15 #include "nav2_route/route_tracker.hpp"
17 #include "nav2_ros_common/rate.hpp"
18 #include "nav2_ros_common/tf2_factories.hpp"
24 nav2::LifecycleNode::SharedPtr node,
25 nav2::TransformBuffer::SharedPtr tf_buffer,
26 std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber,
27 std::shared_ptr<ActionServerTrack> action_server,
28 const std::string & route_frame,
29 const std::string & base_frame)
32 clock_ = node->get_clock();
33 logger_ = node->get_logger();
34 route_frame_ = route_frame;
35 base_frame_ = base_frame;
36 action_server_ = action_server;
37 tf_buffer_ = tf_buffer;
39 radius_threshold_ = node->declare_or_get_parameter(
"radius_to_achieve_node", 2.0);
40 boundary_radius_threshold_ = node->declare_or_get_parameter(
41 "boundary_radius_to_achieve_node", 1.0);
42 tracker_update_rate_ = node->declare_or_get_parameter(
"tracker_update_rate", 50.0);
43 transform_staleness_threshold_ = node->declare_or_get_parameter(
44 "transform_staleness_threshold", 0.0);
45 aggregate_blocked_ids_ = node->declare_or_get_parameter(
46 "aggregate_blocked_ids",
false);
48 operations_manager_ = std::make_unique<OperationsManager>(node, costmap_subscriber);
53 geometry_msgs::msg::PoseStamped pose;
54 if (!nav2_util::getFreshPose(
55 *tf_buffer_, route_frame_, base_frame_, clock_->now(), transform_staleness_threshold_,
64 const geometry_msgs::msg::PoseStamped & pose,
69 const double dx = state.next_node->coords.x - pose.pose.position.x;
70 const double dy = state.next_node->coords.y - pose.pose.position.y;
71 const double dist_mag = std::sqrt(dx * dx + dy * dy);
73 const bool in_radius =
74 (dist_mag <= (is_boundary_node ? boundary_radius_threshold_ : radius_threshold_));
77 if (dist_mag < 1e-4 || (!in_radius && state.within_radius)) {
82 state.within_radius = in_radius;
87 if (is_boundary_node) {
88 return state.within_radius;
98 if (state.within_radius) {
99 NodePtr last_node = state.current_edge->start;
100 const double nx = state.next_node->coords.x - last_node->coords.x;
101 const double ny = state.next_node->coords.y - last_node->coords.y;
102 const double n_mag = std::sqrt(nx * nx + ny * ny);
104 NodePtr future_next_node = route.edges[state.route_edges_idx + 1]->end;
105 const double mx = future_next_node->coords.x - state.next_node->coords.x;
106 const double my = future_next_node->coords.y - state.next_node->coords.y;
107 const double m_mag = std::sqrt(mx * mx + my * my);
110 if (n_mag < 1e-6 || m_mag < 1e-6) {
115 const double bx = nx * m_mag + mx * n_mag;
116 const double by = ny * m_mag + my * n_mag;
117 return utils::normalizedDot(bx, by, dx, dy) <= 0;
128 (state.route_edges_idx ==
static_cast<int>(route.edges.size() - 1)) ||
129 (state.route_edges_idx == -1 && !state.current_edge);
133 const bool rereouted,
134 const unsigned int next_node_id,
135 const unsigned int last_node_id,
136 const unsigned int edge_id,
137 const std::vector<std::string> & operations)
139 auto feedback = std::make_unique<Feedback>();
140 feedback->route = route_msg_;
141 feedback->path = path_;
142 feedback->rerouted = rereouted;
143 feedback->next_node_id = next_node_id;
144 feedback->last_node_id = last_node_id;
145 feedback->current_edge_id = edge_id;
146 feedback->operations_triggered = operations;
147 action_server_->publish_feedback(std::move(feedback));
151 const Route & route,
const nav_msgs::msg::Path & path,
154 route_msg_ = utils::toMsg(route, route_frame_, clock_->now());
157 state.next_node = route.start_node;
162 if (rerouting_info.curr_edge) {
167 state.current_edge = rerouting_info.curr_edge;
168 state.last_node = state.current_edge->start;
170 true, route.start_node->nodeid, state.last_node->nodeid, state.current_edge->edgeid, {});
175 auto node = node_.lock();
181 while (rclcpp::ok()) {
182 bool status_change =
false, completed =
false;
185 if (action_server_->is_cancel_requested()) {
186 return TrackerResult::INTERRUPTED;
187 }
else if (action_server_->is_preempt_requested()) {
188 return TrackerResult::INTERRUPTED;
192 geometry_msgs::msg::PoseStamped robot_pose =
getRobotPose();
194 status_change =
true;
195 state.within_radius =
false;
196 state.last_node = state.next_node;
197 if (++state.route_edges_idx <
static_cast<int>(route.edges.size())) {
198 state.current_edge = route.edges[state.route_edges_idx];
199 state.next_node = state.current_edge->end;
201 state.current_edge =
nullptr;
202 state.next_node =
nullptr;
209 operations_manager_->process(status_change, state, route, robot_pose, rerouting_info);
212 RCLCPP_INFO(logger_,
"Routing to goal completed!");
214 publishFeedback(
false, 0, state.last_node->nodeid, 0, ops_result.operations_triggered);
215 return TrackerResult::COMPLETED;
218 if ((status_change || !ops_result.operations_triggered.empty()) && state.current_edge) {
221 state.next_node->nodeid, state.last_node->nodeid,
222 state.current_edge->edgeid, ops_result.operations_triggered);
225 if (ops_result.reroute) {
226 if (!aggregate_blocked_ids_) {
227 rerouting_info.blocked_ids = ops_result.blocked_ids;
229 rerouting_info.blocked_ids.insert(
230 rerouting_info.blocked_ids.end(),
231 ops_result.blocked_ids.begin(), ops_result.blocked_ids.end());
234 if (state.last_node) {
235 rerouting_info.rerouting_start_id = state.last_node->nodeid;
236 rerouting_info.rerouting_start_pose = robot_pose;
238 rerouting_info.rerouting_start_id = std::numeric_limits<unsigned int>::max();
239 rerouting_info.rerouting_start_pose = geometry_msgs::msg::PoseStamped();
243 rerouting_info.curr_edge = state.current_edge;
244 RCLCPP_INFO(logger_,
"Rerouting requested by route tracking operations!");
245 return TrackerResult::INTERRUPTED;
251 return TrackerResult::EXITED;
A sim-time-aware rate for Nav2 loops.
void configure(nav2::LifecycleNode::SharedPtr node, nav2::TransformBuffer::SharedPtr 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.
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.
TrackerResult trackRoute(const Route &route, const nav_msgs::msg::Path &path, ReroutingState &rerouting_info)
Main function to track route, manage state, and trigger operations.
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.
geometry_msgs::msg::PoseStamped getRobotPose()
Get the current robot's base_frame pose in route_frame.
bool isStartOrEndNode(RouteTrackingState &state, const Route &route)
Determine if a node is the start or last node in the route.
An object to store the nodes in the graph file.
Result information from the operations manager.
State shared to objects to communicate important rerouting data to avoid rerouting over blocked edges...
Current state management of route tracking class.
An ordered set of nodes and edges corresponding to the planned route.