robot reader: query odom pose

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

@@ -11,6 +11,8 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/Clock.h>
 
+#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem/core/error.h>
@@ -203,6 +205,31 @@ namespace armarx::armem::robot_state
             .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
     }
 
+    std::optional<robot::RobotState::Pose>
+    RobotReader::queryOdometryPose(const robot::RobotDescription& description,
+                                   const armem::Time& timestamp) const
+    {
+
+        common::robot_state::localization::TransformQuery query
+        {
+            .header = {
+                .parentFrame = OdometryFrame,
+                .frame = "root",
+                .agent = description.name,
+                .timestamp = timestamp
+            }
+        };
+
+        const auto result = transformReader.lookupTransform(query);
+        if (not result)
+        {
+            return std::nullopt;
+        }
+
+        return result.transform.transform;
+    }
+
+
     std::optional<robot::RobotState::JointMap>
     RobotReader::queryJointState(const robot::RobotDescription& description,
                                  const armem::Time& timestamp) const // Why timestamp?!?!

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

@@ -24,9 +24,9 @@
 #include <mutex>
 #include <optional>
 
-#include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 #include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem_robot/client/interfaces.h>
 #include <RobotAPI/libraries/armem_robot/types.h>
 #include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
@@ -99,9 +99,22 @@ namespace armarx::armem::robot_state
                           const armem::Time& start,
                           const armem::Time& end) const;
 
+        /**
+         * @brief retrieve the robot's pose in the odometry frame. 
+         *
+         * This pose is an integration of the robot's platform velocity and undergoes a significant drift.
+         * 
+         * @param description 
+         * @param timestamp 
+         * @return std::optional<robot::RobotState::Pose> 
+         */
+        std::optional<robot::RobotState::Pose>
+        queryOdometryPose(const robot::RobotDescription& description,
+                          const armem::Time& timestamp) const;
+
     protected:
         // by default, no timeout mechanism
-        armem::Duration syncTimeout  = armem::Duration::MicroSeconds(0);
+        armem::Duration syncTimeout = armem::Duration::MicroSeconds(0);
         armem::Duration sleepAfterFailure = armem::Duration::MicroSeconds(0);
 
     private: