AStarPlanner.h

Go to the documentation of this file.

#pragma once
 
#include <cstddef>
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 
#include "Node.h"
#include "Planner2D.h"
#include <armarx/navigation/algorithms/Costmap.h>
 
namespace armarx::navigation::algorithm::astar
{
 
    /**
 * The A* planner
 */
    class AStarPlanner //: public Planner2D
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        AStarPlanner(const algorithms::Costmap& costmap);
 
        std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start,
                                          const Eigen::Vector2f& goal) /* override */;
 
    private:
        float heuristic(const NodePtr& n1, const NodePtr& n2);
        void createUniformGrid();
        bool fulfillsConstraints(const NodePtr& n);
        NodePtr closestNode(const Eigen::Vector2f& v);
 
        float costs(const NodePtr& a, const NodePtr& b) const;
 
        float computeObstacleDistance(const Eigen::Vector2f& pos,
                                      VirtualRobot::RobotPtr& robot) const;
 
    private:
        const algorithms::Costmap costmap;
 
        std::vector<std::vector<NodePtr>> grid;
 
        const float obstacleDistanceWeightFactor = 0.5;
 
        const float maxObstacleDistance = 1500; // [mm]
    };
} // namespace armarx::navigation::algorithm::astar