Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
goal_intent_extractor.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 <string>
16 #include <memory>
17 #include <mutex>
18 #include <vector>
19 
20 #include "nav2_route/goal_intent_extractor.hpp"
21 #include "nav2_ros_common/tf2_factories.hpp"
22 
23 namespace nav2_route
24 {
25 
26 static float EPSILON = 1e-6;
27 
29  nav2::LifecycleNode::SharedPtr node,
30  Graph & graph,
31  GraphToIDMap * id_to_graph_map,
32  nav2::TransformBuffer::SharedPtr tf,
33  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber,
34  const std::string & route_frame,
35  const std::string & base_frame)
36 {
37  logger_ = node->get_logger();
38  id_to_graph_map_ = id_to_graph_map;
39  graph_ = &graph;
40  tf_ = tf;
41  clock_ = node->get_clock();
42  costmap_subscriber_ = costmap_subscriber;
43  route_frame_ = route_frame;
44  base_frame_ = base_frame;
45  node_spatial_tree_ = std::make_shared<NodeSpatialTree>();
46  node_spatial_tree_->computeTree(graph);
47 
48  prune_goal_ = node->declare_or_get_parameter("prune_goal", true);
49  transform_staleness_threshold_ = node->declare_or_get_parameter(
50  "transform_staleness_threshold", 0.0);
51 
52  max_dist_from_edge_ = static_cast<float>(
53  node->declare_or_get_parameter("max_prune_dist_from_edge", 8.0));
54  min_dist_from_goal_ = static_cast<float>(
55  node->declare_or_get_parameter("min_prune_dist_from_goal", 0.15));
56  min_dist_from_start_ = static_cast<float>(
57  node->declare_or_get_parameter("min_prune_dist_from_start", 0.10));
58 
59  enable_search_ = node->declare_or_get_parameter("enable_nn_search", true);
60  max_nn_search_iterations_ = node->declare_or_get_parameter(
61  "max_nn_search_iterations", 10000);
62 
63  int num_of_nearest_nodes = node->declare_or_get_parameter("num_nearest_nodes", 5);
64  node_spatial_tree_->setNumOfNearestNodes(num_of_nearest_nodes);
65 }
66 
67 void GoalIntentExtractor::setGraph(Graph & graph, GraphToIDMap * id_to_graph_map)
68 {
69  id_to_graph_map_ = id_to_graph_map;
70  graph_ = &graph;
71  node_spatial_tree_->computeTree(graph);
72 }
73 
74 geometry_msgs::msg::PoseStamped GoalIntentExtractor::transformPose(
75  geometry_msgs::msg::PoseStamped & pose,
76  const std::string & target_frame)
77 {
78  if (pose.header.frame_id != target_frame) {
79  RCLCPP_INFO(
80  logger_,
81  "Request pose in %s frame. Converting to route server frame: %s.",
82  pose.header.frame_id.c_str(), target_frame.c_str());
83  if (!nav2_util::transformPoseInTargetFrame(pose, pose, *tf_, target_frame)) {
84  throw nav2_core::RouteTFError("Failed to transform starting pose to: " + target_frame);
85  }
86  }
87  return pose;
88 }
89 
90 void GoalIntentExtractor::overrideStart(const geometry_msgs::msg::PoseStamped & start_pose)
91 {
92  // Override the start pose when rerouting is requested, using the current pose
93  start_ = start_pose;
94 }
95 
96 template<typename GoalT>
97 NodeExtents
98 GoalIntentExtractor::findStartandGoal(const std::shared_ptr<const GoalT> goal)
99 {
100  // If not using the poses, then use the requests Node IDs to establish start and goal
101  if (!goal->use_poses) {
102  unsigned int start_idx = id_to_graph_map_->at(goal->start_id);
103  unsigned int goal_idx = id_to_graph_map_->at(goal->goal_id);
104  const Coordinates & start_coords = graph_->at(start_idx).coords;
105  const Coordinates & goal_coords = graph_->at(goal_idx).coords;
106  start_.pose.position.x = start_coords.x;
107  start_.pose.position.y = start_coords.y;
108  goal_.pose.position.x = goal_coords.x;
109  goal_.pose.position.y = goal_coords.y;
110  return {start_idx, goal_idx};
111  }
112 
113  // Find request start pose
114  geometry_msgs::msg::PoseStamped start_pose, goal_pose = goal->goal;
115  if (goal->use_start) {
116  start_pose = goal->start;
117  } else {
118  if (!nav2_util::getFreshPose(
119  *tf_, route_frame_, base_frame_, clock_->now(), transform_staleness_threshold_,
120  start_pose))
121  {
122  throw nav2_core::RouteTFError("Failed to obtain starting pose in: " + route_frame_);
123  }
124  }
125 
126  // transform to route_frame
127  start_ = transformPose(start_pose, route_frame_);
128  goal_ = transformPose(goal_pose, route_frame_);
129 
130  // Find closest route graph nodes to start and goal to plan between.
131  // Note that these are the location indices in the graph
132  std::vector<unsigned int> start_route, end_route;
133  if (!node_spatial_tree_->findNearestGraphNodesToPose(start_, start_route) ||
134  !node_spatial_tree_->findNearestGraphNodesToPose(goal_, end_route))
135  {
137  "Could not determine node closest to start or goal pose requested!");
138  }
139 
140  unsigned int start_route_loc = start_route.front();
141  unsigned int end_route_loc = end_route.front();
142 
143  // If given cost information, check which of the nearest graph nodes is nearest by
144  // traversability, not just Euclidean distance, in case of obstacles, walls, etc.
145  // However, if the closest node has Line of Sight to the goal, then use that node
146  // skipping the search as we know it is the closest and now optimally traversible node.
147  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap = nullptr;
148  std::string costmap_frame_id;
149  bool enable_search = enable_search_;
150  if (enable_search) {
151  try {
152  costmap = costmap_subscriber_->getCostmap();
153  costmap_frame_id = costmap_subscriber_->getFrameID();
154  } catch (const std::exception & ex) {
155  enable_search = false;
156  RCLCPP_WARN(
157  logger_,
158  "Failed to get costmap for goal intent extractor: %s. "
159  "Falling back to closest euclidean route node instead.", ex.what());
160  }
161  }
162 
163  if (enable_search && start_route.size() > 1u) {
164  // Convert the nearest node candidates to the costmap frame for search
165  std::vector<geometry_msgs::msg::PoseStamped> candidate_nodes;
166  candidate_nodes.reserve(start_route.size());
167  for (const auto & node : start_route) {
168  auto & node_data = graph_->at(node);
169  geometry_msgs::msg::PoseStamped node_pose;
170  node_pose.pose.position.x = node_data.coords.x;
171  node_pose.pose.position.y = node_data.coords.y;
172  node_pose.header.frame_id = node_data.coords.frame_id;
173  node_pose.header.stamp = start_pose.header.stamp;
174  candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id));
175  }
176 
177  auto transformed_start = transformPose(start_, costmap_frame_id);
178  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap->getMutex());
179  GoalIntentSearch::LoSCollisionChecker los_checker(costmap);
180  if (los_checker.worldToMap(
181  candidate_nodes.front().pose.position, transformed_start.pose.position))
182  {
183  if (los_checker.isInCollision()) {
185  if (bfs.search(transformed_start, candidate_nodes, max_nn_search_iterations_)) {
186  start_route_loc = start_route[bfs.getClosestNodeIdx()];
187  }
188  }
189  }
190  }
191 
192  if (enable_search && end_route.size() > 1u) {
193  // Convert the nearest node candidates to the costmap frame for search
194  std::vector<geometry_msgs::msg::PoseStamped> candidate_nodes;
195  candidate_nodes.reserve(end_route.size());
196  for (const auto & node : end_route) {
197  auto & node_data = graph_->at(node);
198  geometry_msgs::msg::PoseStamped node_pose;
199  node_pose.pose.position.x = node_data.coords.x;
200  node_pose.pose.position.y = node_data.coords.y;
201  node_pose.header.frame_id = node_data.coords.frame_id;
202  node_pose.header.stamp = goal_pose.header.stamp;
203  candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id));
204  }
205 
206  auto transformed_end = transformPose(goal_, costmap_frame_id);
207  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap->getMutex());
208  GoalIntentSearch::LoSCollisionChecker los_checker(costmap);
209  if (los_checker.worldToMap(
210  candidate_nodes.front().pose.position, transformed_end.pose.position))
211  {
212  if (los_checker.isInCollision()) {
214  if (bfs.search(transformed_end, candidate_nodes)) {
215  end_route_loc = end_route[bfs.getClosestNodeIdx()];
216  }
217  }
218  }
219  }
220 
221  return {start_route_loc, end_route_loc};
222 }
223 
224 template<typename GoalT>
226  const Route & input_route,
227  const std::shared_ptr<const GoalT> goal,
228  ReroutingState & rerouting_info)
229 {
230  Route pruned_route = input_route;
231 
232  // Grab and update the rerouting state
233  EdgePtr last_curr_edge = rerouting_info.curr_edge;
234  rerouting_info.curr_edge = nullptr;
235  bool first_time = rerouting_info.first_time;
236  rerouting_info.first_time = false;
237 
238  // Cannot prune if no edges to prune or if using nodeIDs in the initial request (no effect)
239  if (input_route.edges.empty() || (!goal->use_poses && first_time)) {
240  return pruned_route;
241  }
242 
243  // Check on pruning the start node
244  NodePtr first = pruned_route.start_node;
245  NodePtr next = pruned_route.edges[0]->end;
246  float vrx = next->coords.x - first->coords.x;
247  float vry = next->coords.y - first->coords.y;
248  float vpx = start_.pose.position.x - first->coords.x;
249  float vpy = start_.pose.position.y - first->coords.y;
250  float dot_prod = utils::normalizedDot(vrx, vry, vpx, vpy);
251  Coordinates closest_pt_on_edge = utils::findClosestPoint(start_, first->coords, next->coords);
252  if (dot_prod > EPSILON && // A projection exists
253  hypotf(vpx, vpy) > min_dist_from_start_ && // We're not on the node to prune entire edge
254  utils::distance(closest_pt_on_edge, start_) <= max_dist_from_edge_) // Close enough to edge
255  {
256  // Record the pruned edge information if its the same edge as previously routed so that
257  // the tracker can seed this information into its state to proceed with its task losslessly
258  if (last_curr_edge && last_curr_edge->edgeid == pruned_route.edges.front()->edgeid) {
259  rerouting_info.closest_pt_on_edge = closest_pt_on_edge;
260  rerouting_info.curr_edge = pruned_route.edges.front();
261  }
262 
263  pruned_route.start_node = next;
264  pruned_route.route_cost -= pruned_route.edges.front()->end->search_state.traversal_cost;
265  pruned_route.edges.erase(pruned_route.edges.begin());
266  }
267 
268  // Don't prune the goal if requested, if given a known goal_id (no effect), or now empty
269  if (!prune_goal_ || !goal->use_poses || pruned_route.edges.empty()) {
270  return pruned_route;
271  }
272 
273  // Check on pruning the goal node
274  next = pruned_route.edges.back()->start;
275  NodePtr last = pruned_route.edges.back()->end;
276  vrx = last->coords.x - next->coords.x;
277  vry = last->coords.y - next->coords.y;
278  vpx = goal_.pose.position.x - last->coords.x;
279  vpy = goal_.pose.position.y - last->coords.y;
280 
281  dot_prod = utils::normalizedDot(vrx, vry, vpx, vpy);
282  closest_pt_on_edge = utils::findClosestPoint(goal_, next->coords, last->coords);
283  if (dot_prod < -EPSILON && // A projection exists
284  hypotf(vpx, vpy) > min_dist_from_goal_ && // We're not on the node to prune entire edge
285  utils::distance(closest_pt_on_edge, goal_) <= max_dist_from_edge_) // Close enough to edge
286  {
287  pruned_route.route_cost -= pruned_route.edges.back()->end->search_state.traversal_cost;
288  pruned_route.edges.pop_back();
289  }
290 
291  return pruned_route;
292 }
293 
294 geometry_msgs::msg::PoseStamped GoalIntentExtractor::getStart()
295 {
296  return start_;
297 }
298 
299 template Route GoalIntentExtractor::pruneStartandGoal<nav2_msgs::action::ComputeRoute::Goal>(
300  const Route & input_route,
301  const std::shared_ptr<const nav2_msgs::action::ComputeRoute::Goal> goal,
302  ReroutingState & rerouting_info);
303 template
304 Route GoalIntentExtractor::pruneStartandGoal<nav2_msgs::action::ComputeAndTrackRoute::Goal>(
305  const Route & input_route,
306  const std::shared_ptr<const nav2_msgs::action::ComputeAndTrackRoute::Goal> goal,
307  ReroutingState & rerouting_info);
308 template NodeExtents GoalIntentExtractor::findStartandGoal<nav2_msgs::action::ComputeRoute::Goal>(
309  const std::shared_ptr<const nav2_msgs::action::ComputeRoute::Goal> goal);
310 template
311 NodeExtents GoalIntentExtractor::findStartandGoal<nav2_msgs::action::ComputeAndTrackRoute::Goal>(
312  const std::shared_ptr<const nav2_msgs::action::ComputeAndTrackRoute::Goal> goal);
313 
314 } // namespace nav2_route
geometry_msgs::msg::PoseStamped transformPose(geometry_msgs::msg::PoseStamped &pose, const std::string &frame_id)
Transforms a pose into the route frame.
void setGraph(Graph &graph, GraphToIDMap *id_to_graph_map)
Sets a new graph when updated.
Route pruneStartandGoal(const Route &input_route, const std::shared_ptr< const GoalT > goal, ReroutingState &rerouting_info)
Prune the start and end nodes in a route if the start or goal poses, respectively,...
void overrideStart(const geometry_msgs::msg::PoseStamped &start_pose)
Override the start pose for use in pruning if it is externally overridden Usually by the rerouting lo...
void configure(nav2::LifecycleNode::SharedPtr node, Graph &graph, GraphToIDMap *id_to_graph_map, nav2::TransformBuffer::SharedPtr tf, std::shared_ptr< nav2_costmap_2d::CostmapSubscriber > costmap_subscriber, const std::string &route_frame, const std::string &base_frame)
Configure extractor.
NodeExtents findStartandGoal(const std::shared_ptr< const GoalT > goal)
Main API to find the start and goal graph IDX (not IDs) for routing.
geometry_msgs::msg::PoseStamped getStart()
gets the desired start pose
bool search(const geometry_msgs::msg::PoseStamped &reference_node, const std::vector< geometry_msgs::msg::PoseStamped > &candidate_nodes, const int max_iterations=std::numeric_limits< int >::max())
Search for the closest node to the given reference node.
unsigned int getClosestNodeIdx()
Get the output closest node in candidate indices.
bool isInCollision()
Check if the line segment is in collision with the costmap.
bool worldToMap(const geometry_msgs::msg::Point &start, const geometry_msgs::msg::Point &end)
Find the line segment in cosmap frame.
An object to store Node coordinates in different frames.
Definition: types.hpp:174
An object representing edges between nodes.
Definition: types.hpp:135
An object to store the nodes in the graph file.
Definition: types.hpp:184
State shared to objects to communicate important rerouting data to avoid rerouting over blocked edges...
Definition: types.hpp:265
An ordered set of nodes and edges corresponding to the planned route.
Definition: types.hpp:212