Feature/synchronize-robot-joints

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};
     }