From e1dae5738d60ee185c20b0c03c55c3823f0997c4 Mon Sep 17 00:00:00 2001
From: Meixner <andre.meixner@kit.edu>
Date: Tue, 13 Feb 2024 11:28:20 +0100
Subject: [PATCH] Added synchronize robot joints to robot reader to be
independent of localization
---
.../client/common/RobotReader.cpp | 29 ++++++++++++++-----
.../client/common/RobotReader.h | 3 ++
.../client/common/VirtualRobotReader.cpp | 24 +++++++++++++++
.../client/common/VirtualRobotReader.h | 3 ++
4 files changed, 51 insertions(+), 8 deletions(-)
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 8f7c057e4..f3b4731b0 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 8397ea9a8..e3ff03089 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 0effa5c23..e23178dfa 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 1d91f0582..d0a672c21 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(),
--
GitLab