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:

[legend]

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 unsigned int index)
	A constructor for nav2_smac_planner::NodeLattice. More...

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

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

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.

unsigned int &	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...

void	getNeighbors (std::function< bool(const unsigned int &, 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 unsigned int	getIndex (const unsigned int &x, const unsigned int &y, const unsigned int &angle)
	Get index at coordinates. More...

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

static float	getHeuristicCost (const Coordinates &node_coords, const Coordinates &goal_coordinates, const nav2_costmap_2d::Costmap2D *costmap)
	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 void	resetObstacleHeuristic (nav2_costmap_2d::Costmap2D *costmap, const unsigned int &start_x, const unsigned int &start_y, const unsigned int &goal_x, const unsigned int &goal_y)
	Compute the wavefront heuristic. More...

static float	getObstacleHeuristic (const Coordinates &node_coords, const Coordinates &goal_coords, const double &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...

Public Attributes
NodeLattice *	parent

Coordinates	pose

Static Public Attributes
static LatticeMotionTable	motion_table

static LookupTable	dist_heuristic_lookup_table

static float	size_lookup = 25

Detailed Description

NodeLattice implementation for graph, Hybrid-A*.

Definition at line 117 of file node_lattice.hpp.

Constructor & Destructor Documentation

◆ NodeLattice()

nav2_smac_planner::NodeLattice::NodeLattice ( const unsigned int index )

explicit

A constructor for nav2_smac_planner::NodeLattice.

Parameters

index The index of this node for self-reference

Definition at line 175 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 556 of file node_lattice.cpp.

References 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 indicies of generated path

Returns: whether the path was able to be backtraced

Definition at line 537 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 183 of file node_lattice.hpp.

◆ getCoords()

static Coordinates nav2_smac_planner::NodeLattice::getCoords	(	const unsigned int &	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 295 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 201 of file node_lattice.hpp.

Referenced by getTraversalCost().

Here is the caller graph for this function:

◆ getDistanceHeuristic()

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

References nav2_smac_planner::LatticeMotionTable::getAngleFromBin().

Referenced by getHeuristicCost().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getHeuristicCost()

float nav2_smac_planner::NodeLattice::getHeuristicCost	(	const Coordinates &	node_coords,
		const Coordinates &	goal_coordinates,
		const nav2_costmap_2d::Costmap2D *	costmap
	)

static

Get cost of heuristic of node.

Parameters

node	Node index current
node	Node index of new
costmap	Costmap ptr to use

Returns: Heuristic cost between the nodes

Definition at line 319 of file node_lattice.cpp.

References getDistanceHeuristic(), and getObstacleHeuristic().

Here is the call graph for this function:

◆ getIndex() [1/2]

unsigned int& nav2_smac_planner::NodeLattice::getIndex ( )

inline

Gets cell index.

Returns: Reference to cell index

Definition at line 227 of file node_lattice.hpp.

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

Here is the caller graph for this function:

◆ getIndex() [2/2]

static unsigned int nav2_smac_planner::NodeLattice::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 279 of file node_lattice.hpp.

References nav2_smac_planner::NodeHybrid::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:

◆ 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 174 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 unsigned int &, 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 461 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:

◆ getObstacleHeuristic()

static float nav2_smac_planner::NodeLattice::getObstacleHeuristic	(	const Coordinates &	node_coords,
		const Coordinates &	goal_coords,
		const double &	cost_penalty
	)

inlinestatic

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 367 of file node_lattice.hpp.

References nav2_smac_planner::NodeHybrid::getObstacleHeuristic().

Referenced by getHeuristicCost().

Here is the call graph for this function:

Here is the caller 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 269 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	(	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 332 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 244 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 205 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 )

inline

operator== for comparisons

Parameters

NodeLattice right hand side node reference

Returns: If cell indicies are equal

Definition at line 142 of file node_lattice.hpp.

◆ precomputeDistanceHeuristic()

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

References nav2_smac_planner::LatticeMotionTable::getAngleFromBin(), and nav2_smac_planner::LatticeMotionTable::getLatticeMetadata().

Here is the call graph for this function:

◆ resetObstacleHeuristic()

static void nav2_smac_planner::NodeLattice::resetObstacleHeuristic	(	nav2_costmap_2d::Costmap2D *	costmap,
		const unsigned int &	start_x,
		const unsigned int &	start_y,
		const unsigned int &	goal_x,
		const unsigned int &	goal_y
	)

inlinestatic

Compute the wavefront heuristic.

Parameters

costmap	Costmap to use
goal_coords	Coordinates to start heuristic expansion at

Definition at line 352 of file node_lattice.hpp.

References nav2_smac_planner::NodeHybrid::resetObstacleHeuristic().

Here is the call graph for this function:

◆ 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 192 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 165 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 151 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 210 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

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Static Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ NodeLattice()

Member Function Documentation

◆ addNodeToPath()

◆ backtracePath()

◆ getAccumulatedCost()

◆ getCoords()

◆ getCost()

◆ getDistanceHeuristic()

◆ getHeuristicCost()

◆ getIndex() [1/2]

◆ getIndex() [2/2]

◆ getMotionPrimitive()

◆ getNeighbors()

◆ getObstacleHeuristic()

◆ getTraversalCost()

◆ initMotionModel()

◆ isBackward()

◆ isNodeValid()

◆ operator==()

◆ precomputeDistanceHeuristic()

◆ resetObstacleHeuristic()

◆ setAccumulatedCost()

◆ setMotionPrimitive()

◆ setPose()

◆ wasVisited()