Class BreadthFirstSearch

Class Documentation

class BreadthFirstSearch

Public Functions

inline explicit BreadthFirstSearch(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap)

Constructor.

Parameters:

costmap – Costmap to check

inline bool search(const geometry_msgs::msg::PoseStamped &reference_node, const std::vector<geometry_msgs::msg::PoseStamped> &candidate_nodes, const int max_iterations = std::numeric_limits<int>::max())

Search for the closest node to the given reference node.

Parameters:

  • reference_node – The reference node to search from

  • candidate_nodes – The candidate nodes to search for

  • max_iterations – The maximum number of iterations to perform

Returns:

True if a candidate node is found, false otherwise

inline std::vector<unsigned int> getNeighbors(const unsigned int current_id)

Get the neighbors of a given node.

Parameters:

current_id – The current node id

Returns:

A vector of neighbor node ids

inline bool inCollision(const unsigned int id)

Check if a node is in collision with the costmap.

Parameters:

id – The node id to check

Returns:

True if the node is in collision, false otherwise

inline unsigned int getClosestNodeIdx()

Get the output closest node in candidate indices.

Returns:

closest_node The closest candidate node index to the given point by search

~BreadthFirstSearch() = default

Destructor.

Protected Attributes

std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_
unsigned int closest_node_idx_ = {0u}