From 158b6d2b22bb05a06bd885da0d917bc7d953356c Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 1 Aug 2024 12:50:33 +0900 Subject: [PATCH 1/2] fix when prepare length is insufficient Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change.param.yaml | 5 +++++ .../utils/data_structs.hpp | 3 +++ .../src/manager.cpp | 7 +++++++ .../src/scene.cpp | 20 +++++++++++++++++++ 4 files changed, 35 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 7a9c466fec260..a7128a0dc546f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -26,6 +26,11 @@ min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + skip_process: + longitudinal_distance_diff_threshold: + prepare: 0.5 + lane_changing: 0.5 + # safety check safety_check: allow_loose_check_for_cancel: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 155fbfdb535f9..9ef47485ec68c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -124,6 +124,9 @@ struct Parameters double min_longitudinal_acc{-1.0}; double max_longitudinal_acc{1.0}; + double skip_process_lon_diff_th_prepare{0.5}; + double skip_process_lon_diff_th_lane_changing{1.0}; + // collision check bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; bool enable_collision_check_for_prepare_phase_in_intersection{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index f3f371ec02a9b..42166c4dff0e0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -354,6 +354,13 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc); } + { + const std::string ns = "lane_change.skip_process.longitudinal_distance_diff_threshold."; + updateParam(parameters, ns + "prepare", p->skip_process_lon_diff_th_prepare); + updateParam( + parameters, ns + "lane_changing", p->skip_process_lon_diff_th_lane_changing); + } + { const std::string ns = "lane_change.safety_check.lane_expansion."; updateParam(parameters, ns + "left_offset", p->lane_expansion_left_offset); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 0e11f38e56db8..022a9cd0f732b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1417,6 +1417,13 @@ bool NormalLaneChange::getLaneChangePaths( continue; } + if (!candidate_paths->empty()) { + const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; + if (std::abs(prev_prep_diff) < lane_change_parameters_->skip_process_lon_diff_th_prepare) { + RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); + continue; + } + } auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); const auto debug_print = [&](const auto & s) { @@ -1488,6 +1495,19 @@ bool NormalLaneChange::getLaneChangePaths( lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing, lane_changing_length); }; + if (!candidate_paths->empty()) { + const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; + const auto lc_length_diff = + candidate_paths->back().info.length.lane_changing - lane_changing_length; + + if ( + std::abs(prev_prep_diff) < eps && + std::abs(lc_length_diff) < + lane_change_parameters_->skip_process_lon_diff_th_lane_changing) { + RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); + continue; + } + } if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); From a41fe971bf4e210e8b57b9bcc28d8052174cf759 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 9 Aug 2024 18:27:50 +0900 Subject: [PATCH 2/2] add reason for comparing prev_prep_diff with eps for lc_length_diff Signed-off-by: Zulfaqar Azmi --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 022a9cd0f732b..90973d02f51b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1500,6 +1500,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto lc_length_diff = candidate_paths->back().info.length.lane_changing - lane_changing_length; + // We only check lc_length_diff if and only if the current prepare_length is equal to the + // previous prepare_length. if ( std::abs(prev_prep_diff) < eps && std::abs(lc_length_diff) <