local changes on A-6 demo account

source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp

@@ -49,10 +49,20 @@ namespace armarx::armem::common::robot_state::localization
                     localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
 
         const armem::Time sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+
+        if (sanitizedTimestamp.isInvalid())
+        {
+            ARMARX_WARNING << deactivateSpam(1) << "No timestamp available.";
+            return {.transform    = {.header = query.header},
+                    .status       = TransformResult::Status::ErrorFrameNotAvailable,
+                    .errorMessage = "Error in TF loookup:  '" + query.header.parentFrame +
+                                    " -> " + query.header.frame +
+                                    "'. No memory data in time range."};
+        }
+
         auto header = query.header;
 
         // ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
-
         header.timestamp = sanitizedTimestamp;
                 
         if (transforms.empty())
@@ -181,24 +191,42 @@ namespace armarx::armem::common::robot_state::localization
     armarx::core::time::DateTime
     TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
     {
-        
         // first we check which the newest timestamp is
-        int64_t timeSinceEpochUs = 0;
+        std::optional<int64_t> timeSinceEpochUs = std::nullopt;
 
-        localizationCoreSegment.forEachEntity([&timeSinceEpochUs, &timestamp](const auto& entity){
+        localizationCoreSegment.forEachEntity([&timeSinceEpochUs, &timestamp](const auto& entity)
+        {
             auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
-            const armem::wm::EntityInstance& item = snapshot->getInstance(0);
-            const auto tf = _convertEntityToTransform(item);
-            
-            const auto& dataTs = tf.header.timestamp;
-
-            timeSinceEpochUs = std::max(timeSinceEpochUs, dataTs.toMicroSecondsSinceEpoch());
+            if (snapshot)
+            {
+                // This caused a segfault when the snapshot does not exist.
+                const armem::wm::EntityInstance& item = snapshot->getInstance(0);
+                const auto tf = _convertEntityToTransform(item);
+
+                const auto& dataTs = tf.header.timestamp;
+
+                if (timeSinceEpochUs.has_value())
+                {
+                    timeSinceEpochUs = std::max(timeSinceEpochUs.value(), dataTs.toMicroSecondsSinceEpoch());
+                }
+                else
+                {
+                    timeSinceEpochUs = dataTs.toMicroSecondsSinceEpoch();
+                }
+            }
         });
 
-        // then we ensure that the timestamp is not more recent than the query timestamp
-        timeSinceEpochUs = std::min(timeSinceEpochUs, timestamp.toMicroSecondsSinceEpoch());
+        if (timeSinceEpochUs.has_value())
+        {
+            // then we ensure that the timestamp is not more recent than the query timestamp
+            timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
 
-        return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs));
+            return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
+        }
+        else
+        {
+            return DateTime::Invalid();
+        }
     }