From 47606efb9c918b25a180073c459b77465dd6a75e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 16 Jul 2024 23:52:37 +0200
Subject: [PATCH] Extending RobotDescription in RobotStateMemory
---
.../libraries/ArmarXObjects/ObjectFinder.cpp | 4 +-
.../ArticulatedObjectWriter.cpp | 4 +-
.../aron/RobotDescription.xml | 104 +++++++++
.../armem_robot_state/aron_conversions.cpp | 4 +
.../client/common/VirtualRobotWriter.cpp | 12 +-
.../armem_robot_state/robot_conversions.cpp | 4 +-
.../server/description/Segment.cpp | 207 +++++++++++++-----
.../libraries/armem_robot_state/types.h | 4 +
8 files changed, 282 insertions(+), 61 deletions(-)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 8b61c578c..1167db200 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -285,7 +285,9 @@ namespace armarx
objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
{
.name = object.idStr(),
- .xml = {modelFile->package, modelFile->relativePath}
+ .xml = {modelFile->package, modelFile->relativePath},
+ .visualization = {},
+ .info = {}
// .dataset = dataset
});
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index d4cf12fee..f293741f9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -38,7 +38,9 @@ namespace armarx::armem::articulated_object
.description = {.name = obj.getType(),
.xml = PackagePath(
armarx::ArmarXDataPath::getProject({package}, fileRelPath),
- obj.getFilename())},
+ obj.getFilename()),
+ .visualization = {},
+ .info = {}},
.instance = obj.getName(),
.config = {.timestamp = timestamp,
.globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
index 325075db3..aae0bba8e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
@@ -7,6 +7,102 @@
<GenerateTypes>
+ <Object name='armarx::armem::arondto::RobotInfoElement'>
+
+ <ObjectChild key='kinematic_chain'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='torso_kinematic_chain'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='tcp'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='force_torque_sensor'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='force_torque_sensor_frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='end_effector'>
+ <string />
+ </ObjectChild>
+ <!-- Legacy old memory -->
+ <!-- <ObjectChild key='memory_hand_name'>
+ <string />
+ </ObjectChild> -->
+ <ObjectChild key='hand_controller_name'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='hand_root_node'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='hand_model'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+ <ObjectChild key='palm_collision_model'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='full_hand_collision_model'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+
+ </Object>
+
+ <Object name='armarx::armem::arondto::RobotInfo'>
+ <ObjectChild key="parts">
+ <Dict>
+ <armarx::armem::arondto::RobotInfoElement />
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::armem::arondto::BodyPartDescription'>
+ <ObjectChild key='xml'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key='root_node_name'>
+ <string/>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::armem::arondto::RobotDescriptionVisualization'>
+
+ <ObjectChild key="body_parts">
+ <Dict>
+ <armarx::armem::arondto::BodyPartDescription />
+ </Dict>
+ </ObjectChild>
+
+
+ <!-- <ObjectChild key='right_hand_closed_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='left_hand_closed_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='right_hand_opened_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='left_hand_opened_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild> -->
+
+
+ </Object>
+
+
<Object name='armarx::armem::arondto::RobotDescription'>
<ObjectChild key='name'>
@@ -17,6 +113,14 @@
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key='visualization'>
+ <armarx::armem::arondto::RobotDescriptionVisualization />
+ </ObjectChild>
+
+ <ObjectChild key='info'>
+ <armarx::armem::arondto::RobotInfo />
+ </ObjectChild>
+
<!-- <ObjectChild key='scaling'>
<float32 />
</ObjectChild> -->
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index e00b27c3d..9d03afe51 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -115,6 +115,8 @@ namespace armarx::armem::robot_state::description
{
aron::fromAron(dto.name, bo.name);
fromAron(dto.xml, bo.xml);
+ aron::fromAron(dto.visualization, bo.visualization);
+ aron::fromAron(dto.info, bo.info);
}
void
@@ -122,6 +124,8 @@ namespace armarx::armem::robot_state::description
{
aron::toAron(dto.name, bo.name);
toAron(dto.xml, bo.xml);
+ aron::toAron(dto.visualization, bo.visualization);
+ aron::toAron(dto.info, bo.info);
}
} // namespace armarx::armem::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index 48dc75da8..68d02546e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -50,8 +50,11 @@ namespace armarx::armem::robot_state
{
const auto filename = robot.getFilename();
+ // FIXME maybe visu and info optional?
const description::RobotDescription desc{.name = robot.getType(),
- .xml = resolvePackagePath(filename)};
+ .xml = resolvePackagePath(filename),
+ .visualization = {},
+ .info = {}};
return RobotWriter::storeDescription(desc, timestamp);
}
@@ -60,10 +63,9 @@ namespace armarx::armem::robot_state
VirtualRobotWriter::storeState(const VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
const RobotState robotState{.timestamp = timestamp,
- .globalPose =
- RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues(),
- .proprioception = std::nullopt};
+ .globalPose = RobotState::Pose(robot.getGlobalPose()),
+ .jointMap = robot.getJointValues(),
+ .proprioception = std::nullopt};
return RobotWriter::storeState(
robotState,
diff --git a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
index 22790b52c..042315b10 100644
--- a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
@@ -32,7 +32,9 @@ namespace armarx::armem::robot_state
description::RobotDescription robotDescription{
.name = "",
- .xml = ::armarx::PackagePath("", fs::path("")) // initialize empty, no default c'tor
+ .xml = ::armarx::PackagePath("", fs::path("")), // initialize empty, no default c'tor
+ .visualization = {},
+ .info = {}
};
fromAron(aronRobotDescription, robotDescription);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 13587664c..0fdb753ba 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -1,43 +1,45 @@
#include "Segment.h"
+
#include <filesystem>
+
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/armem_robot_state/types.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
-#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::server::robot_state::description
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, armem::robot_state::descriptionSegmentID.coreSegmentName, arondto::RobotDescription::ToAronType())
+ Base(memoryToIceAdapter,
+ armem::robot_state::descriptionSegmentID.coreSegmentName,
+ arondto::RobotDescription::ToAronType())
{
}
Segment::~Segment() = default;
-
- void Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
+ void
+ Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
{
robotUnit = robotUnitPrx;
@@ -45,15 +47,17 @@ namespace armarx::armem::server::robot_state::description
updateRobotDescription();
}
-
- void Segment::commitRobotDescription(const armem::robot_state::description::RobotDescription& robotDescription)
+ void
+ Segment::commitRobotDescription(
+ const armem::robot_state::description::RobotDescription& robotDescription)
{
const Time now = Time::Now();
const MemoryID providerID = segmentPtr->id().withProviderSegmentName(robotDescription.name);
if (not segmentPtr->hasProviderSegment(providerID.providerSegmentName))
{
- segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::ToAronType());
+ segmentPtr->addProviderSegment(providerID.providerSegmentName,
+ arondto::RobotDescription::ToAronType());
}
@@ -63,18 +67,40 @@ namespace armarx::armem::server::robot_state::description
arondto::RobotDescription dto;
toAron(dto, robotDescription);
- update.instancesData =
- {
- dto.toAron()
- };
+ update.instancesData = {dto.toAron()};
Commit commit;
commit.updates.push_back(update);
iceMemory.commitLocking(commit);
}
+ // Helper function to handle exceptions
+ // template <typename T>
+ // std::optional<T> tryGet(const std::function<T()>& func) {
+ // try {
+ // return func();
+ // } catch (const std::exception& ex) {
+ // // Handle exception or log error
+ // std::cerr << "Error: " << ex.what() << std::endl;
+ // return std::nullopt; // Return an empty optional if an exception occurs
+ // }
+ // }
+
+ std::string
+ tryGet(const std::function<std::string()>& func)
+ {
+ try
+ {
+ return func();
+ }
+ catch (const std::exception& ex)
+ {
+ return ""; // Return an empty string if an exception occurs
+ }
+ }
- void Segment::updateRobotDescription()
+ void
+ Segment::updateRobotDescription()
{
ARMARX_CHECK_NOT_NULL(robotUnit);
KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
@@ -83,7 +109,8 @@ namespace armarx::armem::server::robot_state::description
const std::string robotName = kinematicUnit->getRobotName();
const std::string robotFilename = kinematicUnit->getRobotFilename();
- const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::vector<std::string> packages =
+ armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
const std::string package = simox::alg::split(robotFilename, "/").front();
@@ -93,25 +120,99 @@ namespace armarx::armem::server::robot_state::description
ARMARX_INFO << VAROUT(robotFilename);
// make sure that the relative path is without the 'package/' prefix
- const std::string robotFileRelPath = [&robotFilename, &package]()-> std::string {
+ const auto fixPath = [](armarx::PackagePath& pp)
+ {
+ auto spp = pp.serialize();
+ if (simox::alg::starts_with(spp.path, spp.package))
+ {
+ // remove "package" + "/"
+ const std::string fixedPath = spp.path.substr(spp.package.size() + 1, -1);
- if(simox::alg::starts_with(robotFilename, package))
+ spp.path = fixedPath;
+
+ pp = armarx::PackagePath{spp};
+ }
+ };
+
+ armarx::PackagePath robotPath{package, robotFilename};
+ fixPath(robotPath);
+
+ const auto fixPathDto = [](armarx::arondto::PackagePath& pp)
+ {
+ if (simox::alg::starts_with(pp.path, pp.package))
{
// remove "package" + "/"
- const std::string fixedRobotFilename = robotFilename.substr(package.size() + 1, -1);
- return fixedRobotFilename;
+ const std::string fixedPath = pp.path.substr(pp.package.size() + 1, -1);
+
+ pp.path = fixedPath;
}
-
- return robotFilename;
- }();
+ };
- ARMARX_INFO << "Robot description '" << VAROUT(robotFileRelPath) << "' found in package " << package;
+ ARMARX_INFO << "Robot description '" << robotPath << "'";
- const armem::robot_state::description::RobotDescription robotDescription
+ auto robotNameHelper = RobotNameHelper::Create(robotPath.toSystemPath());
+
+ arondto::RobotInfo info;
{
+ for (const auto& side :
+ {RobotNameHelper::LocationLeft, RobotNameHelper::LocationRight})
+ {
+ const auto arm = robotNameHelper->getArm(side);
+
+ arondto::RobotInfoElement e;
+ e.end_effector = tryGet([&]() { return arm.getEndEffector(); });
+ e.force_torque_sensor = tryGet([&]() { return arm.getForceTorqueSensor(); });
+ e.force_torque_sensor_frame =
+ tryGet([&]() { return arm.getForceTorqueSensorFrame(); });
+
+ e.full_hand_collision_model.package =
+ tryGet([&]() { return arm.getHandModelPackage(); });
+ e.full_hand_collision_model.path =
+ tryGet([&]() { return arm.getFullHandCollisionModel(); });
+ fixPathDto(e.full_hand_collision_model);
+
+
+ e.hand_controller_name = tryGet([&]() { return arm.getHandControllerName(); });
+
+ e.hand_model.package = tryGet([&]() { return arm.getHandModelPackage(); });
+ e.hand_model.path = tryGet([&]() { return arm.getHandModelPath(); });
+ fixPathDto(e.hand_model);
+
+ e.hand_root_node = tryGet([&]() { return arm.getHandRootNode(); });
+ e.kinematic_chain = tryGet([&]() { return arm.getKinematicChain(); });
+
+ e.palm_collision_model = tryGet([&]() { return arm.getPalmCollisionModel(); });
+
+ e.tcp = tryGet([&]() { return arm.getTCP(); });
+ e.torso_kinematic_chain =
+ tryGet([&]() { return arm.getTorsoKinematicChain(); });
+
+ info.parts.emplace(side + "Arm", e);
+ }
+ }
+
+ arondto::RobotDescriptionVisualization visualization;
+ {
+ for (const auto& side :
+ {RobotNameHelper::LocationLeft, RobotNameHelper::LocationRight})
+ {
+ const auto arm = robotNameHelper->getArm(side);
+
+ armarx::armem::arondto::BodyPartDescription bpd;
+ bpd.root_node_name = tryGet([&]() { return arm.getHandRootNode(); });
+ bpd.xml.package = tryGet([&]() { return arm.getHandModelPackage(); });
+ bpd.xml.path = tryGet([&]() { return arm.getHandModelPath(); });
+ fixPathDto(bpd.xml);
+
+ visualization.body_parts.emplace(side + "Hand", bpd);
+ }
+ }
+
+ const armem::robot_state::description::RobotDescription robotDescription{
.name = kinematicUnit->getRobotName(),
- .xml = {package, robotFileRelPath}
- };
+ .xml = {robotPath.serialize().package, robotPath.serialize().path},
+ .visualization = visualization,
+ .info = info};
// make sure that the package path is valid
ARMARX_CHECK(std::filesystem::exists(robotDescription.xml.toSystemPath()));
@@ -120,45 +221,45 @@ namespace armarx::armem::server::robot_state::description
}
else
{
- ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name
+ << "' does not have a kinematic unit."
<< "\n Cannot commit robot description.";
}
}
-
RobotDescriptionMap
Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
{
- return doLocked([this, ×tamp]()
- {
- return getRobotDescriptions(timestamp);
- });
+ return doLocked([this, ×tamp]() { return getRobotDescriptions(timestamp); });
}
-
RobotDescriptionMap
Segment::getRobotDescriptions(const armem::Time& timestamp) const
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
- (void) timestamp; // Unused
+ (void)timestamp; // Unused
RobotDescriptionMap robotDescriptions;
- segmentPtr->forEachEntity([this, &robotDescriptions](const wm::Entity & entity)
- {
- const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = ::armarx::armem::robot_state::convertRobotDescription(entityInstance);
- if (description)
- {
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
- robotDescriptions.emplace(description->name, *description);
- }
- else
+ segmentPtr->forEachEntity(
+ [this, &robotDescriptions](const wm::Entity& entity)
{
- ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- }
- });
+ const wm::EntityInstance& entityInstance =
+ entity.getLatestSnapshot().getInstance(0);
+ const auto description =
+ ::armarx::armem::robot_state::convertRobotDescription(entityInstance);
+ if (description)
+ {
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ robotDescriptions.emplace(description->name, *description);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ }
+ });
- ARMARX_VERBOSE << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
+ ARMARX_VERBOSE << deactivateSpam(60)
+ << "Number of known robot descriptions: " << robotDescriptions.size();
return robotDescriptions;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index ef1b9c1e9..80d11cc4f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -33,6 +33,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
namespace armarx::armem::robot_state
{
@@ -45,6 +46,9 @@ namespace armarx::armem::robot_state
std::string name;
PackagePath xml{"", std::filesystem::path("")};
+
+ arondto::RobotDescriptionVisualization visualization;
+ arondto::RobotInfo info;
};
using RobotDescriptions = std::vector<RobotDescription>;
--
GitLab