refactor(run_out): reorganize the parameter (autowarefoundation#6064)

* chore(run_out): Reorganize the parameter 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>
kyoichi-sugahara · Jan 23, 2024 · eb6b2b3 · eb6b2b3
1 parent 6b43af1
commit eb6b2b3
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 45 deletions.
diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml
@@ -3,24 +3,16 @@
     run_out:
       detection_method: "Object"  # [-] candidate: Object, ObjectWithoutPath, Points
       use_partition_lanelet: true # [-] whether to use partition lanelet map data
-      suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet:
-      specify_decel_jerk: false   # [-] whether to specify jerk when ego decelerates
+      suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet
+      specify_decel_jerk: true    # [-] whether to specify jerk when ego decelerates
       stop_margin: 2.5            # [m] the vehicle decelerates to be able to stop with this margin
       passing_margin: 1.1         # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin
       deceleration_jerk: -0.3     # [m/s^3] ego decelerates with this jerk when stopping for obstacles
       detection_distance: 45.0    # [m] ahead distance from ego to detect the obstacles
       detection_span: 1.0         # [m] calculate collision with this span to reduce calculation time
       min_vel_ego_kmph: 3.6       # [km/h] min velocity to calculate time to collision
 
-      detection_area:
-        margin_behind: 0.5 # [m] ahead margin for detection area length
-        margin_ahead: 1.0  # [m] behind margin for detection area length
-
-      # This area uses points not filtered by vector map to ensure safety
-      mandatory_area:
-        decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area
-
-      # parameter to create abstracted dynamic obstacles
+      # Parameter to create abstracted dynamic obstacles
       dynamic_obstacle:
         use_mandatory_area: false # [-] whether to use mandatory detection area
         assume_fixed_velocity:
@@ -34,26 +26,42 @@
         time_step: 0.5            # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
         points_interval: 0.1      # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
 
-      # approach if ego has stopped in the front of the obstacle for a certain amount of time
-      approaching:
-        enable: false
-        margin: 0.0           # [m] distance on how close ego approaches the obstacle
-        limit_vel_kmph: 3.0   # [km/h] limit velocity for approaching after stopping
-
-      # 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
-        keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
-
-      # parameter to avoid sudden stopping
+      # Parameter to prevent sudden stopping.
+      # If the deceleration jerk and acceleration exceed this value upon inserting a stop point,
+      # the deceleration will be moderated to stay under this value.
       slow_down_limit:
-        enable: true
+        enable: false
         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
+      # Parameter to 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
+
+      # Typically used when the "detection_method" is set to ObjectWithoutPath or Points
+      # Approach if the ego has stopped in front of the obstacle for a certain period
+      # This avoids the issue of the ego continuously stopping in front of the obstacle
+      approaching:
+        enable: false
+        margin: 0.0           # [m] distance on how close ego approaches the obstacle
+        limit_vel_kmph: 3.0   # [km/h] limit velocity for approaching after stopping
+      # Parameters for state change 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
+          keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
+
+      # Only used when "detection_method" is set to Points
+      # Filters points by the detection area polygon to reduce computational cost
+      # The detection area is calculated based on the current velocity and acceleration and deceleration jerk constraints
+      detection_area:
+        margin_behind: 0.5 # [m] ahead margin for detection area length
+        margin_ahead: 1.0  # [m] behind margin for detection area length
+
+      # Only used when "detection_method" is set to Points
+      # Points in this area are detected even if it is in the no obstacle segmentation area
+      # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints
+      mandatory_area:
+        decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area
diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp
@@ -105,16 +105,14 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     p.enable = getOrDeclareParameter<bool>(node, ns_a + ".enable");
     p.margin = getOrDeclareParameter<double>(node, ns_a + ".margin");
     p.limit_vel_kmph = getOrDeclareParameter<double>(node, ns_a + ".limit_vel_kmph");
-  }
 
-  {
-    auto & p = planner_param_.state_param;
-    const std::string ns_s = ns + ".state";
-    p.stop_thresh = getOrDeclareParameter<double>(node, ns_s + ".stop_thresh");
-    p.stop_time_thresh = getOrDeclareParameter<double>(node, ns_s + ".stop_time_thresh");
-    p.disable_approach_dist = getOrDeclareParameter<double>(node, ns_s + ".disable_approach_dist");
-    p.keep_approach_duration =
-      getOrDeclareParameter<double>(node, ns_s + ".keep_approach_duration");
+    const std::string ns_as = ns_a + ".state";
+    p.state.stop_thresh = getOrDeclareParameter<double>(node, ns_as + ".stop_thresh");
+    p.state.stop_time_thresh = getOrDeclareParameter<double>(node, ns_as + ".stop_time_thresh");
+    p.state.disable_approach_dist =
+      getOrDeclareParameter<double>(node, ns_as + ".disable_approach_dist");
+    p.state.keep_approach_duration =
+      getOrDeclareParameter<double>(node, ns_as + ".keep_approach_duration");
   }
 
   {

diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp
@@ -46,7 +46,7 @@ RunOutModule::RunOutModule(
   planner_param_(planner_param),
   dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)),
   debug_ptr_(debug_ptr),
-  state_machine_(std::make_unique<run_out_utils::StateMachine>(planner_param.state_param))
+  state_machine_(std::make_unique<run_out_utils::StateMachine>(planner_param.approaching.state))
 {
   velocity_factor_.init(PlanningBehavior::UNKNOWN);
 

diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp
@@ -85,13 +85,6 @@ struct MandatoryArea
   float decel_jerk;
 };
 
-struct ApproachingParam
-{
-  bool enable;
-  float margin;
-  float limit_vel_kmph;
-};
-
 struct StateParam
 {
   float stop_thresh;
@@ -100,6 +93,14 @@ struct StateParam
   float keep_approach_duration;
 };
 
+struct ApproachingParam
+{
+  bool enable;
+  float margin;
+  float limit_vel_kmph;
+  StateParam state;
+};
+
 struct SlowDownLimit
 {
   bool enable;
@@ -143,7 +144,6 @@ struct PlannerParam
   DetectionArea detection_area;
   MandatoryArea mandatory_area;
   ApproachingParam approaching;
-  StateParam state_param;
   DynamicObstacleParam dynamic_obstacle;
   SlowDownLimit slow_down_limit;
   Smoother smoother;