aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <optional>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.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/aron/core/data/variant/complex/NDArray.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/algorithms/aron/Room.aron.generated.h>
#include <armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.aron.generated.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.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.robotRadius = bo.params().robotRadius;
 
        dto.frame = armarx::GlobalFrame;
 
        const Eigen::Isometry2f origin =
            bo.origin() * Eigen::Translation2f{bo.getLocalSceneBounds().min};
        dto.origin = navigation::conv::to3D(origin).matrix();
 
        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, .robotRadius = dto.robotRadius};
 
        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 = Eigen::Vector2f::Zero(),
            .max = Eigen::Vector2f{grid.rows() * dto.cellSize, grid.cols() * dto.cellSize}};
 
        const core::Pose2D origin = navigation::conv::to2D(Eigen::Isometry3f{dto.origin});
 
        return {grid, parameters, sceneBounds, mask, origin};
    }
 
    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;
    }
 
    void
    toAron(arondto::Room& dto, const Room& bo)
    {
        dto.height = bo.height;
        dto.name = bo.name;
        dto.zFloor = bo.zFloor;
        dto.polygon = conv::to3D(bo.polygon);
    }
 
    void
    fromAron(const arondto::Room& dto, Room& bo)
    {
        bo.height = dto.height;
        bo.name = dto.name;
        bo.zFloor = dto.zFloor;
        bo.polygon = conv::to2D(dto.polygon);
    }
 
 
} // namespace armarx::navigation::algorithms