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

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

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_coords – Coordinates to start heuristic expansion at

static float adjustedFootprintCost(const float &cost)

Using the inflation layer, find the footprint’s adjusted cost if the robot is non-circular.

Parameters:

cost – Cost to adjust

Returns:

float Cost adjusted

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 std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer
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