From d9571352593c4f73ac361b20ed27ab86b9c39d52 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 12 Jan 2024 00:36:03 +0900 Subject: [PATCH] feat(mission_planner): check shoulder lanelets for check_goal_footprint (#6053) Signed-off-by: kosuke55 --- .../src/lanelet2_plugins/default_planner.cpp | 6 ++- .../lanelet2_plugins/utility_functions.cpp | 50 +++++++++++++++---- .../lanelet2_plugins/utility_functions.hpp | 3 +- 3 files changed, 47 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 5d1c0c68d8208..e98084204ba78 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -292,7 +292,8 @@ bool DefaultPlanner::check_goal_footprint( lanelet::ConstLanelets lanelets; lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); - lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); + lanelet::ConstLanelet combined_lanelets = + combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_); // if next lanelet length longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -351,7 +352,8 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets - lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); + lanelet::ConstLanelet combined_prev_lanelet = + combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_); // 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 690a864fcdacd..e3983b7f3b962 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -54,7 +54,8 @@ void insert_marker_array( a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets) { lanelet::Points3d lefts; lanelet::Points3d rights; @@ -70,17 +71,48 @@ lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) } for (const auto & llt : lanelets) { - if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { - for (const auto & pt : llt.leftBound()) { - lefts.push_back(lanelet::Point3d(pt)); - } + // 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()) { + // if exist, add left bound of SHOULDER lanelet to lefts + add_bound(left_shared_shoulder_it->leftBound(), lefts); + } else if ( + // if not exist, add left bound of lanelet to lefts + std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { + add_bound(llt.leftBound(), lefts); } - if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { - for (const auto & pt : llt.rightBound()) { - rights.push_back(lanelet::Point3d(pt)); - } + + // 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()) { + // if exist, add right bound of SHOULDER lanelet to rights + add_bound(right_shared_shoulder_it->rightBound(), rights); + } else if ( + // if not exist, add right bound of lanelet to rights + std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { + add_bound(llt.rightBound(), rights); } } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 3ea6237f38501..3287267a00dfe 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -49,7 +49,8 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw);