Armar6Converter.cpp

Go to the documentation of this file.

#include "Armar6Converter.h"
#include <cstddef>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/advanced.h>
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
 
#include "ConverterTools.h"
 
 
namespace armarx::armem::server::robot_state::proprioception
{
 
    Armar6Converter::Armar6Converter() :
        tools(std::make_unique<ConverterTools>())
    {
    }
 
 
    Armar6Converter::~Armar6Converter()
    {
    }
 
 
    aron::data::DictPtr
    Armar6Converter::convert(
        const RobotUnitDataStreaming::TimeStep& data,
        const RobotUnitDataStreaming::DataStreamingDescription& description)
    {
        arondto::Proprioception dto;
        dto.iterationID = data.iterationId;
 
        for (const auto& [dataEntryName, dataEntry] : description.entries)
        {
            process(dto, dataEntryName, {data, dataEntry});
        }
        return dto.toAron();
    }
 
 
    void Armar6Converter::process(
        arondto::Proprioception& dto,
        const std::string& entryName,
        const ConverterValue& value)
    {
        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, 5};
        ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
                << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
                << "\n- split: '" << 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 << " | " << entryName;
 
        if (name == "Platform")
        {
            // Platform
            processPlatformEntry(dto.platform, field, value);
        }
        else if (simox::alg::starts_with(name, "FT"))
        {
            // Force Torque
            processForceTorqueEntry(dto.forceTorque, split, value);
        }
        else
        {
            // Joint
            bool processed = processJointEntry(dto.joints, split, value);
            if (not 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;
                    case RobotUnitDataStreaming::NodeTypeBool:
                        dto.extraBools[key] = getValueAs<bool>(value);
                        break;
                    case RobotUnitDataStreaming::NodeTypeInt:
                        dto.extraInts[key] = getValueAs<int>(value);
                        break;
                    case RobotUnitDataStreaming::NodeTypeShort:
                        dto.extraShorts[key] = getValueAs<Ice::Short>(value);
                        break;
                    case RobotUnitDataStreaming::NodeTypeByte:
                        dto.extraBytes[key] = getValueAs<Ice::Byte>(value);
                        break;
                    default:
                        ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
                                     << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
                        break;
                }
            }
        }
    }
 
 
 
    void Armar6Converter::processPlatformEntry(
        prop::arondto::Platform& dto,
        const std::string& fieldName,
        const ConverterValue& value)
    {
        if (findByPrefix(fieldName, tools->platformIgnored))
        {
            return;
        }
        else if (auto getter = findByPrefix(fieldName, tools->platformPoseGetters))
        {
            if (Eigen::Vector3f* dst = getter(dto))
            {
                if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
                {
                    setter(*dst, getValueAs<float>(value));
                }
            }
        }
        else
        {
            // No setter for this field. Put in extra.
            dto.extra[fieldName] = getValueAs<float>(value);
        }
    }
 
 
    void Armar6Converter::processForceTorqueEntry(
        std::map<std::string, prop::arondto::ForceTorque>& fts,
        const std::vector<std::string>& split,
        const ConverterValue& value)
    {
        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 = tools->sidePrefixMap.find(splitName.at(1));
        ARMARX_CHECK(it != tools->sidePrefixMap.end()) << splitName.at(1);
 
        const std::string& side = it->second;
        processForceTorqueEntry(fts[side], split, value);
    }
 
 
    void Armar6Converter::processForceTorqueEntry(
        prop::arondto::ForceTorque& dto,
        const std::vector<std::string>& split,
        const ConverterValue& value)
    {
        const std::string& fieldName = split.at(2);
        if (auto getter = findByPrefix(fieldName, tools->ftGetters))
        {
            if (Eigen::Vector3f* dst = getter(dto))
            {
                if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
                {
                    setter(*dst, getValueAs<float>(value));
                }
            }
        }
        else
        {
            // No setter for this field. Put in extra.
            std::string key = split.size() == 4
                              ? (fieldName + "." + split.at(3))
                              : fieldName;
 
            switch (value.entry.type)
            {
                case RobotUnitDataStreaming::NodeTypeFloat:
                    dto.extra[key] = getValueAs<float>(value);
                    break;
                case RobotUnitDataStreaming::NodeTypeInt:
                    dto.extra[key] = getValueAs<int>(value);
                    break;
                case RobotUnitDataStreaming::NodeTypeLong:
                    dto.extra[key] = getValueAs<long>(value);
                    break;
                default:
                    ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
                                    << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
                    break;
            }
        }
    }
 
 
    bool Armar6Converter::processJointEntry(
        prop::arondto::Joints& dto,
        const std::vector<std::string>& split,
        const ConverterValue& value)
    {
        const std::string& jointName = split.at(1);
        const std::string& fieldName = split.at(2);
        if (false)
        {
            // Only in simulation.
            if (auto getter = findByPrefix(fieldName, tools->jointGetters))
            {
                if (std::map<std::string, float>* map = getter(dto))
                {
                    (*map)[jointName] = getValueAs<float>(value);
                }
            }
        }
 
        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 = tools->jointSetters.find(fieldName); it != tools->jointSetters.end())
        {
            const ConverterTools::JointSetter& setter = it->second;
            setter(dto, split, value);
            return true;
        }
        else
        {
            // ARMARX_DEBUG << "Ignoring unhandled field: '" << simox::alg::join(split, ".") << "'";
            return false;
        }
    }
 
}