diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
index 6054b8a075ccf4e9ca53e91906228b6d4ef4b284..0a56703f5f71820f625a8d30c0aaec065fef0b46 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
@@ -1,6 +1,7 @@
#include "Armar6RobotUnitConverter.h"
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
@@ -92,57 +93,68 @@ namespace armarx::armem::server::robot_state
};
}
{
- jointSetters["position"] = [](prop::arondto::Joints & dto, const std::vector<std::string>& split, const Value & value)
- {
- dto.position[split.at(1)] = getValueAs<float>(value);
- };
-
-
-#define ADD_GETTER(container, name) \
- container [ #name ] = [](prop::arondto::Joints & j) \
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container [ #name ] = []( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const Value & value) \
{ \
- return &j. name; \
+ dto. name [split.at(1)] = getValueAs< type >(value); \
}
- ADD_GETTER(jointGetters, position);
- ADD_GETTER(jointGetters, velocity);
- ADD_GETTER(jointGetters, acceleration);
- ADD_GETTER(jointGetters, relativePosition);
- ADD_GETTER(jointGetters, filteredVelocity);
+ ADD_SCALAR_SETTER(jointSetters, position, float);
+ ADD_SCALAR_SETTER(jointSetters, velocity, float);
+ ADD_SCALAR_SETTER(jointSetters, acceleration, float);
+
+ ADD_SCALAR_SETTER(jointSetters, relativePosition, float);
+ ADD_SCALAR_SETTER(jointSetters, filteredVelocity, float);
+
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, velocityTarget, float);
+
+ ADD_SCALAR_SETTER(jointSetters, torque, float);
+ ADD_SCALAR_SETTER(jointSetters, inertiaTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityCompensatedTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, inverseDynamicsTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, torqueTicks, int);
+
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
- ADD_GETTER(jointGetters, currentTarget);
- ADD_GETTER(jointGetters, positionTarget);
- ADD_GETTER(jointGetters, velocityTarget);
+ // "temperature" handled below
+ // ADD_SCALAR_SETTER(jointSetters, temperature, float);
- ADD_GETTER(jointGetters, torque);
- ADD_GETTER(jointGetters, inertiaTorque);
- ADD_GETTER(jointGetters, gravityTorque);
- ADD_GETTER(jointGetters, gravityCompensatedTorque);
- ADD_GETTER(jointGetters, inverseDynamicsTorque);
- // ADD_GETTER(jointGetters, torqueTicks);
+ ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
+ ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
- ADD_GETTER(jointGetters, positionTarget);
- ADD_GETTER(jointGetters, currentTarget);
- ADD_GETTER(jointGetters, positionTarget);
+ ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
+ ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
- // ADD_GETTER(jointVectorGetters, orientation);
- // ADD_GETTER(jointVectorGetters, angularVelocity);
- // ADD_GETTER(jointVectorGetters, linearAcceleration);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
- // ADD_GETTER(jointGetters, temperature);
- // ADD_GETTER(jointGetters, motorCurrent);
- // ADD_GETTER(jointGetters, maxTargetCurrent);
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container [ #name ] = [this]( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const Value & value) \
+ { \
+ auto& vec = dto. name [split.at(1)]; \
+ auto& setter = this->vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs< type >(value)); \
+ }
- // ADD_GETTER(jointGetters, sensorBoardUpdateRate);
- // ADD_GETTER(jointGetters, I2CUpdateRate);
+ ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
+ ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
- // ADD_GETTER(jointGetters, JointStatusEmergencyStop);
- // ADD_GETTER(jointGetters, JointStatusEnabled);
- // ADD_GETTER(jointGetters, JointStatusError);
- // ADD_GETTER(jointGetters, JointStatusOperation);
-#undef ADD_GETTER
+ // ADD_GETTER(jointVectorGetters, orientation, float);
}
}
@@ -174,6 +186,10 @@ namespace armarx::armem::server::robot_state
{
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};
+ ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
+ << "Data entry name could not be parsed (exected 3 or 4 components between '.'): "
+ << "\n- split: '" << split << "'";
const std::string& category = split.at(0);
const std::string& name = split.at(1);
@@ -188,12 +204,31 @@ namespace armarx::armem::server::robot_state
else if (simox::alg::starts_with(name, "FT"))
{
// Force Torque
- processForceTorqueEntry(dto.forceTorque, name, field, value);
+ processForceTorqueEntry(dto.forceTorque, split, value);
}
else
{
// Joint
- processJointEntry(dto.joints, split, value);
+ bool processed = processJointEntry(dto.joints, split, value);
+
+ if (!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:
+ break;
+ }
+ }
}
}
@@ -247,7 +282,7 @@ namespace armarx::armem::server::robot_state
{
return;
}
- if (auto getter = findByPrefix(fieldName, platformPoseGetters))
+ else if (auto getter = findByPrefix(fieldName, platformPoseGetters))
{
if (Eigen::Vector3f* dst = getter(dto))
{
@@ -257,32 +292,38 @@ namespace armarx::armem::server::robot_state
}
}
}
+ else
+ {
+ // No setter for this field. Put in extra.
+ dto.extra[fieldName] = getValueAs<float>(value);
+ }
}
void Armar6RobotUnitConverter::processForceTorqueEntry(
std::map<std::string, prop::arondto::ForceTorque>& fts,
- const std::string& name,
- const std::string& fieldName,
+ const std::vector<std::string>& split,
const Value& value)
{
- std::vector<std::string> split = simox::alg::split(name, " ", false, false);
- ARMARX_CHECK_EQUAL(split.size(), 2);
- ARMARX_CHECK_EQUAL(split.at(0), "FT");
+ 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 = sidePrefixMap.find(split.at(1));
- ARMARX_CHECK(it != sidePrefixMap.end()) << split.at(1);
+ auto it = sidePrefixMap.find(splitName.at(1));
+ ARMARX_CHECK(it != sidePrefixMap.end()) << splitName.at(1);
const std::string& side = it->second;
- processForceTorqueEntry(fts[side], fieldName, value);
+ processForceTorqueEntry(fts[side], split, value);
}
void Armar6RobotUnitConverter::processForceTorqueEntry(
prop::arondto::ForceTorque& dto,
- const std::string& fieldName,
+ const std::vector<std::string>& split,
const Value& value)
{
+ const std::string& fieldName = split.at(2);
if (auto getter = findByPrefix(fieldName, ftGetters))
{
if (Eigen::Vector3f* dst = getter(dto))
@@ -293,17 +334,25 @@ namespace armarx::armem::server::robot_state
}
}
}
+ else
+ {
+ // No setter for this field. Put in extra.
+ std::string key = split.size() == 4
+ ? (fieldName + "." + split.at(3))
+ : fieldName;
+ dto.extra[key] = getValueAs<float>(value);
+ }
}
- void Armar6RobotUnitConverter::processJointEntry(
+ bool Armar6RobotUnitConverter::processJointEntry(
prop::arondto::Joints& dto,
const std::vector<std::string>& split,
const Value& value)
{
const std::string& jointName = split.at(1);
const std::string& fieldName = split.at(2);
- if (split.size() == 3)
+ if (false)
{
// Only in simulation.
if (auto getter = findByPrefix(fieldName, jointGetters))
@@ -314,18 +363,25 @@ namespace armarx::armem::server::robot_state
}
}
}
- else if (split.size() == 4)
- {
- auto it = jointSetters.find(fieldName);
- ARMARX_CHECK(it != jointSetters.end()) << fieldName << " | " << simox::alg::get_keys(jointSetters);
+ 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 = jointSetters.find(fieldName); it != jointSetters.end())
+ {
const JointSetter& setter = it->second;
setter(dto, split, value);
+ return true;
}
else
{
- ARMARX_CHECK(false) << "Data entry name could not be parsed (exected 3 or 4 components between '.'): "
- << "\n- split: '" << split << "'";
+ ARMARX_DEBUG << "Ignoring unhandled field: '" << simox::alg::join(split, ".") << "'";
+ return false;
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
index e01d7160f220ad2ab739ff954ed0e4b69a054d30..0e4b1c0c7b12fe67305d5b7b4636a00407f836a1 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
@@ -60,16 +60,15 @@ namespace armarx::armem::server::robot_state
void processForceTorqueEntry(
std::map<std::string, prop::arondto::ForceTorque>& fts,
- const std::string& name,
- const std::string& fieldName,
+ const std::vector<std::string>& split,
const Value& value);
void processForceTorqueEntry(
prop::arondto::ForceTorque& ft,
- const std::string& fieldName,
+ const std::vector<std::string>& split,
const Value& value);
- void processJointEntry(
+ bool processJointEntry(
prop::arondto::Joints& dto,
const std::vector<std::string>& split,
const Value& value);
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index 46857c8925f49da40b20c210ba14a677afe177a9..2ddb96a4d263c6d946f4cd3d98b8bfafb63caa97 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -199,6 +199,12 @@ Robot proprioception.
<ObjectChild key="acceleration">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
+
+ <ObjectChild key="extra">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
</Object>
@@ -221,6 +227,12 @@ Robot proprioception.
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
+ <ObjectChild key="extra">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
</Object>
@@ -244,7 +256,20 @@ Robot proprioception.
<armarx::armem::prop::arondto::ForceTorque/>
</Dict>
</ObjectChild>
-
+
+
+ <ObjectChild key="extraFloats">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="extraLongs">
+ <Dict>
+ <Long />
+ </Dict>
+ </ObjectChild>
+
</Object>