Class NodeHybrid

Nested Relationships

Nested Types

Class Documentation

class NodeHybrid

NodeHybrid implementation for graph, Hybrid-A*.

Public Types

typedef NodeHybrid *NodePtr
typedef std::unique_ptr<std::vector<NodeHybrid>> Graph
typedef std::vector<NodePtr> NodeVector
typedef std::vector<Coordinates> CoordinateVector

Public Functions

explicit NodeHybrid(const uint64_t index)

A constructor for nav2_smac_planner::NodeHybrid.

Parameters:

index – The index of this node for self-reference

~NodeHybrid()

A destructor for nav2_smac_planner::NodeHybrid.

inline bool operator==(const NodeHybrid &rhs)

operator== for comparisons

Parameters:

NodeHybrid – 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 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 void setMotionPrimitiveIndex(const unsigned int &idx, const TurnDirection &turn_dir)

Sets the motion primitive index used to achieve node in search.

Parameters:

reference – to motion primitive idx

inline unsigned int &getMotionPrimitiveIndex()

Gets the motion primitive index used to achieve node in search.

Returns:

reference to motion primitive idx

inline TurnDirection &getTurnDirection()

Gets the motion primitive turning direction used to achieve node in search.

Returns:

reference to motion primitive turning direction

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 uint64_t getIndex()

Gets cell index.

Returns:

Reference to cell index

bool 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

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

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

Public Members

NodeHybrid *parent
Coordinates pose

Public Static Functions

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

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

static inline uint64_t 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 uint64_t &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)

Get cost of heuristic of node.

Parameters:
  • node – Node index current

  • node – Node index of new

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 float getObstacleHeuristic(const Coordinates &node_coords, const Coordinates &goal_coords, const float &cost_penalty)

Compute the Obstacle heuristic.

Parameters:
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_coordsCoordinates to get heuristic at

  • goal_coordsCoordinates to compute heuristic to

  • obstacle_heuristic – Value of the obstacle heuristic to compute additional motion heuristics if required

Returns:

heuristic Heuristic value

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)

reset the obstacle heuristic state

Parameters:
  • costmap_ros – Costmap to use

  • goal_coordsCoordinates to start heuristic expansion at

static inline void destroyStaticAssets()

Destroy shared pointer assets at the end of the process that don’t require normal destruction handling.

Public Static Attributes

static float travel_distance_cost
static HybridMotionTable motion_table
static LookupTable obstacle_heuristic_lookup_table
static ObstacleHeuristicQueue obstacle_heuristic_queue
static std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros
static LookupTable dist_heuristic_lookup_table
static float size_lookup
class Coordinates

NodeHybrid implementation of coordinate structure.

Public Functions

inline Coordinates()

A constructor for nav2_smac_planner::NodeHybrid::Coordinates.

inline Coordinates(const float &x_in, const float &y_in, const float &theta_in)

A constructor for nav2_smac_planner::NodeHybrid::Coordinates.

Parameters:
  • x_in – X coordinate

  • y_in – Y coordinate

  • theta_in – Theta coordinate

inline bool operator==(const Coordinates &rhs)
inline bool operator!=(const Coordinates &rhs)

Public Members

float x
float y
float theta