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/aron/core/data/variant/container/List.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/Costmap3D.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/orientation_aware/Costmap3D.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
{
 
    namespace
    {
        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::ListPtr
        toAron(const orientation_aware::Costmap3D::Grid& bo)
        {
            aron::data::ListPtr list{new aron::data::List()};
            for (const auto& layer : bo)
            {
                list->addElement(toAron(layer));
            }
            return list;
        }
    } // namespace
 
    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
    costmapFromAron(const aron::data::DictPtr& dto)
    {
        const auto aronDto = algorithms::arondto::Costmap::FromAron(dto);
        ARMARX_DEBUG << dto->getAllKeys();
 
        const Costmap::Parameters parameters{.binaryGrid = false,
                                             .cellSize = aronDto.cellSize,
                                             .robotRadius = aronDto.robotRadius};
 
        ARMARX_DEBUG << "Converting grid";
        const auto gridNavigator =
            aron::data::NDArray::DynamicCast(dto->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(dto->getElement("mask")))
        {
            mask = aron::converter::AronEigenConverter::ConvertToDynamicMatrix<
                Costmap::Mask::value_type>(*maskNavigator);
        }
 
        const SceneBounds sceneBounds{
            .min = Eigen::Vector2f::Zero(),
            .max = Eigen::Vector2f{grid.rows() * aronDto.cellSize, grid.cols() * aronDto.cellSize}};
 
        const core::Pose2D origin = navigation::conv::to2D(Eigen::Isometry3f{aronDto.origin});
 
        return {grid, parameters, sceneBounds, mask, origin};
    }
 
    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};
    }
 
    armarx::aron::data::DictPtr
    toAron(const orientation_aware::Costmap3D& bo)
    {
        orientation_aware::arondto::Costmap3D dto;
 
        dto.cellSize = bo.params().cellSize;
        dto.orientations = bo.params().orientations;
 
        dto.rootNode = bo.params().robotModel.rootNode;
        dto.robotModelPackage = bo.params().robotModel.packageName;
        dto.robotModelPath = bo.params().robotModel.relativePath;
 
        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;
    }
 
    orientation_aware::Costmap3D
    costmap3dFromAron(const armem::wm::EntityInstance& entityInstance)
    {
        const auto aronDto =
            armem::tryCast<algorithms::orientation_aware::arondto::Costmap3D>(entityInstance);
        ARMARX_CHECK(aronDto) << "Failed casting to Costmap3D";
        const auto& dto = *aronDto;
 
        ARMARX_DEBUG << entityInstance.data()->getAllKeys();
 
        const orientation_aware::Costmap3D::Parameters parameters{
            .binaryGrid = false,
            .cellSize = dto.cellSize,
            .orientations = dto.orientations,
            .robotModel = {.rootNode = dto.rootNode,
                           .packageName = dto.robotModelPackage,
                           .relativePath = dto.robotModelPath}};
 
        ARMARX_DEBUG << "Converting grid";
        orientation_aware::Costmap3D::Grid grid;
        const auto gridNavigator =
            aron::data::List::DynamicCast(entityInstance.data()->getElement("grid"));
        const auto layerNavigators = gridNavigator->getElements();
        for (const auto& layerNavigator : layerNavigators)
        {
            const auto ndArrayNavigator = aron::data::NDArray::DynamicCast(layerNavigator);
            ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
            grid.emplace_back(
                aron::converter::AronEigenConverter::ConvertToDynamicMatrix<
                    orientation_aware::Costmap3D::Grid::value_type::value_type>(*ndArrayNavigator));
        }
 
        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[0].rows() * dto.cellSize, grid[0].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