From ac450a973db8e9accbc1da66976650394c05d775 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 14 Mar 2024 13:55:19 +0900 Subject: [PATCH 1/7] feat(avoidance): wait and see the ambiguous stopped vehicle Signed-off-by: satoshi-ota --- .../src/shift_line_generator.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 5a8fd35f6d618..b65457ee7ab7d 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -276,6 +276,22 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); + + if (o.is_ambiguous) { + const auto prepare_distance = helper_->getMinimumPrepareDistance(); + const auto constant_distance = helper_->getFrontConstantDistance(o); + const auto avoidance_distance = helper_->getMinAvoidanceDistance(desire_shift_length); + const auto decelaration_distance = + std::max(10.0, helper_->getFeasibleDecelDistance(0.0, false)); + if ( + o.longitudinal > + prepare_distance + constant_distance + avoidance_distance + decelaration_distance) { + o.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + o.is_avoidable = feasible_shift_profile.has_value(); + continue; + } + } + if (!feasible_shift_profile.has_value()) { if (o.avoid_required && is_forward_object(o)) { break; From 4cc3008fc62e92b46095221a5442e9c3059d0d9f Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 14 Mar 2024 13:58:05 +0900 Subject: [PATCH 2/7] fix(avoidance): wait and see objects around ego straight lane Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/utils.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index a7e41825bfe96..9cb67f377a7b9 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -647,6 +647,11 @@ bool isAmbiguousStoppedVehicle( } if (object.is_within_intersection) { + std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "straight") { + return true; + } + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); return false; } @@ -819,11 +824,6 @@ bool isSatisfiedWithVehicleCondition( } if (object.is_within_intersection) { - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { - return true; - } - if (object.behavior == ObjectData::Behavior::NONE) { object.reason = "ParallelToEgoLane"; return false; From ddd8066b70e411071eef759e4b86ea65d9b7acbb Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 14 Mar 2024 19:14:44 +0900 Subject: [PATCH 3/7] tmp Signed-off-by: satoshi-ota --- .../src/shift_line_generator.cpp | 2 +- planning/behavior_path_avoidance_module/src/utils.cpp | 5 ----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index b65457ee7ab7d..955a99f296750 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -277,7 +277,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - if (o.is_ambiguous) { + if (o.is_ambiguous && o.behavior == ObjectData::Behavior::MERGING) { const auto prepare_distance = helper_->getMinimumPrepareDistance(); const auto constant_distance = helper_->getFrontConstantDistance(o); const auto avoidance_distance = helper_->getMinAvoidanceDistance(desire_shift_length); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 9cb67f377a7b9..09cf6301c962b 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -647,11 +647,6 @@ bool isAmbiguousStoppedVehicle( } if (object.is_within_intersection) { - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { - return true; - } - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); return false; } From fb4b21ced19023dab3a746361892274281ab61c0 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 15 Mar 2024 10:45:15 +0900 Subject: [PATCH 4/7] refactor(avoidance): filtering logic for vehicle type objects Signed-off-by: satoshi-ota --- .../src/debug.cpp | 2 + .../src/shift_line_generator.cpp | 2 +- .../src/utils.cpp | 196 ++++++++++++------ 3 files changed, 134 insertions(+), 66 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 3c80a62a4fe2c..7d029277fcbc4 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -579,6 +579,8 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); addObjects(data.other_objects, std::string("UnstableObject")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 955a99f296750..b65457ee7ab7d 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -277,7 +277,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - if (o.is_ambiguous && o.behavior == ObjectData::Behavior::MERGING) { + if (o.is_ambiguous) { const auto prepare_distance = helper_->getMinimumPrepareDistance(); const auto constant_distance = helper_->getFrontConstantDistance(o); const auto avoidance_distance = helper_->getMinAvoidanceDistance(desire_shift_length); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 09cf6301c962b..3a93e63854365 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -525,7 +525,8 @@ bool isMergingToEgoLane(const ObjectData & object) } bool isParkedVehicle( - ObjectData & object, const std::shared_ptr & route_handler, + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & route_handler, const std::shared_ptr & parameters) { using lanelet::geometry::distance2d; @@ -622,57 +623,36 @@ bool isParkedVehicle( object.shiftable_ratio > parameters->object_check_shiftable_ratio; } - return is_left_side_parked_vehicle || is_right_side_parked_vehicle; -} - -bool isAmbiguousStoppedVehicle( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) -{ - const auto stop_time_longer_than_threshold = - object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; - - if (!stop_time_longer_than_threshold) { + if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { return false; } const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto is_moving_distance_longer_than_threshold = - tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > - parameters->force_avoidance_distance_threshold; - - if (is_moving_distance_longer_than_threshold) { - return false; - } - - if (object.is_within_intersection) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); - return false; - } - - const auto rh = planner_data->route_handler; - - if ( - !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) && - !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane."); + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { return false; } - if (!object.is_on_ego_lane) { - return true; - } + return true; +} +bool isCloseToStopFactor( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle - bool not_parked_object = true; + bool is_close_to_stop_factor = false; // check traffic light const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); { - not_parked_object = + is_close_to_stop_factor = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; } @@ -684,12 +664,89 @@ bool isAmbiguousStoppedVehicle( const auto stop_for_crosswalk = to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; - not_parked_object = not_parked_object || stop_for_crosswalk; + is_close_to_stop_factor = is_close_to_stop_factor || stop_for_crosswalk; } object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); - return !not_parked_object; + return is_close_to_stop_factor; +} + +bool isNeverAvoidanceTarget( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > + parameters->force_avoidance_distance_threshold; + if (is_moving_distance_longer_than_threshold) { + object.reason = AvoidanceDebugFactor::MOVING_OBJECT; + return true; + } + + if (object.is_within_intersection) { + if (object.behavior == ObjectData::Behavior::NONE) { + object.reason = "ParallelToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + } + + if (object.is_on_ego_lane) { + if ( + !!planner_data->route_handler->getRoutingGraphPtr()->right(object.overhang_lanelet) && + !!planner_data->route_handler->getRoutingGraphPtr()->left(object.overhang_lanelet)) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); + return true; + } + } + + if (isCloseToStopFactor(object, data, planner_data, parameters)) { + if (object.is_on_ego_lane && !object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it."); + return true; + } + } + + return false; +} + +bool isObviousAvoidanceTarget( + ObjectData & object, [[maybe_unused]] const AvoidancePlanningData & data, + [[maybe_unused]] const std::shared_ptr & planner_data, + [[maybe_unused]] const std::shared_ptr & parameters) +{ + if (!object.is_within_intersection) { + if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle."); + return true; + } + + if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle."); + return true; + } + } + + if (!object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + } + + return false; } bool isSatisfiedWithCommonCondition( @@ -792,45 +849,53 @@ bool isSatisfiedWithVehicleCondition( { object.behavior = getObjectBehavior(object, parameters); object.is_on_ego_lane = isOnEgoLane(object); - object.is_ambiguous = isAmbiguousStoppedVehicle(object, data, planner_data, parameters); - // from here condition check for vehicle type objects. - if (object.is_ambiguous && parameters->enable_force_avoidance_for_stopped_vehicle) { + if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) { + return false; + } + + if (isObviousAvoidanceTarget(object, data, planner_data, parameters)) { return true; } - // check vehicle shift ratio - if (object.is_on_ego_lane) { - if (object.is_parked) { - return true; - } else { - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - return false; - } + // from here, filtering for ambiguous vehicle. + + if (!parameters->enable_force_avoidance_for_stopped_vehicle) { + object.reason = "AmbiguousStoppedVehicle"; + return false; } - // Object is on center line -> ignore. - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; - if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + const auto stop_time_longer_than_threshold = + object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; + if (!stop_time_longer_than_threshold) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; return false; } if (object.is_within_intersection) { - if (object.behavior == ObjectData::Behavior::NONE) { - object.reason = "ParallelToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.is_ambiguous = true; + return true; + } + } else { + if (object.behavior == ObjectData::Behavior::MERGING) { + object.is_ambiguous = true; + return true; } - } - if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.is_ambiguous = true; + return true; + } + + if (object.behavior == ObjectData::Behavior::NONE) { + object.is_ambiguous = false; + return true; + } } - return true; + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + return false; } bool isNoNeedAvoidanceBehavior( @@ -1757,7 +1822,8 @@ void filterTargetObjects( if (filtering_utils::isVehicleTypeObject(o)) { o.is_within_intersection = filtering_utils::isWithinIntersection(o, planner_data->route_handler); - o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters); + o.is_parked = + filtering_utils::isParkedVehicle(o, data, planner_data->route_handler, parameters); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { From 4700aa146ec0456718684e5bf003b16af18503dc Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 15 Mar 2024 14:32:27 +0900 Subject: [PATCH 5/7] fix(avoidance): wait with unsafe avoidance path Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/helper.hpp | 28 +++++++++++++++++++ .../src/scene.cpp | 3 +- .../src/shift_line_generator.cpp | 16 ----------- .../src/utils.cpp | 3 ++ 4 files changed, 33 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 5ade0dd0062b0..c1c1f98e73cb4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -289,6 +289,34 @@ class AvoidanceHelper }); } + bool isReady(const ObjectDataArray & objects) const + { + if (objects.empty()) { + return true; + } + + const auto object = objects.front(); + + if (!object.is_ambiguous) { + return true; + } + + if (!object.avoid_margin.has_value()) { + return true; + } + + const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto desire_shift_length = + getShiftLength(object, is_object_on_right, object.avoid_margin.value()); + + constexpr double BUFFER = 10.0; + const auto prepare_distance = getMinimumPrepareDistance(); + const auto constant_distance = getFrontConstantDistance(object); + const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); + + return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + BUFFER; + } + bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const { if (std::abs(current_shift_length) < 1e-3) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 67eeab9e4ea66..8e3f1ff7bb7a9 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -522,7 +522,8 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ data.comfortable = helper_->isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); - data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()); + data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()) && + helper_->isReady(data.target_objects); } void AvoidanceModule::fillEgoStatus( diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index b65457ee7ab7d..5a8fd35f6d618 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -276,22 +276,6 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - - if (o.is_ambiguous) { - const auto prepare_distance = helper_->getMinimumPrepareDistance(); - const auto constant_distance = helper_->getFrontConstantDistance(o); - const auto avoidance_distance = helper_->getMinAvoidanceDistance(desire_shift_length); - const auto decelaration_distance = - std::max(10.0, helper_->getFeasibleDecelDistance(0.0, false)); - if ( - o.longitudinal > - prepare_distance + constant_distance + avoidance_distance + decelaration_distance) { - o.reason = "AmbiguousStoppedVehicle(wait-and-see)"; - o.is_avoidable = feasible_shift_profile.has_value(); - continue; - } - } - if (!feasible_shift_profile.has_value()) { if (o.avoid_required && is_forward_object(o)) { break; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 3a93e63854365..663f680718522 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -874,16 +874,19 @@ bool isSatisfiedWithVehicleCondition( if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; object.is_ambiguous = true; return true; } } else { if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; object.is_ambiguous = true; return true; } if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; object.is_ambiguous = true; return true; } From bae21fe88929ef5e2caa95a38d81eb3573e54267 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 15 Mar 2024 17:03:40 +0900 Subject: [PATCH 6/7] fix(avoidance): use getRightLanelet Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 663f680718522..ca8c6cfff477d 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -702,8 +702,8 @@ bool isNeverAvoidanceTarget( if (object.is_on_ego_lane) { if ( - !!planner_data->route_handler->getRoutingGraphPtr()->right(object.overhang_lanelet) && - !!planner_data->route_handler->getRoutingGraphPtr()->left(object.overhang_lanelet)) { + planner_data->route_handler->getRightLanelet(object.overhang_lanelet).has_value() && + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet).has_value()) { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); return true; From 378632af8cca49ce196da96c61898f7f320f358f Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sun, 17 Mar 2024 14:56:54 +0900 Subject: [PATCH 7/7] refactor(avoidance): parameterize Signed-off-by: satoshi-ota --- .../src/manager.cpp | 11 +++++++---- .../config/avoidance.param.yaml | 6 ++++-- .../behavior_path_avoidance_module/data_structs.hpp | 7 ++++--- .../behavior_path_avoidance_module/helper.hpp | 5 +++-- .../parameter_helper.hpp | 13 +++++++------ .../behavior_path_avoidance_module/src/utils.cpp | 6 +++--- 6 files changed, 28 insertions(+), 20 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 19e03181e1dcb..799364437955c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -133,10 +133,13 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 10e5c2a05d902..50b5aef92cf60 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -155,8 +155,10 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: enable: false # [-] - time_threshold: 1.0 # [s] - distance_threshold: 1.0 # [m] + closest_distance_to_wait_and_see: 10.0 # [m] + condition: + time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.0 # [m] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index a6ee85fbeae5c..ab79b49d67125 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -100,7 +100,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_force_avoidance_for_stopped_vehicle{false}; + bool enable_avoidance_for_ambiguous_vehicle{false}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -184,8 +184,9 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; - double force_avoidance_distance_threshold{0.0}; + double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + double time_threshold_for_ambiguous_vehicle{0.0}; + double distance_threshold_for_ambiguous_vehicle{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index c1c1f98e73cb4..6d2eb81b328f4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -309,12 +309,13 @@ class AvoidanceHelper const auto desire_shift_length = getShiftLength(object, is_object_on_right, object.avoid_margin.value()); - constexpr double BUFFER = 10.0; const auto prepare_distance = getMinimumPrepareDistance(); const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); - return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + BUFFER; + return object.longitudinal < + prepare_distance + constant_distance + avoidance_distance + + parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; } bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 82f1eddb03c01..d23f5c8729edd 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -141,12 +141,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.force_avoidance_distance_threshold = - getOrDeclareParameter(*node, ns + "distance_threshold"); + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index ca8c6cfff477d..f83a07bafef48 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -680,7 +680,7 @@ bool isNeverAvoidanceTarget( const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto is_moving_distance_longer_than_threshold = tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > - parameters->force_avoidance_distance_threshold; + parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return true; @@ -860,13 +860,13 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. - if (!parameters->enable_force_avoidance_for_stopped_vehicle) { + if (!parameters->enable_avoidance_for_ambiguous_vehicle) { object.reason = "AmbiguousStoppedVehicle"; return false; } const auto stop_time_longer_than_threshold = - object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; + object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; return false;