Use correct timestamp in getGlobalRobotPose

source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp

@@ -21,6 +21,7 @@
  */
 
 #include "RobotStateMemory.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
 
 #include <RobotAPI/interface/core/PoseBase.h>
 #include <RobotAPI/libraries/core/Pose.h>
@@ -33,7 +34,10 @@
 #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include <IceUtil/Time.h>
+
 #include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/contains.h>
 #include <SimoxUtility/algorithm/string.h>
 
 
@@ -187,9 +191,11 @@ namespace armarx::armem::server::robot_state
 
     armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current& /*unused*/)
     {
-        auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::now());
+        auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::microSeconds(timestamp));
         
-        bool robotNameFound = !(poseMap.find(robotName) == poseMap.end());
+        ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << armem::Time::microSeconds(timestamp);
+
+        bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
         ARMARX_CHECK(robotNameFound)
             << "Robot with name " << robotName << " does not exist. "
             << "Available robots are: " << simox::alg::get_keys(poseMap);

source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h

@@ -77,7 +77,7 @@ namespace armarx::armem::server::robot_state
 
 
         // GlobalRobotPoseProvider interface
-        virtual armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+        armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
 
 
     protected:

source/RobotAPI/interface/core/RobotLocalization.ice

@@ -30,6 +30,7 @@
 
 module armarx{
     interface GlobalRobotPoseProvider extends armem::server::MemoryInterface {
+        // timestamp in microseconds
         PoseBase getGlobalRobotPose(long timestamp, string robotName);
     }