From 4db9fc5231c530d8ad15ea46050f3adc7fdfaade Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Fri, 10 Nov 2023 03:31:22 +0900
Subject: [PATCH] fix(mission_planner): revert "refactor(mission_planner): use
 combineLaneletsShape in lanele2_extension (#5535)" (#5543)

Revert "refactor(mission_planner): use combineLaneletsShape in lanelet2_extension (#5535)"

This reverts commit c4ca645cbbd0a7b919ba1c2cdf78742bb81b87b5.
---
 .../src/lanelet2_plugins/default_planner.cpp  |  4 +--
 .../lanelet2_plugins/utility_functions.cpp    | 33 +++++++++++++++++++
 .../lanelet2_plugins/utility_functions.hpp    |  1 +
 3 files changed, 36 insertions(+), 2 deletions(-)

diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
index 0fce63827b5e7..673519b6f7a0e 100644
--- 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 = lanelet::utils::combineLaneletsShape(lanelets);
+    lanelet::ConstLanelet combined_lanelets = combine_lanelets(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 = lanelet::utils::combineLaneletsShape(path_lanelets);
+  lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(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
index 126c673108b3c..690a864fcdacd 100644
--- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp
+++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp
@@ -54,6 +54,39 @@ 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
index df9d42bab9444..3ea6237f38501 100644
--- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp
+++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp
@@ -49,6 +49,7 @@ 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);