From 3a2dd14ba693499b4a64d53dd8023c96c9f36c53 Mon Sep 17 00:00:00 2001
From: kosuke55 <kosuke.tnp@gmail.com>
Date: Fri, 31 Jan 2025 16:08:26 +0900
Subject: [PATCH] feat(goal_planner): resample path interval for lane departure
check accuracy
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
.../src/pull_over_planner/geometric_pull_over.cpp | 5 +++++
.../src/pull_over_planner/shift_pull_over.cpp | 8 ++++++--
2 files changed, 11 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp
index b7b4ad83de362..14307261ce7f5 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp
+++ b/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(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp
index c8e0788e39b6f..83628bb483e56 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp
+++ b/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 {};