From cb64a6a8daf6286e46bd89c129057510970142d3 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Mon, 4 Dec 2023 18:50:02 +0900 Subject: [PATCH] refactor(avoidance_by_lane_change): rework execution condition Signed-off-by: Zulfaqar Azmi --- .../lane_change/avoidance_by_lane_change.cpp | 43 +++++++++++-------- 1 file changed, 24 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index befd458153e6e..e9243173120f3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -44,28 +44,37 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const return false; } + const auto & target_objects = avoidance_data_.target_objects; + if(target_objects.empty()){ + return false; + } + const auto & object_parameters = avoidance_parameters_->object_parameters; - const auto is_more_than_threshold = - std::any_of(object_parameters.begin(), object_parameters.end(), [&](const auto & p) { - const auto & objects = avoidance_data_.target_objects; - - const size_t num = std::count_if(objects.begin(), objects.end(), [&p](const auto & object) { - const auto target_class = - utils::getHighestProbLabel(object.object.classification) == p.first; - return target_class && object.avoid_required; - }); - return num >= p.second.execute_num; - }); + const auto is_more_than_threshold = [&](const auto & p) { + + const auto is_type_to_avoid = [&p](const auto & object) { + const auto target_class = utils::getHighestProbLabel(object.object.classification) == p.first; + return target_class && object.avoid_required; + }; - if (!is_more_than_threshold) { + const auto num_of_objects = std::count_if(target_objects.begin(), target_objects.end(), is_type_to_avoid); + + return num_of_objects >= 2; + }; + + if (!std::any_of(object_parameters.begin(), object_parameters.end(), is_more_than_threshold)) { return false; } const auto & front_object = data.target_objects.front(); + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(status_.current_lanes.back()); + const auto common_param = getCommonParam(); + const auto minimum_lane_change_length = utils::calcMinimumLaneChangeLength( + common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); - const auto THRESHOLD = avoidance_parameters_->execute_object_longitudinal_margin; - if (front_object.longitudinal < THRESHOLD) { + if (front_object.longitudinal < minimum_lane_change_length) { return false; } @@ -77,11 +86,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const avoidance_parameters_->execute_only_when_lane_change_finish_before_object; const auto not_enough_distance = front_object.longitudinal < to_lc_end; - if (execute_only_when_lane_change_finish_before_object && not_enough_distance) { - return false; - } - - return true; + return !(execute_only_when_lane_change_finish_before_object && not_enough_distance); } bool AvoidanceByLaneChange::specialExpiredCheck() const