Takes a processing route request and tracks it for progress in accordance with executing behavioral operations. More...

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

Collaboration diagram for nav2_route::RouteTracker:

[legend]

Public Types
using	ActionServerTrack = nav2::SimpleActionServer< nav2_msgs::action::ComputeAndTrackRoute >

using	Feedback = nav2_msgs::action::ComputeAndTrackRoute::Feedback

Public Member Functions
	RouteTracker ()=default
	A constructor for nav2_route::RouteTracker.

	~RouteTracker ()=default
	A constructor for nav2_route::RouteTracker.

void	configure (nav2::LifecycleNode::SharedPtr node, std::shared_ptr< tf2_ros::Buffer > tf_buffer, std::shared_ptr< nav2_costmap_2d::CostmapSubscriber > costmap_subscriber, typename ActionServerTrack::SharedPtr action_server, const std::string &route_frame, const std::string &base_frame)
	Configure route tracker. More...

bool	nodeAchieved (const geometry_msgs::msg::PoseStamped &pose, RouteTrackingState &state, const Route &route)
	Determine if a node is to be considered achieved at the current position. More...

bool	isStartOrEndNode (RouteTrackingState &state, const Route &route)
	Determine if a node is the start or last node in the route. More...

geometry_msgs::msg::PoseStamped	getRobotPose ()
	Get the current robot's base_frame pose in route_frame. More...

void	publishFeedback (const bool rereouted, const unsigned int next_node_id, const unsigned int last_node_id, const unsigned int edge_id, const std::vector< std::string > &operations)
	A utility to publish feedback for the action on important changes. More...

TrackerResult	trackRoute (const Route &route, const nav_msgs::msg::Path &path, ReroutingState &rerouting_info)
	Main function to track route, manage state, and trigger operations. More...

Protected Attributes
nav2_msgs::msg::Route	route_msg_

nav_msgs::msg::Path	path_

std::string	route_frame_

std::string	base_frame_

rclcpp::Clock::SharedPtr	clock_

rclcpp::Logger	logger_ {rclcpp::get_logger("RouteTracker")}

double	radius_threshold_

double	boundary_radius_threshold_

double	tracker_update_rate_

bool	aggregate_blocked_ids_

ActionServerTrack::SharedPtr	action_server_

std::unique_ptr< OperationsManager >	operations_manager_

std::shared_ptr< tf2_ros::Buffer >	tf_buffer_

Detailed Description

Takes a processing route request and tracks it for progress in accordance with executing behavioral operations.

Definition at line 38 of file route_tracker.hpp.

Member Function Documentation

◆ configure()

void nav2_route::RouteTracker::configure	(	nav2::LifecycleNode::SharedPtr	node,
		std::shared_ptr< tf2_ros::Buffer >	tf_buffer,
		std::shared_ptr< nav2_costmap_2d::CostmapSubscriber >	costmap_subscriber,
		typename ActionServerTrack::SharedPtr	action_server,
		const std::string &	route_frame,
		const std::string &	base_frame
	)

Configure route tracker.

Parameters

node	Node to grab info from
route_frame	Frame of route navigation

Definition at line 20 of file route_tracker.cpp.

◆ getRobotPose()

geometry_msgs::msg::PoseStamped nav2_route::RouteTracker::getRobotPose ( )

Get the current robot's base_frame pose in route_frame.

Returns: Robot pose

Definition at line 51 of file route_tracker.cpp.

◆ isStartOrEndNode()

bool nav2_route::RouteTracker::isStartOrEndNode	(	RouteTrackingState &	state,
		const Route &	route
	)

Determine if a node is the start or last node in the route.

Parameters

idx	Idx of the current edge being tracked
route	Route to check

Returns: bool if this node is the last node

Definition at line 120 of file route_tracker.cpp.

Referenced by nodeAchieved().

Here is the caller graph for this function:

◆ nodeAchieved()

bool nav2_route::RouteTracker::nodeAchieved	(	const geometry_msgs::msg::PoseStamped &	pose,
		RouteTrackingState &	state,
		const Route &	route
	)

Determine if a node is to be considered achieved at the current position.

Parameters

pose	Current robot pose to query
state	Tracker state
route	Route to check against

Returns: bool if node is achieved

Definition at line 60 of file route_tracker.cpp.

References isStartOrEndNode().

Here is the call graph for this function:

◆ publishFeedback()

void nav2_route::RouteTracker::publishFeedback	(	const bool	rereouted,
		const unsigned int	next_node_id,
		const unsigned int	last_node_id,
		const unsigned int	edge_id,
		const std::vector< std::string > &	operations
	)

A utility to publish feedback for the action on important changes.

Parameters

rerouted	If the route has been rerouted
next_node_id	Id of the next node the route is to pass
last_node_id	Id of the last node the route passed
edge_id	Id of the current edge being processed
operations	A set of operations which were performed this iteration

Definition at line 129 of file route_tracker.cpp.

Referenced by trackRoute().

Here is the caller graph for this function:

◆ trackRoute()

TrackerResult nav2_route::RouteTracker::trackRoute	(	const Route &	route,
		const nav_msgs::msg::Path &	path,
		ReroutingState &	rerouting_info
	)

Main function to track route, manage state, and trigger operations.

Parameters

route	Route to track progress of
path	Path that comprises this route for publication of feedback messages
blocked_ids	A set of blocked IDs to modify if rerouting is necessary

Returns: TrackerResult if the route is completed, should be rerouted, or was interrupted

Definition at line 147 of file route_tracker.cpp.

References publishFeedback().

Here is the call graph for this function:

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

nav2_route/include/nav2_route/route_tracker.hpp
nav2_route/src/route_tracker.cpp

Public Types

Public Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ configure()

◆ getRobotPose()

◆ isStartOrEndNode()

◆ nodeAchieved()

◆ publishFeedback()

◆ trackRoute()