Update shift pull out behavior and add debug data

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -13,7 +13,7 @@
       center_line_path_interval: 1.0
       # shift pull out
       enable_shift_pull_out: true
-      check_shift_path_lane_departure: false
+      check_shift_path_lane_departure: true
       minimum_shift_pull_out_distance: 0.0
       deceleration_interval: 15.0
       lateral_jerk: 0.5

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

@@ -46,6 +46,7 @@ struct StartPlannerDebugData
   std::vector<PoseWithVelocityStamped> ego_predicted_path;
   // collision check debug map
   CollisionCheckDebugMap collision_check;
+  lanelet::ConstLanelets drivable_lanes;
 
   Pose refined_start_pose;
   std::vector<Pose> start_pose_candidates;

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -102,7 +102,7 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
     // check lane departure
     if (
       parameters_.check_shift_path_lane_departure &&
-      lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_start_to_end)) {
+      lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_shift_start_to_end)) {
       continue;
     }
 

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -23,6 +23,7 @@
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <lanelet2_extension/visualization/visualization.hpp>
 #include <magic_enum.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -1199,8 +1200,12 @@ bool StartPlannerModule::isSafePath() const
   const double hysteresis_factor =
     status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate;
 
-  behavior_path_planner::updateSafetyCheckDebugData(
-    debug_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
+  // debug
+  {
+    debug_data_.filtered_objects = filtered_objects;
+    debug_data_.target_objects_on_lane = target_objects_on_lane;
+    debug_data_.ego_predicted_path = ego_predicted_path;
+  }
 
   return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
     pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane,
@@ -1337,6 +1342,10 @@ void StartPlannerModule::updateDrivableLanes()
       shift_pull_out->setDrivableLanes(drivable_lanes);
     }
   }
+  // debug
+  {
+    debug_data_.drivable_lanes = drivable_lanes;
+  }
 }
 
 lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const
@@ -1365,6 +1374,7 @@ lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const
 
 void StartPlannerModule::setDebugData()
 {
+  using lanelet::visualization::laneletsAsTriangleMarkerArray;
   using marker_utils::addFootprintMarker;
   using marker_utils::createFootprintMarkerArray;
   using marker_utils::createObjectsMarkerArray;
@@ -1483,6 +1493,10 @@ void StartPlannerModule::setDebugData()
     }
 
     add(pull_out_path_footprint_marker_array);
+    std::cerr << "path_shift_start_to_end is outside of drivable lanes: "
+              << lane_departure_checker_->checkPathWillLeaveLane(
+                   debug_data_.drivable_lanes, path_shift_start_to_end)
+              << std::endl;
   }
 
   // safety check
@@ -1532,6 +1546,10 @@ void StartPlannerModule::setDebugData()
     planner_type_marker_array.markers.push_back(marker);
     add(planner_type_marker_array);
   }
+
+  add(laneletsAsTriangleMarkerArray(
+    "drivable_lanes_for_shift_pull_out_path", debug_data_.drivable_lanes,
+    createMarkerColor(0.16, 1.0, 0.69, 0.2)));
 }
 
 void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const