From fc638b8a7284f232c6ba42ca9657c94d1e0417cd Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 28 Mar 2024 11:39:15 +0100
Subject: [PATCH] simplifying robot state and virtual robot reader: only robot
name is required as input parameter instead of robot description
---
.../client/common/RobotReader.cpp | 80 +++++++++----------
.../client/common/RobotReader.h | 31 +++----
.../client/common/VirtualRobotReader.cpp | 33 +-------
.../client/common/VirtualRobotReader.h | 6 --
4 files changed, 54 insertions(+), 96 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 9827cf6b7..c7dc200af 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -105,23 +105,23 @@ namespace armarx::armem::robot_state::client::common
}
bool
- RobotReader::synchronize(Robot& obj, const armem::Time& timestamp) const
+ RobotReader::synchronize(Robot& robot, const armem::Time& timestamp) const
{
const auto tsStartFunctionInvokation = armem::Time::Now();
while (true)
{
- auto state = queryState(obj.description, timestamp);
+ auto state = queryState(robot.description.name, timestamp);
if (not state) /* c++20 [[unlikely]] */
{
- ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
// if the syncTime is non-positive there will be no retry
const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
if (elapsedTime > syncTimeout)
{
- ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
return false;
}
@@ -129,7 +129,7 @@ namespace armarx::armem::robot_state::client::common
Clock::WaitFor(sleepAfterFailure);
}
- obj.config = std::move(*state);
+ robot.config = std::move(*state);
return true;
}
}
@@ -183,17 +183,17 @@ namespace armarx::armem::robot_state::client::common
}
std::optional<RobotState>
- RobotReader::queryState(const description::RobotDescription& description,
+ RobotReader::queryState(const std::string& robotName,
const armem::Time& timestamp) const
{
- std::optional<RobotState> robotState = queryJointState(description, timestamp);
+ std::optional<RobotState> robotState = queryJointState(robotName, timestamp);
if (robotState)
{
- const auto globalPose = queryGlobalPose(description, timestamp);
+ const auto globalPose = queryGlobalPose(robotName, timestamp);
if (not globalPose)
{
- ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
+ ARMARX_VERBOSE << "Failed to query global pose for robot " << robotName;
return std::nullopt;
}
robotState->globalPose = *globalPose;
@@ -203,14 +203,14 @@ namespace armarx::armem::robot_state::client::common
}
std::optional<RobotState>
- RobotReader::queryJointState(const description::RobotDescription& description,
+ RobotReader::queryJointState(const std::string& robotName,
const armem::Time& timestamp) const
{
- const auto proprioception = queryProprioception(description, timestamp);
+ const auto proprioception = queryProprioception(robotName, timestamp);
if (not proprioception.has_value())
{
- ARMARX_VERBOSE << "Failed to query proprioception for robot '" << description.name
+ ARMARX_VERBOSE << "Failed to query proprioception for robot '" << robotName
<< "'.";
return std::nullopt;
}
@@ -223,14 +223,14 @@ namespace armarx::armem::robot_state::client::common
}
std::optional<::armarx::armem::robot_state::localization::Transform>
- RobotReader::queryOdometryPose(const description::RobotDescription& description,
+ RobotReader::queryOdometryPose(const std::string& robotName,
const armem::Time& timestamp) const
{
robot_state::localization::TransformQuery query{
.header = {.parentFrame = OdometryFrame,
.frame = constants::robotRootNodeName,
- .agent = description.name,
+ .agent = robotName,
.timestamp = timestamp}};
// try
@@ -250,7 +250,7 @@ namespace armarx::armem::robot_state::client::common
}
std::optional<armarx::armem::arondto::Proprioception>
- RobotReader::queryProprioception(const description::RobotDescription& description,
+ RobotReader::queryProprioception(const std::string& robotName,
const armem::Time& timestamp) const // Why timestamp?!?!
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -258,12 +258,12 @@ namespace armarx::armem::robot_state::client::common
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying robot description for robot: " << description;
+ ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -281,7 +281,7 @@ namespace armarx::armem::robot_state::client::common
return std::nullopt;
}
- return getRobotProprioception(qResult.memory, description.name);
+ return getRobotProprioception(qResult.memory, robotName);
}
catch (...)
{
@@ -292,19 +292,19 @@ namespace armarx::armem::robot_state::client::common
}
RobotReader::JointTrajectory
- RobotReader::queryJointStates(const description::RobotDescription& description,
+ RobotReader::queryJointStates(const std::string& robotName,
const armem::Time& begin,
const armem::Time& end) const
{
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+ ARMARX_DEBUG << "Querying robot joint states for robot: `" << robotName
<< "` on time interval [" << begin << "," << end << "]";
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().timeRange(begin, end);
// clang-format on
@@ -322,7 +322,7 @@ namespace armarx::armem::robot_state::client::common
return {};
}
- return getRobotJointStates(qResult.memory, description.name);
+ return getRobotJointStates(qResult.memory, robotName);
}
catch (...)
{
@@ -333,7 +333,7 @@ namespace armarx::armem::robot_state::client::common
}
std::optional<PlatformState>
- RobotReader::queryPlatformState(const description::RobotDescription& description,
+ RobotReader::queryPlatformState(const std::string& robotName,
const armem::Time& timestamp) const
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -341,12 +341,12 @@ namespace armarx::armem::robot_state::client::common
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying robot description for robot: " << description;
+ ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -364,7 +364,7 @@ namespace armarx::armem::robot_state::client::common
return std::nullopt;
}
- return getRobotPlatformState(qResult.memory, description.name);
+ return getRobotPlatformState(qResult.memory, robotName);
}
catch (...)
{
@@ -375,13 +375,13 @@ namespace armarx::armem::robot_state::client::common
}
std::optional<RobotState::Pose>
- RobotReader::queryGlobalPose(const description::RobotDescription& description,
+ RobotReader::queryGlobalPose(const std::string& robotName,
const armem::Time& timestamp) const
{
try
{
const auto result = transformReader.getGlobalPose(
- description.name, constants::robotRootNodeName, timestamp);
+ robotName, constants::robotRootNodeName, timestamp);
if (not result)
{
return std::nullopt;
@@ -521,19 +521,19 @@ namespace armarx::armem::robot_state::client::common
// force torque for left and right
std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
- RobotReader::queryForceTorque(const description::RobotDescription& description,
+ RobotReader::queryForceTorque(const std::string& robotName,
const armem::Time& timestamp) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+ ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -551,7 +551,7 @@ namespace armarx::armem::robot_state::client::common
return std::nullopt;
}
- return getForceTorque(qResult.memory, description.name);
+ return getForceTorque(qResult.memory, robotName);
}
catch (...)
{
@@ -563,7 +563,7 @@ namespace armarx::armem::robot_state::client::common
// force torque for left and right
std::optional<std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
- RobotReader::queryForceTorques(const description::RobotDescription& description,
+ RobotReader::queryForceTorques(const std::string& robotName,
const armem::Time& start,
const armem::Time& end) const
{
@@ -571,12 +571,12 @@ namespace armarx::armem::robot_state::client::common
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+ ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::proprioceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().timeRange(start, end);
// clang-format on
@@ -594,7 +594,7 @@ namespace armarx::armem::robot_state::client::common
return std::nullopt;
}
- return getForceTorques(qResult.memory, description.name);
+ return getForceTorques(qResult.memory, robotName);
}
catch (...)
{
@@ -605,18 +605,18 @@ namespace armarx::armem::robot_state::client::common
}
std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
- RobotReader::queryToF(const description::RobotDescription& description,
+ RobotReader::queryToF(const std::string& robotName,
const armem::Time& timestamp) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
- ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+ ARMARX_DEBUG << "Querying ToF data for robot: " << robotName;
// clang-format off
qb
.coreSegments().withName(constants::exteroceptionCoreSegment)
- .providerSegments().withName(description.name) // agent
+ .providerSegments().withName(robotName) // agent
.entities().all() // TODO
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
@@ -634,7 +634,7 @@ namespace armarx::armem::robot_state::client::common
return std::nullopt;
}
- return getToF(qResult.memory, description.name);
+ return getToF(qResult.memory, robotName);
}
catch (...)
{
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 833783371..30032e0c9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -64,29 +64,26 @@ namespace armarx::armem::robot_state::client::common
std::vector<description::RobotDescription>
queryDescriptions(const armem::Time& timestamp) const;
- std::optional<RobotState> queryState(const description::RobotDescription& description,
+ std::optional<RobotState> queryState(const std::string& robotName,
const armem::Time& timestamp) const;
- std::optional<RobotState> queryJointState(const description::RobotDescription& description,
+ std::optional<RobotState> queryJointState(const std::string& robotName,
const armem::Time& timestamp) const;
std::optional<armarx::armem::arondto::Proprioception>
- queryProprioception(const description::RobotDescription& description,
- const armem::Time& timestamp) const;
+ queryProprioception(const std::string& robotName, const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, RobotState::JointMap>;
- JointTrajectory queryJointStates(const description::RobotDescription& description,
+ JointTrajectory queryJointStates(const std::string& robotName,
const armem::Time& begin,
const armem::Time& end) const;
- std::optional<RobotState::Pose>
- queryGlobalPose(const description::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<RobotState::Pose> queryGlobalPose(const std::string& robotName,
+ const armem::Time& timestamp) const;
- std::optional<PlatformState>
- queryPlatformState(const description::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<PlatformState> queryPlatformState(const std::string& robotName,
+ const armem::Time& timestamp) const;
void setSyncTimeout(const armem::Duration& duration);
void setSleepAfterSyncFailure(const armem::Duration& duration);
@@ -98,19 +95,17 @@ namespace armarx::armem::robot_state::client::common
};
std::optional<std::map<Hand, proprioception::ForceTorque>>
- queryForceTorque(const description::RobotDescription& description,
- const armem::Time& timestamp) const;
+ queryForceTorque(const std::string& robotName, const armem::Time& timestamp) const;
std::optional<
std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
- queryForceTorques(const description::RobotDescription& description,
+ queryForceTorques(const std::string& robotName,
const armem::Time& start,
const armem::Time& end) const;
std::optional<std::map<Hand, exteroception::ToF>>
- queryToF(const description::RobotDescription& description,
- const armem::Time& timestamp) const;
+ queryToF(const std::string& robotName, const armem::Time& timestamp) const;
/**
* @brief retrieve the robot's pose in the odometry frame.
@@ -122,8 +117,7 @@ namespace armarx::armem::robot_state::client::common
* @return std::optional<RobotState::Pose>
*/
std::optional<::armarx::armem::robot_state::localization::Transform>
- queryOdometryPose(const description::RobotDescription& description,
- const armem::Time& timestamp) const;
+ queryOdometryPose(const std::string& robotName, const armem::Time& timestamp) const;
protected:
// by default, no timeout mechanism
@@ -174,7 +168,6 @@ namespace armarx::armem::robot_state::client::common
} // namespace armarx::armem::robot_state::client::common
-
namespace armarx::armem::robot_state
{
using client::common::RobotReader;
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 e7a7838a6..b57642a1e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -15,24 +15,12 @@
namespace armarx::armem::robot_state::client::common
{
- // TODO(fabian.reister): register property defs
- void
- VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
- {
- RobotReader::registerPropertyDefinitions(def);
- }
bool
VirtualRobotReader::synchronizeRobot(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 description::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{"", ""}};
-
- const auto robotState = queryState(robotDescription, timestamp);
+ const auto robotState = queryState(robot.getName(), timestamp);
if (not robotState)
{
ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
@@ -51,13 +39,7 @@ namespace armarx::armem::robot_state::client::common
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 description::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{"", ""}};
-
- const auto robotState = queryJointState(robotDescription, timestamp);
+ const auto robotState = queryJointState(robot.getName(), timestamp);
if (not robotState)
{
ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
@@ -145,16 +127,5 @@ namespace armarx::armem::robot_state::client::common
return nullptr;
}
- std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
- VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
- {
- const auto description = queryDescription(name, timestamp);
- if (not description.has_value())
- {
- return std::nullopt;
- }
- return RobotReader::queryForceTorque(description.value(), timestamp);
- }
-
} // namespace armarx::armem::robot_state::client::common
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 e0142d356..f4f7f2f44 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -44,8 +44,6 @@ namespace armarx::armem::robot_state::client::common
~VirtualRobotReader() override = default;
- void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
-
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
const armem::Time& timestamp) const;
@@ -71,10 +69,6 @@ namespace armarx::armem::robot_state::client::common
VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
- std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
- queryForceTorque(const std::string& name, const armem::Time& timestamp);
- using RobotReader::queryForceTorque;
-
private:
[[nodiscard]] VirtualRobot::RobotPtr
_getSynchronizedRobot(const std::string& name,
--
GitLab