NodeHybrid implementation for graph, Hybrid-A*. More...

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

Collaboration diagram for nav2_smac_planner::NodeHybrid:

Classes
class	Coordinates
	NodeHybrid implementation of coordinate structure. More...

Public Types
typedef NodeHybrid *	NodePtr

typedef std::unique_ptr< std::vector< NodeHybrid > >	Graph

typedef std::vector< NodePtr >	NodeVector

typedef std::vector< Coordinates >	CoordinateVector

Public Member Functions
	NodeHybrid (const uint64_t index)
	A constructor for nav2_smac_planner::NodeHybrid. More...

	~NodeHybrid ()
	A destructor for nav2_smac_planner::NodeHybrid.

bool	operator== (const NodeHybrid &rhs)
	operator== for comparisons More...

void	setPose (const Coordinates &pose_in)
	setting continuous coordinate search poses (in partial-cells) More...

void	reset ()
	Reset method for new search.

float	getAccumulatedCost ()
	Gets the accumulated cost at this node. More...

void	setAccumulatedCost (const float &cost_in)
	Sets the accumulated cost at this node. More...

void	setMotionPrimitiveIndex (const unsigned int &idx, const TurnDirection &turn_dir)
	Sets the motion primitive index used to achieve node in search. More...

unsigned int &	getMotionPrimitiveIndex ()
	Gets the motion primitive index used to achieve node in search. More...

TurnDirection &	getTurnDirection ()
	Gets the motion primitive turning direction used to achieve node in search. More...

float	getCost ()
	Gets the costmap cost at this node. More...

bool	wasVisited ()
	Gets if cell has been visited in search. More...

void	visited ()
	Sets if cell has been visited in search.

uint64_t	getIndex ()
	Gets cell index. More...

bool	isNodeValid (const bool &traverse_unknown, GridCollisionChecker *collision_checker)
	Check if this node is valid. More...

float	getTraversalCost (const NodePtr &child)
	Get traversal cost of parent node to child node. More...

void	getNeighbors (std::function< bool(const uint64_t &, nav2_smac_planner::NodeHybrid &)> &validity_checker, GridCollisionChecker collision_checker, const bool &traverse_unknown, NodeVector &neighbors)
	Retrieve all valid neighbors of a node. More...

bool	backtracePath (CoordinateVector &path)
	Set the starting pose for planning, as a node index. More...

Static Public Member Functions
static uint64_t	getIndex (const unsigned int &x, const unsigned int &y, const unsigned int &angle, const unsigned int &width, const unsigned int &angle_quantization)
	Get index at coordinates. More...

static uint64_t	getIndex (const unsigned int &x, const unsigned int &y, const unsigned int &angle)
	Get index at coordinates. More...

static Coordinates	getCoords (const uint64_t &index, const unsigned int &width, const unsigned int &angle_quantization)
	Get coordinates at index. More...

static float	getHeuristicCost (const Coordinates &node_coords, const CoordinateVector &goals_coords)
	Get cost of heuristic of node. More...

static void	initMotionModel (const MotionModel &motion_model, unsigned int &size_x, unsigned int &size_y, unsigned int &angle_quantization, SearchInfo &search_info)
	Initialize motion models. More...

static void	precomputeDistanceHeuristic (const float &lookup_table_dim, const MotionModel &motion_model, const unsigned int &dim_3_size, const SearchInfo &search_info)
	Compute the SE2 distance heuristic. More...

static float	getObstacleHeuristic (const Coordinates &node_coords, const Coordinates &goal_coords, const float &cost_penalty)
	Compute the Obstacle heuristic. More...

static float	getDistanceHeuristic (const Coordinates &node_coords, const Coordinates &goal_coords, const float &obstacle_heuristic)
	Compute the Distance heuristic. More...

static void	resetObstacleHeuristic (std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros, const unsigned int &start_x, const unsigned int &start_y, const unsigned int &goal_x, const unsigned int &goal_y)
	reset the obstacle heuristic state More...

static void	destroyStaticAssets ()
	Destroy shared pointer assets at the end of the process that don't require normal destruction handling.

Public Attributes
NodeHybrid *	parent

Coordinates	pose

Static Public Attributes
static float	travel_distance_cost = sqrtf(2.0f)

static HybridMotionTable	motion_table

static LookupTable	obstacle_heuristic_lookup_table

static ObstacleHeuristicQueue	obstacle_heuristic_queue

static std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros = nullptr

static LookupTable	dist_heuristic_lookup_table

static float	size_lookup = 25

Detailed Description

NodeHybrid implementation for graph, Hybrid-A*.

Definition at line 142 of file node_hybrid.hpp.

Constructor & Destructor Documentation

◆ NodeHybrid()

nav2_smac_planner::NodeHybrid::NodeHybrid ( const uint64_t index )

explicit

A constructor for nav2_smac_planner::NodeHybrid.

Parameters

index The index of this node for self-reference

Definition at line 346 of file node_hybrid.cpp.

Member Function Documentation

◆ backtracePath()

bool nav2_smac_planner::NodeHybrid::backtracePath ( CoordinateVector & path )

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

Parameters

path	Reference to a vector of indices of generated path

Returns: whether the path was able to be backtraced

Definition at line 878 of file node_hybrid.cpp.

◆ getAccumulatedCost()

float nav2_smac_planner::NodeHybrid::getAccumulatedCost ( )

inline

Gets the accumulated cost at this node.

Returns: accumulated cost

Definition at line 224 of file node_hybrid.hpp.

◆ getCoords()

static Coordinates nav2_smac_planner::NodeHybrid::getCoords	(	const uint64_t &	index,
		const unsigned int &	width,
		const unsigned int &	angle_quantization
	)

inlinestatic

Get coordinates at index.

Parameters

index	Index of point
width	Width of costmap
angle_quantization	Theta size of costmap

Returns: Coordinates

Definition at line 359 of file node_hybrid.hpp.

◆ getCost()

float nav2_smac_planner::NodeHybrid::getCost ( )

inline

Gets the costmap cost at this node.

Returns: costmap cost

Definition at line 270 of file node_hybrid.hpp.

Referenced by getTraversalCost().

Here is the caller graph for this function:

◆ getDistanceHeuristic()

float nav2_smac_planner::NodeHybrid::getDistanceHeuristic	(	const Coordinates &	node_coords,
		const Coordinates &	goal_coords,
		const float &	obstacle_heuristic
	)

static

Compute the Distance heuristic.

Parameters

node_coords	Coordinates to get heuristic at
goal_coords	Coordinates to compute heuristic to
obstacle_heuristic	Value of the obstacle heuristic to compute additional motion heuristics if required

Returns: heuristic Heuristic value

Definition at line 722 of file node_hybrid.cpp.

Referenced by getHeuristicCost().

Here is the caller graph for this function:

◆ getHeuristicCost()

float nav2_smac_planner::NodeHybrid::getHeuristicCost	(	const Coordinates &	node_coords,
		const CoordinateVector &	goals_coords
	)

static

Get cost of heuristic of node.

Parameters

node	Node index current
node	Node index of new

Returns: Heuristic cost between the nodes

Definition at line 443 of file node_hybrid.cpp.

References getDistanceHeuristic(), and getObstacleHeuristic().

Here is the call graph for this function:

◆ getIndex() [1/3]

uint64_t nav2_smac_planner::NodeHybrid::getIndex ( )

inline

Gets cell index.

Returns: Reference to cell index

Definition at line 296 of file node_hybrid.hpp.

Referenced by nav2_simple_commander.costmap_2d.PyCostmap2D::getCostXY(), getIndex(), nav2_smac_planner::NodeLattice::getIndex(), getNeighbors(), and nav2_simple_commander.costmap_2d.PyCostmap2D::setCost().

Here is the caller graph for this function:

◆ getIndex() [2/3]

static uint64_t nav2_smac_planner::NodeHybrid::getIndex	(	const unsigned int &	x,
		const unsigned int &	y,
		const unsigned int &	angle
	)

inlinestatic

Get index at coordinates.

Parameters

x	X coordinate of point
y	Y coordinate of point
angle	Theta coordinate of point

Returns: Index

Definition at line 344 of file node_hybrid.hpp.

References getIndex().

Referenced by nav2_simple_commander.costmap_2d.PyCostmap2D::getCostXY(), and nav2_simple_commander.costmap_2d.PyCostmap2D::setCost().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getIndex() [3/3]

static uint64_t nav2_smac_planner::NodeHybrid::getIndex	(	const unsigned int &	x,
		const unsigned int &	y,
		const unsigned int &	angle,
		const unsigned int &	width,
		const unsigned int &	angle_quantization
	)

inlinestatic

Get index at coordinates.

Parameters

x	X coordinate of point
y	Y coordinate of point
angle	Theta coordinate of point
width	Width of costmap
angle_quantization	Number of theta bins

Returns: Index

Definition at line 327 of file node_hybrid.hpp.

Referenced by nav2_simple_commander.costmap_2d.PyCostmap2D::getCostXY(), and nav2_simple_commander.costmap_2d.PyCostmap2D::setCost().

Here is the caller graph for this function:

◆ getMotionPrimitiveIndex()

unsigned int& nav2_smac_planner::NodeHybrid::getMotionPrimitiveIndex ( )

inline

Gets the motion primitive index used to achieve node in search.

Returns: reference to motion primitive idx

Definition at line 252 of file node_hybrid.hpp.

Referenced by getTraversalCost().

Here is the caller graph for this function:

◆ getNeighbors()

void nav2_smac_planner::NodeHybrid::getNeighbors	(	std::function< bool(const uint64_t &, nav2_smac_planner::NodeHybrid *&)> &	validity_checker,
		GridCollisionChecker *	collision_checker,
		const bool &	traverse_unknown,
		NodeVector &	neighbors
	)

Retrieve all valid neighbors of a node.

Parameters

validity_checker	Functor for state validity checking
collision_checker	Collision checker to use
traverse_unknown	If unknown costs are valid to traverse
neighbors	Vector of neighbors to be filled

Definition at line 840 of file node_hybrid.cpp.

References getIndex(), nav2_smac_planner::HybridMotionTable::getProjections(), isNodeValid(), setMotionPrimitiveIndex(), setPose(), and wasVisited().

Here is the call graph for this function:

◆ getObstacleHeuristic()

float nav2_smac_planner::NodeHybrid::getObstacleHeuristic	(	const Coordinates &	node_coords,
		const Coordinates &	goal_coords,
		const float &	cost_penalty
	)

static

Compute the Obstacle heuristic.

Parameters

node_coords	Coordinates to get heuristic at
goal_coords	Coordinates to compute heuristic to

Returns: heuristic Heuristic value

Definition at line 549 of file node_hybrid.cpp.

Referenced by getHeuristicCost(), and nav2_smac_planner::NodeLattice::getObstacleHeuristic().

Here is the caller graph for this function:

◆ getTraversalCost()

float nav2_smac_planner::NodeHybrid::getTraversalCost ( const NodePtr & child )

Get traversal cost of parent node to child node.

Parameters

child Node pointer to child

Returns: traversal cost

Definition at line 391 of file node_hybrid.cpp.

References getCost(), getMotionPrimitiveIndex(), and getTurnDirection().

Here is the call graph for this function:

◆ getTurnDirection()

TurnDirection& nav2_smac_planner::NodeHybrid::getTurnDirection ( )

inline

Gets the motion primitive turning direction used to achieve node in search.

Returns: reference to motion primitive turning direction

Definition at line 261 of file node_hybrid.hpp.

Referenced by getTraversalCost().

Here is the caller graph for this function:

◆ initMotionModel()

void nav2_smac_planner::NodeHybrid::initMotionModel	(	const MotionModel &	motion_model,
		unsigned int &	size_x,
		unsigned int &	size_y,
		unsigned int &	angle_quantization,
		SearchInfo &	search_info
	)

static

Initialize motion models.

Parameters

motion_model	Motion model enum to use
size_x	Size of X of graph
size_y	Size of y of graph
angle_quantization	Size of theta bins of graph
search_info	Search info to use

Definition at line 459 of file node_hybrid.cpp.

◆ isNodeValid()

bool nav2_smac_planner::NodeHybrid::isNodeValid	(	const bool &	traverse_unknown,
		GridCollisionChecker *	collision_checker
	)

Check if this node is valid.

Parameters

traverse_unknown	If we can explore unknown nodes on the graph
collision_checker	Collision checker object

Returns: whether this node is valid and collision free

Definition at line 376 of file node_hybrid.cpp.

References nav2_smac_planner::GridCollisionChecker::getCost(), and nav2_smac_planner::GridCollisionChecker::inCollision().

Referenced by getNeighbors().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ operator==()

bool nav2_smac_planner::NodeHybrid::operator== ( const NodeHybrid & rhs )

inline

operator== for comparisons

Parameters

NodeHybrid right hand side node reference

Returns: If cell indices are equal

Definition at line 201 of file node_hybrid.hpp.

◆ precomputeDistanceHeuristic()

void nav2_smac_planner::NodeHybrid::precomputeDistanceHeuristic	(	const float &	lookup_table_dim,
		const MotionModel &	motion_model,
		const unsigned int &	dim_3_size,
		const SearchInfo &	search_info
	)

static

Compute the SE2 distance heuristic.

Parameters

lookup_table_dim	Size, in costmap pixels, of the each lookup table dimension to populate
motion_model	Motion model to use for state space
dim_3_size	Number of quantization bins for caching
search_info	Info containing minimum radius to use

Definition at line 791 of file node_hybrid.cpp.

◆ resetObstacleHeuristic()

void nav2_smac_planner::NodeHybrid::resetObstacleHeuristic	(	std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros,
		const unsigned int &	start_x,
		const unsigned int &	start_y,
		const unsigned int &	goal_x,
		const unsigned int &	goal_y
	)

static

reset the obstacle heuristic state

Parameters

costmap_ros	Costmap to use
goal_coords	Coordinates to start heuristic expansion at

Definition at line 493 of file node_hybrid.cpp.

Referenced by nav2_smac_planner::NodeLattice::resetObstacleHeuristic().

Here is the caller graph for this function:

◆ setAccumulatedCost()

void nav2_smac_planner::NodeHybrid::setAccumulatedCost ( const float & cost_in )

inline

Sets the accumulated cost at this node.

Parameters

reference to accumulated cost

Definition at line 233 of file node_hybrid.hpp.

◆ setMotionPrimitiveIndex()

void nav2_smac_planner::NodeHybrid::setMotionPrimitiveIndex	(	const unsigned int &	idx,
		const TurnDirection &	turn_dir
	)

inline

Sets the motion primitive index used to achieve node in search.

Parameters

reference to motion primitive idx

Definition at line 242 of file node_hybrid.hpp.

Referenced by getNeighbors().

Here is the caller graph for this function:

◆ setPose()

void nav2_smac_planner::NodeHybrid::setPose ( const Coordinates & pose_in )

inline

setting continuous coordinate search poses (in partial-cells)

Parameters

Pose pose

Definition at line 210 of file node_hybrid.hpp.

Referenced by getNeighbors().

Here is the caller graph for this function:

◆ wasVisited()

bool nav2_smac_planner::NodeHybrid::wasVisited ( )

inline

Gets if cell has been visited in search.

Parameters

If	cell was visited

Definition at line 279 of file node_hybrid.hpp.

Referenced by getNeighbors().

Here is the caller graph for this function:

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

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
nav2_smac_planner/src/node_hybrid.cpp

Classes

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Static Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ NodeHybrid()

Member Function Documentation

◆ backtracePath()

◆ getAccumulatedCost()

◆ getCoords()

◆ getCost()

◆ getDistanceHeuristic()

◆ getHeuristicCost()

◆ getIndex() [1/3]

◆ getIndex() [2/3]

◆ getIndex() [3/3]

◆ getMotionPrimitiveIndex()

◆ getNeighbors()

◆ getObstacleHeuristic()

◆ getTraversalCost()

◆ getTurnDirection()

◆ initMotionModel()

◆ isNodeValid()

◆ operator==()

◆ precomputeDistanceHeuristic()

◆ resetObstacleHeuristic()

◆ setAccumulatedCost()

◆ setMotionPrimitiveIndex()

◆ setPose()

◆ wasVisited()