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 8f7c057e4dbd935d5574e6ee4914280611c7808a..f3b4731b0ab07e0fb93ce563ae6729163fd2ac05 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp +++ b/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}; } 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 8397ea9a8d8b8de46a7c2b1ec1167396d8383d06..e3ff0308939a3a6e6b7858c7e8b1cdfb198230b8 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h +++ b/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; diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp index 0effa5c23d6763ff984610100b1770024e452bee..e23178dfa4a884654b8ba924a71006dbd88e8e8b 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp +++ b/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, diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h index 1d91f05829b1c49fc5a07782ecbead486e075a5c..d0a672c21ba5ae953d3bc3b8bed2333a9f6efa97 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h +++ b/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(),