feat(behavior_velocity): improve occlusion spot detection area polygon

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -10,6 +10,8 @@
       motion:
         safety_ratio: 0.8                   # [-] jerk/acceleration ratio for safety
         max_slow_down_acceleration: -1.5    # [m/s^2] minimum deceleration for safe deceleration.
+        non_effective_jerk: -0.3            # [m/s^3] too weak jerk for velocity planning.
+        non_effective_acceleration: -1.0    # [m/s^2] too weak deceleration for velocity planning.
         min_allowed_velocity: 1.0           # [m/s] minimum velocity allowed
         delay_time: 0.1                     # [s] safety time buffer for delay system response
         safe_margin: 1.0                    # [m] maximum safety distance for any error

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -78,6 +78,8 @@ struct Velocity
   double max_stop_jerk;         // [m/s^3] emergency braking system jerk
   double max_stop_accel;        // [m/s^2] emergency braking system deceleration
   double max_slow_down_accel;   // [m/s^2] maximum allowed deceleration
+  double non_effective_jerk;    // [m/s^3] too weak jerk for velocity planning.
+  double non_effective_accel;   // [m/s^2] too weak deceleration for velocity planning.
   double min_allowed_velocity;  // [m/s]   minimum allowed velocity not to stop
   double a_ego;                 // [m/s^2] current ego acceleration
   double v_ego;                 // [m/s]   current ego velocity
@@ -95,10 +97,11 @@ struct PlannerParam
 {
   bool debug;  // [-]
   // parameters in yaml
-  double detection_area_length;  // [m]
-  double stuck_vehicle_vel;      // [m/s]
-  double lateral_distance_thr;   // [m] lateral distance threshold to consider
-  double pedestrian_vel;         // [m/s]
+  double detection_area_length;      // [m]
+  double detection_area_max_length;  // [m]
+  double stuck_vehicle_vel;          // [m/s]
+  double lateral_distance_thr;       // [m] lateral distance threshold to consider
+  double pedestrian_vel;             // [m/s]
 
   double dist_thr;   // [m]
   double angle_thr;  // [rad]

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp

@@ -82,7 +82,8 @@ void buildSlices(
     for (int i = 0; i <= num_step; i++) {
       idx = s + i;
       const double arc_length_from_ego = std::max(0.0, static_cast<double>(idx - min_length));
-      if (idx >= max_index) continue;
+      if (arc_length_from_ego > param.detection_area_max_length + min_length) break;
+      if (idx >= max_index) break;
       const auto & c0 = center_line.at(idx);
       const auto & c1 = center_line.at(idx + 1);
       /**

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp

@@ -94,6 +94,9 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
   pp.v.delay_time = node.declare_parameter(ns + ".motion.delay_time", 0.1);
   pp.v.safe_margin = node.declare_parameter(ns + ".motion.safe_margin", 1.0);
   pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel", -1.5);
+  pp.v.non_effective_jerk = node.declare_parameter(ns + ".motion.non_effective_jerk", -0.3);
+  pp.v.non_effective_accel =
+    node.declare_parameter(ns + ".motion.non_effective_acceleration", -1.0);
   pp.v.min_allowed_velocity = node.declare_parameter(ns + ".motion.min_allowed_velocity", 1.0);
   // detection_area param
   pp.detection_area.min_occlusion_spot_size =

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp

@@ -56,6 +56,9 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity(
     param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold;
     param_.v.v_ego = planner_data_->current_velocity->twist.linear.x;
     param_.v.a_ego = planner_data_->current_accel.get();
+    param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit(
+      param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_jerk, param_.v.non_effective_accel,
+      0.0);
   }
   const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose;
   const auto & lanelet_map_ptr = planner_data_->lanelet_map;

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp

@@ -56,6 +56,9 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
     param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold;
     param_.v.v_ego = planner_data_->current_velocity->twist.linear.x;
     param_.v.a_ego = planner_data_->current_accel.get();
+    param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit(
+      param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_jerk, param_.v.non_effective_accel,
+      0.0);
   }
   const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose;
   const auto & lanelet_map_ptr = planner_data_->lanelet_map;

planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp

@@ -35,28 +35,33 @@ TEST(safeMotion, delay_jerk_acceleration)
    * case2 delay + jerk
    * case3 delay + jerk + acc
    */
-  utils::Velocity v{1.0, -3.0, -4.5, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0};
+  utils::Velocity v{};
+  v.safety_ratio = 1.0;
+  v.max_stop_jerk = -3.0;
+  v.max_stop_accel = -4.5;
+  v.delay_time = 0.5;
   double ttc = 0.0;
+  const double eps = 1e-3;
   // case 1 delay
   {
     ttc = 0.5;
     utils::SafeMotion sm = utils::calculateSafeMotion(v, ttc);
-    EXPECT_DOUBLE_EQ(sm.safe_velocity, 0.0);
-    EXPECT_DOUBLE_EQ(sm.stop_dist, 0.0);
+    EXPECT_NEAR(sm.safe_velocity, 0.0, eps);
+    EXPECT_NEAR(sm.stop_dist, 0.0, eps);
   }
   // case 2 delay + jerk
   {
     ttc = 1.5;
     utils::SafeMotion sm = utils::calculateSafeMotion(v, ttc);
-    EXPECT_DOUBLE_EQ(sm.safe_velocity, 1.5);
-    EXPECT_DOUBLE_EQ(sm.stop_dist, 1.25);
+    EXPECT_NEAR(sm.safe_velocity, 1.5, eps);
+    EXPECT_NEAR(sm.stop_dist, 1.25, eps);
   }
   // case 3 delay + jerk + acc
   {
     ttc = 3.25;
     utils::SafeMotion sm = utils::calculateSafeMotion(v, ttc);
-    EXPECT_DOUBLE_EQ(sm.safe_velocity, 9);
-    EXPECT_DOUBLE_EQ(std::round(sm.stop_dist * 100.0) / 100.0, 13.92);
+    EXPECT_NEAR(sm.safe_velocity, 9, eps);
+    EXPECT_NEAR(std::round(sm.stop_dist * 100.0) / 100.0, 13.92, eps);
   }
 }