From 1ca97cfa099c9c534acc1baa7090369690418a61 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 19 Mar 2024 08:13:12 +0900 Subject: [PATCH] feat(avoidance): wait and see ambiguous stopped vehicle (#6631) * feat(avoidance): wait and see the ambiguous stopped vehicle Signed-off-by: satoshi-ota * fix(avoidance): wait and see objects around ego straight lane Signed-off-by: satoshi-ota * tmp Signed-off-by: satoshi-ota * refactor(avoidance): filtering logic for vehicle type objects Signed-off-by: satoshi-ota * fix(avoidance): wait with unsafe avoidance path Signed-off-by: satoshi-ota * fix(avoidance): use getRightLanelet Signed-off-by: satoshi-ota * refactor(avoidance): parameterize Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/manager.cpp | 11 +- .../config/avoidance.param.yaml | 6 +- .../data_structs.hpp | 7 +- .../behavior_path_avoidance_module/helper.hpp | 29 +++ .../parameter_helper.hpp | 13 +- .../src/debug.cpp | 2 + .../src/scene.cpp | 3 +- .../src/utils.cpp | 198 ++++++++++++------ 8 files changed, 186 insertions(+), 83 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 5ade0dd0062b0..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 @@ -289,6 +289,35 @@ 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()); + + 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 + + parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + } + 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/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/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/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 5a41359fe6a65..0ecaf76e8c2d1 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/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index f24a514a9ac72..fdb2b9b71c782 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -399,7 +399,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; @@ -496,57 +497,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; } @@ -558,12 +538,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->distance_threshold_for_ambiguous_vehicle; + 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->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; + } + } + + 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( @@ -666,50 +723,56 @@ 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_avoidance_for_ambiguous_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->time_threshold_for_ambiguous_vehicle; + if (!stop_time_longer_than_threshold) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; return false; } if (object.is_within_intersection) { - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { + 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::NONE) { - object.reason = "ParallelToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; + return true; } - } - if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::NONE) { + object.is_ambiguous = false; + return true; + } } - return true; + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + return false; } bool isNoNeedAvoidanceBehavior( @@ -1636,7 +1699,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)) {