diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 3816605266f1bc44dce40a752bfe94b14af8177b..0241caff84c93709619c0bcabae2736d443e5c1b 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -172,7 +172,7 @@ namespace armarx
// HandUnitInterface interface
public:
- std::string getHandName(const Ice::Current&) override;
+ std::string getHandName(const Ice::Current& = Ice::emptyCurrent) override;
void setJointForces(const NameValueMap& targetJointForces, const Ice::Current&) override;
void sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&) override;
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 4b61a29ecf41a7af19336ccdc81d30c7dc32189a..f5516353eecf459e4d2cde12db3737b396908972 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -104,6 +104,7 @@ namespace armarx
{
return findObject(id.dataset(), id.className());
}
+
std::optional<ObjectInfo> ObjectFinder::findObject(const objpose::ObjectPose& obj) const
{
return findObject(obj.objectID);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 00c5efd4a4b52bd95743e2427f661108a80891f7..6d20564f660aee832698ca9d1eda297b56de9f3c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -91,6 +91,36 @@ namespace armarx
return file(".xml");
}
+ PackageFileLocation ObjectInfo::urdf() const
+ {
+ return file(".urdf");
+ }
+
+ PackageFileLocation ObjectInfo::sdf() const
+ {
+ return file(".sdf");
+ }
+
+ std::optional<PackageFileLocation> ObjectInfo::getModel() const
+ {
+ if (fs::is_regular_file(simoxXML().absolutePath))
+ {
+ return simoxXML();
+ }
+ else if (fs::is_regular_file(urdf().absolutePath))
+ {
+ return urdf();
+ }
+ else if (fs::is_regular_file(sdf().absolutePath))
+ {
+ return sdf();
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
PackageFileLocation ObjectInfo::articulatedSimoxXML() const
{
return file(".xml", "_articulated", true);
@@ -101,6 +131,11 @@ namespace armarx
return file(".urdf", "_articulated", true);
}
+ PackageFileLocation ObjectInfo::articulatedSdf() const
+ {
+ return file(".sdf", "_articulated");
+ }
+
std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
{
if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
@@ -111,6 +146,10 @@ namespace armarx
{
return articulatedUrdf();
}
+ else if (fs::is_regular_file(articulatedSdf().absolutePath))
+ {
+ return articulatedSdf();
+ }
else
{
return std::nullopt;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index abc1d9dd7ba13e05a578a873427f81668274f136..ccbd4eb1be32116d8fc7ced05d8148f027fefccf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -80,10 +80,15 @@ namespace armarx
PackageFileLocation simoxXML() const;
+ PackageFileLocation urdf() const;
+ PackageFileLocation sdf() const;
+ /// Return the Simox XML, URDF or SDF, if one exists.
+ std::optional<PackageFileLocation> getModel() const;
PackageFileLocation articulatedSimoxXML() const;
PackageFileLocation articulatedUrdf() const;
- /// Return the articulated Simox XML or URDF, if one exists.
+ PackageFileLocation articulatedSdf() const;
+ /// Return the articulated Simox XML, URDF or SDF, if one exists.
std::optional<PackageFileLocation> getArticulatedModel() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index cf42e169e67f0f18aef427c65fd24a73160983bf..446b625e4e91359fadd8dd43b4dacdc460a9d513 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -83,7 +83,7 @@ void armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
j["instanceName"] = rhs.instanceName;
j["collection"] = rhs.collection;
j["position"] = rhs.position;
- j["orientation"] = rhs.orientation;
+ j["orientation"] = rhs.orientation.normalized();
if (rhs.isStatic.has_value())
{
j["isStatic"] = rhs.isStatic.value();
@@ -106,6 +106,7 @@ void armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
j.at("collection").get_to(rhs.collection);
j.at("position").get_to(rhs.position);
j.at("orientation").get_to(rhs.orientation);
+ rhs.orientation.normalize();
if (j.count("isStatic"))
{
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index ec17f944c60747160a3306032fa89c13899d6b86..e7ceabba97898778af1c13d9ad9c805357f79ac2 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -6,11 +6,10 @@
#include <map>
#include <optional>
#include <ostream>
+#include <type_traits>
#include <utility>
#include <vector>
-#include <type_traits>
-
// ICE
#include <IceUtil/Handle.h>
#include <IceUtil/Time.h>
@@ -34,15 +33,13 @@
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
// RobotAPI Armem
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
@@ -62,21 +59,22 @@ namespace armarx::armem::laser_scans::client
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
-
}
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
- << constants::memoryName << "' ...";
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
+ << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName << "'";
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -91,27 +89,43 @@ namespace armarx::armem::laser_scans::client
armarx::armem::client::query::Builder qb;
ARMARX_VERBOSE << "Query for agent: " << query.agent
- << " memory name: " << constants::memoryName;
+ << " memory name: " << constants::memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().all()
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
else
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
return qb;
@@ -126,16 +140,14 @@ namespace armarx::armem::laser_scans::client
ARMARX_WARNING << "No entities!";
}
- const auto convert =
- [](const auto& aronLaserScanStamped,
- const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert = [](const auto& aronLaserScanStamped,
+ const wm::EntityInstance& ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
const auto ndArrayNavigator =
- aron::data::NDArray::DynamicCast(
- ei.data()->getElement("scan"));
+ aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
@@ -145,47 +157,48 @@ namespace armarx::armem::laser_scans::client
};
// loop over all entities and their snapshots
- providerSegment.forEachEntity([&outV, &convert](const wm::Entity & entity)
- {
- // If we don't need this warning, we could directly iterate over the snapshots.
- if (entity.empty())
+ providerSegment.forEachEntity(
+ [&outV, &convert](const wm::Entity& entity)
{
- ARMARX_WARNING << "Empty history for " << entity.id();
- }
- ARMARX_DEBUG << "History size: " << entity.size();
-
- entity.forEachInstance([&outV, &convert](const wm::EntityInstance & entityInstance)
- {
- if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
{
- outV.push_back(convert(*o, entityInstance));
+ ARMARX_WARNING << "Empty history for " << entity.id();
}
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&outV, &convert](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ return true;
+ });
return true;
});
- return true;
- });
return outV;
}
- Reader::Result Reader::queryData(const Query& query) const
+ Reader::Result
+ Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
ARMARX_DEBUG << "[MappingDataReader] query ... ";
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
if (not qResult.success)
{
- ARMARX_WARNING << "Failed to query data from memory: "
- << qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
@@ -195,16 +208,17 @@ namespace armarx::armem::laser_scans::client
const auto laserScans = asLaserScans(providerSegment);
std::vector<std::string> sensors;
- providerSegment.forEachEntity([&sensors](const wm::Entity & entity)
- {
- sensors.push_back(entity.name());
- return true;
- });
+ providerSegment.forEachEntity(
+ [&sensors](const wm::Entity& entity)
+ {
+ sensors.push_back(entity.name());
+ return true;
+ });
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 70703521336f8ac0b1fdf9649df4a3777d95f431..55beab204b12a7f72432ef2e38789f1856058934 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -72,7 +72,8 @@ namespace armarx::armem::laser_scans::client
{
std::string agent;
- TimeRange timeRange;
+ // if not provided, only latest is queried
+ std::optional<TimeRange> timeRange;
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
index 2387e7319e2646857d9c821647f32d136ffdd969..551e4792c2eba3b5e19d2ebf468f2d090a53af41 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
@@ -301,8 +301,7 @@ namespace armarx::armem::server::laser_scans
if (robots.count(name) == 0)
{
ARMARX_CHECK_NOT_NULL(virtualRobotReader);
- const auto robot = virtualRobotReader->getRobot(
- name, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ const auto robot = virtualRobotReader->getRobot(name);
if (robot)
{
@@ -324,7 +323,7 @@ namespace armarx::armem::server::laser_scans
}
else
{
- ARMARX_VERBOSE << "Faield to synchronize robot `" << name << "`";
+ ARMARX_INFO << deactivateSpam(10) << "Failed to synchronize robot `" << name << "`";
}
}
return entry.first;
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index a90821817037a2deb733474743f33221048ab24b..e2d695429d3c4193ad97df106821f5bb912a0028 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -27,6 +27,22 @@ Core segment type of Object/Class.
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key="urdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedUrdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="sdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedSdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
<ObjectChild key="meshWrlPath">
<armarx::arondto::PackagePath />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index a95e98d011e52c7f6251a2119b66b971b9d5a045..4dcf3262724cd6a9626a967f9582d4514a478d89 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -191,7 +191,11 @@ namespace armarx::armem::server::obj::clazz
}
};
setPathIfExists(data.simoxXmlPath, info.simoxXML());
+ setPathIfExists(data.urdfPath, info.urdf());
+ setPathIfExists(data.sdfPath, info.sdf());
setPathIfExists(data.articulatedSimoxXmlPath, info.articulatedSimoxXML());
+ setPathIfExists(data.articulatedUrdfPath, info.articulatedUrdf());
+ setPathIfExists(data.articulatedSdfPath, info.articulatedSdf());
setPathIfExists(data.meshObjPath, info.wavefrontObj());
setPathIfExists(data.meshWrlPath, info.meshWrl());
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 0fd61b04c6305584e714cf38eb26c2370db2afc9..3b22bf1d1368a51be9e3ea71da20b8a343f3079b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1311,10 +1311,9 @@ namespace armarx::armem::server::obj::instance
{
// Try to fetch the robot.
ARMARX_CHECK_NOT_NULL(reader);
- bool warnings = false;
VirtualRobot::RobotPtr robot = reader->getRobot(
robotName, Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eStructure, warnings);
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
if (robot)
{
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 345564d4b859af306acecdc82d00096c4a559351..6f29809973ed4ae1d0a70e9c0830ac0ab0003037 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -40,6 +40,9 @@ namespace armarx::armem::robot
Eigen::Vector3f torque;
};
+ using ToFArray = Eigen::MatrixXf;
+
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index c7ac7670d399329bb6674dad3243a048153534f8..e84ccf38297fee314fc3d4372386d8ca26e0e8d9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -235,6 +235,13 @@
</Object>
+ <Object name="armarx::armem::prop::arondto::ToF">
+
+ <ObjectChild key="array">
+ <Matrix rows="-1" cols="-1" type="float32" />
+ </ObjectChild>
+
+ </Object>
<Object name="armarx::armem::arondto::Proprioception">
@@ -257,6 +264,12 @@
</Dict>
</ObjectChild>
+ <!-- TODO move to separate segment -->
+ <ObjectChild key="tof">
+ <Dict>
+ <armarx::armem::prop::arondto::ToF/>
+ </Dict>
+ </ObjectChild>
<ObjectChild key="extraFloats">
<Dict>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 1c18fe5b78799b1b2f9555a33a8997e7edb60d28..b079cec2e38c92510dac533980c070ef9acf0f09 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -97,4 +97,14 @@ namespace armarx::armem
aron::toAron(dto.torque, bo.torque);
}
+
+ void fromAron(const armarx::armem::prop::arondto::ToF& dto, robot::ToFArray& bo){
+ bo = dto.array;
+ }
+
+ void toAron(armarx::armem::prop::arondto::ToF& dto, const robot::ToFArray& bo){
+ dto.array = bo;
+ }
+
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 75c339c7fc4aad7f24b5b52befdf2e7f1451b345..1912e6c69092982ae5a5677615bfa6658a46c9a9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -38,6 +38,7 @@ namespace armarx::armem
{
struct Platform;
struct ForceTorque;
+ struct ToF;
}
namespace arondto
@@ -66,4 +67,8 @@ namespace armarx::armem
void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+ void fromAron(const armarx::armem::prop::arondto::ToF& dto, robot::ToFArray& bo);
+ void toAron(armarx::armem::prop::arondto::ToF& dto, const robot::ToFArray& bo);
+
+
} // namespace armarx::armem
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 611fa0e0d3f5bb931f5d64a60c70735d698c623f..d5f4844b574d3c6522f1d4461b7d9372c9f029c0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -121,7 +121,7 @@ namespace armarx::armem::robot_state
const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
if (elapsedTime > syncTimeout)
{
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
return false;
}
@@ -137,9 +137,7 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotDescription>
RobotReader::queryDescription(
const std::string& name,
- const armem::Time& timestamp,
- bool warnings
- )
+ const armem::Time& timestamp)
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -159,11 +157,8 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- if (warnings)
- {
- ARMARX_WARNING << "Memory reader is null. Did you forget to call "
- "RobotReader::connect() in onConnectComponent()?";
- }
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -182,10 +177,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- if (warnings)
- {
- ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
- }
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
}
return std::nullopt;
@@ -198,14 +190,14 @@ namespace armarx::armem::robot_state
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
- ARMARX_WARNING << "Failed to query joint state for robot '" << description.name << "'.";
+ ARMARX_VERBOSE << "Failed to query joint state for robot '" << description.name << "'.";
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
{
- ARMARX_WARNING << "Failed to query global pose for robot " << description.name;
+ ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
return std::nullopt;
}
@@ -235,7 +227,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << GetHandledExceptionString();
+ ARMARX_VERBOSE << GetHandledExceptionString();
return std::nullopt;
}
}
@@ -268,7 +260,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -276,7 +268,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query joint state. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint state. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -306,7 +298,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return {};
}
@@ -339,7 +331,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -362,7 +354,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query global pose. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query global pose. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -381,7 +373,7 @@ namespace armarx::armem::robot_state
if (providerSegment.empty())
{
- ARMARX_WARNING << "No entity found";
+ ARMARX_VERBOSE << "No entity found";
return std::nullopt;
}
@@ -394,9 +386,10 @@ namespace armarx::armem::robot_state
instance = &i;
return false; // break
});
- if (!instance)
+
+ if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found";
+ ARMARX_VERBOSE << "No entity snapshots found";
return std::nullopt;
}
@@ -436,6 +429,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&jointMap](const wm::Entity& entity)
{
+ if(not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -525,7 +523,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -558,7 +556,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -566,6 +564,37 @@ namespace armarx::armem::robot_state
}
+
+ std::optional<std::map<RobotReader::Hand, robot::ToFArray>> RobotReader::queryToF(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getToF(qResult.memory, description.name);
+ }
+
+
std::optional<robot::PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -580,6 +609,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&platformState](const wm::Entity& entity)
{
+ if(not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -623,6 +657,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&forceTorques](const wm::Entity& entity)
{
+ if(not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -675,6 +714,48 @@ namespace armarx::armem::robot_state
return forceTorques;
}
+ std::map<RobotReader::Hand, robot::ToFArray>
+ RobotReader::getToF(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::map<RobotReader::Hand, robot::ToFArray> tofs;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(constants::proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&tofs](const wm::Entity& entity)
+ {
+ ARMARX_DEBUG << "Processing ToF element";
+
+ if(not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : proprioception->tof)
+ {
+ ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
+
+ robot::ToFArray tof;
+ fromAron(dtoFt, tof);
+
+ const auto hand = fromHandName(handName);
+ tofs.emplace(hand, tof);
+ }
+ });
+
+ return tofs;
+ }
+
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
@@ -691,7 +772,7 @@ namespace armarx::armem::robot_state
{ instance = &i; });
if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found in provider segment `" << name << "`";
+ ARMARX_VERBOSE << "No entity snapshots found in provider segment `" << name << "`";
return std::nullopt;
}
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 bb63c714180325961c434d61e4a8e31b78a84045..da57e74afa0fedc79570a31a064ef0661a0cd8f3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -50,7 +50,7 @@ namespace armarx::armem::robot_state
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
std::optional<robot::Robot> get(const std::string& name,
const armem::Time& timestamp) override;
@@ -58,8 +58,7 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp,
- bool warnings = true);
+ const armem::Time& timestamp);
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
const armem::Time& timestamp);
@@ -100,6 +99,10 @@ namespace armarx::armem::robot_state
const armem::Time& start,
const armem::Time& end) const;
+
+ std::optional<std::map<Hand, robot::ToFArray>> queryToF(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
/**
* @brief retrieve the robot's pose in the odometry frame.
*
@@ -142,6 +145,9 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::map<RobotReader::Hand, robot::ToFArray>
+ getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
struct Properties
{
} properties;
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 8c1f8dfb0ad4f6f833d27191588fc8d766998235..4f03377e988f5a573e966a146bc335ed6494c2a7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -47,7 +47,7 @@ namespace armarx::armem::robot_state
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << "` "
+ ARMARX_VERBOSE << "Querying robot state failed for robot `" << robot.getName() << "` "
<< "(type `"<< robot.getType() << "`)!";
return false;
}
@@ -61,19 +61,16 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotPtr
VirtualRobotReader::getRobot(const std::string& name,
const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode,
- bool warnings)
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
ARMARX_INFO << deactivateSpam(60)
<< "Querying robot description for robot '" << name << "'";
- const auto description = queryDescription(name, timestamp, warnings);
+ const auto description = queryDescription(name, timestamp);
if (not description)
{
- if (warnings)
- {
- ARMARX_WARNING << "The description of robot `" << name << "` is not a available!";
- }
+ ARMARX_VERBOSE << "The description of robot `" << name << "` is not a available!";
+
return nullptr;
}
@@ -134,7 +131,7 @@ namespace armarx::armem::robot_state
Clock::WaitFor(sleepAfterFailure);
}
- ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
+ ARMARX_VERBOSE << "Failed to get synchronized robot `" << name << "`";
return nullptr;
}
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 d19e0483fdfa04f00579ba85b1e830f58feee241..19a184e8c4cee71e2a7866206f2ae55cd51f0d27 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -53,8 +53,7 @@ namespace armarx::armem::robot_state
getRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure,
- bool warnings = true);
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
[[nodiscard]] VirtualRobot::RobotPtr
getSynchronizedRobot(const std::string& name,
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 169d59f1a86d6a1bea1b9e54033409d23fb3cd7f..887cc2eec912ffc8bedcff94d5b19495d8fc12c1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -44,6 +44,7 @@ armarx_add_library(
proprioception/RobotUnitReader.h
proprioception/converters/Armar6Converter.h
+ proprioception/converters/ArmarDEConverter.h
proprioception/converters/ConverterTools.h
proprioception/converters/ConverterRegistry.h
proprioception/converters/ConverterInterface.h
@@ -63,6 +64,7 @@ armarx_add_library(
proprioception/RobotUnitReader.cpp
proprioception/converters/Armar6Converter.cpp
+ proprioception/converters/ArmarDEConverter.cpp
proprioception/converters/ConverterTools.cpp
proprioception/converters/ConverterRegistry.cpp
proprioception/converters/ConverterInterface.cpp
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a4ac7a8c3a2daf23f80cbb5d88c3290915190129
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp
@@ -0,0 +1,296 @@
+#include "ArmarDEConverter.h"
+#include <cmath>
+#include <string>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+
+#include "ConverterTools.h"
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ ArmarDEConverter::ArmarDEConverter() :
+ tools(std::make_unique<ConverterTools>())
+ {
+ }
+
+
+ ArmarDEConverter::~ArmarDEConverter()
+ {
+ }
+
+
+ aron::data::DictPtr
+ ArmarDEConverter::convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
+ {
+ arondto::Proprioception dto;
+ dto.iterationID = data.iterationId;
+
+ for (const auto& [dataEntryName, dataEntry] : description.entries)
+ {
+ process(dto, dataEntryName, {data, dataEntry});
+ }
+
+ // resize to square
+ for(auto& [_, tof] : dto.tof)
+ {
+ const int sr = std::sqrt(tof.array.size());
+ const bool isSquare = (sr * sr == tof.array.size());
+
+ if(tof.array.size() > 0 and isSquare)
+ {
+ tof.array.resize(sr, sr);
+ }
+ }
+
+
+ return dto.toAron();
+ }
+
+
+ void ArmarDEConverter::process(
+ arondto::Proprioception& dto,
+ const std::string& entryName,
+ const ConverterValue& value)
+ {
+ const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
+ ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
+ const std::set<size_t> acceptedSizes{3, 4, 5};
+ ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
+ << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
+ << "\n- split: '" << split << "'";
+
+ const std::string& category = split.at(0);
+ const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
+
+ if (name == "Platform")
+ {
+ // Platform
+ processPlatformEntry(dto.platform, field, value);
+ }
+ else if (simox::alg::starts_with(name, "FT"))
+ {
+ // Force Torque
+ processForceTorqueEntry(dto.forceTorque, split, value);
+ }
+ else if (simox::alg::contains(field, "tofDepth"))
+ {
+ // ARMARX_DEBUG << "Processing ToF sensor data";
+ processToFEntry(dto.tof, split, value);
+ }
+ else
+ {
+ // Joint
+ bool processed = processJointEntry(dto.joints, split, value);
+ if (not processed)
+ {
+ // Fallback: Put in extra.
+ const std::vector<std::string> comps{simox::alg::advanced(split.begin(), 1), split.end()};
+ const std::string key = simox::alg::join(comps, ".");
+
+ switch (value.entry.type)
+ {
+ case RobotUnitDataStreaming::NodeTypeFloat:
+ dto.extraFloats[key] = getValueAs<float>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeLong:
+ dto.extraLongs[key] = getValueAs<long>(value);
+ break;
+ default:
+ ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
+ << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
+ break;
+ }
+ }
+ }
+ }
+
+
+
+ void ArmarDEConverter::processPlatformEntry(
+ prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ const ConverterValue& value)
+ {
+ if (findByPrefix(fieldName, tools->platformIgnored))
+ {
+ return;
+ }
+ else if (auto getter = findByPrefix(fieldName, tools->platformPoseGetters))
+ {
+ if (Eigen::Vector3f* dst = getter(dto))
+ {
+ if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
+ {
+ setter(*dst, getValueAs<float>(value));
+ }
+ }
+ }
+ else
+ {
+ // No setter for this field. Put in extra.
+ dto.extra[fieldName] = getValueAs<float>(value);
+ }
+ }
+
+
+ void ArmarDEConverter::processForceTorqueEntry(
+ std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ const std::string& name = split.at(1);
+ std::vector<std::string> splitName = simox::alg::split(name, " ", false, false);
+ ARMARX_CHECK_EQUAL(splitName.size(), 2);
+ ARMARX_CHECK_EQUAL(splitName.at(0), "FT");
+
+ auto it = tools->sidePrefixMap.find(splitName.at(1));
+ ARMARX_CHECK(it != tools->sidePrefixMap.end()) << splitName.at(1);
+
+ const std::string& side = it->second;
+ processForceTorqueEntry(fts[side], split, value);
+ }
+
+ void ArmarDEConverter::processToFEntry(
+ std::map<std::string, prop::arondto::ToF>& tofs,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
+
+ ARMARX_CHECK_EQUAL(split.size(), 4);
+ ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
+
+ const std::string& name = split.at(1);
+
+ // element 0 sens
+ // element 1 is either LeftHand or RightHand
+
+ const std::map<std::string, std::string> sidePrefixMap
+ {
+ {"LeftHand", "Left"},
+ {"RightHand", "Right"}
+ };
+
+ auto it = sidePrefixMap.find(name);
+ ARMARX_CHECK(it != sidePrefixMap.end()) << name;
+
+ const std::string& side = it->second;
+ processToFEntry(tofs[side], split, value);
+ }
+
+ void ArmarDEConverter::processToFEntry(prop::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split, e.g., element_12
+ const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
+ ARMARX_CHECK_EQUAL(elements.size(), 2);
+
+ const int idx = std::stoi(elements.at(1));
+
+ if(tof.array.size() < (idx +1))
+ {
+ tof.array.resize(idx+1, 1);
+ }
+
+ tof.array(idx) = getValueAs<float>(value);
+ }
+
+
+ void ArmarDEConverter::processForceTorqueEntry(
+ prop::arondto::ForceTorque& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ const std::string& fieldName = split.at(2);
+ if (auto getter = findByPrefix(fieldName, tools->ftGetters))
+ {
+ if (Eigen::Vector3f* dst = getter(dto))
+ {
+ if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
+ {
+ setter(*dst, getValueAs<float>(value));
+ }
+ }
+ }
+ else
+ {
+ // No setter for this field. Put in extra.
+ std::string key = split.size() == 4
+ ? (fieldName + "." + split.at(3))
+ : fieldName;
+
+ switch (value.entry.type)
+ {
+ case RobotUnitDataStreaming::NodeTypeFloat:
+ dto.extra[key] = getValueAs<float>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeInt:
+ dto.extra[key] = getValueAs<int>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeLong:
+ dto.extra[key] = getValueAs<long>(value);
+ break;
+ default:
+ ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
+ << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
+ break;
+ }
+ }
+ }
+
+
+ bool ArmarDEConverter::processJointEntry(
+ prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ const std::string& jointName = split.at(1);
+ const std::string& fieldName = split.at(2);
+ if (false)
+ {
+ // Only in simulation.
+ if (auto getter = findByPrefix(fieldName, tools->jointGetters))
+ {
+ if (std::map<std::string, float>* map = getter(dto))
+ {
+ (*map)[jointName] = getValueAs<float>(value);
+ }
+ }
+ }
+
+ const std::string tempSuffix = "Temperature";
+ if (simox::alg::ends_with(split.at(2), tempSuffix))
+ {
+ // Handle "dieTemperature" etc
+ const std::string name = split.at(2).substr(0, split.at(2).size() - tempSuffix.size());
+ dto.temperature[split.at(1)][name] = getValueAs<float>(value);
+ return true;
+ }
+ else if (auto it = tools->jointSetters.find(fieldName); it != tools->jointSetters.end())
+ {
+ const ConverterTools::JointSetter& setter = it->second;
+ setter(dto, split, value);
+ return true;
+ }
+ else
+ {
+ // ARMARX_DEBUG << "Ignoring unhandled field: '" << simox::alg::join(split, ".") << "'";
+ return false;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f5c287159d10fa252ae2f21215e1328c52cf6e8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h
@@ -0,0 +1,78 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
+#include "ConverterInterface.h"
+
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ struct ConverterValue;
+ class ConverterTools;
+
+
+ class ArmarDEConverter : public ConverterInterface
+ {
+ public:
+
+ ArmarDEConverter();
+ virtual ~ArmarDEConverter() override;
+
+
+ aron::data::DictPtr
+ convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+ protected:
+
+ void process(arondto::Proprioception& dto, const std::string& entryName, const ConverterValue& value);
+
+
+
+ private:
+
+ void processPlatformEntry(
+ prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ const ConverterValue& value);
+
+ void processForceTorqueEntry(
+ std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processForceTorqueEntry(
+ prop::arondto::ForceTorque& ft,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processToFEntry(
+ std::map<std::string, prop::arondto::ToF>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processToFEntry(
+ prop::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ bool processJointEntry(
+ prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+
+ private:
+
+ std::unique_ptr<ConverterTools> tools;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
index 0c6baa3b770e2136543be7885b1e856a7e93bb78..48ea1537572cdc50488190da9c6c69a7cbb024ae 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
@@ -1,6 +1,7 @@
#include "ConverterRegistry.h"
#include "Armar6Converter.h"
+#include "ArmarDEConverter.h"
#include <SimoxUtility/algorithm/get_map_keys_values.h>
@@ -11,7 +12,7 @@ namespace armarx::armem::server::robot_state::proprioception
ConverterRegistry::ConverterRegistry()
{
add<Armar6Converter>("Armar6");
- add<Armar6Converter>("ArmarDE");
+ add<ArmarDEConverter>("ArmarDE");
add<Armar6Converter>("Armar7");
}