From 4b1a8c780a3c8c8b4077caaf952f793dd118d565 Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Sat, 25 Feb 2023 17:52:43 +0100
Subject: [PATCH] ARMAR-DE RobotStreaming converter: adding ToF data
---
source/RobotAPI/libraries/armem_robot/types.h | 3 +
.../armem_robot_state/aron/Proprioception.xml | 13 +
.../armem_robot_state/aron_conversions.cpp | 10 +
.../armem_robot_state/aron_conversions.h | 5 +
.../armem_robot_state/server/CMakeLists.txt | 2 +
.../converters/ArmarDEConverter.cpp | 296 ++++++++++++++++++
.../converters/ArmarDEConverter.h | 78 +++++
.../converters/ConverterRegistry.cpp | 3 +-
8 files changed, 409 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 345564d4b..6f2980997 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 c7ac7670d..e84ccf382 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 1c18fe5b7..b079cec2e 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 75c339c7f..1912e6c69 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/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 169d59f1a..887cc2eec 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 000000000..a4ac7a8c3
--- /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 000000000..6f5c28715
--- /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 0c6baa3b7..48ea15375 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");
}
--
GitLab