From ced46edcee3a7889a895097fe8354dfdda389ee3 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 12 Apr 2024 16:39:09 +0900 Subject: [PATCH] fix(avoidance): add target filtering threshold for merging/deviating vehicle Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 4 ++ .../data_structs.hpp | 3 ++ .../schema/avoidance.schema.json | 13 ++++++ .../src/debug.cpp | 1 + .../src/utils.cpp | 40 +++++++++++++++++++ 5 files changed, 61 insertions(+) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 5d6044df64b2f..3339653702eb2 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -132,6 +132,10 @@ th_shiftable_ratio: 0.8 # [-] min_road_shoulder_width: 0.5 # [m] FOR DEVELOPER + # for merging/deviating vehicle + merging_vehicle: + th_overhang_distance: 0.5 # [m] + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: enable: true # [-] 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 e74b546f31fc4..f52912787c9e1 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 @@ -189,6 +189,9 @@ struct AvoidanceParameters double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; + // for merging/deviating vehicle + double th_overhang_distance{0.0}; + // parameters for safety check area bool enable_safety_check{false}; bool check_current_lane{false}; diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json index 79882beb805f8..92f6830eef0ad 100644 --- a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -681,6 +681,18 @@ ], "additionalProperties": false }, + "merging_vehicle": { + "type": "object", + "properties": { + "th_overhang_distance": { + "type": "number", + "description": "Distance threshold between overhang point and ego lane's centerline. If the nearest overhang point of merging/deviating vehicle is less than this param, the module never avoid it. (Basically, the ego stopps behind of it.)", + "default": 0.5 + } + }, + "required": ["th_overhang_distance"], + "additionalProperties": false + }, "parked_vehicle": { "type": "object", "properties": { @@ -803,6 +815,7 @@ "object_check_return_pose_distance", "max_compensation_time", "detection_area", + "merging_vehicle", "parked_vehicle", "avoidance_for_ambiguous_vehicle", "intersection" diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 7d029277fcbc4..4595a77a84b3b 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -578,6 +578,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("TooNearToGoal")); addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("DeviatingFromEgoLane")); addObjects(data.other_objects, std::string("UnstableObject")); addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle")); addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)")); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index bef3976be7e1e..856ae6db1f8f6 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -574,6 +574,46 @@ bool isNeverAvoidanceTarget( } } + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + if ( + isOnRight(object) && + object.overhang_points.front().first > parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(__func__), + "merging vehicle. but overhang distance is larger than threshold."); + return true; + } + if ( + !isOnRight(object) && + object.overhang_points.front().first < -1.0 * parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(__func__), + "merging vehicle. but overhang distance is larger than threshold."); + return true; + } + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "DeviatingFromEgoLane"; + if ( + isOnRight(object) && + object.overhang_points.front().first > parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(__func__), + "deviating vehicle. but overhang distance is larger than threshold."); + return true; + } + if ( + !isOnRight(object) && + object.overhang_points.front().first < -1.0 * parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(__func__), + "deviating vehicle. but overhang distance is larger than threshold."); + return true; + } + } + if (object.is_on_ego_lane) { const auto right_lane = planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false);