From a11c9db4cb1c39d63f4ed17b682d68fed65f9dd8 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 12 Oct 2023 16:56:58 +0900 Subject: [PATCH] feat(avoidance): make detection area dynamically Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 9 +++++++-- .../utils/avoidance/avoidance_module_data.hpp | 6 +++--- .../utils/avoidance/helper.hpp | 16 ++++++++++++++++ .../utils/avoidance/utils.hpp | 2 +- .../scene_module/avoidance/avoidance_module.cpp | 7 ++++--- .../src/scene_module/avoidance/manager.cpp | 15 +++++++++++---- .../src/scene_module/lane_change/manager.cpp | 15 +++++++++++---- .../src/utils/avoidance/utils.cpp | 11 +++-------- 8 files changed, 56 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 2b38536341d7c..25e42d20d144a 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -118,8 +118,6 @@ object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] object_check_goal_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] @@ -128,6 +126,13 @@ # lost object compensation object_last_seen_threshold: 2.0 + # detection area generation parameters + detection_area: + static: true # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + # For safety check safety_check: # safety check configuration diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 5ba0085ded5b7..d04b35a3b185b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -145,9 +145,9 @@ struct AvoidanceParameters double object_ignore_section_crosswalk_behind_distance{0.0}; // distance to avoid object detection - double object_check_forward_distance{0.0}; - - // continue to detect backward vehicles as avoidance targets until they are this distance away + bool use_static_detection_area{true}; + double object_check_min_forward_distance{0.0}; + double object_check_max_forward_distance{0.0}; double object_check_backward_distance{0.0}; // if the distance between object and goal position is less than this parameter, the module ignore diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 77fe2e29c4978..11388dd6bb74a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -173,6 +173,22 @@ class AvoidanceHelper : std::max(shift_length, getRightShiftBound()); } + double getForwardDetectionRange() const + { + if (parameters_->use_static_detection_area) { + return parameters_->object_check_max_forward_distance; + } + + const auto max_shift_length = std::max( + std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); + const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk( + max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); + + return std::clamp( + 1.5 * dynamic_distance, parameters_->object_check_min_forward_distance, + parameters_->object_check_max_forward_distance); + } + void alignShiftLinesOrder(AvoidLineArray & lines, const bool align_shift_length = true) const { if (lines.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index d012fd5f612fb..5e55e14d9825c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -164,7 +164,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const bool is_running, DebugData & debug); + const double object_check_forward_distance, const bool is_running, DebugData & debug); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index c94406e466219..b4b0a871eec62 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -322,11 +322,12 @@ void AvoidanceModule::fillAvoidanceTargetObjects( getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. + const auto sparse_resample_path = utils::resamplePathWithSpline( + helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); const auto [object_within_target_lane, object_outside_target_lane] = utils::avoidance::separateObjectsByPath( - utils::resamplePathWithSpline( - helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output), - planner_data_, data, parameters_, is_running, debug); + sparse_resample_path, planner_data_, data, parameters_, helper_.getForwardDetectionRange(), + is_running, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 500ebe873a2d8..763cf17890721 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -114,10 +114,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - getOrDeclareParameter(*node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "object_check_backward_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -130,6 +126,17 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + // safety check general params { std::string ns = "avoidance.safety_check."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 3d7cf07308a79..c6995ada1bfa6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -329,10 +329,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - getOrDeclareParameter(*node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "object_check_backward_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -345,6 +341,17 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + // safety check { std::string ns = "avoidance.safety_check."; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index ed2e42bf0c5ff..234b7c58daa53 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -977,11 +977,6 @@ void filterTargetObjects( data.other_objects.push_back(o); continue; } - if (o.longitudinal > parameters->object_check_forward_distance) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; - data.other_objects.push_back(o); - continue; - } // Target object is behind the path goal -> ignore. if (o.longitudinal > dist_to_goal) { @@ -1465,7 +1460,7 @@ lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & parameters, const bool is_right_shift) { const auto & rh = planner_data->route_handler; - const auto & forward_distance = parameters->object_check_forward_distance; + const auto & forward_distance = parameters->object_check_max_forward_distance; const auto & backward_distance = parameters->safety_check_backward_distance; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; @@ -1605,7 +1600,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const bool is_running, DebugData & debug) + const double object_check_forward_distance, const bool is_running, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1628,7 +1623,7 @@ std::pair separateObjectsByPath( const auto & p_ego_back = path.points.at(i + 1).point.pose; const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); - if (distance_from_ego > parameters->object_check_forward_distance) { + if (distance_from_ego > object_check_forward_distance) { break; }