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< uint64_t, 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 uint64_t &, NodeT *&)>	NodeGetter

typedef GoalManager< NodeT >	GoalManagerT

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 int &terminal_checking_interval, 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, std::function< bool()> cancel_checker, std::vector< std::tuple< float, float, float >> *expansions_log=nullptr)
	Creating path from given costmap, start, and goal. More...

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

void	setGoal (const float &mx, const float &my, const unsigned int &dim_3, const GoalHeadingMode &goal_heading_mode=GoalHeadingMode::DEFAULT, const int &coarse_search_resolution=1)
	Set the goal for planning, as a node index. More...

void	setStart (const float &mx, const float &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...

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

unsigned int	getCoarseSearchResolution ()
	Get the resolution of the coarse search. More...

GoalManagerT	getGoalManager ()
	Get the goals manager class. More...

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

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

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

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 uint64_t &index)
	Adds node to graph. 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 heuristic queue of nodes to search.

void	clearGraph ()
	Clear graph of nodes searched.

bool	onVisitationCheckNode (const NodePtr &node)
	Check if node has been visited. More...

void	populateExpansionsLog (const NodePtr &node, std::vector< std::tuple< float, float, float >> *expansions_log)
	Populate a debug log of expansions for Hybrid-A* for visualization. More...

void	populateExpansionsLog (const NodePtr &node, std::vector< std::tuple< float, float, float >> *expansions_log)

Protected Attributes
bool	_traverse_unknown

bool	_is_initialized

int	_max_iterations

int	_max_on_approach_iterations

int	_terminal_checking_interval

double	_max_planning_time

float	_tolerance

unsigned int	_x_size

unsigned int	_y_size

unsigned int	_dim3_size

unsigned int	_coarse_search_resolution

SearchInfo	_search_info

NodePtr	_start

GoalManagerT	_goal_manager

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 47 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 476 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 uint64_t & index )

inlineprotected

Adds node to graph.

Parameters

index Node index to add

Definition at line 125 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 302 of file a_star.cpp.

◆ createPath()

template<typename NodeT >

bool nav2_smac_planner::AStarAlgorithm< NodeT >::createPath	(	CoordinateVector &	path,
		int &	num_iterations,
		const float &	tolerance,
		std::function< bool()>	cancel_checker,
		std::vector< std::tuple< float, float, float >> *	expansions_log = `nullptr`
	)

Creating path from given costmap, start, and goal.

Parameters

path	Reference to a vector of indices of generated path
num_iterations	Reference to number of iterations to create plan
tolerance	Reference to tolerance in costmap nodes
cancel_checker	Function to check if the task has been canceled
expansions_log	Optional expansions logged for debug

Returns: if plan was successful

Definition at line 327 of file a_star.cpp.

◆ getCoarseSearchResolution()

template<typename NodeT >

unsigned int nav2_smac_planner::AStarAlgorithm< NodeT >::getCoarseSearchResolution

Get the resolution of the coarse search.

Returns: Size of the goals to expand

Definition at line 554 of file a_star.cpp.

◆ getGoalManager()

template<typename NodeT >

AStarAlgorithm< NodeT >::GoalManagerT nav2_smac_planner::AStarAlgorithm< NodeT >::getGoalManager

Get the goals manager class.

Returns: Goal manager class

Definition at line 560 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 484 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 518 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 467 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-approach iterations parameter

Definition at line 524 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 548 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 536 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 542 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 461 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 530 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 int &	terminal_checking_interval,
		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.
terminal_checking_interval	Number of iterations to check if the task has been canceled or or planning time exceeded
max_planning_time	Maximum time (in seconds) to wait for a plan, createPath returns false after this timeout
lookup_table_size	Size of the lookup table to store heuristic values
dim_3_size	Number of quantization bins

Definition at line 59 of file a_star.cpp.

◆ onVisitationCheckNode()

template<typename NodeT >

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

inlineprotected

Check if node has been visited.

Parameters

current_node Node to check if visited

Returns: if node has been visited

Definition at line 497 of file a_star.cpp.

◆ populateExpansionsLog()

template<typename NodeT >

void nav2_smac_planner::AStarAlgorithm< NodeT >::populateExpansionsLog	(	const NodePtr &	node,
		std::vector< std::tuple< float, float, float >> *	expansions_log
	)

inlineprotected

Populate a debug log of expansions for Hybrid-A* for visualization.

Parameters

node	Node expanded
expansions_log	Log to add not expanded to

Definition at line 179 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 107 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 float &	mx,
		const float &	my,
		const unsigned int &	dim_3,
		const GoalHeadingMode &	goal_heading_mode = `GoalHeadingMode::DEFAULT`,
		const int &	coarse_search_resolution = `1`
	)

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
goal_heading_mode	The goal heading mode to use
coarse_search_resolution	The resolution to search for goal heading

Definition at line 215 of file a_star.cpp.

◆ setStart()

template<typename NodeT >

void nav2_smac_planner::AStarAlgorithm< NodeT >::setStart	(	const float &	mx,
		const float &	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 153 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()

◆ getCoarseSearchResolution()

◆ getGoalManager()

◆ getHeuristicCost()

◆ getMaxIterations()

◆ getNextNode()

◆ getOnApproachMaxIterations()

◆ getSizeDim3()

◆ getSizeX()

◆ getSizeY()

◆ getStart()

◆ getToleranceHeuristic()

◆ initialize()

◆ onVisitationCheckNode()

◆ populateExpansionsLog()

◆ setCollisionChecker()

◆ setGoal()

◆ setStart()

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