refactor(mission_planner): use combineLaneletsShape in lanelet2_exten…

…sion (autowarefoundation#5535) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Nov 9, 2023 · c4ca645 · c4ca645
1 parent 2cbd0c3
commit c4ca645
Show file tree

Hide file tree

Showing 3 changed files with 2 additions and 36 deletions.
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
@@ -288,7 +288,7 @@ 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 = lanelet::utils::combineLaneletsShape(lanelets);
 
     // if next lanelet length longer than vehicle longitudinal offset
     if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
@@ -347,7 +347,7 @@ 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 = lanelet::utils::combineLaneletsShape(path_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
@@ -54,39 +54,6 @@ 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::Points3d lefts;
-  lanelet::Points3d rights;
-  lanelet::Points3d centers;
-  std::vector<uint64_t> left_bound_ids;
-  std::vector<uint64_t> right_bound_ids;
-
-  for (const auto & llt : lanelets) {
-    if (llt.id() != 0) {
-      left_bound_ids.push_back(llt.leftBound().id());
-      right_bound_ids.push_back(llt.rightBound().id());
-    }
-  }
-
-  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));
-      }
-    }
-    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));
-      }
-    }
-  }
-  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);
-  return std::move(combined_lanelet);
-}
-
 std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet)
 {
   std::vector<geometry_msgs::msg::Point> centerline_points;

diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp
@@ -49,7 +49,6 @@ 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);
 std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
 geometry_msgs::msg::Pose convertBasicPoint3dToPose(
   const lanelet::BasicPoint3d & point, const double lane_yaw);