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
struct	NodeContext

Public Types
typedef NodeHybrid *	NodePtr

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

typedef std::vector< NodePtr >	NodeVector

using	Coordinates = nav2_smac_planner::Coordinates

typedef std::vector< Coordinates >	CoordinateVector

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

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

bool	operator== (const NodeHybrid &rhs) const
	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...

float	getHeuristicCost (const Coordinates &node_coords, const CoordinateVector &goals_coords)
	Get cost of heuristic of 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 Coordinates	getCoords (const uint64_t &index, const unsigned int &width, const unsigned int &angle_quantization)
	Get coordinates at index. More...

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

Public Attributes
NodeHybrid *	parent

Coordinates	pose

Detailed Description

NodeHybrid implementation for graph, Hybrid-A*.

Definition at line 131 of file node_hybrid.hpp.

Constructor & Destructor Documentation

◆ NodeHybrid()

nav2_smac_planner::NodeHybrid::NodeHybrid	(	const uint64_t	index,
		NodeContext *	ctx
	)

explicit

A constructor for nav2_smac_planner::NodeHybrid.

Parameters

index The index of this node for self-reference

Definition at line 340 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 517 of file node_hybrid.cpp.

References nav2_smac_planner::HybridMotionTable::getAngleFromBin().

Here is the call graph for this function:

◆ getAccumulatedCost()

float nav2_smac_planner::NodeHybrid::getAccumulatedCost ( )

inline

Gets the accumulated cost at this node.

Returns: accumulated cost

Definition at line 193 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 313 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 239 of file node_hybrid.hpp.

Referenced by getTraversalCost().

Here is the caller graph for this function:

◆ getHeuristicCost()

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

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 436 of file node_hybrid.cpp.

◆ getIndex() [1/2]

uint64_t nav2_smac_planner::NodeHybrid::getIndex ( )

inline

Gets cell index.

Returns: Reference to cell index

Definition at line 265 of file node_hybrid.hpp.

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

Here is the caller graph for this function:

◆ getIndex() [2/2]

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 296 of file node_hybrid.hpp.

Referenced by nav2_simple_commander.costmap_2d.PyCostmap2D::getCostXY(), nav2_simple_commander.occupancy_grid.PyOccupancyGrid::getCostXY(), nav2_simple_commander.occupancy_grid.PyOccupancyGrid::setCost(), 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 221 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 479 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:

◆ 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 386 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 230 of file node_hybrid.hpp.

Referenced by getTraversalCost().

Here is the caller graph for this function:

◆ initMotionModel()

void nav2_smac_planner::NodeHybrid::initMotionModel	(	NodeContext *	ctx,
		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 455 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 371 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 ) const

inline

operator== for comparisons

Parameters

NodeHybrid right hand side node reference

Returns: If cell indices are equal

Definition at line 170 of file node_hybrid.hpp.

◆ 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 202 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 211 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 179 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 248 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

Detailed Description

Constructor & Destructor Documentation

◆ NodeHybrid()

Member Function Documentation

◆ backtracePath()

◆ getAccumulatedCost()

◆ getCoords()

◆ getCost()

◆ getHeuristicCost()

◆ getIndex() [1/2]

◆ getIndex() [2/2]

◆ getMotionPrimitiveIndex()

◆ getNeighbors()

◆ getTraversalCost()

◆ getTurnDirection()

◆ initMotionModel()

◆ isNodeValid()

◆ operator==()

◆ setAccumulatedCost()

◆ setMotionPrimitiveIndex()

◆ setPose()

◆ wasVisited()