feat(behavior_velocity_run_out): ignore momentary detection caused by…

… false positive (autowarefoundation#5359)

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

Signed-off-by: Tomohito Ando <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Tomohito Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     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

@@ -132,7 +132,7 @@ bool RunOutModule::modifyPathVelocity(
 }
 
 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.");
@@ -176,13 +176,19 @@ boost::optional<DynamicObstacle> RunOutModule::detectCollision(
       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 {};
 }
 
@@ -807,4 +813,21 @@ void RunOutModule::publishDebugValue(
   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

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