SPFA.cpp

Go to the documentation of this file.

#include "SPFA.h"
 
#include <algorithm>
#include <iterator>
#include <mutex>
#include <optional>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/Robot.h>
#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/core/basic_types.h>
#include <armarx/navigation/algorithms/smoothing/ChainApproximation.h>
#include <armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.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/SPFAParams.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>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
 
namespace armarx::navigation::global_planning
{
 
    // SPFAParams
 
    Algorithms
    SPFAParams::algorithm() const
    {
        return Algorithms::SPFA;
    }
 
    aron::data::DictPtr
    SPFAParams::toAron() const
    {
        arondto::SPFAParams dto;
 
        aron_conv::toAron(dto, *this);
 
        return dto.toAron();
    }
 
    SPFAParams
    SPFAParams::FromAron(const aron::data::DictPtr& dict)
    {
        ARMARX_CHECK_NOT_NULL(dict);
 
        // ARMARX_DEBUG << dict->getAllKeysAsString();
 
        arondto::SPFAParams dto;
        dto.fromAron(dict);
 
        SPFAParams bo;
        aron_conv::fromAron(dto, bo);
 
        return bo;
    }
 
    // SPFA
 
    SPFA::SPFA(const SPFAParams& params, const core::Scene& ctx) :
        GlobalPlanner(ctx), params_(params)
    {
    }
 
    std::optional<GlobalPlannerResult>
    SPFA::plan(const core::Pose& goal)
    {
        const core::Pose start(scene.robot->getGlobalPose());
        return plan(start, goal);
    }
 
    std::optional<GlobalPlannerResult>
    SPFA::plan(const core::Pose& start, const core::Pose& goal)
    {
        ARMARX_TRACE;
 
        // FIXME check if costmap is available
 
        algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams = params_.algo;
 
        ARMARX_CHECK(scene.staticScene.has_value());
        ARMARX_CHECK(scene.staticScene->distanceToObstaclesCostmap.has_value());
        algorithms::spfa::ShortestPathFasterAlgorithm planner(
            *scene.staticScene->distanceToObstaclesCostmap, spfaParams);
 
        const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
 
        const auto timeStart = IceUtil::Time::now();
 
        algorithms::spfa::ShortestPathFasterAlgorithm::PlanningResult plan;
        try
        {
            plan = planner.plan(conv::to2D(start.translation()), goalPos);
            ARMARX_CHECK(plan);
        }
        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_VERBOSE << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
        ARMARX_VERBOSE << "Path contains " << plan.path.size() << " points";
 
        ARMARX_DEBUG << "The plan consists of the following positions:";
        for (const auto& position : plan.path)
        {
            ARMARX_DEBUG << position;
        }
 
        ARMARX_TRACE;
        const auto plan3d = conv::to3D(plan.path);
 
        std::vector<core::Position> wpts;
        // when the position is already reached (but not the orientation) the planned path is empty
        if (plan3d.size() >= 2)
        {
            for (size_t i = 1; i < (plan3d.size() - 1); i++)
            {
                wpts.push_back(plan3d.at(i));
            }
        }
 
        // ARMARX_TRACE;
        // auto smoothPlan = postProcessPath(plan.path);
        // 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();
 
        const std::vector<float> velocities = [&]()
        {
            const float defaultVelocity = params_.linearVelocity;
 
            core::Positions positions;
            positions.reserve(wpts.size() + 2);
 
            positions.emplace_back(start.translation());
            positions.insert(positions.end(), wpts.begin(), wpts.end());
            positions.emplace_back(goal.translation());
 
            const auto maxVelocityBasedOnObstacles = [&](const core::Position& position) -> float
            {
                const auto minDistanceToObstaclesOpt =
                    scene.staticScene->distanceToObstaclesCostmap->value(
                        Eigen::Vector2f{position.head<2>()});
 
                if (not minDistanceToObstaclesOpt.has_value())
                {
                    return defaultVelocity;
                }
 
                const float clippedObstacleDistance =
                    std::min(minDistanceToObstaclesOpt.value(), spfaParams.obstacleMaxDistance);
 
                const float ds =
                    std::pow(1.F - clippedObstacleDistance / spfaParams.obstacleMaxDistance,
                             spfaParams.obstacleCostExponent);
 
                const float obstacleFactor = 1 / (1 + spfaParams.obstacleDistanceWeight * ds);
 
                return defaultVelocity * obstacleFactor;
            };
 
            return positions | ranges::views::transform(maxVelocityBasedOnObstacles) |
                   ranges::to_vector;
        }();
 
        ARMARX_TRACE;
        auto trajectory = core::GlobalTrajectory::FromPath(start, wpts, goal, velocities);
 
        // TODO(fabian.reister): resampling of trajectory
 
        // FIXME param
        std::optional<core::GlobalTrajectory> resampledTrajectory;
 
        try
        {
            // if (params.resampleDistance > 0 or false)
            // {
            resampledTrajectory = trajectory.resample(200);
            // }
            // else
            // {
            // resampledTrajectory = trajectory;
            // }
        }
        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(), params_.optimizerParams);
        auto result = optimizer.optimize();
 
        if (not result)
        {
            ARMARX_ERROR << "Optimizer failure";
            return std::nullopt;
        }
 
        // TODO circular path smoothing should be done now
 
        // algorithm::CircularPathSmoothing smoothing;
        // auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
        // smoothTrajectory.setMaxVelocity(params.linearVelocity);
 
        ARMARX_CHECK(scene.staticScene->distanceToObstaclesCostmap.has_value());
        const auto& costmap = scene.staticScene->distanceToObstaclesCostmap.value();
 
 
        for (auto& point : result.trajectory->mutablePoints())
        {
            const float distance = std::min<float>(
                spfaParams.obstacleMaxDistance,
                costmap.value(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()})
                    .value_or(0.F));
 
            if (spfaParams.obstacleDistanceCosts)
            {
                point.velocity = params_.linearVelocity /
                                 (1.F + spfaParams.obstacleDistanceWeight *
                                            std::pow(1 - distance / spfaParams.obstacleMaxDistance,
                                                     spfaParams.obstacleCostExponent));
            }
        }
 
 
        ARMARX_TRACE;
        return GlobalPlannerResult{.trajectory = result.trajectory.value()};
    }
 
    std::vector<Eigen::Vector2f>
    SPFA::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;
    }
 
 
} // namespace armarx::navigation::global_planning