Skip to content

Commit 7fc3418

Browse files
committedMay 17, 2024
Improve the search for closest lanelet
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 90fa4fe commit 7fc3418

File tree

1 file changed

+19
-11
lines changed

1 file changed

+19
-11
lines changed
 

‎planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

+19-11
Original file line numberDiff line numberDiff line change
@@ -325,18 +325,26 @@ bool DefaultPlanner::is_goal_valid(
325325
return true;
326326
}
327327
}
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-
});
337328
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;
340348
}
341349

342350
const auto local_vehicle_footprint = vehicle_info_.createFootprint();

0 commit comments

Comments
 (0)