AStar.cpp

Go to the documentation of this file.

#include "AStar.h"
 
#include <algorithm>
#include <mutex>
#include <optional>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
 
#include <armarx/navigation/algorithms/astar/AStarPlanner.h>
#include <armarx/navigation/algorithms/smoothing/ChainApproximation.h>
#include <armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/aron/AStarParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron_conversions.h>
#include <armarx/navigation/global_planning/core.h>
#include <armarx/navigation/global_planning/optimization/OrientationOptimizer.h>
 
 
namespace armarx::navigation::global_planning
{
 
    // AStarParams
 
    Algorithms
    AStarParams::algorithm() const
    {
        return Algorithms::AStar;
    }
 
    aron::data::DictPtr
    AStarParams::toAron() const
    {
        arondto::AStarParams dto;
 
        AStarParams bo;
        aron_conv::toAron(dto, bo);
 
        return dto.toAron();
    }
 
    AStarParams
    AStarParams::FromAron(const aron::data::DictPtr& dict)
    {
        ARMARX_CHECK_NOT_NULL(dict);
 
        // ARMARX_DEBUG << dict->getAllKeysAsString();
 
        arondto::AStarParams dto;
        dto.fromAron(dict);
 
        AStarParams bo;
        aron_conv::fromAron(dto, bo);
 
        return bo;
    }
 
    // AStar
 
    AStar::AStar(const AStarParams& params, const core::Scene& ctx) :
        GlobalPlanner(ctx), params(params)
    {
    }
 
    // auto
    // robotCollisionModel()
    // {
 
    //     constexpr float platformHeight = 600;
    //     constexpr float bodyHeight = 1400;
    //     constexpr float fullHeight = platformHeight + bodyHeight;
 
    //     constexpr float platformRadius = 600.F;
    //     constexpr float bodyRadius = 300.F;
    //     constexpr float simpleRadius = std::max(platformRadius, bodyRadius);
 
    //     // combined model made out of two stacked cylinders.
    //     [[maybe_unused]] const auto createCombinedVisuModel = []()
    //     {
    //         VirtualRobot::CoinVisualizationFactory factory;
 
    //         auto cylinderPlatform = factory.createCylinder(platformRadius, platformHeight);
    //         factory.applyDisplacement(
    //             cylinderPlatform,
    //             core::Pose(Eigen::Translation3f{platformHeight / 2 * Eigen::Vector3f::UnitY()})
    //                 .matrix());
 
    //         auto cylinderBody = factory.createCylinder(bodyRadius, bodyHeight);
    //         factory.applyDisplacement(
    //             cylinderPlatform,
    //             core::Pose(Eigen::Translation3f{(bodyHeight / 2 + platformHeight) *
    //                                             Eigen::Vector3f::UnitY()})
    //                 .matrix());
 
 
    //         return factory.createUnitedVisualization({cylinderPlatform, cylinderBody});
    //     };
 
    //     // simple model made of one cylinder only
    //     const auto createSimpleVisuModel = []()
    //     {
    //         VirtualRobot::CoinVisualizationFactory factory;
 
    //         auto cylinderPlatform = factory.createCylinder(simpleRadius, fullHeight);
    //         factory.applyDisplacement(
    //             cylinderPlatform,
    //             core::Pose(Eigen::Translation3f{fullHeight / 2 * Eigen::Vector3f::UnitY()})
    //                 .matrix());
 
    //         return cylinderPlatform;
    //     };
 
    //     const auto visuModel = createSimpleVisuModel();
 
    //     VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(visuModel));
 
    //     return collisionModel;
    // }
 
    std::vector<Eigen::Vector2f>
    AStar::postProcessPath(const std::vector<Eigen::Vector2f>& path)
    {
        /// chain approximation
        algorithm::ChainApproximation approx(
            path, algorithm::ChainApproximation::Params{.distanceTh = 200.F});
        approx.approximate();
        const auto p = approx.approximatedChain();
 
        // visualizePath(p, "approximated", simox::Color::green());
 
        // algo::CircularPathSmoothing smoothing;
        // const auto points = smoothing.smooth(p);
 
        return p;
    }
 
    std::optional<GlobalPlannerResult>
    AStar::plan(const core::Pose& goal)
    {
        const core::Pose start(scene.robot->getGlobalPose());
        return plan(start, goal);
    }
 
    std::optional<GlobalPlannerResult>
    AStar::plan(const core::Pose& start, const core::Pose& goal)
    {
        ARMARX_TRACE;
 
        // FIXME check if costmap is available
        algorithm::astar::AStarPlanner planner(*scene.staticScene->distanceToObstaclesCostmap);
 
        const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
 
        const auto timeStart = IceUtil::Time::now();
 
        std::vector<Eigen::Vector2f> plan;
        try
        {
            plan = planner.plan(conv::to2D(start.translation()), goalPos);
        }
        catch (...)
        {
            ARMARX_INFO << "Could not plan collision-free path from"
                        << "(" << start.translation().x() << "," << start.translation().y() << ")"
                        << " to "
                        << "(" << goal.translation().x() << "," << goal.translation().y() << ")"
                        << " due to exception " << GetHandledExceptionString();
 
            return std::nullopt;
        }
        const auto timeEnd = IceUtil::Time::now();
 
        ARMARX_IMPORTANT << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
        ARMARX_IMPORTANT << "Path contains " << plan.size() << " points";
 
        if (plan.size() < 2) // failure
        {
            ARMARX_INFO << "Could not plan collision-free path from"
                        << "(" << start.translation().x() << "," << start.translation().y() << ")"
                        << " to "
                        << "(" << goal.translation().x() << "," << goal.translation().y() << ")";
            return std::nullopt;
        }
 
        ARMARX_DEBUG << "The plan consists of the following positions:";
        for (const auto& position : plan)
        {
            ARMARX_DEBUG << position;
        }
 
        const core::Pose startPose(Eigen::Translation3f(conv::to3D(plan.front())));
        const core::Pose goalPose(Eigen::Translation3f(conv::to3D(plan.back())));
 
        const auto plan3d = conv::to3D(plan);
 
        // std::vector<core::Pose> plan3d = plan3dtmp | ranges::views::transform([](const core::Position& p) -> core::Pose{
        //     return core::Pose(Eigen::Translation3f(p));
        // }) | ranges::to_vector;
 
        // auto waypoints =
        //     // plan3d | ranges::views::counted(plan3d.begin(), 2) |
        //     plan3d | ranges::make_subrange(plan3d.begin(), plan3d.end()) |
        //     ranges::to_vector;
 
        std::vector<core::Position> wpts;
        for (size_t i = 1; i < (plan3d.size() - 1); i++)
        {
            wpts.push_back(plan3d.at(i));
        }
 
        // TODO remove start and goal and use plan front and back instead
        // return GlobalPlannerResult{
        //     .trajectory = core::Trajectory::FromPath(
        //         startPose, wpts, goalPose, params.linearVelocity)}; // FIXME remove
 
        ARMARX_TRACE;
        auto smoothPlan = postProcessPath(plan);
        ARMARX_IMPORTANT << "Smooth path contains " << smoothPlan.size() << " points";
 
        ARMARX_TRACE;
        // we need to strip the first and the last points from the plan as they encode the start and goal position
        smoothPlan.erase(smoothPlan.begin());
        smoothPlan.pop_back();
 
        ARMARX_TRACE;
        auto trajectory = core::GlobalTrajectory::FromPath(
            start, conv::to3D(smoothPlan), goal, params.linearVelocity);
 
        // TODO(fabian.reister): resampling of trajectory
 
        // FIXME param
        std::optional<core::GlobalTrajectory> resampledTrajectory;
 
        if (params.resampleDistance < 0)
        {
            resampledTrajectory = trajectory;
        }
        else
        {
            try
            {
                resampledTrajectory = trajectory.resample(params.resampleDistance);
            }
            catch (...)
            {
                ARMARX_INFO << "Caught exception during resampling: "
                            << GetHandledExceptionString();
                // return std::nullopt;
                resampledTrajectory = trajectory;
            }
        }
 
        ARMARX_VERBOSE << "Resampled trajectory contains " << resampledTrajectory->points().size()
                       << " points";
 
        resampledTrajectory->setMaxVelocity(params.linearVelocity);
 
 
        // ARMARX_CHECK(resampledTrajectory.hasMaxDistanceBetweenWaypoints(params.resampleDistance));
 
        if (resampledTrajectory->points().size() == 2)
        {
            ARMARX_INFO << "Only start and goal provided. Not optimizing orientation";
            ARMARX_TRACE;
            return GlobalPlannerResult{.trajectory = resampledTrajectory.value()};
        }
 
        ARMARX_TRACE;
        OrientationOptimizer optimizer(resampledTrajectory.value(), OrientationOptimizer::Params{});
        const auto result = optimizer.optimize();
 
        if (not result)
        {
            ARMARX_WARNING << "Optimizer failure";
            return std::nullopt;
        }
 
        ARMARX_TRACE;
 
        // TODO circular path smoothing should be done now
        // algorithm::CircularPathSmoothing smoothing;
        // auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
        // smoothTrajectory.setMaxVelocity(params.linearVelocity);
 
        ARMARX_TRACE;
        return GlobalPlannerResult{.trajectory = result.trajectory.value()};
    }
} // namespace armarx::navigation::global_planning