ConverterTools.cpp

Go to the documentation of this file.

#include "ConverterTools.h"
 
#include <SimoxUtility/algorithm/advanced.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
 
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
namespace armarx::armem::server::robot_state
{
 
    std::optional<std::string>
    exteroception::findByPrefix(const std::string& key, const std::set<std::string>& prefixes)
    {
        for (const auto& prefix : prefixes)
        {
            if (simox::alg::starts_with(key, prefix))
            {
                return prefix;
            }
        }
        return std::nullopt;
    }
} // namespace armarx::armem::server::robot_state
 
namespace armarx::armem::server::robot_state::exteroception
{
    ConverterTools::ConverterTools()
    {
        {
            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["relativePosition"] = [](prop::arondto::Platform& p)
            { return &p.relativePosition; };
            platformPoseGetters["velocity"] = [](prop::arondto::Platform& p)
            { return &p.velocity; };
            platformIgnored.insert("absolutePosition");
        }
        {
            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.acceleration; };
            jointGetters["gravityTorque"] = [](prop::arondto::Joints& j)
            { return &j.gravityTorque; };
            jointGetters["inertiaTorque"] = [](prop::arondto::Joints& j)
            { return &j.inertiaTorque; };
            jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints& j)
            { return &j.inverseDynamicsTorque; };
            jointGetters["position"] = [](prop::arondto::Joints& j) { return &j.position; };
            jointGetters["torque"] = [](prop::arondto::Joints& j) { return &j.torque; };
            jointGetters["velocity"] = [](prop::arondto::Joints& j) { return &j.velocity; };
        }
        {
 
#define ADD_SCALAR_SETTER(container, name, type)                                                   \
    container[#name] = [](prop::arondto::Joints& dto,                                              \
                          const std::vector<std::string>& split,                                   \
                          const ConverterValue& value)                                             \
    { dto.name[split.at(1)] = getValueAs<type>(value); }
 
            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);
 
            // "temperature" handled below
            // ADD_SCALAR_SETTER(jointSetters, temperature, float);
 
            ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
            ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
 
            ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
            ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
 
            ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
            ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
            ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
            ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
 
 
#define ADD_VECTOR3_SETTER(container, name, type)                                                  \
    container[#name] = [this](prop::arondto::Joints& dto,                                          \
                              const std::vector<std::string>& split,                               \
                              const ConverterValue& value)                                         \
    {                                                                                              \
        auto& vec = dto.name[split.at(1)];                                                         \
        auto& setter = this -> vector3fSetters.at(split.at(3));                                    \
        setter(vec, getValueAs<type>(value));                                                      \
    }
 
            ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
            ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
 
            // ADD_GETTER(jointVectorGetters, orientation, float);
        }
    }
 
} // namespace armarx::armem::server::robot_state::exteroception