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

#include <nav2_smac_planner/include/nav2_smac_planner/a_star.hpp>

Collaboration diagram for nav2_smac_planner::AStarAlgorithm< NodeT >:

[legend]

Classes
struct	NodeComparator

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 Member Functions
	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. More...

bool	createPath (CoordinateVector &path, int &num_iterations, const float &tolerance)
	Creating path from given costmap, start, and goal. More...

void	setCollisionChecker (GridCollisionChecker *collision_checker)
	Sets the collision checker to use. More...

void	setGoal (const unsigned int &mx, const unsigned int &my, const unsigned int &dim_3)
	Set the goal for planning, as a node index. More...

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. More...

int &	getMaxIterations ()
	Get maximum number of iterations to plan. More...

NodePtr &	getStart ()
	Get pointer reference to starting node. More...

NodePtr &	getGoal ()
	Get pointer reference to goal node. More...

int &	getOnApproachMaxIterations ()
	Get maximum number of on-approach iterations after within threshold. More...

float &	getToleranceHeuristic ()
	Get tolerance, in node nodes. More...

unsigned int &	getSizeX ()
	Get size of graph in X. More...

unsigned int &	getSizeY ()
	Get size of graph in Y. More...

unsigned int &	getSizeDim3 ()
	Get number of angle quantization bins (SE2) or Z coordinate (XYZ) More...

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

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

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

Protected Member Functions
NodePtr	getNextNode ()
	Get pointer to next goal in open set. More...

void	addNode (const float &cost, NodePtr &node)
	Add a node to the open set. More...

NodePtr	addToGraph (const unsigned int &index)
	Adds node to graph. More...

bool	isGoal (NodePtr &node)
	Check if this node is the goal node. More...

float	getHeuristicCost (const NodePtr &node)
	Get cost of heuristic of node. More...

bool	areInputsValid ()
	Check if inputs to planner are valid. More...

void	clearQueue ()
	Clear hueristic queue of nodes to search.

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

Detailed Description

template<typename NodeT>
class nav2_smac_planner::AStarAlgorithm< NodeT >

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

Definition at line 46 of file a_star.hpp.

Member Function Documentation

◆ addNode()

template<typename NodeT >

void nav2_smac_planner::AStarAlgorithm< NodeT >::addNode	(	const float &	cost,
		NodePtr &	node
	)

inlineprotected

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

Definition at line 369 of file a_star.cpp.

References nav2_smac_planner::NodeBasic< NodeT >::populateSearchNode().

Here is the call graph for this function:

◆ addToGraph()

template<typename NodeT >

AStarAlgorithm< NodeT >::NodePtr nav2_smac_planner::AStarAlgorithm< NodeT >::addToGraph ( const unsigned int & index )

inlineprotected

Adds node to graph.

Parameters

index Node index to add

Definition at line 121 of file a_star.cpp.

◆ areInputsValid()

template<typename NodeT >

bool nav2_smac_planner::AStarAlgorithm< NodeT >::areInputsValid

inlineprotected

Check if inputs to planner are valid.

Returns: Are valid

Definition at line 198 of file a_star.cpp.

◆ createPath()

template<typename NodeT >

bool nav2_smac_planner::AStarAlgorithm< NodeT >::createPath	(	CoordinateVector &	path,
		int &	num_iterations,
		const float &	tolerance
	)

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

Returns: if plan was successful

Definition at line 226 of file a_star.cpp.

◆ getGoal()

template<typename NodeT >

AStarAlgorithm< NodeT >::NodePtr & nav2_smac_planner::AStarAlgorithm< NodeT >::getGoal

Get pointer reference to goal node.

Returns: Node pointer reference to goal node

Definition at line 354 of file a_star.cpp.

◆ getHeuristicCost()

template<typename NodeT >

float nav2_smac_planner::AStarAlgorithm< NodeT >::getHeuristicCost ( const NodePtr & node )

inlineprotected

Get cost of heuristic of node.

Parameters

node	Node pointer to get heuristic for

Returns: Heuristic cost for node

Definition at line 377 of file a_star.cpp.

◆ getMaxIterations()

template<typename NodeT >

int & nav2_smac_planner::AStarAlgorithm< NodeT >::getMaxIterations

Get maximum number of iterations to plan.

Returns: Reference to Maximum iterations parameter

Definition at line 407 of file a_star.cpp.

◆ getNextNode()

template<typename NodeT >

AStarAlgorithm< NodeT >::NodePtr nav2_smac_planner::AStarAlgorithm< NodeT >::getNextNode

inlineprotected

Get pointer to next goal in open set.

Returns: Node pointer reference to next heuristically scored node

Definition at line 360 of file a_star.cpp.

References nav2_smac_planner::NodeBasic< NodeT >::processSearchNode().

Here is the call graph for this function:

◆ getOnApproachMaxIterations()

template<typename NodeT >

int & nav2_smac_planner::AStarAlgorithm< NodeT >::getOnApproachMaxIterations

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

Returns: Reference to Maximum on-appraoch iterations parameter

Definition at line 413 of file a_star.cpp.

◆ getSizeDim3()

template<typename NodeT >

unsigned int & nav2_smac_planner::AStarAlgorithm< NodeT >::getSizeDim3

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

Returns: Number of angle bins / Z dimension

Definition at line 437 of file a_star.cpp.

◆ getSizeX()

template<typename NodeT >

unsigned int & nav2_smac_planner::AStarAlgorithm< NodeT >::getSizeX

Get size of graph in X.

Returns: Size in X

Definition at line 425 of file a_star.cpp.

◆ getSizeY()

template<typename NodeT >

unsigned int & nav2_smac_planner::AStarAlgorithm< NodeT >::getSizeY

Get size of graph in Y.

Returns: Size in Y

Definition at line 431 of file a_star.cpp.

◆ getStart()

template<typename NodeT >

AStarAlgorithm< NodeT >::NodePtr & nav2_smac_planner::AStarAlgorithm< NodeT >::getStart

Get pointer reference to starting node.

Returns: Node pointer reference to starting node

Definition at line 348 of file a_star.cpp.

◆ getToleranceHeuristic()

template<typename NodeT >

float & nav2_smac_planner::AStarAlgorithm< NodeT >::getToleranceHeuristic

Get tolerance, in node nodes.

Returns: Reference to tolerance parameter

Definition at line 419 of file a_star.cpp.

◆ initialize()

template<typename NodeT >

void nav2_smac_planner::AStarAlgorithm< NodeT >::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

Definition at line 59 of file a_star.cpp.

◆ isGoal()

template<typename NodeT >

bool nav2_smac_planner::AStarAlgorithm< NodeT >::isGoal ( NodePtr & node )

inlineprotected

Check if this node is the goal node.

Parameters

node	Node pointer to check if its the goal node

Returns: if node is goal

Definition at line 342 of file a_star.cpp.

◆ setCollisionChecker()

template<typename NodeT >

void nav2_smac_planner::AStarAlgorithm< NodeT >::setCollisionChecker ( GridCollisionChecker * collision_checker )

Sets the collision checker to use.

Parameters

collision_checker Collision checker to use for checking state validity

Definition at line 103 of file a_star.cpp.

References nav2_costmap_2d::FootprintCollisionChecker< CostmapT >::getCostmap(), and nav2_costmap_2d::Costmap2D::getSizeInCellsX().

Here is the call graph for this function:

◆ setGoal()

template<typename NodeT >

void nav2_smac_planner::AStarAlgorithm< NodeT >::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

Definition at line 173 of file a_star.cpp.

◆ setStart()

template<typename NodeT >

void nav2_smac_planner::AStarAlgorithm< NodeT >::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

Definition at line 145 of file a_star.cpp.

The documentation for this class was generated from the following files:

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
nav2_smac_planner/src/a_star.cpp

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

template<typename NodeT> class nav2_smac_planner::AStarAlgorithm< NodeT >

Member Function Documentation

◆ addNode()

◆ addToGraph()

◆ areInputsValid()

◆ createPath()

◆ getGoal()

◆ getHeuristicCost()

◆ getMaxIterations()

◆ getNextNode()

◆ getOnApproachMaxIterations()

◆ getSizeDim3()

◆ getSizeX()

◆ getSizeY()

◆ getStart()

◆ getToleranceHeuristic()

◆ initialize()

◆ isGoal()

◆ setCollisionChecker()

◆ setGoal()

◆ setStart()

template<typename NodeT>
class nav2_smac_planner::AStarAlgorithm< NodeT >