Template Class AStarAlgorithm

Nested Relationships

Nested Types

Class Documentation

template<typename NodeT>
class AStarAlgorithm

An A* implementation for planning in a costmap. Templated based on the Node type.

Public Types

typedef NodeT *NodePtr
typedef robin_hood::unordered_node_map<unsigned int, NodeT> Graph
typedef std::vector<NodePtr> NodeVector
typedef std::pair<float, NodeBasic<NodeT>> NodeElement
typedef NodeT::Coordinates Coordinates
typedef NodeT::CoordinateVector CoordinateVector
typedef NodeVector::iterator NeighborIterator
typedef std::function<bool(const unsigned int&, NodeT*&)> NodeGetter
typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue

Public Functions

explicit AStarAlgorithm(const MotionModel &motion_model, const SearchInfo &search_info)

A constructor for nav2_smac_planner::AStarAlgorithm.

~AStarAlgorithm()

A destructor for nav2_smac_planner::AStarAlgorithm.

void initialize(const bool &allow_unknown, int &max_iterations, const int &max_on_approach_iterations, const double &max_planning_time, const float &lookup_table_size, const unsigned int &dim_3_size)

Initialization of the planner with defaults.

Parameters:

  • allow_unknown – Allow search in unknown space, good for navigation while mapping

  • max_iterations – Maximum number of iterations to use while expanding search

  • max_on_approach_iterations – Maximum number of iterations before returning a valid path once within thresholds to refine path comes at more compute time but smoother paths.

  • max_planning_time – Maximum time (in seconds) to wait for a plan, createPath returns false after this timeout

bool createPath(CoordinateVector &path, int &num_iterations, const float &tolerance, std::vector<std::tuple<float, float>> *expansions_log = nullptr)

Creating path from given costmap, start, and goal.

Parameters:

  • path – Reference to a vector of indicies of generated path

  • num_iterations – Reference to number of iterations to create plan

  • tolerance – Reference to tolerance in costmap nodes

  • expansions_log – Optional expansions logged for debug

Returns:

if plan was successful

void setCollisionChecker(GridCollisionChecker *collision_checker)

Sets the collision checker to use.

Parameters:

collision_checker – Collision checker to use for checking state validity

void setGoal(const unsigned int &mx, const unsigned int &my, const unsigned int &dim_3)

Set the goal for planning, as a node index.

Parameters:

  • mx – The node X index of the goal

  • my – The node Y index of the goal

  • dim_3 – The node dim_3 index of the goal

void setStart(const unsigned int &mx, const unsigned int &my, const unsigned int &dim_3)

Set the starting pose for planning, as a node index.

Parameters:

  • mx – The node X index of the goal

  • my – The node Y index of the goal

  • dim_3 – The node dim_3 index of the goal

int &getMaxIterations()

Get maximum number of iterations to plan.

Returns:

Reference to Maximum iterations parameter

NodePtr &getStart()

Get pointer reference to starting node.

Returns:

Node pointer reference to starting node

NodePtr &getGoal()

Get pointer reference to goal node.

Returns:

Node pointer reference to goal node

int &getOnApproachMaxIterations()

Get maximum number of on-approach iterations after within threshold.

Returns:

Reference to Maximum on-appraoch iterations parameter

float &getToleranceHeuristic()

Get tolerance, in node nodes.

Returns:

Reference to tolerance parameter

unsigned int &getSizeX()

Get size of graph in X.

Returns:

Size in X

unsigned int &getSizeY()

Get size of graph in Y.

Returns:

Size in Y

unsigned int &getSizeDim3()

Get number of angle quantization bins (SE2) or Z coordinate (XYZ)

Returns:

Number of angle bins / Z dimension

Protected Functions

inline NodePtr getNextNode()

Get pointer to next goal in open set.

Returns:

Node pointer reference to next heuristically scored node

inline void addNode(const float &cost, NodePtr &node)

Add a node to the open set.

Parameters:

  • cost – The cost to sort into the open set of the node

  • node – Node pointer reference to add to open set

inline NodePtr addToGraph(const unsigned int &index)

Adds node to graph.

Parameters:

index – Node index to add

inline bool isGoal(NodePtr &node)

Check if this node is the goal node.

Parameters:

node – Node pointer to check if its the goal node

Returns:

if node is goal

inline float getHeuristicCost(const NodePtr &node)

Get cost of heuristic of node.

Parameters:

node – Node pointer to get heuristic for

Returns:

Heuristic cost for node

inline bool areInputsValid()

Check if inputs to planner are valid.

Returns:

Are valid

inline void clearQueue()

Clear hueristic queue of nodes to search.

inline void clearGraph()

Clear graph of nodes searched.

Protected Attributes

int _timing_interval = 5000
bool _traverse_unknown
bool _is_initialized
int _max_iterations
int _max_on_approach_iterations
double _max_planning_time
float _tolerance
unsigned int _x_size
unsigned int _y_size
unsigned int _dim3_size
SearchInfo _search_info
Coordinates _goal_coordinates
NodePtr _start
NodePtr _goal
Graph _graph
NodeQueue _queue
MotionModel _motion_model
NodeHeuristicPair _best_heuristic_node
GridCollisionChecker *_collision_checker
nav2_costmap_2d::Costmap2D *_costmap
std::unique_ptr<AnalyticExpansion<NodeT>> _expander
struct NodeComparator

Public Functions

inline bool operator()(const NodeElement &a, const NodeElement &b) const