Added synchronize robot joints to robot reader to be independent of localization

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -185,6 +185,26 @@ namespace armarx::armem::robot_state
     std::optional<robot::RobotState>
     RobotReader::queryState(const robot::RobotDescription& description,
                             const armem::Time& timestamp) const
+    {
+        std::optional<robot::RobotState> robotState = queryJointState(description, timestamp);
+
+        if (robotState)
+        {
+            const auto globalPose = queryGlobalPose(description, timestamp);
+            if (not globalPose)
+            {
+                ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
+                return std::nullopt;
+            }
+            robotState->globalPose = *globalPose;
+        }
+
+        return robotState;
+    }
+
+    std::optional<robot::RobotState>
+    RobotReader::queryJointState(const robot::RobotDescription& description,
+                                 const armem::Time& timestamp) const
     {
         const auto proprioception = queryProprioception(description, timestamp);
 
@@ -196,15 +216,8 @@ namespace armarx::armem::robot_state
         }
         const auto jointMap = proprioception->joints.position;
 
-        const auto globalPose = queryGlobalPose(description, timestamp);
-        if (not globalPose)
-        {
-            ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
-            return std::nullopt;
-        }
-
         return robot::RobotState{.timestamp = timestamp,
-                                 .globalPose = *globalPose,
+                                 .globalPose = robot::RobotState::Pose::Identity(),
                                  .jointMap = jointMap,
                                  .proprioception = proprioception};
     }

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h

@@ -67,6 +67,9 @@ namespace armarx::armem::robot_state
         std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
                                                     const armem::Time& timestamp) const;
 
+        std::optional<robot::RobotState> queryJointState(const robot::RobotDescription& description,
+                                                         const armem::Time& timestamp) const;
+
         std::optional<armarx::armem::arondto::Proprioception>
         queryProprioception(const robot::RobotDescription& description,
                             const armem::Time& timestamp) const;

source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp

@@ -47,6 +47,30 @@ namespace armarx::armem::robot_state
         return true;
     }
 
+    bool
+    VirtualRobotReader::synchronizeRobotJoints(VirtualRobot::Robot& robot,
+                                               const armem::Time& timestamp) const
+    {
+        // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+        // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
+
+        const robot::RobotDescription robotDescription{.name = robot.getName(),
+                                                       .xml = PackagePath{"", ""}};
+
+        const auto robotState = queryJointState(robotDescription, timestamp);
+        if (not robotState)
+        {
+            ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
+                           << robot.getName() << "` "
+                           << "(type `" << robot.getType() << "`)!";
+            return false;
+        }
+
+        robot.setJointValues(robotState->jointMap);
+
+        return true;
+    }
+
     VirtualRobot::RobotPtr
     VirtualRobotReader::getRobot(const std::string& name,
                                  const armem::Time& timestamp,

source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h

@@ -49,6 +49,9 @@ namespace armarx::armem::robot_state
         [[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
                                             const armem::Time& timestamp) const;
 
+        [[nodiscard]] bool synchronizeRobotJoints(VirtualRobot::Robot& robot,
+                                                  const armem::Time& timestamp) const;
+
         [[nodiscard]] VirtualRobot::RobotPtr
         getRobot(const std::string& name,
                  const armem::Time& timestamp = armem::Time::Invalid(),