ice_conversions.cpp

Go to the documentation of this file.

#include "ice_conversions.h"
 
#include <algorithm>
#include <iterator>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
 
namespace armarx::navigation::client
{
 
    client::GlobalPlanningStrategy
    defrost(const client::detail::GlobalPlanningStrategy& strategy)
    {
        client::GlobalPlanningStrategy strgy = client::GlobalPlanningStrategy::Point2Point;
 
        switch (strategy)
        {
            case client::detail::GlobalPlanningStrategy::Free:
                ARMARX_IMPORTANT << "Free navigation";
                strgy = client::GlobalPlanningStrategy::Free;
                break;
            case client::detail::GlobalPlanningStrategy::Point2Point:
                ARMARX_IMPORTANT << "Point2Point navigation";
                strgy = client::GlobalPlanningStrategy::Point2Point;
                break;
            default:
                ARMARX_UNEXPECTED_ENUM_VALUE(client::detail::GlobalPlanningStrategy, strategy);
        }
 
        return strgy;
    }
 
    WaypointTarget
    defrost(const client::detail::Waypoint& waypoint)
    {
        ARMARX_CHECK((not waypoint.locationId.empty()) or waypoint.pose)
            << "Either `locationId` or `pose` has to be set.";
 
        if (not waypoint.locationId.empty())
        {
            return WaypointTarget{
                .locationId = waypoint.locationId, .strategy = defrost(waypoint.strategy)
                // .config = defrost(waypoint.config)
            };
        }
 
        if (waypoint.pose)
        {
            return WaypointTarget{
                .pose = core::Pose(armarx::fromIce(waypoint.pose)),
                .strategy = defrost(waypoint.strategy)
                // .config = defrost(waypoint.config)
            };
        }
 
        throw armarx::LocalException("Invalid use of client::detail::Waypoint: either `locationId` "
                                     "or `pose` has to be set!");
    }
 
    WaypointTargets
    defrost(const client::detail::Waypoints& waypoints)
    {
        WaypointTargets targets;
        targets.reserve(waypoints.size());
 
        std::transform(waypoints.begin(),
                       waypoints.end(),
                       std::back_inserter(targets),
                       [](const auto& wp) { return defrost(wp); });
 
        return targets;
    }
 
    core::TargetAlternative
    defrost(const client::detail::TargetAlternative& alternative)
    {
        return core::TargetAlternative{
            .target = core::Pose(alternative.target),
            .priority = alternative.priority,
        };
    }
 
    std::vector<core::TargetAlternative>
    defrost(const client::detail::TargetAlternatives& alternatives)
    {
        std::vector<core::TargetAlternative> result;
        result.reserve(alternatives.size());
 
        std::transform(alternatives.begin(),
                       alternatives.end(),
                       std::back_inserter(result),
                       [](const auto& alt) { return defrost(alt); });
 
        return result;
    }
 
    client::detail::GlobalPlanningStrategy
    freeze(const client::GlobalPlanningStrategy& strategy)
    {
        client::detail::GlobalPlanningStrategy strgy =
            client::detail::GlobalPlanningStrategy::Point2Point;
 
        switch (strategy)
        {
            case client::GlobalPlanningStrategy::Free:
                strgy = client::detail::GlobalPlanningStrategy::Free;
                break;
            case client::GlobalPlanningStrategy::Point2Point:
                strgy = client::detail::GlobalPlanningStrategy::Point2Point;
                break;
            default:
                ARMARX_UNEXPECTED_ENUM_VALUE(client::GlobalPlanningStrategy, strategy);
        }
 
        return strgy;
    }
 
    client::detail::Waypoint
    freeze(const client::WaypointTarget& target)
    {
        return client::detail::Waypoint{.locationId = target.locationId.value_or(""),
                                        .pose =
                                            target.pose ? toIce(target.pose->matrix()) : nullptr,
                                        .strategy = freeze(target.strategy)};
    }
 
    client::detail::Waypoints
    freeze(const client::WaypointTargets& targets)
    {
        client::detail::Waypoints waypoints;
        waypoints.reserve(targets.size());
 
        std::transform(targets.begin(),
                       targets.end(),
                       std::back_inserter(waypoints),
                       [](const auto& wp) { return freeze(wp); });
 
        return waypoints;
    }
 
    client::detail::TargetAlternative
    freeze(const core::TargetAlternative& alternative)
    {
        return client::detail::TargetAlternative{
            .target = alternative.target.matrix(),
            .priority = alternative.priority,
        };
    }
 
    client::detail::TargetAlternatives
    freeze(const std::vector<core::TargetAlternative>& alternatives)
    {
        client::detail::TargetAlternatives result;
        result.reserve(alternatives.size());
 
        std::transform(alternatives.begin(),
                       alternatives.end(),
                       std::back_inserter(result),
                       [](const auto& alt) { return freeze(alt); });
 
        return result;
    }
 
} // namespace armarx::navigation::client