From 244821026689b33669801c3bcbf14a8db1d9188a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 15 May 2024 16:20:28 +0900 Subject: [PATCH 1/5] remove road_lanelets_ Signed-off-by: Maxime CLEMENT --- .../src/lanelet2_plugins/default_planner.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 4bc0f21dd947b..5b300af25611a 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -186,7 +186,6 @@ void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg) lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); - road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); is_graph_ready_ = true; } @@ -331,8 +330,16 @@ bool DefaultPlanner::is_goal_valid( } } - lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { + constexpr auto max_queries = 50; + lanelet::ConstLanelets goal_candidate_lanelets; + lanelet::BasicPoint2d goal_point{goal.position.x, goal.position.y}; + auto nb_queries = 0; + lanelet_map_ptr_->laneletLayer.nearestUntil(goal_point, [&](const auto &, const auto & ll) { + if (route_handler_.isRoadLanelet(ll)) goal_candidate_lanelets.push_back(ll); + return (++nb_queries >= max_queries); + }); + lanelet::ConstLanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLanelet(goal_candidate_lanelets, goal, &closest_lanelet)) { return false; } From fa1cadf5115f40f8662cd9f0465e7d55b7c87deb Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 15 May 2024 16:39:21 +0900 Subject: [PATCH 2/5] use route_handler to get the left/right shoulder lanelet Signed-off-by: Maxime CLEMENT --- .../src/lanelet2_plugins/default_planner.cpp | 4 +- .../lanelet2_plugins/utility_functions.cpp | 39 +++++++------------ .../lanelet2_plugins/utility_functions.hpp | 5 ++- 3 files changed, 18 insertions(+), 30 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 5b300af25611a..c22bc075ad4a5 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -281,7 +281,7 @@ bool DefaultPlanner::check_goal_footprint_inside_lanes( lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); lanelet::ConstLanelet combined_lanelets = - combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_); + combine_lanelets_with_shoulder(lanelets, route_handler_); // if next lanelet length is longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -352,7 +352,7 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets const lanelet::ConstLanelet combined_prev_lanelet = - combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_); + combine_lanelets_with_shoulder(path_lanelets, route_handler_); // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 4ef4bd6f20ed0..3552ef6abd28b 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -42,7 +42,7 @@ void insert_marker_array( } lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets) + const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler) { lanelet::Points3d lefts; lanelet::Points3d rights; @@ -57,30 +57,18 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder( } } + // lambda to add bound to target_bound + const auto add_bound = [](const auto & bound, auto & target_bound) { + std::transform( + bound.begin(), bound.end(), std::back_inserter(target_bound), + [](const auto & pt) { return lanelet::Point3d(pt); }); + }; for (const auto & llt : lanelets) { - // lambda to check if shoulder lane which share left bound with lanelets exist - const auto find_bound_shared_shoulder = - [&shoulder_lanelets](const auto & lanelet_bound, const auto & get_shoulder_bound) { - return std::find_if( - shoulder_lanelets.begin(), shoulder_lanelets.end(), - [&lanelet_bound, &get_shoulder_bound](const auto & shoulder_llt) { - return lanelet_bound.id() == get_shoulder_bound(shoulder_llt).id(); - }); - }; - - // lambda to add bound to target_bound - const auto add_bound = [](const auto & bound, auto & target_bound) { - std::transform( - bound.begin(), bound.end(), std::back_inserter(target_bound), - [](const auto & pt) { return lanelet::Point3d(pt); }); - }; - // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist - const auto left_shared_shoulder_it = find_bound_shared_shoulder( - llt.leftBound(), [](const auto & shoulder_llt) { return shoulder_llt.rightBound(); }); - if (left_shared_shoulder_it != shoulder_lanelets.end()) { + const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt); + if (left_shared_shoulder) { // if exist, add left bound of SHOULDER lanelet to lefts - add_bound(left_shared_shoulder_it->leftBound(), lefts); + add_bound(left_shared_shoulder->leftBound(), lefts); } else if ( // if not exist, add left bound of lanelet to lefts // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`, @@ -90,11 +78,10 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder( } // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist - const auto right_shared_shoulder_it = find_bound_shared_shoulder( - llt.rightBound(), [](const auto & shoulder_llt) { return shoulder_llt.leftBound(); }); - if (right_shared_shoulder_it != shoulder_lanelets.end()) { + const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt); + if (right_shared_shoulder) { // if exist, add right bound of SHOULDER lanelet to rights - add_bound(right_shared_shoulder_it->rightBound(), rights); + add_bound(right_shared_shoulder->rightBound(), rights); } else if ( // if not exist, add right bound of lanelet to rights // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`, diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 3eb38638e17b7..cc95d736574f6 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -16,6 +16,7 @@ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #include +#include #include #include @@ -50,10 +51,10 @@ void insert_marker_array( * @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost * bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively * @param lanelets topologically sorted sequence of lanelets - * @param shoulder_lanelets list of possible road_shoulder lanelets to combine with lanelets + * @param route_handler route handler to query the lanelet map */ lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets); + const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( From 7e6119eff3f2bad470abd9cbd91945f5128fbd03 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 15 May 2024 16:40:05 +0900 Subject: [PATCH 3/5] update copyright date Signed-off-by: Maxime CLEMENT --- .../mission_planner/src/lanelet2_plugins/default_planner.cpp | 2 +- .../mission_planner/src/lanelet2_plugins/utility_functions.cpp | 2 +- .../mission_planner/src/lanelet2_plugins/utility_functions.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index c22bc075ad4a5..1dfdcb4fc7dab 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 3552ef6abd28b..e7797946aa619 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index cc95d736574f6..428cd979a2526 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From f8efd4eaaa4872996c05c061f51bd2eee0af8dfc Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 15 May 2024 16:47:11 +0900 Subject: [PATCH 4/5] get shoulder lanelets at goal pose using route handler function Signed-off-by: Maxime CLEMENT --- .../src/lanelet2_plugins/default_planner.cpp | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 1dfdcb4fc7dab..907ebbdb8dd07 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -185,8 +185,6 @@ void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg) lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); is_graph_ready_ = true; } @@ -316,17 +314,15 @@ bool DefaultPlanner::is_goal_valid( // check if goal is in shoulder lanelet lanelet::Lanelet closest_shoulder_lanelet; + const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(goal); if (lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { - if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); - const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); - if (std::abs(angle_diff) < th_angle) { - return true; - } + shoulder_lanelets, goal, &closest_shoulder_lanelet)) { + const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); + const auto goal_yaw = tf2::getYaw(goal.orientation); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + if (std::abs(angle_diff) < th_angle) { + return true; } } From 76db48e46bd7024a61631e11a73d5226e622fbde Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 17 May 2024 12:11:21 +0900 Subject: [PATCH 5/5] Improve the search for closest lanelet Signed-off-by: Maxime CLEMENT --- .../src/lanelet2_plugins/default_planner.cpp | 30 ++++++++++++------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 907ebbdb8dd07..1942649c4d70c 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -325,18 +325,26 @@ bool DefaultPlanner::is_goal_valid( return true; } } - - constexpr auto max_queries = 50; - lanelet::ConstLanelets goal_candidate_lanelets; - lanelet::BasicPoint2d goal_point{goal.position.x, goal.position.y}; - auto nb_queries = 0; - lanelet_map_ptr_->laneletLayer.nearestUntil(goal_point, [&](const auto &, const auto & ll) { - if (route_handler_.isRoadLanelet(ll)) goal_candidate_lanelets.push_back(ll); - return (++nb_queries >= max_queries); - }); lanelet::ConstLanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLanelet(goal_candidate_lanelets, goal, &closest_lanelet)) { - return false; + const auto road_lanelets_at_goal = route_handler_.getRoadLaneletsAtPose(goal); + if (!lanelet::utils::query::getClosestLanelet(road_lanelets_at_goal, goal, &closest_lanelet)) { + // if no road lanelets directly at the goal, find the closest one + const lanelet::BasicPoint2d goal_point{goal.position.x, goal.position.y}; + auto closest_dist = std::numeric_limits::max(); + const auto closest_road_lanelet_found = lanelet_map_ptr_->laneletLayer.nearestUntil( + goal_point, [&](const auto & bbox, const auto & ll) { + // this search is done by increasing distance between the bounding box and the goal + // we stop the search when the bounding box is further than the closest dist found + if (lanelet::geometry::distance2d(bbox, goal_point) > closest_dist) + return true; // stop the search + const auto dist = lanelet::geometry::distance2d(goal_point, ll.polygon2d()); + if (route_handler_.isRoadLanelet(ll) && dist < closest_dist) { + closest_dist = dist; + closest_lanelet = ll; + } + return false; // continue the search + }); + if (!closest_road_lanelet_found) return false; } const auto local_vehicle_footprint = vehicle_info_.createFootprint();