AStar.cpp

Go to the documentation of this file.

#include "AStar.h"
 
#include <cstddef>
#include <optional>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <IceUtil/Time.h>
 
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/algorithms/astar/AStarPlanner.h>
#include <armarx/navigation/algorithms/smoothing/ChainApproximation.h>
#include <armarx/navigation/conversions/eigen.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/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 Params& params,
                 const core::GeneralConfig& generalConfig,
                 const core::Scene& ctx) :
        GlobalPlanner(generalConfig, ctx),
        impl_(params,
              generalConfig,
              scene.staticScene.has_value() ? scene.staticScene->distanceToObstaclesCostmap
                                            : std::nullopt)
    {
    }
 
    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)
    {
        if (scene.staticScene.has_value())
        {
            impl_.updateCostmap(scene.staticScene->distanceToObstaclesCostmap);
        }
        return impl_.plan(start, goal);
    }
 
    // AStarImpl
 
    AStarImpl::AStarImpl(const Params& params,
                         const core::GeneralConfig& generalConfig,
                         const std::optional<navigation::algorithms::Costmap>& costmap) :
        params(params), costmap_(costmap)
    {
    }
 
    void
    AStarImpl::updateCostmap(const std::optional<navigation::algorithms::Costmap>& costmap)
    {
        if (costmap.has_value())
        {
            costmap_.emplace(costmap.value());
        }
        else
        {
            costmap_.reset();
        }
    }
 
    std::vector<Eigen::Vector2f>
    AStarImpl::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();
        return p;
    }
 
    std::optional<GlobalPlannerResult>
    AStarImpl::plan(const core::Pose& start, const core::Pose& goal)
    {
        ARMARX_TRACE;
 
        // FIXME check if costmap is available
        algorithm::astar::AStarPlanner planner(*costmap_);
 
        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::Position> wpts;
        for (size_t i = 1; i < (plan3d.size() - 1); i++)
        {
            wpts.push_back(plan3d.at(i));
        }
 
        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, generalConfig_.maxVel.linear);
 
        // 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(generalConfig_.maxVel.linear);
 
        if (resampledTrajectory->points().size() == 2)
        {
            ARMARX_INFO << "Only start and goal provided. Not optimizing orientation";
            ARMARX_TRACE;
            return GlobalPlannerResult{.trajectory = resampledTrajectory.value(),
                                       .helperTrajectory = std::nullopt};
        }
 
        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(generalConfig.maxVel.linear);
 
        ARMARX_TRACE;
        return GlobalPlannerResult{.trajectory = result.trajectory.value(),
                                   .helperTrajectory = std::nullopt};
    }
} // namespace armarx::navigation::global_planning