feat(goal_planner): resample path interval for lane departure check a…

…ccuracy

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

@@ -16,6 +16,7 @@
 
 #include "autoware/behavior_path_goal_planner_module/util.hpp"
 #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
+#include "autoware/behavior_path_planner_common/utils/path_utils.hpp"
 
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
@@ -77,6 +78,10 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   const auto arc_path = planner_.getArcPath();
 
   // check lane departure with road and shoulder lanes
+  // To improve the accuracy of lane departure detection, make the sampling interval finer
+  // todo: Implement lane departure detection that does not depend on the footprint
+  const auto resampled_arc_path =
+    utils::resamplePathWithSpline(arc_path, parameters_.center_line_path_interval / 2);
   if (lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, arc_path)) return {};
 
   auto pull_over_path_opt = PullOverPath::create(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -299,8 +299,12 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
 
   const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
     pull_over_lanes, *planner_data->route_handler, left_side_parking_);
-  const bool is_in_lanes = !lane_departure_checker_.checkPathWillLeaveLane(
-    {departure_check_lane}, pull_over_path.parking_path());
+  // To improve the accuracy of lane departure detection, make the sampling interval finer
+  // todo: Implement lane departure detection that does not depend on the footprint
+  const auto resampled_parking_path = utils::resamplePathWithSpline(
+    pull_over_path.parking_path(), parameters_.center_line_path_interval / 2);
+  const bool is_in_lanes =
+    !lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, resampled_parking_path);
 
   if (!is_in_parking_lots && !is_in_lanes) {
     return {};