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

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

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

Constructor & Destructor Documentation

◆ NodeLattice()

nav2_smac_planner::NodeLattice::NodeLattice ( const uint64_t index )

explicit

A constructor for nav2_smac_planner::NodeLattice.

Parameters

index The index of this node for self-reference

Definition at line 187 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 585 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 indices of generated path

Returns: whether the path was able to be backtraced

Definition at line 566 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 186 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 298 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 204 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 378 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 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 345 of file node_lattice.cpp.

References getDistanceHeuristic(), and getObstacleHeuristic().

Here is the call graph for this function:

◆ getIndex() [1/2]

uint64_t nav2_smac_planner::NodeLattice::getIndex ( )

inline

Gets cell index.

Returns: Reference to cell index

Definition at line 230 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 uint64_t 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 282 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 177 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 493 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 368 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 295 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 362 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 247 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 219 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 indices are equal

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

◆ resetObstacleHeuristic()

static void nav2_smac_planner::NodeLattice::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
	)

inlinestatic

Compute the wavefront heuristic.

Parameters

costmap	Costmap to use
goal_coords	Coordinates to start heuristic expansion at

Definition at line 353 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 195 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 168 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 154 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 213 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()