Class RouteTracker

• Defined in File route_tracker.hpp

Class Documentation

class RouteTracker

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

Public Types

using ActionServerTrack = nav2_util::SimpleActionServer<nav2_msgs::action::ComputeAndTrackRoute>

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

Public Functions

RouteTracker() = default

A constructor for nav2_route::RouteTracker.

~RouteTracker() = default

A constructor for nav2_route::RouteTracker.

void configure(nav2_util::LifecycleNode::SharedPtr node, std::shared_ptr<tf2_ros::Buffer> tf_buffer, std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber, std::shared_ptr<ActionServerTrack> 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

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.

Parameters:

• pose – Current robot pose to query

• state – Tracker state

• route – Route to check against

Returns:

bool if node is achieved

bool 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

geometry_msgs::msg::PoseStamped getRobotPose()

Get the current robot’s base_frame pose in route_frame.

Returns:

Robot pose

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.

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

TrackerResult 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

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_

std::shared_ptr<ActionServerTrack> action_server_

std::unique_ptr<OperationsManager> operations_manager_

std::shared_ptr<tf2_ros::Buffer> tf_buffer_