Add method to get global robot pose to RobotStateMemory

Tested it with Python; it works and throws a useful exception if invalid robot name is given ;-)

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

@@ -21,7 +21,10 @@
  */
 
 #include "RobotStateMemory.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
 
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/Pose.h>
 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
 
 #include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
@@ -31,6 +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>
 
 
@@ -182,6 +189,22 @@ namespace armarx::armem::server::robot_state
     }
 
 
+    armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp_us, const std::string& robotName, const ::Ice::Current& /*unused*/)
+    {
+        auto timestamp = armem::Time::microSeconds(timestamp_us);
+        ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << timestamp << ".";
+
+        auto poseMap = localizationSegment.getRobotGlobalPoses(timestamp);
+        
+        bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
+        ARMARX_CHECK(robotNameFound)
+            << "Robot with name " << robotName << " does not exist at or before timestamp " << timestamp << ".\n"
+            << "Available robots are: " << simox::alg::get_keys(poseMap);
+
+        return new Pose(poseMap[robotName].matrix());
+    }
+
+
     /*************************************************************/
     // RobotUnit Streaming functions
     /*************************************************************/