ArmarDEConverter.cpp

Go to the documentation of this file.

#include "ArmarDEConverter.h"
 
#include <cmath>
#include <cstddef>
#include <map>
#include <set>
#include <string>
#include <vector>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include "ConverterTools.h"
 
namespace armarx::armem::server::robot_state::exteroception
{
 
    ArmarDEConverter::ArmarDEConverter(const Properties& properties) : properties_(properties)
    {
    }
 
    ArmarDEConverter::~ArmarDEConverter() = default;
 
    aron::data::DictPtr
    ArmarDEConverter::convert(const RobotUnitDataStreaming::TimeStep& data,
                              const RobotUnitDataStreaming::DataStreamingDescription& description)
    {
        arondto::Exteroception dto;
        dto.iterationID = data.iterationId;
 
        // initialize fields
        const std::size_t singleDimSize = static_cast<std::size_t>(std::sqrt(properties_.tofSize));
        ARMARX_CHECK_EQUAL(singleDimSize * singleDimSize, properties_.tofSize);
 
        // initialize empty
        for (const auto& id : {"Left", "Right"})
        {
            // FIXME change if unsigned
            dto.tof[id].depth.resize(singleDimSize, singleDimSize);
            dto.tof[id].depth.setConstant(-1); // in order to unset fields faults
 
            // FIXME change if unsigned
            dto.tof[id].sigma.resize(singleDimSize, singleDimSize);
            dto.tof[id].sigma.setConstant(-1); // in order to unset fields faults
 
            // FIXME change if unsigned
            dto.tof[id].targetDetected.resize(singleDimSize, singleDimSize);
            dto.tof[id].targetDetected.setConstant(-1); // in order to unset fields faults
        }
 
 
        for (const auto& [dataEntryName, dataEntry] : description.entries)
        {
            process(dto, dataEntryName, {.data = data, .entry = dataEntry});
        }
 
        // Ensure that we only generate updates if data has changed. As the ToF sensors might only generate data at a lower rate (e.g., 15Hz) and the
        // read loop in the unit might run at a higher rate, we might end up with a lot of duplicate data. Therefore, we check if the data is still
        // the same and remove those entries from the update.
        {
            std::vector<std::string> keysToRemove;
            for (const auto& [id, data] : dto.tof)
            {
                // Check if this is the first time we got data
                if (lastData.count(id) == 0)
                {
                    lastData.emplace(id, data);
 
                    {
                        // This is a robust HACK: as it might happen that the connection to the ToF sensor was lost at some point, the robot unit streaming 
                        // might report some old data which does not change. Therefore, we initialize lastData and do not report it unless there is a change.
                        keysToRemove.push_back(id);
                    }
                    
                    continue; // new data can be kept
                }
 
                // Check if the data is similar and remove it from the update. Here, we maintain a separate vector to avoid interference with the for loop
                if (data == lastData.at(id))
                {
                    keysToRemove.push_back(id);
                }
 
                // Update the history
                lastData[id] = data;
            }
 
            // remove the found elements from the update
            for (const auto& key : keysToRemove)
            {
                dto.tof.erase(key);
            }
        }
 
 
        // If the data has not changed at all, we indicate this by a nullptr
        if (dto.tof.empty())
        {
            return nullptr;
        }
 
 
        // resize to square
        // for (auto& [_, tof] : dto.tof)
        // {
        //     const int sr = std::sqrt(tof.array.size());
        //     const bool isSquare = (sr * sr == tof.array.size());
 
        //     if (tof.array.size() > 0 and isSquare)
        //     {
        //         tof.array.resize(sr, sr);
        //     }
        // }
 
 
        return dto.toAron();
    }
 
    void
    ArmarDEConverter::process(arondto::Exteroception& 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 (simox::alg::contains(field, "tofDepth"))
        {
            // ARMARX_DEBUG << "Processing ToF sensor data";
            processToFDepthEntry(dto.tof, split, value);
        }
 
        if (simox::alg::contains(field, "tofSigma"))
        {
            // ARMARX_DEBUG << "Processing ToF sensor data";
            processToFSigmaEntry(dto.tof, split, value);
        }
 
        if (simox::alg::contains(field, "tofTargetDetected"))
        {
            // ARMARX_DEBUG << "Processing ToF sensor data";
            processToFTargetDetectedEntry(dto.tof, split, value);
        }
    }
 
    void
    ArmarDEConverter::processToFDepthEntry(
        std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
        const std::vector<std::string>& split,
        const ConverterValue& value)
    {
        // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
 
        ARMARX_CHECK_EQUAL(split.size(), 4);
        ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
 
        const std::string& name = split.at(1);
 
        // element 0 sens
        // element 1 is either LeftHand or RightHand
 
        const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
                                                               {"RightHand", "Right"}};
 
        auto it = sidePrefixMap.find(name);
        ARMARX_CHECK(it != sidePrefixMap.end()) << name;
 
        const std::string& side = it->second;
        processToFDepthEntry(tofs[side], split, value);
    }
 
    void
    ArmarDEConverter::processToFDepthEntry(armarx::armem::exteroception::arondto::ToF& tof,
                                           const std::vector<std::string>& split,
                                           const ConverterValue& value)
    {
        // split, e.g., element_12
        const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
        ARMARX_CHECK_EQUAL(elements.size(), 2);
 
        const int idx = std::stoi(elements.at(1));
 
        ARMARX_CHECK_LESS(idx, tof.depth.size());
 
        // we don't allow dynamic ToF size atm
        // if (tof.array.size() < (idx + 1))
        // {
        //     tof.array.resize(idx + 1, 1);
        // }
 
        tof.depth(idx) = getValueAs<float>(value);
    }
 
    void
    ArmarDEConverter::processToFSigmaEntry(
        std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
        const std::vector<std::string>& split,
        const ConverterValue& value)
    {
        // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
 
        ARMARX_CHECK_EQUAL(split.size(), 4);
        ARMARX_CHECK_EQUAL(split.at(2), "tofSigma");
 
        const std::string& name = split.at(1);
 
        // element 0 sens
        // element 1 is either LeftHand or RightHand
 
        const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
                                                               {"RightHand", "Right"}};
 
        auto it = sidePrefixMap.find(name);
        ARMARX_CHECK(it != sidePrefixMap.end()) << name;
 
        const std::string& side = it->second;
        processToFSigmaEntry(tofs[side], split, value);
    }
 
    void
    ArmarDEConverter::processToFSigmaEntry(armarx::armem::exteroception::arondto::ToF& tof,
                                           const std::vector<std::string>& split,
                                           const ConverterValue& value)
    {
        // split, e.g., element_12
        const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
        ARMARX_CHECK_EQUAL(elements.size(), 2);
 
        const int idx = std::stoi(elements.at(1));
 
        ARMARX_CHECK_LESS(idx, tof.sigma.size());
 
        // we don't allow dynamic ToF size atm
        // if (tof.array.size() < (idx + 1))
        // {
        //     tof.array.resize(idx + 1, 1);
        // }
 
        tof.sigma(idx) = getValueAs<float>(value);
    }
 
    void
    ArmarDEConverter::processToFTargetDetectedEntry(
        std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
        const std::vector<std::string>& split,
        const ConverterValue& value)
    {
        // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
 
        ARMARX_CHECK_EQUAL(split.size(), 4);
        ARMARX_CHECK_EQUAL(split.at(2), "tofTargetDetected");
 
        const std::string& name = split.at(1);
 
        // element 0 sens
        // element 1 is either LeftHand or RightHand
 
        const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
                                                               {"RightHand", "Right"}};
 
        auto it = sidePrefixMap.find(name);
        ARMARX_CHECK(it != sidePrefixMap.end()) << name;
 
        const std::string& side = it->second;
        processToFTargetDetectedEntry(tofs[side], split, value);
    }
 
    void
    ArmarDEConverter::processToFTargetDetectedEntry(armarx::armem::exteroception::arondto::ToF& tof,
                                                    const std::vector<std::string>& split,
                                                    const ConverterValue& value)
    {
        // split, e.g., element_12
        const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
        ARMARX_CHECK_EQUAL(elements.size(), 2);
 
        const int idx = std::stoi(elements.at(1));
 
        ARMARX_CHECK_LESS(idx, tof.targetDetected.size());
 
        // we don't allow dynamic ToF size atm
        // if (tof.array.size() < (idx + 1))
        // {
        //     tof.array.resize(idx + 1, 1);
        // }
 
        tof.targetDetected(idx) = getValueAs<float>(value);
    }
 
 
} // namespace armarx::armem::server::robot_state::exteroception