aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.aron.generated.h>
#include <armarx/navigation/conversions/eigen.h>
 
 
namespace armarx::navigation::algorithms
{
 
    inline aron::data::NDArrayPtr
    toAron(const Costmap::Grid& grid)
    {
        return aron::converter::AronEigenConverter::ConvertFromMatrix(grid);
    }
 
 
    inline aron::data::NDArrayPtr
    toAron(const std::optional<Costmap::Mask>& mask)
    {
        if (not mask.has_value())
        {
            return nullptr; // maybe other type?
        }
 
        return aron::converter::AronEigenConverter::ConvertFromMatrix(mask.value());
    }
 
 
    inline Costmap::Mask
    fronAron(const aron::data::NDArray& nav)
    {
        return aron::converter::AronEigenConverter::ConvertToDynamicMatrix<
            Costmap::Mask::value_type>(nav);
    }
 
    armarx::aron::data::DictPtr
    toAron(const Costmap& bo)
    {
        arondto::Costmap dto;
 
        dto.cellSize = bo.params().cellSize;
 
        dto.frame = armarx::GlobalFrame;
        // FIXME: integrate costmap origin
        dto.origin = conv::to3D(bo.getLocalSceneBounds().min);
 
        auto arn = dto.toAron();
        arn->setElementCopy("mask", toAron(bo.getMask()));
        arn->setElementCopy("grid", toAron(bo.getGrid()));
 
        return arn;
    }
 
    Costmap
    fromAron(const armem::wm::EntityInstance& entityInstance)
    {
        const auto aronDto = armem::tryCast<algorithms::arondto::Costmap>(entityInstance);
        ARMARX_CHECK(aronDto) << "Failed casting to Costmap";
        const auto& dto = *aronDto;
 
        ARMARX_DEBUG << entityInstance.data()->getAllKeys();
 
        const Costmap::Parameters parameters{.binaryGrid = false, .cellSize = dto.cellSize};
 
        ARMARX_DEBUG << "Converting grid";
        const auto gridNavigator =
            aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("grid"));
 
        ARMARX_CHECK_NOT_NULL(gridNavigator);
 
        Costmap::Grid grid =
            aron::converter::AronEigenConverter::ConvertToDynamicMatrix<Costmap::Grid::value_type>(
                *gridNavigator);
 
        ARMARX_DEBUG << "Converting mask";
        std::optional<Costmap::Mask> mask;
 
        if (const auto maskNavigator =
                aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("mask")))
        {
            mask = aron::converter::AronEigenConverter::ConvertToDynamicMatrix<
                Costmap::Mask::value_type>(*maskNavigator);
        }
 
        const SceneBounds sceneBounds{
            .min = dto.origin.head<2>(),
            .max = dto.origin.head<2>() +
                   Eigen::Vector2f{grid.rows() * dto.cellSize, grid.cols() * dto.cellSize}};
 
        return {grid, parameters, sceneBounds, mask};
    }
 
    void
    toAron(arondto::ShortestPathFasterAlgorithmParams& dto,
           const spfa::ShortestPathFasterAlgorithm::Parameters& bo)
    {
        dto.obstacleDistanceCosts = bo.obstacleDistanceCosts;
        dto.obstacleMaxDistance = bo.obstacleMaxDistance;
        dto.obstacleDistanceWeight = bo.obstacleDistanceWeight;
        dto.obstacleCostExponent = bo.obstacleCostExponent;
    }
 
 
    void
    fromAron(const arondto::ShortestPathFasterAlgorithmParams& dto,
             spfa::ShortestPathFasterAlgorithm::Parameters& bo)
    {
        bo.obstacleDistanceCosts = dto.obstacleDistanceCosts;
        bo.obstacleMaxDistance = dto.obstacleMaxDistance;
        bo.obstacleDistanceWeight = dto.obstacleDistanceWeight;
        bo.obstacleCostExponent = dto.obstacleCostExponent;
    }
 
 
} // namespace armarx::navigation::algorithms