20 #include "nav2_route/goal_intent_extractor.hpp"
21 #include "nav2_ros_common/tf2_factories.hpp"
26 static float EPSILON = 1e-6;
29 nav2::LifecycleNode::SharedPtr node,
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)
37 logger_ = node->get_logger();
38 id_to_graph_map_ = id_to_graph_map;
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);
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);
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));
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);
63 int num_of_nearest_nodes = node->declare_or_get_parameter(
"num_nearest_nodes", 5);
64 node_spatial_tree_->setNumOfNearestNodes(num_of_nearest_nodes);
69 id_to_graph_map_ = id_to_graph_map;
71 node_spatial_tree_->computeTree(graph);
75 geometry_msgs::msg::PoseStamped & pose,
76 const std::string & target_frame)
78 if (pose.header.frame_id != target_frame) {
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)) {
96 template<
typename GoalT>
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};
114 geometry_msgs::msg::PoseStamped start_pose, goal_pose = goal->goal;
115 if (goal->use_start) {
116 start_pose = goal->start;
118 if (!nav2_util::getFreshPose(
119 *tf_, route_frame_, base_frame_, clock_->now(), transform_staleness_threshold_,
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))
137 "Could not determine node closest to start or goal pose requested!");
140 unsigned int start_route_loc = start_route.front();
141 unsigned int end_route_loc = end_route.front();
147 std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap =
nullptr;
148 std::string costmap_frame_id;
149 bool enable_search = enable_search_;
152 costmap = costmap_subscriber_->getCostmap();
153 costmap_frame_id = costmap_subscriber_->getFrameID();
154 }
catch (
const std::exception & ex) {
155 enable_search =
false;
158 "Failed to get costmap for goal intent extractor: %s. "
159 "Falling back to closest euclidean route node instead.", ex.what());
163 if (enable_search && start_route.size() > 1u) {
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));
177 auto transformed_start =
transformPose(start_, costmap_frame_id);
178 std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap->getMutex());
181 candidate_nodes.front().pose.position, transformed_start.pose.position))
185 if (bfs.
search(transformed_start, candidate_nodes, max_nn_search_iterations_)) {
192 if (enable_search && end_route.size() > 1u) {
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));
206 auto transformed_end =
transformPose(goal_, costmap_frame_id);
207 std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap->getMutex());
210 candidate_nodes.front().pose.position, transformed_end.pose.position))
214 if (bfs.
search(transformed_end, candidate_nodes)) {
221 return {start_route_loc, end_route_loc};
224 template<
typename GoalT>
226 const Route & input_route,
227 const std::shared_ptr<const GoalT> goal,
230 Route pruned_route = input_route;
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;
239 if (input_route.edges.empty() || (!goal->use_poses && first_time)) {
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 &&
253 hypotf(vpx, vpy) > min_dist_from_start_ &&
254 utils::distance(closest_pt_on_edge, start_) <= max_dist_from_edge_)
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();
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());
269 if (!prune_goal_ || !goal->use_poses || pruned_route.edges.empty()) {
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;
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 &&
284 hypotf(vpx, vpy) > min_dist_from_goal_ &&
285 utils::distance(closest_pt_on_edge, goal_) <= max_dist_from_edge_)
287 pruned_route.route_cost -= pruned_route.edges.back()->end->search_state.traversal_cost;
288 pruned_route.edges.pop_back();
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,
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,
308 template NodeExtents GoalIntentExtractor::findStartandGoal<nav2_msgs::action::ComputeRoute::Goal>(
309 const std::shared_ptr<const nav2_msgs::action::ComputeRoute::Goal> goal);
311 NodeExtents GoalIntentExtractor::findStartandGoal<nav2_msgs::action::ComputeAndTrackRoute::Goal>(
312 const std::shared_ptr<const nav2_msgs::action::ComputeAndTrackRoute::Goal> goal);
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.
An object representing edges between nodes.
An object to store the nodes in the graph file.
State shared to objects to communicate important rerouting data to avoid rerouting over blocked edges...
An ordered set of nodes and edges corresponding to the planned route.