feat(merge_from_private): make stop_distance_threshold tunable (autow…

…arefoundation#5086) * feat(merge_from_private): make stop_distance_threshold tunable Signed-off-by: Takayuki Murooka <[email protected]> * update Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Sep 25, 2023 · 80ba7ef · 80ba7ef
1 parent 457c04a
commit 80ba7ef
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 11 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -2,7 +2,7 @@
   ros__parameters:
     intersection:
       common:
-        attention_area_margin: 0.5 # [m]
+        attention_area_margin: 0.75 # [m]
         attention_area_length: 200.0 # [m]
         attention_area_angle_threshold: 0.785 # [rad]
         stop_line_margin: 3.0
@@ -14,8 +14,7 @@
         consider_wrong_direction_vehicle: false
       stuck_vehicle:
         use_stuck_stopline: true # stopline generated before the first conflicting area
-        stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
-        stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
+        stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
         stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
         # enable_front_car_decel_prediction: false # By default this feature is disabled
         # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
@@ -46,14 +45,15 @@
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        possible_object_bbox: [1.0, 2.0] # [m x m]
-        ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h
-        stop_release_margin_time: 1.0 # [s]
+        possible_object_bbox: [1.5, 2.5] # [m x m]
+        ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h
+        stop_release_margin_time: 1.5 # [s]
 
-      enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
-        intersection: true
+      enable_rtc:
+        intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
         intersection_to_occlusion: true
 
     merge_from_private:
       stop_line_margin: 3.0
       stop_duration_sec: 1.0
+      stop_distance_threshold: 1.0
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -277,6 +277,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node
   mp.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
   mp.path_interpolation_ds =
     node.get_parameter("intersection.common.path_interpolation_ds").as_double();
+  mp.stop_distance_threshold = getOrDeclareParameter<double>(node, ns + ".stop_distance_threshold");
 }
 
 void MergeFromPrivateModuleManager::launchNewModules(

diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
@@ -134,12 +134,11 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
     const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength(
       path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position);
 
-    constexpr double distance_threshold = 2.0;
     if (
-      signed_arc_dist_to_stop_point < distance_threshold &&
+      signed_arc_dist_to_stop_point < planner_param_.stop_distance_threshold &&
       planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
       state_machine_.setState(StateMachine::State::GO);
-      if (signed_arc_dist_to_stop_point < -distance_threshold) {
+      if (signed_arc_dist_to_stop_point < -planner_param_.stop_distance_threshold) {
         RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO");
       }
     }

diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp
@@ -59,6 +59,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
     double attention_area_length;
     double stop_line_margin;
     double stop_duration_sec;
+    double stop_distance_threshold;
     double path_interpolation_ds;
     double occlusion_attention_area_length;
     bool consider_wrong_direction_vehicle;