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..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 @@ -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,21 @@ 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; + + // 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) < + 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!!");