AStarPlanner.cpp

Go to the documentation of this file.

#include "AStarPlanner.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <map>
#include <memory>
#include <vector>
 
#include <boost/geometry.hpp> // IWYU pragma: keep
#include <boost/geometry/algorithms/detail/intersects/interface.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/BoundingBox.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/astar/Node.h>
 
#include <omp.h>
#include <range/v3/algorithm/reverse.hpp>
 
namespace bg = boost::geometry;
 
namespace armarx::navigation::algorithm::astar
{
 
    bool
    intersects(const VirtualRobot::BoundingBox& bb1, const VirtualRobot::BoundingBox& bb2)
    {
        using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
        using box_t = bg::model::box<point_t>;
 
        const auto toPoint = [](const Eigen::Vector3f& pt)
        { return point_t(pt.x(), pt.y(), pt.z()); };
 
        const box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
        const box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
 
        return bg::intersects(box1, box2);
    }
 
    AStarPlanner::AStarPlanner(const algorithms::Costmap& costmap) : costmap(costmap)
    {
    }
 
    float
    AStarPlanner::heuristic(const NodePtr& n1, const NodePtr& n2)
    {
        return (1.F - obstacleDistanceWeightFactor) * (n1->position - n2->position).norm();
    }
 
    void
    AStarPlanner::createUniformGrid()
    {
        ARMARX_TRACE;
 
        const size_t rows = costmap.getGrid().rows();
        const size_t cols = costmap.getGrid().cols();
 
        // create the grid with the correct size
        ARMARX_VERBOSE << "Creating grid";
        for (size_t r = 0; r < rows; r++)
        {
            grid.push_back(std::vector<NodePtr>(cols));
        }
 
        // intitializing grid
        for (size_t r = 0; r < rows; r++)
        {
            for (size_t c = 0; c < cols; c++)
            {
                const auto pos = costmap.toPositionGlobal({r, c});
                const float obstacleDistance = costmap.getGrid()(r, c);
 
                grid[r][c] = std::make_shared<Node>(pos, obstacleDistance);
            }
        }
 
        ARMARX_VERBOSE << "Creating graph";
 
        // Init successors
        for (size_t r = 0; r < rows; r++)
        {
            for (size_t c = 0; c < cols; c++)
            {
                NodePtr candidate = grid[r][c];
                if (!fulfillsConstraints(candidate))
                {
                    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]->successors.push_back(candidate);
                    }
                }
            }
        }
 
        ARMARX_VERBOSE << "Done.";
    }
 
    bool
    AStarPlanner::fulfillsConstraints(const NodePtr& n)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(n);
 
        return not costmap.isInCollision(n->position);
    }
 
    NodePtr
    AStarPlanner::closestNode(const Eigen::Vector2f& v)
    {
        const auto vertex = costmap.toVertex(v);
 
        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);
 
        const int rows = static_cast<int>(costmap.getGrid().rows());
        const int cols = static_cast<int>(costmap.getGrid().cols());
 
        ARMARX_CHECK_LESS_EQUAL(r, rows - 1);
        ARMARX_CHECK_LESS_EQUAL(c, cols - 1);
 
        return grid[static_cast<int>(r)][static_cast<int>(c)];
    }
 
    std::vector<Eigen::Vector2f>
    AStarPlanner::plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal)
    {
        ARMARX_TRACE;
        grid.clear();
        std::vector<Eigen::Vector2f> result;
 
        createUniformGrid();
        ARMARX_DEBUG << "Setting start node for position " << start;
        NodePtr nodeStart = closestNode(start);
        // FIXME activate ARMARX_CHECK(fulfillsConstraints(nodeStart)) << "Start node in collision!";
 
        ARMARX_DEBUG << "Setting goal node for position " << goal;
        NodePtr nodeGoal = closestNode(goal);
        ARMARX_CHECK(fulfillsConstraints(nodeGoal)) << "Goal node in collision!";
 
        std::vector<NodePtr> closedSet;
 
        std::vector<NodePtr> openSet;
        openSet.push_back(nodeStart);
 
        std::map<NodePtr, float> gScore;
        gScore[nodeStart] = 0;
 
        std::map<NodePtr, float> fScore;
        fScore[nodeStart] = gScore[nodeStart] + heuristic(nodeStart, nodeGoal);
 
        while (!openSet.empty())
        {
            float lowestScore = fScore[openSet[0]];
            auto currentIT = openSet.begin();
            for (auto it = openSet.begin() + 1; it != openSet.end(); it++)
            {
                if (fScore[*it] < lowestScore)
                {
                    lowestScore = fScore[*it];
                    currentIT = it;
                }
            }
            NodePtr currentBest = *currentIT;
            if (currentBest == nodeGoal)
            {
                break;
            }
 
            openSet.erase(currentIT);
            closedSet.push_back(currentBest);
            for (size_t i = 0; i < currentBest->successors.size(); i++)
            {
                NodePtr neighbor = currentBest->successors[i];
                if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end())
                {
                    continue;
                }
 
                float tentativeGScore = gScore[currentBest] + costs(currentBest, neighbor);
                bool notInOS = std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end();
                if (notInOS || tentativeGScore < gScore[neighbor])
                {
                    neighbor->predecessor = currentBest;
                    gScore[neighbor] = tentativeGScore;
                    fScore[neighbor] = tentativeGScore + heuristic(neighbor, nodeGoal);
                    if (notInOS)
                    {
                        openSet.push_back(neighbor);
                    }
                }
            }
        }
 
        // Found solution, now retrieve path from goal to start
        if (nodeGoal)
        {
            result = nodeGoal->traversePredecessors();
        }
 
        // Since the graph was traversed from goal to start, we have to reverse the order
        ranges::reverse(result);
 
        return result;
    }
 
    float
    AStarPlanner::costs(const NodePtr& n1, const NodePtr& n2) const
    {
        const float euclideanDistance = (n1->position - n2->position).norm();
 
        // additional costs if
        const float obstacleDistanceN1 = std::min(n1->obstacleDistance, maxObstacleDistance);
        const float obstacleDistanceN2 = std::min(n2->obstacleDistance, maxObstacleDistance);
 
        // const float obstacleProximityChangeCosts = std::max(obstacleDistanceN1 - obstacleDistanceN2, 0.F);
        const float obstacleProximityChangeCosts = obstacleDistanceN1 - obstacleDistanceN2;
 
        const float costs =
            euclideanDistance + obstacleProximityChangeCosts * obstacleDistanceWeightFactor;
        return std::max(costs, 0.F);
    }
 
 
} // namespace armarx::navigation::algorithm::astar