diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp index 7a4305c6fe26ea6bf54aab17e9e87843688aa0a1..beebabd2977e8f7e46e976892ee4da3dc18f2bad 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp +++ b/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?!?! diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h index 6682fcd79130501530d54f770d342d02676fd069..e72b5fc64744a7ad3481990705568e92a8c56208 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h +++ b/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: