Add RobotUnitConverter base class and Armar6 version

source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp

+#include "Armar6RobotUnitConverter.h"
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+    Armar6RobotUnitConverter::Armar6RobotUnitConverter()
+    {
+        {
+            vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
+            {
+                v.x() = f;
+            };
+            vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
+            {
+                v.y() = f;
+            };
+            vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
+            {
+                v.z() = f;
+            };
+            vector3fSetters["x"] = vector3fSetters["X"];
+            vector3fSetters["y"] = vector3fSetters["Y"];
+            vector3fSetters["z"] = vector3fSetters["Z"];
+            vector3fSetters["Rotation"] = vector3fSetters["Z"];
+        }
+        {
+            platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
+            {
+                return &p.acceleration;
+            };
+            platformPoseGetters["absolutePosition"] = [](prop::arondto::Platform & p)
+            {
+                return &p.absolutePosition;
+            };
+            platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
+            {
+                return &p.relativePosition;
+            };
+            platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
+            {
+                return &p.velocity;
+            };
+        }
+        {
+            ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
+            {
+                return &ft.gravityCompensationForce;
+            };
+            ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
+            {
+                return &ft.gravityCompensationTorque;
+            };
+            ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
+            {
+                return &ft.force;
+            };
+            ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
+            {
+                return &ft.torque;
+            };
+        }
+        {
+            jointGetters["acceleration"] = [](prop::arondto::Joints & j)
+            {
+                return &j.accellerations;
+            };
+            jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
+            {
+                return &j.gravityTorques;
+            };
+            jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
+            {
+                return &j.inertiaTorques;
+            };
+            jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
+            {
+                return &j.inverseDynamicsTorques;
+            };
+            jointGetters["position"] = [](prop::arondto::Joints & j)
+            {
+                return &j.positions;
+            };
+            jointGetters["torque"] = [](prop::arondto::Joints & j)
+            {
+                return &j.torques;
+            };
+            jointGetters["velocity"] = [](prop::arondto::Joints & j)
+            {
+                return &j.velocities;
+            };
+        }
+    }
+
+
+    Armar6RobotUnitConverter::~Armar6RobotUnitConverter()
+    {
+    }
+
+
+    arondto::Proprioception Armar6RobotUnitConverter::convert(
+        const RobotUnitDataStreaming::TimeStep& data,
+        const RobotUnitDataStreaming::DataStreamingDescription& description)
+    {
+        arondto::Proprioception dto;
+        dto.iterationID = data.iterationId;
+
+        for (const auto& [dataEntryName, dataEntry] : description.entries)
+        {
+            switch (dataEntry.type)
+            {
+                case RobotUnitDataStreaming::NodeTypeFloat:
+                {
+                    const float value = RobotUnitDataStreamingReceiver::GetAs<float>(data, dataEntry);
+
+                    const std::vector<std::string> split = simox::alg::split(dataEntryName, ".", false, false);
+                    ARMARX_CHECK_EQUAL(split.size(), 3) << 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 << " | " << dataEntryName;
+
+                    if (name == "Platform")
+                    {
+                        // Platform
+                        processPlatformEntry(dto.platform, field, value);
+                    }
+                    else if (simox::alg::starts_with(name, "FT"))
+                    {
+                        // Force Torque
+                        processForceTorqueEntry(dto.forceTorque, name, field, value);
+                    }
+                    else
+                    {
+                        // Joint
+                        processJointEntry(dto.joints, name, field, value);
+                    }
+                }
+                break;
+
+                default:
+                {
+                    ARMARX_DEBUG << "Ignoring non-float entry '" << dataEntryName << "'.";
+                }
+                break;
+            }
+        }
+        return dto;
+    }
+
+
+    template <class ValueT>
+    ValueT findByPrefix(const std::string& key,
+                        const std::map<std::string, ValueT>& map)
+    {
+        for (const auto& [prefix, value] : map)
+        {
+            if (simox::alg::starts_with(key, prefix))
+            {
+                return value;
+            }
+        }
+        return nullptr;
+    }
+    template <class ValueT>
+    ValueT findBySuffix(const std::string& key,
+                        const std::map<std::string, ValueT>& map)
+    {
+        for (const auto& [suffix, value] : map)
+        {
+            if (simox::alg::ends_with(key, suffix))
+            {
+                return value;
+            }
+        }
+        return nullptr;
+    }
+
+
+    void Armar6RobotUnitConverter::processPlatformEntry(
+        prop::arondto::Platform& dto,
+        const std::string& fieldName,
+        float value)
+    {
+        if (auto getter = findByPrefix(fieldName, platformPoseGetters))
+        {
+            if (Eigen::Vector3f* dst = getter(dto))
+            {
+                if (auto setter = findBySuffix(fieldName, vector3fSetters))
+                {
+                    setter(*dst, value);
+                }
+            }
+        }
+    }
+
+
+    void Armar6RobotUnitConverter::processForceTorqueEntry(
+        std::map<std::string, prop::arondto::ForceTorque>& fts,
+        const std::string& name,
+        const std::string& fieldName,
+        float 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");
+
+        auto it = sidePrefixMap.find(split.at(1));
+        ARMARX_CHECK(it != sidePrefixMap.end()) << split.at(1);
+
+        const std::string& side = it->second;
+        processForceTorqueEntry(fts[side], fieldName, value);
+    }
+
+
+    void Armar6RobotUnitConverter::processForceTorqueEntry(
+        prop::arondto::ForceTorque& dto,
+        const std::string& fieldName,
+        float value)
+    {
+        if (auto getter = findByPrefix(fieldName, ftGetters))
+        {
+            if (Eigen::Vector3f* dst = getter(dto))
+            {
+                if (auto setter = findBySuffix(fieldName, vector3fSetters))
+                {
+                    setter(*dst, value);
+                }
+            }
+        }
+    }
+
+
+    void Armar6RobotUnitConverter::processJointEntry(
+        prop::arondto::Joints& dto,
+        const std::string& name,
+        const std::string& fieldName,
+        float value)
+    {
+        if (auto getter = findByPrefix(fieldName, jointGetters))
+        {
+            if (std::map<std::string, float>* map = getter(dto))
+            {
+                (*map)[name] = value;
+            }
+        }
+    }
+
+}

source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h

+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
+#include "RobotUnitConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state
+{
+
+    class Armar6RobotUnitConverter : public RobotUnitConverterInterface
+    {
+    public:
+
+        Armar6RobotUnitConverter();
+        virtual ~Armar6RobotUnitConverter() override;
+
+
+        arondto::Proprioception convert(
+            const RobotUnitDataStreaming::TimeStep& data,
+            const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+    private:
+
+        void processPlatformEntry(
+            prop::arondto::Platform& dto,
+            const std::string& fieldName,
+            float value);
+
+        void processForceTorqueEntry(
+            std::map<std::string, prop::arondto::ForceTorque>& fts,
+            const std::string& name,
+            const std::string& fieldName,
+            float value);
+
+        void processForceTorqueEntry(
+            prop::arondto::ForceTorque& ft,
+            const std::string& fieldName,
+            float value);
+
+        void processJointEntry(
+            prop::arondto::Joints& dto,
+            const std::string& name,
+            const std::string& fieldName,
+            float value);
+
+
+    private:
+
+        std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+
+        std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+        std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+        std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+
+        std::map<std::string, std::string> sidePrefixMap
+        {
+            { "R", "Right" },
+            { "L", "Left" },
+        };
+
+    };
+}
+

source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt

@@ -20,6 +20,9 @@ set(SOURCES
     RobotStateWriter.cpp
     RobotUnitData.cpp
     RobotUnitReader.cpp
+
+    RobotUnitConverterInterface.cpp
+    Armar6RobotUnitConverter.cpp
 )
 set(HEADERS
     aron_conversions.h
@@ -27,6 +30,9 @@ set(HEADERS
     RobotStateWriter.h
     RobotUnitData.h
     RobotUnitReader.h
+
+    RobotUnitConverterInterface.h
+    Armar6RobotUnitConverter.h
 )
 
 armarx_add_component("${SOURCES}" "${HEADERS}")

source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.cpp

+#include "RobotUnitConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state
+{
+
+    RobotUnitConverterInterface::~RobotUnitConverterInterface()
+    {
+
+    }
+
+}

source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.h

+#pragma once
+
+
+namespace armarx::RobotUnitDataStreaming
+{
+    struct TimeStep;
+    struct DataStreamingDescription;
+    struct DataEntry;
+}
+namespace armarx::armem::arondto
+{
+    class Proprioception;
+}
+
+namespace armarx::armem::server::robot_state
+{
+    class RobotUnitConverterInterface
+    {
+    public:
+
+        virtual ~RobotUnitConverterInterface();
+
+        virtual arondto::Proprioception convert(
+            const RobotUnitDataStreaming::TimeStep& data,
+            const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
+
+    };
+}
+