Feat: working concept, but still need to fix the behind part

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp

@@ -96,6 +96,8 @@ MarkerArray makeOverhangToRoadShoulderMarkerArray(
 MarkerArray createOvehangFurthestLineStringMarkerArray(
   const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r,
   const double g, const double b);
+
+MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings);
 }  // namespace marker_utils
 
 std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -509,9 +509,8 @@ void BehaviorPathPlannerNode::run()
   publishDebugMarker(bt_manager_->getDebugMarkers());
 
   if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
-    const auto drivable_area_lines = marker_utils::createOvehangFurthestLineStringMarkerArray(
-      util::getDrivableAreaforAllSharedLinestringLanelets(planner_data_), "drivables", 0.984, 0.831,
-      0.725);
+    const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray(
+      util::getDrivableAreaforAllSharedLinestringLanelets(planner_data_));
     debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines);
   }
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");

planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp

@@ -557,6 +557,95 @@ MarkerArray createOvehangFurthestLineStringMarkerArray(
 
   return msg;
 }
+
+MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings)
+{
+  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
+
+  MarkerArray msg;
+
+  Marker marker{};
+  marker.header.frame_id = "map";
+  marker.header.stamp = current_time;
+  marker.ns = "shared_linestring_lanelets";
+  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+
+
+  marker.type = Marker::LINE_STRIP;
+  marker.action = Marker::ADD;
+  marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
+  marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.0, 0.0);
+  marker.color = tier4_autoware_utils::createMarkerColor(0.996, 0.658, 0.466, 0.999);
+
+  const auto reserve_size = linestrings.size() / 2;
+
+  lanelet::ConstLineStrings3d lefts;
+  lanelet::ConstLineStrings3d rights;
+  lefts.reserve(reserve_size);
+  rights.reserve(reserve_size);
+
+  for (size_t idx = 1; idx < linestrings.size(); idx+=2) {
+    lefts.emplace_back(linestrings.at(idx - 1));
+    rights.emplace_back(linestrings.at(idx));
+  }
+
+  const auto & first_ls = linestrings.front().basicLineString();
+  for(const auto & ls:first_ls){
+  Point p;
+  p.x = ls.x();
+  p.y = ls.y();
+  p.z = ls.z();
+  marker.points.push_back(p);
+  }
+
+  for (auto idx = lefts.cbegin() + 1; idx != lefts.cend(); ++idx) {
+    const auto marker_back = marker.points.back();
+    const auto left_front = idx->basicLineString().front();
+    Point front;
+    front.x = left_front.x();
+    front.y = left_front.y();
+    front.z = left_front.z();
+    const auto left_inverted_front = idx->invert().basicLineString().front();
+    Point front_inverted;
+    front_inverted.x = left_inverted_front.x();
+    front_inverted.y = left_inverted_front.y();
+    front_inverted.z = left_inverted_front.z();
+    const bool cond = tier4_autoware_utils::calcDistance2d(marker_back, front) < tier4_autoware_utils::calcDistance2d(marker_back, front_inverted);
+    const auto & left_ls = (cond) ? idx->basicLineString() : idx->invert().basicLineString();
+    for (auto ls = left_ls.cbegin(); ls != left_ls.cend(); ++ls) {
+      Point p;
+      p.x = ls->x();
+      p.y = ls->y();
+      p.z = ls->z();
+      marker.points.push_back(p);
+    }
+  }
+  for (auto idx = rights.crbegin(); idx != rights.crend(); ++idx) {
+    const auto marker_back = marker.points.back();
+    const auto right_front = idx->basicLineString().front();
+    Point front;
+    front.x = right_front.x();
+    front.y = right_front.y();
+    front.z = right_front.z();
+    const auto right_inverted_front = idx->invert().basicLineString().front();
+    Point front_inverted;
+    front_inverted.x = right_inverted_front.x();
+    front_inverted.y = right_inverted_front.y();
+    front_inverted.z = right_inverted_front.z();
+    const bool cond = tier4_autoware_utils::calcDistance2d(marker_back, front) > tier4_autoware_utils::calcDistance2d(marker_back, front_inverted);
+    const auto & right_ls = (cond) ? idx->basicLineString() : idx->invert().basicLineString();
+    for (auto ls = right_ls.crbegin(); ls != right_ls.crend(); ++ls) {
+      Point p;
+      p.x = ls->x();
+      p.y = ls->y();
+      p.z = ls->z();
+      marker.points.push_back(p);
+    }
+  }
+  marker.points.push_back(marker.points.front());
+  msg.markers.push_back(marker);
+  return msg;
+}
 }  // namespace marker_utils
 
 std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr)
@@ -593,7 +682,6 @@ std::string toStrInfo(const behavior_path_planner::AvoidPointArray & ap_arr)
   }
   return ss.str();
 }
-
 std::string toStrInfo(const behavior_path_planner::AvoidPoint & ap)
 {
   std::stringstream pids;