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

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

Collaboration diagram for nav2_smac_planner::NodeLattice:

Classes
struct	NodeContext

Public Types
typedef NodeLattice *	NodePtr

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

typedef std::vector< NodePtr >	NodeVector

typedef NodeHybrid::Coordinates	Coordinates

typedef NodeHybrid::CoordinateVector	CoordinateVector

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

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

bool	operator== (const NodeLattice &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.

void	setMotionPrimitive (MotionPrimitive *prim)
	Sets the motion primitive used to achieve node in search. More...

MotionPrimitive *&	getMotionPrimitive ()
	Gets the motion primitive used to achieve node in search. More...

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

void	setAccumulatedCost (const float &cost_in)
	Sets the accumulated cost at this node. 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...

void	backwards (bool back=true)
	Sets that this primitive is moving in reverse.

bool	isBackward ()
	Gets if this primitive is moving in reverse. More...

bool	isNodeValid (const bool &traverse_unknown, GridCollisionChecker collision_checker, MotionPrimitive primitive=nullptr, bool is_backwards=false)
	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::NodeLattice &)> &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...

void	addNodeToPath (NodePtr current_node, CoordinateVector &path)
	add node to the path 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
NodeLattice *	parent

Coordinates	pose

Detailed Description

NodeLattice implementation for graph, Hybrid-A*.

Definition at line 124 of file node_lattice.hpp.

Constructor & Destructor Documentation

◆ NodeLattice()

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

explicit

A constructor for nav2_smac_planner::NodeLattice.

Parameters

index The index of this node for self-reference

Definition at line 189 of file node_lattice.cpp.

Member Function Documentation

◆ addNodeToPath()

void nav2_smac_planner::NodeLattice::addNodeToPath	(	NodeLattice::NodePtr	current_node,
		NodeLattice::CoordinateVector &	path
	)

add node to the path

Parameters

current_node

Definition at line 488 of file node_lattice.cpp.

References nav2_smac_planner::LatticeMotionTable::getAngleFromBin(), getMotionPrimitive(), and isBackward().

Referenced by backtracePath().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ backtracePath()

bool nav2_smac_planner::NodeLattice::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 469 of file node_lattice.cpp.

References addNodeToPath().

Here is the call graph for this function:

◆ getAccumulatedCost()

float nav2_smac_planner::NodeLattice::getAccumulatedCost ( )

inline

Gets the accumulated cost at this node.

Returns: accumulated cost

Definition at line 206 of file node_lattice.hpp.

◆ getCoords()

static Coordinates nav2_smac_planner::NodeLattice::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 319 of file node_lattice.hpp.

◆ getCost()

float nav2_smac_planner::NodeLattice::getCost ( )

inline

Gets the costmap cost at this node.

Returns: costmap cost

Definition at line 224 of file node_lattice.hpp.

Referenced by getTraversalCost().

Here is the caller graph for this function:

◆ getHeuristicCost()

float nav2_smac_planner::NodeLattice::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 358 of file node_lattice.cpp.

◆ getIndex() [1/2]

uint64_t nav2_smac_planner::NodeLattice::getIndex ( )

inline

Gets cell index.

Returns: Reference to cell index

Definition at line 250 of file node_lattice.hpp.

Referenced by nav2_simple_commander.costmap_2d.PyCostmap2D::getCostXY(), nav2_simple_commander.occupancy_grid.PyOccupancyGrid::getCostXY(), 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::NodeLattice::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 304 of file node_lattice.hpp.

References nav2_smac_planner::NodeHybrid::getIndex().

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 call graph for this function:

Here is the caller graph for this function:

◆ getMotionPrimitive()

MotionPrimitive* & nav2_smac_planner::NodeLattice::getMotionPrimitive ( )

inline

Gets the motion primitive used to achieve node in search.

Returns: pointer to motion primitive

Definition at line 197 of file node_lattice.hpp.

Referenced by addNodeToPath(), and getTraversalCost().

Here is the caller graph for this function:

◆ getNeighbors()

void nav2_smac_planner::NodeLattice::getNeighbors	(	std::function< bool(const uint64_t &, nav2_smac_planner::NodeLattice *&)> &	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 395 of file node_lattice.cpp.

References backwards(), getIndex(), nav2_smac_planner::LatticeMotionTable::getMotionPrimitives(), isNodeValid(), setMotionPrimitive(), setPose(), and wasVisited().

Here is the call graph for this function:

◆ getTraversalCost()

float nav2_smac_planner::NodeLattice::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 299 of file node_lattice.cpp.

References getCost(), getMotionPrimitive(), and isBackward().

Here is the call graph for this function:

◆ initMotionModel()

void nav2_smac_planner::NodeLattice::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 378 of file node_lattice.cpp.

◆ isBackward()

bool nav2_smac_planner::NodeLattice::isBackward ( )

inline

Gets if this primitive is moving in reverse.

Returns: backwards If moving in reverse

Definition at line 267 of file node_lattice.hpp.

Referenced by addNodeToPath(), and getTraversalCost().

Here is the caller graph for this function:

◆ isNodeValid()

bool nav2_smac_planner::NodeLattice::isNodeValid	(	const bool &	traverse_unknown,
		GridCollisionChecker *	collision_checker,
		MotionPrimitive *	primitive = `nullptr`,
		bool	is_backwards = `false`
	)

Check if this node is valid.

Parameters

traverse_unknown	If we can explore unknown nodes on the graph
collision_checker	Collision checker object to aid in validity checking
primitive	Optional argument if needing to check over a primitive not only a terminal pose
is_backwards	Optional argument if needed to check if prim expansion is in reverse

Returns: whether this node is valid and collision free

Definition at line 222 of file node_lattice.cpp.

References nav2_smac_planner::LatticeMotionTable::getAngleFromBin(), nav2_smac_planner::GridCollisionChecker::getCost(), nav2_smac_planner::GridCollisionChecker::getPrecomputedAngles(), 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::NodeLattice::operator== ( const NodeLattice & rhs ) const

inline

operator== for comparisons

Parameters

NodeLattice right hand side node reference

Returns: If cell indices are equal

Definition at line 165 of file node_lattice.hpp.

◆ setAccumulatedCost()

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

inline

Sets the accumulated cost at this node.

Parameters

reference to accumulated cost

Definition at line 215 of file node_lattice.hpp.

◆ setMotionPrimitive()

void nav2_smac_planner::NodeLattice::setMotionPrimitive ( MotionPrimitive * prim )

inline

Sets the motion primitive used to achieve node in search.

Parameters

pointer to motion primitive

Definition at line 188 of file node_lattice.hpp.

Referenced by getNeighbors().

Here is the caller graph for this function:

◆ setPose()

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

inline

setting continuous coordinate search poses (in partial-cells)

Parameters

Pose pose

Definition at line 174 of file node_lattice.hpp.

Referenced by getNeighbors().

Here is the caller graph for this function:

◆ wasVisited()

bool nav2_smac_planner::NodeLattice::wasVisited ( )

inline

Gets if cell has been visited in search.

Parameters

If	cell was visited

Definition at line 233 of file node_lattice.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_lattice.hpp
nav2_smac_planner/src/node_lattice.cpp

Classes

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ NodeLattice()

Member Function Documentation

◆ addNodeToPath()

◆ backtracePath()

◆ getAccumulatedCost()

◆ getCoords()

◆ getCost()

◆ getHeuristicCost()

◆ getIndex() [1/2]

◆ getIndex() [2/2]

◆ getMotionPrimitive()

◆ getNeighbors()

◆ getTraversalCost()

◆ initMotionModel()

◆ isBackward()

◆ isNodeValid()

◆ operator==()

◆ setAccumulatedCost()

◆ setMotionPrimitive()

◆ setPose()

◆ wasVisited()