feat(behavior_velocity_run_out): ignore momentary detection caused by false positive

planning/behavior_velocity_run_out_module/README.md

@@ -217,6 +217,11 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
 | `max_jerk`                 | double | [m/s^3] minimum jerk deceleration for safe brake.             |
 | `max_acc`                  | double | [m/s^2] minimum accel deceleration for safe brake.            |
 
+| Parameter /ignore_momentary_detection | Type   | Description                                                       |
+| ------------------------------------- | ------ | ----------------------------------------------------------------- |
+| `enable`                              | bool   | [-] whether to ignore momentary detection                         |
+| `time_threshold`                      | double | [sec] ignores detections that persist for less than this duration |
+
 ### Future extensions / Unimplemented parts
 
 - Calculate obstacle's min velocity and max velocity from covariance

planning/behavior_velocity_run_out_module/config/run_out.param.yaml

@@ -48,3 +48,8 @@
         enable: true
         max_jerk: -0.7  # [m/s^3] minimum jerk deceleration for safe brake.
         max_acc : -2.0  # [m/s^2] minimum accel deceleration for safe brake.
+
+      # prevent abrupt stops caused by false positives in perception
+      ignore_momentary_detection:
+        enable: true
+        time_threshold: 0.15  # [sec] ignores detections that persist for less than this duration

planning/behavior_velocity_run_out_module/src/manager.cpp

@@ -119,6 +119,13 @@
     p.max_acc = getOrDeclareParameter<double>(node, ns_m + ".max_acc");
   }
 
+  {
+    auto & p = planner_param_.ignore_momentary_detection;
+    const std::string ns_param = ns + ".ignore_momentary_detection";
+    p.enable = getOrDeclareParameter<bool>(node, ns_param + ".enable");
+    p.time_threshold = getOrDeclareParameter<double>(node, ns_param + ".time_threshold");
+  }
+
   debug_ptr_ = std::make_shared<RunOutDebug>(node);
   setDynamicObstacleCreator(node, debug_ptr_);
 }

planning/behavior_velocity_run_out_module/src/scene.cpp

@@ -137,57 +137,63 @@
 }
 
 boost::optional<DynamicObstacle> RunOutModule::detectCollision(
-  const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path) const
+  const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path)
 {
   if (path.points.size() < 2) {
     RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points.");
     return {};
   }
 
   // detect collision with obstacles from the nearest path point to the end
   // ignore the travel time from current pose to nearest path point?
   float travel_time = 0.0;
   float dist_sum = 0.0;
   for (size_t idx = 1; idx < path.points.size(); idx++) {
     const auto & p1 = path.points.at(idx - 1).point;
     const auto & p2 = path.points.at(idx).point;
     const float prev_vel =
       std::max(p1.longitudinal_velocity_mps, planner_param_.run_out.min_vel_ego_kmph / 3.6f);
     const float ds = tier4_autoware_utils::calcDistance2d(p1, p2);
 
     // calculate travel time from nearest point to p2
     travel_time += ds / prev_vel;
     dist_sum += ds;
 
     // skip collision detection to reduce calculation time
     if (idx != 1 && dist_sum < planner_param_.run_out.detection_span) {
       continue;
     }
     dist_sum = 0.0;
 
     const auto vehicle_poly = createVehiclePolygon(p2.pose);
 
     debug_ptr_->pushPredictedVehiclePolygons(vehicle_poly);
     debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0);
 
     auto obstacles_collision =
       checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time);
     if (obstacles_collision.empty()) {
       continue;
     }
 
     const auto obstacle_selected = findNearestCollisionObstacle(path, p2.pose, obstacles_collision);
     if (!obstacle_selected) {
       continue;
     }
 
+    // ignore momentary obstacle detection to avoid sudden stopping by false positive
+    if (isMomentaryDetection()) {
+      return {};
+    }
+
     debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points);
     debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point);
 
     return obstacle_selected;
   }
 
   // no collision detected
+  first_detected_time_.reset();
   return {};
 }
 
@@ -812,4 +818,21 @@
   debug_ptr_->publishDebugValue();
 }
 
+bool RunOutModule::isMomentaryDetection()
+{
+  if (!planner_param_.ignore_momentary_detection.enable) {
+    return false;
+  }
+
+  if (!first_detected_time_) {
+    first_detected_time_ = std::make_shared<rclcpp::Time>(clock_->now());
+    return true;
+  }
+
+  const auto now = clock_->now();
+  const double elapsed_time_since_detection = (now - *first_detected_time_).seconds();
+  RCLCPP_DEBUG_STREAM(logger_, "elapsed_time_since_detection: " << elapsed_time_since_detection);
+
+  return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold;
+}
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_run_out_module/src/scene.hpp

@@ -62,13 +62,13 @@ class RunOutModule : public SceneModuleInterface
   std::unique_ptr<DynamicObstacleCreator> dynamic_obstacle_creator_;
   std::shared_ptr<RunOutDebug> debug_ptr_;
   std::unique_ptr<run_out_utils::StateMachine> state_machine_;
+  std::shared_ptr<rclcpp::Time> first_detected_time_;
 
   // Function
   Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const;
 
   boost::optional<DynamicObstacle> detectCollision(
-    const std::vector<DynamicObstacle> & dynamic_obstacles,
-    const PathWithLaneId & path_points) const;
+    const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path_points);
 
   float calcCollisionPositionOfVehicleSide(
     const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const;
@@ -141,6 +141,8 @@ class RunOutModule : public SceneModuleInterface
     const PathWithLaneId & path, const std::vector<DynamicObstacle> extracted_obstacles,
     const boost::optional<DynamicObstacle> & dynamic_obstacle,
     const geometry_msgs::msg::Pose & current_pose) const;
+
+  bool isMomentaryDetection();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_run_out_module/src/utils.hpp

@@ -126,6 +126,12 @@ struct DynamicObstacleParam
   float points_interval;      // [m]
 };
 
+struct IgnoreMomentaryDetection
+{
+  bool enable;
+  double time_threshold;
+};
+
 struct PlannerParam
 {
   CommonParam common;
@@ -138,6 +144,7 @@ struct PlannerParam
   DynamicObstacleParam dynamic_obstacle;
   SlowDownLimit slow_down_limit;
   Smoother smoother;
+  IgnoreMomentaryDetection ignore_momentary_detection;
 };
 
 enum class DetectionMethod {