aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <optional>
 
#include "RobotAPI/libraries/aron/core/aron_conversions.h"
 
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/global_planning/AStarWithOrientation.h>
#include <armarx/navigation/global_planning/aron/AStarParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron/GlobalPlannerParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron/OrientationOptimizerParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron/Point2PointParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron/SPFAParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron/AStarWithOrientationParams.aron.generated.h>
#include <armarx/navigation/global_planning/optimization/OrientationOptimizer.h>
 
namespace armarx::navigation::global_planning::aron_conv
{
    void
    toAron(arondto::GlobalPlannerParams& dto, const GlobalPlannerParams& bo)
    {
        ARMARX_ERROR << "Trying to convert GlobalPlannerParams. This is probably not what you want";
    }
 
    void
    fromAron(const arondto::GlobalPlannerParams& dto, GlobalPlannerParams& bo)
    {
        ARMARX_ERROR << "Trying to convert GlobalPlannerParams. This is probably not what you want";
    }
 
    void
    toAron(arondto::Point2PointParams& dto, const Point2PointParams& bo)
    {
        // fromAron(static_cast<const arondto::GlobalPlannerParams&>(dto), static_cast<const GlobalPlannerParams&>(bo));
        aron::toAron(dto.includeStartPose, bo.includeStartPose);
    }
 
    void
    fromAron(const arondto::Point2PointParams& dto, Point2PointParams& bo)
    {
        // fromAron(static_cast<arondto::GlobalPlannerParams&>(dto), static_cast<GlobalPlannerParams&>(bo));
        aron::fromAron(dto.includeStartPose, bo.includeStartPose);
    }
 
    void
    toAron(arondto::AStarParams& dto, const AStarParams& bo)
    {
        // toAron(static_cast<arondto::GlobalPlannerParams&>(dto),
        //        static_cast<const GlobalPlannerParams&>(bo));
 
        aron::toAron(dto.resampleDistance, bo.resampleDistance);
    }
 
    void
    fromAron(const arondto::AStarParams& dto, AStarParams& bo)
    {
        // fromAron(static_cast<const arondto::GlobalPlannerParams&>(dto),
        //          static_cast<GlobalPlannerParams&>(bo));
 
        aron::fromAron(dto.resampleDistance, bo.resampleDistance);
    }
 
    void
    toAron(arondto::SPFAParams& dto, const SPFAParams& bo)
    {
        // toAron(static_cast<arondto::GlobalPlannerParams&>(dto),
        //        static_cast<const GlobalPlannerParams&>(bo));
 
        aron::toAron(dto.resampleDistance, bo.resampleDistance);
        algorithms::toAron(dto.algo, bo.algo);
 
        toAron(dto.optimizer, bo.optimizerParams);
    }
 
    void
    fromAron(const arondto::SPFAParams& dto, SPFAParams& bo)
    {
        // fromAron(static_cast<const arondto::GlobalPlannerParams&>(dto),
        //          static_cast<GlobalPlannerParams&>(bo));
 
        aron::fromAron(dto.resampleDistance, bo.resampleDistance);
 
        algorithms::fromAron(dto.algo, bo.algo);
 
        fromAron(dto.optimizer, bo.optimizerParams);
    }
 
    // std::optional
    template <typename DtoT, typename BoT>
    void
    toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
    {
        if (bo.has_value())
        {
            dto = DtoT{};
            toAron(*dto, *bo);
        }
        else
        {
            dto = std::nullopt;
        }
    }
 
    void
    toAron(arondto::OrientationOptimizerParams& dto,
           const optimization::OrientationOptimizerParams& bo)
    {
        aron::toAron(dto.iterations, bo.iterations);
        aron::toAron(dto.movementDirWeightStart, bo.movementDirWeightStart);
        aron::toAron(dto.movementDirWeightEnd, bo.movementDirWeightEnd);
 
        aron::toAron(dto.smoothnessWeight, bo.smoothnessWeight);
        aron::toAron(dto.smoothnessWeightStartGoal, bo.smoothnessWeightStartGoal);
 
        aron::toAron(dto.priorStartWeight, bo.priorStartWeight);
        aron::toAron(dto.priorEndWeight, bo.priorEndWeight);
 
        aron::toAron(dto.startGoalDistanceThreshold, bo.startGoalDistanceThreshold);
 
        toAron(dto.predefinedRotationDirection, bo.predefinedRotationDirection);
    }
 
    template <typename DtoT, typename BoT>
    void
    fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
    {
        if (dto.has_value())
        {
            bo = BoT{};
            fromAron(*dto, *bo);
        }
        else
        {
            bo = std::nullopt;
        }
    }
 
    void
    fromAron(const arondto::OrientationOptimizerParams& dto,
             optimization::OrientationOptimizerParams& bo)
    {
        aron::fromAron(dto.iterations, bo.iterations);
        aron::fromAron(dto.movementDirWeightStart, bo.movementDirWeightStart);
        aron::fromAron(dto.movementDirWeightEnd, bo.movementDirWeightEnd);
 
        aron::fromAron(dto.smoothnessWeight, bo.smoothnessWeight);
        aron::fromAron(dto.smoothnessWeightStartGoal, bo.smoothnessWeightStartGoal);
 
        aron::fromAron(dto.priorStartWeight, bo.priorStartWeight);
        aron::fromAron(dto.priorEndWeight, bo.priorEndWeight);
 
        aron::fromAron(dto.startGoalDistanceThreshold, bo.startGoalDistanceThreshold);
 
        fromAron(dto.predefinedRotationDirection, bo.predefinedRotationDirection);
    }
 
    void
    toAron(arondto::RotationDirection& dto, const optimization::RotationDirection& bo)
    {
        switch (bo)
        {
            case optimization::RotationDirection::Any:
                dto.value = arondto::RotationDirection::ANY;
                break;
            case optimization::RotationDirection::Clockwise:
                dto.value = arondto::RotationDirection::CLOCK_WISE;
                break;
            case optimization::RotationDirection::CounterClockwise:
                dto.value = arondto::RotationDirection::COUNTER_CLOCK_WISE;
                break;
            default:
                ARMARX_ERROR << "Error converting RotationDirection: switch-case not comprehensive!";
        }
    }
 
    void
    fromAron(const arondto::RotationDirection& dto, optimization::RotationDirection& bo)
    {
        switch (dto.value)
        {
            case arondto::rotation_direction_details::Enum::ANY:
                bo = optimization::RotationDirection::Any;
                break;
            case arondto::rotation_direction_details::Enum::CLOCK_WISE:
                bo = optimization::RotationDirection::Clockwise;
                break;
            case arondto::rotation_direction_details::Enum::COUNTER_CLOCK_WISE:
                bo = optimization::RotationDirection::CounterClockwise;
                break;
            default:
                ARMARX_ERROR << "Error converting RotationDirection: switch-case not comprehensive!";
        }
    }
 
    void
    toAron(arondto::AStarWithOrientationParams& dto, const AStarWithOrientationParams& bo)
    {
 
    }
 
    void
    fromAron(const arondto::AStarWithOrientationParams& dto, AStarWithOrientationParams& bo)
    {
    }
 
 
} // namespace armarx::navigation::global_planning::aron_conv