feat(avoidance): make detection area dynamically (#5303)

Signed-off-by: satoshi-ota <[email protected]>

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

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

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()) {

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -167,7 +167,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
   const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & 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<const PlannerData> & planner_data,

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;

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<double>(*node, ns + "object_ignore_section_crosswalk_behind_distance");
-    p.object_check_forward_distance =
-      getOrDeclareParameter<double>(*node, ns + "object_check_forward_distance");
-    p.object_check_backward_distance =
-      getOrDeclareParameter<double>(*node, ns + "object_check_backward_distance");
     p.object_check_goal_distance =
       getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
     p.threshold_distance_object_is_on_center =
@@ -130,6 +126,17 @@ AvoidanceModuleManager::AvoidanceModuleManager(
       getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
   }
 
+  {
+    std::string ns = "avoidance.target_filtering.detection_area.";
+    p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");
+    p.object_check_min_forward_distance =
+      getOrDeclareParameter<double>(*node, ns + "min_forward_distance");
+    p.object_check_max_forward_distance =
+      getOrDeclareParameter<double>(*node, ns + "max_forward_distance");
+    p.object_check_backward_distance =
+      getOrDeclareParameter<double>(*node, ns + "backward_distance");
+  }
+
   // safety check general params
   {
     std::string ns = "avoidance.safety_check.";

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<double>(*node, ns + "object_ignore_section_crosswalk_behind_distance");
-    p.object_check_forward_distance =
-      getOrDeclareParameter<double>(*node, ns + "object_check_forward_distance");
-    p.object_check_backward_distance =
-      getOrDeclareParameter<double>(*node, ns + "object_check_backward_distance");
     p.object_check_goal_distance =
       getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
     p.threshold_distance_object_is_on_center =
@@ -345,6 +341,17 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
       getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
   }
 
+  {
+    std::string ns = "avoidance.target_filtering.detection_area.";
+    p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");
+    p.object_check_min_forward_distance =
+      getOrDeclareParameter<double>(*node, ns + "min_forward_distance");
+    p.object_check_max_forward_distance =
+      getOrDeclareParameter<double>(*node, ns + "max_forward_distance");
+    p.object_check_backward_distance =
+      getOrDeclareParameter<double>(*node, ns + "backward_distance");
+  }
+
   // safety check
   {
     std::string ns = "avoidance.safety_check.";

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -991,7 +991,7 @@ void filterTargetObjects(
       data.other_objects.push_back(o);
       continue;
     }
-    if (o.longitudinal > parameters->object_check_forward_distance) {
+    if (o.longitudinal > parameters->object_check_max_forward_distance) {
       o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD;
       data.other_objects.push_back(o);
       continue;
@@ -1479,7 +1479,7 @@ lanelet::ConstLanelets getAdjacentLane(
   const std::shared_ptr<AvoidanceParameters> & 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;
 
@@ -1619,7 +1619,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
   const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & 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;
@@ -1642,7 +1642,7 @@ std::pair<PredictedObjects, PredictedObjects> 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;
     }