Point2Point.cpp

Go to the documentation of this file.

#include "Point2Point.h"
 
#include <optional>
#include <vector>
 
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/aron/Point2PointParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron_conversions.h>
#include <armarx/navigation/global_planning/core.h>
 
namespace armarx::navigation::global_planning
{
 
    // Point2PointParams
 
    Algorithms
    Point2PointParams::algorithm() const
    {
        return Algorithms::Point2Point;
    }
 
    aron::data::DictPtr
    Point2PointParams::toAron() const
    {
        arondto::Point2PointParams dto;
 
        Point2PointParams bo;
        aron_conv::toAron(dto, bo);
 
        return dto.toAron();
    }
 
    Point2PointParams
    Point2PointParams::FromAron(const aron::data::DictPtr& dict)
    {
        ARMARX_CHECK_NOT_NULL(dict);
 
        arondto::Point2PointParams dto;
        dto.fromAron(dict);
 
        Point2PointParams bo;
        aron_conv::fromAron(dto, bo);
 
        return bo;
    }
 
    // Point2Point
 
    Point2Point::Point2Point(const Point2Point::Params& params, const core::Scene& ctx) :
        GlobalPlanner(ctx), params(params)
    {
    }
 
    std::optional<GlobalPlannerResult>
    Point2Point::plan(const core::Pose& goal)
    {
        std::vector<core::GlobalTrajectoryPoint> trajectory;
 
        if (params.includeStartPose)
        {
            trajectory.push_back(core::GlobalTrajectoryPoint{
                .waypoint = core::Waypoint{.pose = core::Pose(scene.robot->getGlobalPose())},
                .velocity = params.velocity});
        }
 
        trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
                                                         .velocity = params.velocity});
 
        return GlobalPlannerResult{.trajectory = trajectory};
    }
 
    std::optional<GlobalPlannerResult>
    Point2Point::plan(const core::Pose& start, const core::Pose& goal)
    {
        std::vector<core::GlobalTrajectoryPoint> trajectory;
 
        if (params.includeStartPose)
        {
            trajectory.push_back(core::GlobalTrajectoryPoint{
                .waypoint = core::Waypoint{.pose = start}, .velocity = params.velocity});
        }
 
        trajectory.push_back(core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
                                                         .velocity = params.velocity});
 
        return GlobalPlannerResult{.trajectory = trajectory};
    }
 
} // namespace armarx::navigation::global_planning