Class RoutePlanner

Class Documentation

class RoutePlanner

An optimal planner to compute a route from a start to a goal in an arbitrary graph.

Public Functions

RoutePlanner() = default

A constructor for nav2_route::RoutePlanner.

virtual ~RoutePlanner() = default

A destructor for nav2_route::RoutePlanner.

void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node, const std::shared_ptr<tf2_ros::Buffer> tf_buffer, const std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber)

Configure the route planner, get parameters.

Parameters:

  • node – Node object to get parametersfrom

  • tf_buffer – TF buffer to use for transformations

  • costmap_subscriber – Costmap subscriber to use for blocked nodes

virtual Route findRoute(Graph &graph, unsigned int start_index, unsigned int goal_index, const std::vector<unsigned int> &blocked_ids, const RouteRequest &route_request)

Find the route from start to goal on the graph.

Parameters:

  • graph – Graph to search

  • start – Start index in the graph of the start node

  • goal – Goal index in the graph of the goal node

  • blocked_ids – A set of blocked node and edge IDs not to traverse

Returns:

Route object containing the navigation graph route

Protected Functions

inline void resetSearchStates(Graph &graph)

Reset the search state of the graph nodes.

Parameters:

graph – Graph to reset

void findShortestGraphTraversal(Graph &graph, const NodePtr start_node, const NodePtr goal_node, const std::vector<unsigned int> &blocked_ids, const RouteRequest &route_request)

Dikstra’s algorithm search on the graph.

Parameters:

  • graph – Graph to search

  • start – Start Node pointer

  • goal – Goal node pointer

  • blocked_ids – A set of blocked node and edge IDs not to traverse

inline bool getTraversalCost(const EdgePtr edge, float &score, const std::vector<unsigned int> &blocked_ids, const RouteRequest &route_request)

Gets the traversal cost for an edge using edge scorers.

Parameters:

  • edge – Edge pointer to find traversal cost for

  • travel – cost

  • blocked_ids – A set of blocked node and edge IDs not to traverse

Returns:

if this edge is valid for search

inline NodeElement getNextNode()

Gets the next node in the priority queue for search.

Returns:

Next node pointer in queue with cost

inline void addNode(const float cost, const NodePtr node)

Adds a node to the priority queue for search.

Parameters:

  • cost – Priority level

  • node – Node pointer to insert

inline EdgeVector &getEdges(const NodePtr node)

Gets the edges from a given node.

Parameters:

node – Node pointer to check

Returns:

A vector of edges that the node contains

inline void clearQueue()

Clears the priority queue.

inline bool isGoal(const NodePtr node)

Checks if a given node is the goal node.

Parameters:

node – Node to check

Returns:

bool If this node is the goal

inline bool isStart(const NodePtr node)

Checks if a given node is the start node.

Parameters:

node – Node to check

Returns:

bool If this node is the start

nav2_route::EdgeType classifyEdge(const EdgePtr edge)

Checks edge is a start or end edge.

Parameters:

edge – Edge to check

Returns:

EdgeType identifying whether the edge is start, end or none

Protected Attributes

int max_iterations_ = {0}
unsigned int start_id_ = {0}
unsigned int goal_id_ = {0}
NodeQueue queue_
std::unique_ptr<EdgeScorer> edge_scorer_
std::shared_ptr<tf2_ros::Buffer> tf_buffer_