Add getGlobalRobotPose to RobotStateMemory

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

@@ -22,6 +22,8 @@
 
 #include "RobotStateMemory.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 +33,7 @@
 #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
 #include <SimoxUtility/algorithm/string.h>
 
 
@@ -182,6 +185,19 @@ 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());
+        
+        bool robotNameFound = !(poseMap.find(robotName) == poseMap.end());
+        ARMARX_CHECK(robotNameFound)
+            << "Robot with name " << robotName << " does not exist. "
+            << "Available robots are: " << simox::alg::get_keys(poseMap);
+
+        return new Pose(poseMap[robotName].matrix());
+    }
+
+
     /*************************************************************/
     // RobotUnit Streaming functions
     /*************************************************************/

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

@@ -39,6 +39,8 @@
 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
 
+#include <RobotAPI/interface/core/RobotLocalization.h>
+
 
 namespace armarx::plugins
 {
@@ -62,7 +64,8 @@ namespace armarx::armem::server::robot_state
     class RobotStateMemory :
         virtual public armarx::Component,
         virtual public armem::server::ReadWritePluginUser,
-        virtual public armarx::ArVizComponentPluginUser
+        virtual public armarx::ArVizComponentPluginUser,
+        virtual public armarx::GlobalRobotPoseProvider
     {
     public:
 
@@ -73,6 +76,10 @@ namespace armarx::armem::server::robot_state
         std::string getDefaultName() const override;
 
 
+        // GlobalRobotPoseProvider interface
+        virtual armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+
+
     protected:
 
         armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;

source/RobotAPI/interface/core/RobotLocalization.ice

@@ -24,10 +24,14 @@
 
 #pragma once
 
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
 #include <RobotAPI/interface/core/GeometryBase.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
 
 module armarx{
-
+    interface GlobalRobotPoseProvider extends armem::server::MemoryInterface {
+        PoseBase getGlobalRobotPose(long timestamp, string robotName);
+    }
 
     interface GlobalRobotPoseLocalizationListener{
         void reportGlobalRobotPose(TransformStamped currentPose);

source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp

@@ -12,6 +12,7 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/core/PackagePath.h>
 
 #include <RobotAPI/libraries/armem/core/Time.h>
 
@@ -79,7 +80,7 @@ namespace armarx::armem::server::robot_state
     {
         for (const robot::Robot& robot : robots)
         {
-            const data::PackagePath xmlPath = robot.description.xml.serialize();
+            const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
 
             // clang-format off
             viz::Robot robotVisu = viz::Robot(robot.description.name)