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

#include <nav2_route/include/nav2_route/route_planner.hpp>

Public Member 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. More...

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

Protected Member Functions
void	resetSearchStates (Graph &graph)
	Reset the search state of the graph nodes. More...

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

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

NodeElement	getNextNode ()
	Gets the next node in the priority queue for search. More...

void	addNode (const float cost, const NodePtr node)
	Adds a node to the priority queue for search. More...

EdgeVector &	getEdges (const NodePtr node)
	Gets the edges from a given node. More...

void	clearQueue ()
	Clears the priority queue.

bool	isGoal (const NodePtr node)
	Checks if a given node is the goal node. More...

bool	isStart (const NodePtr node)
	Checks if a given node is the start node. More...

nav2_route::EdgeType	classifyEdge (const EdgePtr edge)
	Checks edge is a start or end edge. More...

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_

Detailed Description

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

Definition at line 40 of file route_planner.hpp.

Member Function Documentation

◆ addNode()

void nav2_route::RoutePlanner::addNode	(	const float	cost,
		const NodePtr	node
	)

inlineprotected

Adds a node to the priority queue for search.

Parameters

cost	Priority level
node	Node pointer to insert

Definition at line 182 of file route_planner.cpp.

Referenced by findShortestGraphTraversal().

Here is the caller graph for this function:

◆ classifyEdge()

nav2_route::EdgeType nav2_route::RoutePlanner::classifyEdge ( const EdgePtr edge )

protected

Checks edge is a start or end edge.

Parameters

edge	Edge to check

Returns: EdgeType identifying whether the edge is start, end or none

Definition at line 208 of file route_planner.cpp.

References isStart().

Referenced by getTraversalCost().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ configure()

void nav2_route::RoutePlanner::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

Definition at line 27 of file route_planner.cpp.

◆ findRoute()

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

virtual

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

Definition at line 43 of file route_planner.cpp.

References findShortestGraphTraversal().

Here is the call graph for this function:

◆ findShortestGraphTraversal()

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

protected

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

Definition at line 86 of file route_planner.cpp.

References addNode(), clearQueue(), getEdges(), getNextNode(), getTraversalCost(), isGoal(), and resetSearchStates().

Referenced by findRoute().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getEdges()

EdgeVector & nav2_route::RoutePlanner::getEdges ( const NodePtr node )

inlineprotected

Gets the edges from a given node.

Parameters

node	Node pointer to check

Returns: A vector of edges that the node contains

Definition at line 187 of file route_planner.cpp.

Referenced by findShortestGraphTraversal().

Here is the caller graph for this function:

◆ getNextNode()

NodeElement nav2_route::RoutePlanner::getNextNode ( )

inlineprotected

Gets the next node in the priority queue for search.

Returns: Next node pointer in queue with cost

Definition at line 175 of file route_planner.cpp.

Referenced by findShortestGraphTraversal().

Here is the caller graph for this function:

◆ getTraversalCost()

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

inlineprotected

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

Definition at line 148 of file route_planner.cpp.

References classifyEdge().

Referenced by findShortestGraphTraversal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isGoal()

bool nav2_route::RoutePlanner::isGoal ( const NodePtr node )

inlineprotected

Checks if a given node is the goal node.

Parameters

node	Node to check

Returns: bool If this node is the goal

Definition at line 198 of file route_planner.cpp.

Referenced by findShortestGraphTraversal().

Here is the caller graph for this function:

◆ isStart()

bool nav2_route::RoutePlanner::isStart ( const NodePtr node )

inlineprotected

Checks if a given node is the start node.

Parameters

node	Node to check

Returns: bool If this node is the start

Definition at line 203 of file route_planner.cpp.

Referenced by classifyEdge().

Here is the caller graph for this function:

◆ resetSearchStates()

void nav2_route::RoutePlanner::resetSearchStates ( Graph & graph )

inlineprotected

Reset the search state of the graph nodes.

Parameters

graph Graph to reset

Definition at line 77 of file route_planner.cpp.

Referenced by findShortestGraphTraversal().

Here is the caller graph for this function:

The documentation for this class was generated from the following files:

nav2_route/include/nav2_route/route_planner.hpp
nav2_route/src/route_planner.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ addNode()

◆ classifyEdge()

◆ configure()

◆ findRoute()

◆ findShortestGraphTraversal()

◆ getEdges()

◆ getNextNode()

◆ getTraversalCost()

◆ isGoal()

◆ isStart()

◆ resetSearchStates()