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/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
 
#include "ConverterInterface.h"
 
 
namespace armarx::armem::server::robot_state::proprioception
{
 
    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" },
        };
 
    };
}