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 29ba4e43f0d2b..a4a42fdf2300e 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 @@ -27,7 +27,7 @@ longitudinal_distance_diff_threshold: prepare: 1.0 lane_changing: 1.0 - + # delay lane change delay_lane_change: enable: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 982de0c05608e..fc60fde4ac740 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -132,9 +132,28 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); +/** + * @brief Checks if delaying of lane change maneuver is necessary + * + * @details Scans through the provided target objects (assumed to be ordered from closest to + * furthest), and returns true if any of the objects satisfy the following conditions: + * - Not near the end of current lanes + * - There is sufficient distance from object to next one to do lane change + * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects + * which pass isParkedObject() check will be considered. + * + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, + * and transient data. + * @param lane_change_path Cadidate lane change path to apply checks on. + * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include + * target lane leading static objects). + * @param object_debug Collision check debug struct to be updated if any of the traget objects + * satisfy the conditions. + * @return bool True if conditions to delay lane change are met + */ bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & target_lane_static_objects, + const std::vector & target_objects, CollisionCheckDebugMap & object_debug); bool passed_parked_objects( 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 03feb520116aa..f4114dd5bc9c3 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 @@ -224,13 +224,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change delay - p.delay.enable = getOrDeclareParameter(*node, parameter("delay.enable")); + p.delay.enable = getOrDeclareParameter(*node, parameter("delay_lane_change.enable")); p.delay.check_only_parked_vehicle = - getOrDeclareParameter(*node, parameter("delay.check_only_parked_vehicle")); + getOrDeclareParameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); p.delay.min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("delay.min_road_shoulder_width")); - p.delay.th_parked_vehicle_shift_ratio = - getOrDeclareParameter(*node, parameter("delay.th_parked_vehicle_shift_ratio")); + getOrDeclareParameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( + *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); // lane change cancel p.cancel.enable_on_prepare_phase = 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 942902b836c95..5360d76c9941c 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 @@ -1338,7 +1338,7 @@ bool NormalLaneChange::check_candidate_path_safety( } if ( - !is_stuck && !utils::lane_change::passed_parked_objects( + !is_stuck && utils::lane_change::is_delay_lane_change( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( @@ -1519,10 +1519,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, true}; } - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); - - if (!has_passed_parked_objects) { + if (utils::lane_change::is_delay_lane_change( + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data)) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); return {false, false}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 41d1d678d5780..f4f087b031c79 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -741,23 +741,17 @@ bool isParkedObject( bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & target_lane_objects, + const std::vector & target_objects, CollisionCheckDebugMap & object_debug) { const auto & current_lane_path = common_data_ptr->current_lanes_path; if ( - target_lane_objects.empty() || lane_change_path.path.points.empty() || + target_objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { return false; } - const auto vel_threshold = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; - auto is_static = [&vel_threshold](const ExtendedPredictedObject & obj) { - const auto obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - return obj_vel_norm < vel_threshold; - }; - const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end; const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min; auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) { @@ -765,24 +759,26 @@ bool is_delay_lane_change( return dist_obj_to_end <= dist_buffer; }; - const auto lc_length = lane_change_path.info.length.lane_changing; - auto is_sufficient_gap = [&lc_length]( + const auto ego_vel = common_data_ptr->get_ego_speed(); + const auto min_lon_acc = common_data_ptr->lc_param_ptr->min_longitudinal_acc; + const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); + auto is_sufficient_gap = [&gap_threshold]( const ExtendedPredictedObject & current_obj, const ExtendedPredictedObject & next_obj) { const auto curr_obj_half_length = current_obj.shape.dimensions.x; const auto next_obj_half_length = next_obj.shape.dimensions.x; const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length; - return gap_length > lc_length; + return gap_length > gap_threshold; }; const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - auto it = target_lane_objects.begin(); - for (; it < target_lane_objects.end(); ++it) { + auto it = target_objects.begin(); + for (; it < target_objects.end(); ++it) { if (is_near_end(*it)) break; - if (!is_static(*it) || it->dist_from_ego < lc_length) continue; + if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue; if ( delay_lc_param.check_only_parked_vehicle && @@ -793,7 +789,7 @@ bool is_delay_lane_change( } auto next_it = std::next(it); - if (next_it == target_lane_objects.end() || is_sufficient_gap(*it, *next_it)) { + if (next_it == target_objects.end() || is_sufficient_gap(*it, *next_it)) { auto debug = utils::path_safety_checker::createObjectDebug(*it); debug.second.unsafe_reason = "delay lane change"; utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);