Class NodeLattice

Class Documentation

class NodeLattice

NodeLattice implementation for graph, Hybrid-A*.

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 Functions

explicit NodeLattice(const unsigned int index)

A constructor for nav2_smac_planner::NodeLattice.

Parameters:

index – The index of this node for self-reference

~NodeLattice()

A destructor for nav2_smac_planner::NodeLattice.

inline bool operator==(const NodeLattice &rhs)

operator== for comparisons

Parameters:

NodeLattice – right hand side node reference

Returns:

If cell indicies are equal

inline void setPose(const Coordinates &pose_in)

setting continuous coordinate search poses (in partial-cells)

Parameters:

Pose – pose

void reset()

Reset method for new search.

inline void setMotionPrimitive(MotionPrimitive *prim)

Sets the motion primitive used to achieve node in search.

Parameters:

pointer – to motion primitive

inline MotionPrimitive *&getMotionPrimitive()

Gets the motion primitive used to achieve node in search.

Returns:

pointer to motion primitive

inline float &getAccumulatedCost()

Gets the accumulated cost at this node.

Returns:

accumulated cost

inline void setAccumulatedCost(const float &cost_in)

Sets the accumulated cost at this node.

Parameters:

reference – to accumulated cost

inline float &getCost()

Gets the costmap cost at this node.

Returns:

costmap cost

inline bool &wasVisited()

Gets if cell has been visited in search.

Parameters:

If – cell was visited

inline void visited()

Sets if cell has been visited in search.

inline unsigned int &getIndex()

Gets cell index.

Returns:

Reference to cell index

inline void backwards(bool back = true)

Sets that this primitive is moving in reverse.

inline bool isBackward()

Gets if this primitive is moving in reverse.

Returns:

backwards If moving in reverse

bool 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

float getTraversalCost(const NodePtr &child)

Get traversal cost of parent node to child node.

Parameters:

child – Node pointer to child

Returns:

traversal cost

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.

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

bool 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

void addNodeToPath(NodePtr current_node, CoordinateVector &path)

add node to the path

Parameters:

current_node –

Public Members

NodeLattice *parent
Coordinates pose

Public Static Functions

static inline unsigned int getIndex(const unsigned int &x, const unsigned int &y, const unsigned int &angle)

Get index at coordinates.

Parameters:

  • x – X coordinate of point

  • y – Y coordinate of point

  • angle – Theta coordinate of point

Returns:

Index

static inline Coordinates getCoords(const unsigned int &index, const unsigned int &width, const unsigned int &angle_quantization)

Get coordinates at index.

Parameters:

  • index – Index of point

  • width – Width of costmap

  • angle_quantization – Theta size of costmap

Returns:

Coordinates

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

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

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.

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

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.

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

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

Parameters:

  • costmap – Costmap to use

  • goal_coords – Coordinates to start heuristic expansion at

static inline float getObstacleHeuristic(const Coordinates &node_coords, const Coordinates &goal_coords, const double &cost_penalty)

Compute the Obstacle heuristic.

Parameters:

  • node_coords – Coordinates to get heuristic at

  • goal_coords – Coordinates to compute heuristic to

Returns:

heuristic Heuristic value

static float getDistanceHeuristic(const Coordinates &node_coords, const Coordinates &goal_coords, const float &obstacle_heuristic)

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

Public Static Attributes

static LatticeMotionTable motion_table
static LookupTable dist_heuristic_lookup_table
static float size_lookup