Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
route_tracker.cpp
1 // Copyright (c) 2025, Open Navigation LLC
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #include "nav2_route/route_tracker.hpp"
16 
17 #include "nav2_ros_common/rate.hpp"
18 #include "nav2_ros_common/tf2_factories.hpp"
19 
20 namespace nav2_route
21 {
22 
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)
30 {
31  node_ = node;
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;
38 
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);
47 
48  operations_manager_ = std::make_unique<OperationsManager>(node, costmap_subscriber);
49 }
50 
51 geometry_msgs::msg::PoseStamped RouteTracker::getRobotPose()
52 {
53  geometry_msgs::msg::PoseStamped pose;
54  if (!nav2_util::getFreshPose(
55  *tf_buffer_, route_frame_, base_frame_, clock_->now(), transform_staleness_threshold_,
56  pose))
57  {
58  throw nav2_core::RouteTFError("Unable to get robot pose in route frame: " + route_frame_);
59  }
60  return pose;
61 }
62 
64  const geometry_msgs::msg::PoseStamped & pose,
65  RouteTrackingState & state,
66  const Route & route)
67 {
68  // check if inside a *generous* radius window
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);
72  const bool is_boundary_node = isStartOrEndNode(state, route);
73  const bool in_radius =
74  (dist_mag <= (is_boundary_node ? boundary_radius_threshold_ : radius_threshold_));
75 
76  // Within 0.1mm is achieved or within radius and now not, consider node achieved
77  if (dist_mag < 1e-4 || (!in_radius && state.within_radius)) {
78  return true;
79  }
80 
81  // Update the state for the next iteration
82  state.within_radius = in_radius;
83 
84  // If start or end node, use the radius check only since the final node may not pass
85  // threshold depending on the configurations. The start node has no last_node for
86  // computing the vector bisector. If this is an issue, please file a ticket to discuss.
87  if (is_boundary_node) {
88  return state.within_radius;
89  }
90 
91  // We can evaluate the unit distance vector from the node w.r.t. the unit vector bisecting
92  // the last and current edges to find the average whose orthogonal is an imaginary
93  // line representing the migration from one edge's spatial domain to the other.
94  // When the dot product is negative, it means that there exists a projection between
95  // the vectors and that the robot position has passed this imaginary orthogonal line.
96  // This enables a more refined definition of when a node is considered achieved while
97  // enabling the use of dynamic behavior that may deviate from the path non-trivially
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);
103 
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);
108 
109  // If nodes overlap so there is no vector, use radius check only (divide by zero)
110  if (n_mag < 1e-6 || m_mag < 1e-6) {
111  return true;
112  }
113 
114  // Unnormalized Bisector = |n|*m + |m|*n
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;
118  }
119 
120  return false;
121 }
122 
124 {
125  // Check if current_edge is nullptr in case we have a rerouted previous
126  // edge to use for the refined node achievement vectorized estimate
127  return
128  (state.route_edges_idx == static_cast<int>(route.edges.size() - 1)) ||
129  (state.route_edges_idx == -1 && !state.current_edge);
130 }
131 
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)
138 {
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));
148 }
149 
151  const Route & route, const nav_msgs::msg::Path & path,
152  ReroutingState & rerouting_info)
153 {
154  route_msg_ = utils::toMsg(route, route_frame_, clock_->now());
155  path_ = path;
156  RouteTrackingState state;
157  state.next_node = route.start_node;
158 
159  // If we're rerouted but still covering the same previous edge to
160  // start, retain the state so we can continue as previously set with
161  // refined node achievement logic and performing edge operations on exit
162  if (rerouting_info.curr_edge) {
163  // state.next_node is not updated since the first edge is removed from route when rerouted
164  // along the same edge in the goal intent extractor. Thus, state.next_node is still the
165  // future node to reach in this case and we add in the state.last_node and state.current_edge
166  // to represent the 'currently' progressing edge that is omitted from the route (and its start)
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, {});
171  } else {
172  publishFeedback(true, route.start_node->nodeid, 0, 0, {});
173  }
174 
175  auto node = node_.lock();
176  if (!node) {
177  throw nav2_core::RouteException("Route tracker node expired");
178  }
179 
180  nav2::Rate r(node, tracker_update_rate_);
181  while (rclcpp::ok()) {
182  bool status_change = false, completed = false;
183 
184  // Check if OK to keep processing
185  if (action_server_->is_cancel_requested()) {
186  return TrackerResult::INTERRUPTED;
187  } else if (action_server_->is_preempt_requested()) {
188  return TrackerResult::INTERRUPTED;
189  }
190 
191  // Update the tracking state
192  geometry_msgs::msg::PoseStamped robot_pose = getRobotPose();
193  if (nodeAchieved(robot_pose, state, route)) {
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;
200  } else { // At achieved the last node in the route
201  state.current_edge = nullptr;
202  state.next_node = nullptr;
203  completed = true;
204  }
205  }
206 
207  // Process any operations necessary
208  OperationsResult ops_result =
209  operations_manager_->process(status_change, state, route, robot_pose, rerouting_info);
210 
211  if (completed) {
212  RCLCPP_INFO(logger_, "Routing to goal completed!");
213  // Publishing last feedback
214  publishFeedback(false, 0, state.last_node->nodeid, 0, ops_result.operations_triggered);
215  return TrackerResult::COMPLETED;
216  }
217 
218  if ((status_change || !ops_result.operations_triggered.empty()) && state.current_edge) {
220  false, // No rerouting occurred
221  state.next_node->nodeid, state.last_node->nodeid,
222  state.current_edge->edgeid, ops_result.operations_triggered);
223  }
224 
225  if (ops_result.reroute) {
226  if (!aggregate_blocked_ids_) {
227  rerouting_info.blocked_ids = ops_result.blocked_ids;
228  } else {
229  rerouting_info.blocked_ids.insert(
230  rerouting_info.blocked_ids.end(),
231  ops_result.blocked_ids.begin(), ops_result.blocked_ids.end());
232  }
233 
234  if (state.last_node) {
235  rerouting_info.rerouting_start_id = state.last_node->nodeid;
236  rerouting_info.rerouting_start_pose = robot_pose;
237  } else {
238  rerouting_info.rerouting_start_id = std::numeric_limits<unsigned int>::max();
239  rerouting_info.rerouting_start_pose = geometry_msgs::msg::PoseStamped();
240  }
241 
242  // Update so during rerouting we can check if we are continuing on the same edge
243  rerouting_info.curr_edge = state.current_edge;
244  RCLCPP_INFO(logger_, "Rerouting requested by route tracking operations!");
245  return TrackerResult::INTERRUPTED;
246  }
247 
248  r.sleep();
249  }
250 
251  return TrackerResult::EXITED;
252 }
253 
254 } // namespace nav2_route
A sim-time-aware rate for Nav2 loops.
Definition: rate.hpp:61
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.
Definition: types.hpp:184
Result information from the operations manager.
Definition: types.hpp:124
State shared to objects to communicate important rerouting data to avoid rerouting over blocked edges...
Definition: types.hpp:265
Current state management of route tracking class.
Definition: types.hpp:249
An ordered set of nodes and edges corresponding to the planned route.
Definition: types.hpp:212