armem_robot_state: TransformHelper: setting timestamp properly

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

@@ -6,6 +6,9 @@
 
 #include <ArmarXCore/core/exceptions/LocalException.h>
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
 
 #include <RobotAPI/libraries/core/FramedPose.h>
 
@@ -41,6 +44,14 @@ namespace armarx::armem::common::robot_state::localization
 
         const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
                     localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+
+        const armem::Time sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+        auto header = query.header;
+
+        ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
+
+        header.timestamp = sanitizedTimestamp;
+                
         if (transforms.empty())
         {
             ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
@@ -58,7 +69,7 @@ namespace armarx::armem::common::robot_state::localization
 
         ARMARX_DEBUG << "Found valid transform";
 
-        return {.transform = {.header = query.header, .transform = transform},
+        return {.transform = {.header = header, .transform = transform},
                 .status    = TransformResult::Status::Success};
     }
 
@@ -187,6 +198,30 @@ namespace armarx::armem::common::robot_state::localization
         return chain;
     }
 
+    template <class ...Args>
+    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;
+
+        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());
+        });
+
+        // then we ensure that the timestamp is not more recent than the query timestamp
+        timeSinceEpochUs = std::min(timeSinceEpochUs, timestamp.toMicroSecondsSinceEpoch());
+
+        return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs));
+    }
+
 
     template <class ...Args>
     std::vector<Eigen::Affine3f>

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

@@ -111,6 +111,10 @@ namespace armarx::armem::common::robot_state::localization
             const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
             const armem::Time& timestamp);
 
+        template <class ...Args>
+        static
+        armarx::core::time::DateTime
+        _obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp);
 
         static
         Eigen::Affine3f