ConverterTools.h

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <set>
#include <string>
 
#include <Eigen/Core>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
 
#include "ConverterInterface.h"
 
namespace armarx::armem::server::robot_state::exteroception
{
 
    struct ConverterValue
    {
        const RobotUnitDataStreaming::TimeStep& data;
        const RobotUnitDataStreaming::DataEntry& entry;
    };
 
    template <class T>
    T
    getValueAs(const ConverterValue& value)
    {
        return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
    }
 
    /**
     * @brief Search
     * @param key
     * @param prefixes
     * @return
     */
    std::optional<std::string> findByPrefix(const std::string& key,
                                            const std::set<std::string>& prefixes);
 
    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;
    }
 
    class ConverterTools
    {
    public:
        ConverterTools();
 
 
    public:
        std::map<std::string, std::function<void(Eigen::Vector3f&, float)>> vector3fSetters;
 
        std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)>>
            jointGetters;
        std::map<std::string,
                 std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)>>
            jointVectorGetters;
 
        using JointSetter = std::function<void(prop::arondto::Joints& dto,
                                               const std::vector<std::string>& split,
                                               const ConverterValue& value)>;
        std::map<std::string, JointSetter> jointSetters;
 
        std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)>>
            platformPoseGetters;
        std::set<std::string> platformIgnored;
 
        std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)>>
            ftGetters;
 
 
        std::map<std::string, std::string> sidePrefixMap{
            {"R", "Right"},
            {"L", "Left"},
        };
    };
} // namespace armarx::armem::server::robot_state::exteroception