AStarPlanner.cpp

Go to the documentation of this file.

#include "AStarPlanner.h"
 
#include <math.h>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <experimental/memory>
#include <map>
#include <memory>
#include <optional>
#include <queue>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <qnamespace.h>
 
#include <range/v3/algorithm/reverse.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/map.hpp>
#include <range/v3/view/transform.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/algorithms/orientation_aware/Node.h>
#include <armarx/navigation/algorithms/orientation_aware/io.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <omp.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
 
    Grid::Grid(int rows, int cols, int orientations, const Costmap3D& costmap, float clearance)
    {
        this->clearance = clearance;
        initialize(rows, cols, orientations, costmap);
    }
 
    void
    Grid::clear()
    {
        for (int r = 0; r < rows; r++)
        {
            for (int c = 0; c < cols; c++)
            {
                for (int rot_idx = 0; rot_idx < orientations; rot_idx++)
                {
                    Node& node = getNode(r, c, rot_idx);
                    node.inOpenSet = false;
                    node.inClosedSet = false;
                    node.fScore = -1.F;
                    node.gScore = -1.F;
                }
            }
        }
    }
 
    Node&
    Grid::getNode(int row, int col, int orientation)
    {
        return data_[((row * cols) + col) * orientations + orientation];
    }
 
    void
    Grid::initialize(int rows, int cols, int orientations, const Costmap3D& costmap)
    {
        ARMARX_TRACE;
        this->rows = rows;
        this->cols = cols;
        this->orientations = orientations;
 
        // Measure time
        armarx::core::time::DateTime t1 = armarx::core::time::DateTime::Now();
 
        // create the grid with the correct size
        ARMARX_VERBOSE << "Creating grid";
 
        data_ = std::make_unique<Node[]>(rows * cols * orientations);
 
        // intitializing grid
        for (int r = 0; r < rows; r++)
        {
            for (int c = 0; c < cols; c++)
            {
                for (int rot_idx = 0; rot_idx < orientations; rot_idx++)
                {
                    const auto pos = costmap.toPositionGlobal({r, c});
                    const float obstacleDistance = costmap.value_ignore_mask(
                        Costmap3D::Index{r, c}, Costmap3D::RotationIndex{rot_idx});
                    const auto rotation_deg =
                        costmap.rotationFromIndex(Costmap3D::RotationIndex{rot_idx}).degrees;
 
                    getNode(r, c, rot_idx).initialize(pos, obstacleDistance, rotation_deg);
                }
            }
        }
 
 
        ARMARX_VERBOSE << "Creating graph";
 
        ARMARX_CHECK(costmap.params().orientations == orientations);
 
        // Init successors
        for (int r = 0; r < rows; r++)
        {
            for (int c = 0; c < cols; c++)
            {
                for (int rot_idx = 0; rot_idx < orientations; rot_idx++)
                {
                    Node& candidate = getNode(r, c, rot_idx);
                    if (!fulfillsConstraints(candidate, costmap, this->clearance))
                    {
                        continue;
                    }
 
                    // Add the valid node as successor to all its neighbors
                    for (int nR = -1; nR <= 1; nR++)
                    {
                        for (int nC = -1; nC <= 1; nC++)
                        {
                            const int neighborIndexC = c + nC;
                            const int neighborIndexR = r + nR;
                            if (neighborIndexC < 0 || neighborIndexR < 0 || (nR == 0 && nC == 0))
                            {
                                continue;
                            }
 
                            if (neighborIndexR >= static_cast<int>(rows) ||
                                neighborIndexC >= static_cast<int>(cols))
                            {
                                continue;
                            }
 
                            //grid[neighborIndexR][neighborIndexC][rot_idx]->successors.push_back(candidate);
 
                            for (int nRot = -3; nRot <= 3; nRot++)
                            {
                                int neighborRotIndex = rot_idx + nRot;
                                if (neighborRotIndex < 0)
                                {
                                    neighborRotIndex += orientations;
                                }
                                if (neighborRotIndex >= orientations)
                                {
                                    neighborRotIndex -= orientations;
                                }
                                getNode(neighborIndexR, neighborIndexC, neighborRotIndex)
                                    .successors.emplace_back(&candidate);
                            }
                        }
                    }
 
                    // Add successors by just rotating in place
                    bool enableRotationInPlace =
                        false; // currently has problems with zero-lenght vectors for distance to previous node (would need to be handled separately)
                    for (int nRot = -1; enableRotationInPlace && nRot <= 1; nRot++)
                    {
                        if (nRot == 0)
                        {
                            continue;
                        }
                        int neighborRotIndex = rot_idx + nRot;
                        if (neighborRotIndex < 0)
                        {
                            neighborRotIndex += orientations;
                        }
                        if (neighborRotIndex >= orientations)
                        {
                            neighborRotIndex -= orientations;
                        }
                        getNode(r, c, neighborRotIndex).successors.emplace_back(&candidate);
                    }
                }
            }
        }
 
        armarx::core::time::DateTime t2 = armarx::core::time::DateTime::Now();
        ARMARX_VERBOSE << "Grid created in " << (t2 - t1).toMilliSeconds() << " ms";
    }
 
    bool
    Grid::fulfillsConstraints(const Node& n, const Costmap3D& costmap, float clearance)
    {
        ARMARX_TRACE;
 
        // Treat nodes within the clearance zone as collision.
        return n.obstacleDistance > clearance;
    }
 
    AStarPlanner::AStarPlanner(const Costmap3D& costmap3d, io::AStarWithOrientationParams params) :
        costmap3d(costmap3d), params(params)
    {
    }
 
    float
    AStarPlanner::heuristic(const Node& n1, const Node& n2)
    {
        // TODO: Update heuristic and cost functions
        // Cost function for orientation change should not be 0, otherwise the robot would rotate more than what we want
        //
        return std::max(1.F - params.obstacleDistanceWeightFactor, 0.F) *
               (n1.position - n2.position).norm();
 
        // maybe we also need to include the orientation in the heuristic?
    }
 
    Node&
    AStarPlanner::closestNode(const core::Pose2D& pose)
    {
        float rot_deg = Eigen::Rotation2Df{pose.rotation()}.angle() * 180.F / M_PI;
        if (rot_deg <= 0.F)
        {
            rot_deg += 360.F;
        }
        ARMARX_DEBUG << VAROUT(rot_deg);
 
        const auto vertex = costmap3d.toVertex(pose.translation());
        const auto rot = costmap3d.closestRotationFromDegrees(rot_deg);
        ARMARX_DEBUG << "Closest rotation: " << rot.index << " (" << rot.degrees << " deg)";
 
        const int r = vertex.index.x();
        const int c = vertex.index.y();
 
        ARMARX_CHECK_GREATER_EQUAL(r, 0.F);
        ARMARX_CHECK_GREATER_EQUAL(c, 0.F);
        ARMARX_CHECK_GREATER_EQUAL(rot.index, 0);
 
        const int rows = static_cast<int>(costmap3d.getSize().x());
        const int cols = static_cast<int>(costmap3d.getSize().y());
 
        ARMARX_CHECK_LESS_EQUAL(r, rows - 1);
        ARMARX_CHECK_LESS_EQUAL(c, cols - 1);
        ARMARX_CHECK_LESS_EQUAL(rot.index, costmap3d.params().orientations - 1);
 
        return grid->getNode(static_cast<int>(r), static_cast<int>(c), rot.index);
    }
 
    namespace
    {
 
        float
        angleBetween(const Eigen::Vector2f& a, const Eigen::Vector2f& b)
        {
            float dotProd = a.dot(b);
            float magA = a.norm();
            float magB = b.norm();
 
            if (magA == 0.0f || magB == 0.0f)
            {
                // This is expected if in-place rotations are allowed. However, in other cases this would be unexpected behavior.
                return 0.F;
            }
 
            float cosTheta = dotProd / (magA * magB);
 
            // Clamp to [-1, 1] to avoid NaNs due to floating-point errors
            if (cosTheta > 1.0f)
                cosTheta = 1.0f;
            if (cosTheta < -1.0f)
                cosTheta = -1.0f;
 
            return std::acos(cosTheta); // returns radians
        }
 
        float
        smoothnessCosts(const Node& n1,
                        const Node& n2,
                        const std::optional<Node>& n3,
                        const std::optional<Node>& n4)
        {
            float cost = 0;
 
            auto angle_to_cost = [&](float angle) { return angle * angle; };
 
            if (n3.has_value())
            {
                const Eigen::Vector2f v21 = n1.position - n2.position;
                const Eigen::Vector2f v23 = n3.value().position - n2.position;
                float angle = std::abs(angleBetween(v21, -v23));
                ARMARX_CHECK(angle <= M_PI + 0.001);
                cost += angle_to_cost(angle);
            }
            if (false && n3.has_value() && n4.has_value())
            {
                const Eigen::Vector2f v32 = n2.position - n3.value().position;
                const Eigen::Vector2f v34 = n4.value().position - n3.value().position;
                float angle = angleBetween(v32, -v34);
                ARMARX_CHECK(angle <= M_PI + 0.001);
                cost += angle_to_cost(angle);
            }
            return cost;
        }
    } // namespace
 
    std::vector<core::Pose2D>
    AStarPlanner::plan(const core::Pose2D& start, const core::Pose2D& goal)
    {
        ARMARX_TRACE;
 
        if (grid.has_value())
        {
            grid->clear();
        }
        else
        {
            // initialize grid
            const auto size = costmap3d.getSize();
            grid = {size.x(), size.y(), costmap3d.params().orientations, costmap3d, params.clearance};
        }
        std::vector<core::Pose2D> result;
        expandedNodes = 0;
 
        ARMARX_DEBUG << "Setting start node for position " << start.matrix();
        Node& nodeStart = closestNode(start);
        ARMARX_INFO << "Start node orientation: " << nodeStart.orientation_deg;
        ARMARX_INFO << "Start node rotation index: "
                    << costmap3d.closestRotationFromDegrees(nodeStart.orientation_deg).index;
        ARMARX_CHECK(Grid::fulfillsConstraints(nodeStart, costmap3d, params.clearance))
            << "Start node in collision (within clearance=" << params.clearance << "mm)!";
 
        ARMARX_DEBUG << "Setting goal node for position " << goal.matrix();
        Node& nodeGoal = closestNode(goal);
        ARMARX_INFO << "Goal node orientation: " << nodeGoal.orientation_deg;
        ARMARX_INFO << "Goal node rotation index: "
                    << costmap3d.closestRotationFromDegrees(nodeGoal.orientation_deg).index;
        ARMARX_CHECK(Grid::fulfillsConstraints(nodeGoal, costmap3d, params.clearance))
            << "Goal node in collision (within clearance=" << params.clearance << "mm)!";
 
        // gScore - (estimated) cost from start node to a given node
        nodeStart.gScore = 0;
 
        // fScore - (estimated) cost from the start over the given node to the end (gScore + heuristic)
        nodeStart.fScore = nodeStart.gScore + heuristic(nodeStart, nodeGoal);
 
        // open set
        auto cmp = [&](const Node* left, const Node* right)
        { return left->fScore > right->fScore; };
        std::priority_queue<Node*, std::vector<Node*>, decltype(cmp)> openSetPq(cmp);
        openSetPq.push(&nodeStart);
        nodeStart.inOpenSet = true;
 
        bool foundSolution = false;
        while (!openSetPq.empty())
        {
            Node& currentBest = *openSetPq.top();
            currentBest.inOpenSet = false;
            openSetPq.pop();
            if (&currentBest == &nodeGoal)
            {
                foundSolution = true;
                break;
            }
 
            currentBest.inClosedSet = true;
            ++expandedNodes;
            for (size_t i = 0; i < currentBest.successors.size(); i++)
            {
                Node& neighbor = *currentBest.successors[i];
                if (neighbor.inClosedSet)
                {
                    continue;
                }
 
                Costs cost;
                try
                {
                    cost = costs(currentBest, neighbor);
                }
                catch (const std::exception& e)
                {
                    ARMARX_ERROR << "Cost calculation failed for neighbor at position "
                                 << neighbor.position.transpose() << " with orientation "
                                 << neighbor.orientation_deg << " deg";
                    ARMARX_ERROR << "Current node position: " << currentBest.position.transpose()
                                 << " with orientation " << currentBest.orientation_deg << " deg";
                    throw;
                }
                const auto pred1 = currentBest.predecessor
                                       ? std::make_optional(*currentBest.predecessor)
                                       : std::nullopt;
                const auto pred2 = pred1.has_value()
                                       ? pred1->predecessor
                                             ? std::make_optional(*(pred1->predecessor))
                                             : std::nullopt
                                       : std::nullopt;
                cost.smoothnessCosts = params.smoothnessWeightFactor *
                                       smoothnessCosts(neighbor, currentBest, pred1, pred2);
                float tentativeGScore = currentBest.gScore + cost.combined();
                if ((not neighbor.inOpenSet) || tentativeGScore < neighbor.gScore)
                {
                    neighbor.predecessor = std::experimental::make_observer(&currentBest);
                    neighbor.predecessorCosts = cost;
                    neighbor.gScore = tentativeGScore;
                    neighbor.fScore = tentativeGScore + heuristic(neighbor, nodeGoal);
                    if (not neighbor.inOpenSet)
                    {
                        openSetPq.push(&neighbor);
                        neighbor.inOpenSet = true;
                    }
                }
            }
        }
 
        // Found solution, now retrieve path from goal to start
        if (foundSolution)
        {
            const auto resultNodes = nodeGoal.traversePredecessors();
            result = resultNodes |
                     ranges::views::transform(
                         [this](const Node* node) noexcept {
                             return costmap3d.globalPose(costmap3d.toVertex(node->position).index,
                                                         node->orientation_deg);
                         }) |
                     ranges::to_vector;
            auto predecessorCosts = resultNodes |
                                    ranges::views::transform([](const Node* node) noexcept
                                                             { return node->predecessorCosts; }) |
                                    ranges::to_vector;
            ranges::reverse(predecessorCosts);
            ARMARX_DEBUG << "Predecessor costs";
            Costs combinedCosts = {}; // default-initialize to 0
            for (const auto& c : predecessorCosts)
            {
                ARMARX_DEBUG << "------------------";
                if (not c.has_value())
                {
                    continue;
                }
                ARMARX_DEBUG << "euclidian: " << c->euclidian;
                ARMARX_DEBUG << "obs_dist: " << c->obstacleProximity;
                ARMARX_DEBUG << "obs_dist_2: " << c->obstacleProximity2;
                ARMARX_DEBUG << "ori_diff: " << c->orientationDifference;
                ARMARX_DEBUG << "forward: " << c->forwardMovement;
                ARMARX_DEBUG << "smoothness: " << c->smoothnessCosts;
                combinedCosts = combinedCosts + c.value();
            }
 
            ARMARX_INFO << "==================";
            ARMARX_INFO << "Total costs:";
            ARMARX_INFO << "euclidian: " << combinedCosts.euclidian;
            ARMARX_INFO << "obs_dist: " << combinedCosts.obstacleProximity;
            ARMARX_INFO << "obs_dist_2: " << combinedCosts.obstacleProximity2;
            ARMARX_INFO << "ori_diff: " << combinedCosts.orientationDifference;
            ARMARX_INFO << "forward: " << combinedCosts.forwardMovement;
            ARMARX_INFO << "smoothness: " << combinedCosts.smoothnessCosts;
            ARMARX_INFO << "==================";
        }
 
        // Since the graph was traversed from goal to start, we have to reverse the order
        ranges::reverse(result);
 
        return result;
    }
 
    Costs
    AStarPlanner::costs(const Node& n1, const Node& n2) const
    {
        const auto vectorDiff = n1.position - n2.position;
        const float euclideanDistance = vectorDiff.norm();
 
        // additional costs if
        const float obstacleDistanceN1 = std::min(n1.obstacleDistance, params.maxObstacleDistance);
        const float obstacleDistanceN2 = std::min(n2.obstacleDistance, params.maxObstacleDistance);
 
        // const float obstacleProximityChangeCosts = std::max(obstacleDistanceN1 - obstacleDistanceN2, 0.F);
        const float obstacleProximityChangeCosts = obstacleDistanceN1 - obstacleDistanceN2;
 
        // Small note for heuristic:
        // obstacleProximityChangeCosts can be negative
 
        // Additional obstacle distance cost (penalty for being near an obstacle for longer than necessary)
        // Shift by clearance: the "boundary" is now at clearance mm, not at 0 mm.
        float effectiveDistN2 = std::max(obstacleDistanceN2 - params.clearance, 0.0f);
        float ratio = (params.maxObstacleDistance - effectiveDistN2) / params.maxObstacleDistance;
        ratio = std::clamp(ratio, 0.0f, 1.0f);
        const float obstacleProximityN2CostsNormalized =
            std::pow(ratio, params.obstacleDistanceContinuousExponent);
 
        // orientation costs
        const float orientationDist = std::abs(n1.orientation_deg - n2.orientation_deg);
 
        // prefer to move forward
        auto pose_n1 = costmap3d.globalPose({0, 0}, n1.orientation_deg);
        auto pose_n2 = costmap3d.globalPose({0, 0}, n2.orientation_deg);
        auto ori_vec_n1 = pose_n1.linear() * Eigen::Vector2f::UnitY();
        auto ori_vec_n2 = pose_n2.linear() * Eigen::Vector2f::UnitY();
        Eigen::Vector2f ori_vec = ori_vec_n1 + ori_vec_n2;
        ori_vec.normalize();
        const Eigen::Vector2f vectorDiff_norm = vectorDiff.normalized();
        float forward_angle = 0;
        if (vectorDiff.norm() == 0.F)
        {
            ARMARX_DEBUG << "Zero-length vector between nodes, cannot calculate forward movement "
                            "cost. n1 position: "
                         << n1.position.transpose() << ", n2 position: " << n2.position.transpose();
            ARMARX_DEBUG << "Using 0 instead";
        }
        else
        {
            forward_angle = std::abs(angleBetween(ori_vec, -vectorDiff_norm));
        }
 
        // Dynamic forward weight: prefer forward movement more strongly near obstacles.
        // When close to a wall, sideways/backward motion is more likely to collide.
        float obstacleProximityRatio = obstacleDistanceN2 / params.maxObstacleDistance;
        float dynamicForwardWeight = params.forwardWeightFactor
                                     * (1.0F + params.forwardWeightObstacleScale
                                                    * (1.0F - obstacleProximityRatio));
 
        Costs costs = {
            .euclidian = euclideanDistance,
            .obstacleProximity = obstacleProximityChangeCosts * params.obstacleDistanceWeightFactor,
            .obstacleProximity2 =
                obstacleProximityN2CostsNormalized * params.obstacleDistanceContinuousWeightFactor,
            .orientationDifference = orientationDist * params.orientationWeightFactor,
            .forwardMovement = forward_angle * dynamicForwardWeight,
            .smoothnessCosts = 0 // TODO set
        };
        return costs;
    }
 
 
} // namespace armarx::navigation::algorithms::orientation_aware