@@ -325,18 +325,26 @@ bool DefaultPlanner::is_goal_valid(
325
325
return true ;
326
326
}
327
327
}
328
-
329
- constexpr auto max_queries = 50 ;
330
- lanelet::ConstLanelets goal_candidate_lanelets;
331
- lanelet::BasicPoint2d goal_point{goal.position .x , goal.position .y };
332
- auto nb_queries = 0 ;
333
- lanelet_map_ptr_->laneletLayer .nearestUntil (goal_point, [&](const auto &, const auto & ll) {
334
- if (route_handler_.isRoadLanelet (ll)) goal_candidate_lanelets.push_back (ll);
335
- return (++nb_queries >= max_queries);
336
- });
337
328
lanelet::ConstLanelet closest_lanelet;
338
- if (!lanelet::utils::query::getClosestLanelet (goal_candidate_lanelets, goal, &closest_lanelet)) {
339
- return false ;
329
+ const auto road_lanelets_at_goal = route_handler_.getRoadLaneletsAtPose (goal);
330
+ if (!lanelet::utils::query::getClosestLanelet (road_lanelets_at_goal, goal, &closest_lanelet)) {
331
+ // if no road lanelets directly at the goal, find the closest one
332
+ const lanelet::BasicPoint2d goal_point{goal.position .x , goal.position .y };
333
+ auto closest_dist = std::numeric_limits<double >::max ();
334
+ const auto closest_road_lanelet_found = lanelet_map_ptr_->laneletLayer .nearestUntil (
335
+ goal_point, [&](const auto & bbox, const auto & ll) {
336
+ // this search is done by increasing distance between the bounding box and the goal
337
+ // we stop the search when the bounding box is further than the closest dist found
338
+ if (lanelet::geometry::distance2d (bbox, goal_point) > closest_dist)
339
+ return true ; // stop the search
340
+ const auto dist = lanelet::geometry::distance2d (goal_point, ll.polygon2d ());
341
+ if (route_handler_.isRoadLanelet (ll) && dist < closest_dist) {
342
+ closest_dist = dist;
343
+ closest_lanelet = ll;
344
+ }
345
+ return false ; // continue the search
346
+ });
347
+ if (!closest_road_lanelet_found) return false ;
340
348
}
341
349
342
350
const auto local_vehicle_footprint = vehicle_info_.createFootprint ();
0 commit comments