feat(run_out): avoid chattering of state transition (#1975)

* feat: keep approach state to avoid chattering of detection

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

* add parameter

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

* update parameter

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

* update documents

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

* revert changed parameter

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

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

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml

@@ -33,9 +33,10 @@
 
       # parameters for the change of state. used only when approaching is enabled
       state:
-        stop_thresh: 0.01          # [m/s] threshold to decide if ego is stopping
-        stop_time_thresh: 3.0      # [sec] threshold for stopping time to transit to approaching state
-        disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
+        stop_thresh: 0.01           # [m/s] threshold to decide if ego is stopping
+        stop_time_thresh: 3.0       # [sec] threshold for stopping time to transit to approaching state
+        disable_approach_dist: 3.0  # [m] end the approaching state if distance to the obstacle is longer than this value
+        keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
 
       # parameter to avoid sudden stopping
       slow_down_limit:

planning/behavior_velocity_planner/config/run_out.param.yaml

@@ -33,9 +33,10 @@
 
       # parameters for the change of state. used only when approaching is enabled
       state:
-        stop_thresh: 0.01          # [m/s] threshold to decide if ego is stopping
-        stop_time_thresh: 3.0      # [sec] threshold for stopping time to transit to approaching state
-        disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
+        stop_thresh: 0.01           # [m/s] threshold to decide if ego is stopping
+        stop_time_thresh: 3.0       # [sec] threshold for stopping time to transit to approaching state
+        disable_approach_dist: 3.0  # [m] end the approaching state if distance to the obstacle is longer than this value
+        keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
 
       # parameter to avoid sudden stopping
       slow_down_limit:

planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp

@@ -44,13 +44,17 @@ class StateMachine
 
   explicit StateMachine(const StateParam & state_param) { state_param_ = state_param; }
   State getCurrentState() const { return state_; }
+  boost::optional<DynamicObstacle> getTargetObstacle() const { return target_obstacle_; }
   std::string toString(const State & state) const;
   void updateState(const StateInput & state_input, rclcpp::Clock & clock);
 
 private:
   StateParam state_param_;
   State state_{State::GO};
   rclcpp::Time stop_time_;
+  rclcpp::Time prev_approach_time_;
+  boost::optional<DynamicObstacle> prev_obstacle_{};
+  boost::optional<DynamicObstacle> target_obstacle_{};
 };
 }  // namespace run_out_utils
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp

@@ -82,6 +82,7 @@ struct StateParam
   float stop_thresh;
   float stop_time_thresh;
   float disable_approach_dist;
+  float keep_approach_duration;
 };
 
 struct SlowDownLimit

planning/behavior_velocity_planner/run-out-design.md

@@ -147,7 +147,21 @@ The decision to approach the obstacle is determined by a simple state transition
 
 ![brief](./docs/run_out/insert_velocity_to_approach.svg)
 
-![brief](./docs/run_out/state_transition.svg)
+```plantuml
+@startuml
+hide empty description
+left to right direction
+title State transition for approaching the obstacle
+
+[*] --> GO
+GO --> STOP : Current velocity is less than threshold
+STOP --> GO : Current velocity is larger than threshold
+
+STOP --> APPROACH : Stop duration is larger than threshold
+APPROACH --> GO : There are no obstacles or \n distance to the obstacle is larger than threshold
+APPROACH --> APPROACH : Approach duration is less than threshold
+@enduml
+```
 
 ##### Limit velocity with specified jerk and acc limit
 
@@ -190,11 +204,12 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
 | `margin`               | double | [m] distance on how close ego approaches the obstacle |
 | `limit_vel_kmph`       | double | [km/h] limit velocity for approaching after stopping  |
 
-| Parameter /state        | Type   | Description                                                                         |
-| ----------------------- | ------ | ----------------------------------------------------------------------------------- |
-| `stop_thresh`           | double | [m/s] threshold to decide if ego is stopping                                        |
-| `stop_time_thresh`      | double | [sec] threshold for stopping time to transit to approaching state                   |
-| `disable_approach_dist` | double | [m] end the approaching state if distance to the obstacle is longer than this value |
+| Parameter /state         | Type   | Description                                                                         |
+| ------------------------ | ------ | ----------------------------------------------------------------------------------- |
+| `stop_thresh`            | double | [m/s] threshold to decide if ego is stopping                                        |
+| `stop_time_thresh`       | double | [sec] threshold for stopping time to transit to approaching state                   |
+| `disable_approach_dist`  | double | [m] end the approaching state if distance to the obstacle is longer than this value |
+| `keep_approach_duration` | double | [sec] keep approach state for this duration to avoid chattering of state transition |
 
 | Parameter /slow_down_limit | Type   | Description                                                   |
 | -------------------------- | ------ | ------------------------------------------------------------- |

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

@@ -94,6 +94,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     p.stop_thresh = node.declare_parameter(ns_s + ".stop_thresh", 0.01);
     p.stop_time_thresh = node.declare_parameter(ns_s + ".stop_time_thresh", 3.0);
     p.disable_approach_dist = node.declare_parameter(ns_s + ".disable_approach_dist", 4.0);
+    p.keep_approach_duration = node.declare_parameter(ns_s + ".keep_approach_duration", 1.0);
   }
 
   {

planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp

@@ -674,27 +674,28 @@ void RunOutModule::insertVelocityForState(
 
   // get updated state and target obstacle to decelerate
   const auto state = state_machine_->getCurrentState();
+  const auto target_obstacle = state_machine_->getTargetObstacle();
 
   // no obstacles to decelerate
-  if (!dynamic_obstacle) {
+  if (!target_obstacle) {
     return;
   }
 
   // insert velocity for each state
   switch (state) {
     case State::GO: {
-      insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path);
+      insertStoppingVelocity(target_obstacle, current_pose, current_vel, current_acc, output_path);
       break;
     }
 
     case State::STOP: {
-      insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path);
+      insertStoppingVelocity(target_obstacle, current_pose, current_vel, current_acc, output_path);
       break;
     }
 
     case State::APPROACH: {
       insertApproachingVelocity(
-        *dynamic_obstacle, current_pose, planner_param.approaching.limit_vel_kmph / 3.6,
+        *target_obstacle, current_pose, planner_param.approaching.limit_vel_kmph / 3.6,
         planner_param.approaching.margin, output_path);
       debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP);
       break;