renamed to common.stop_velocity_threshold

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
autowarefoundation · Aug 10, 2023 · 8cf1fd5 · 8cf1fd5
1 parent 1783844
commit 8cf1fd5
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 5 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -12,6 +12,7 @@
         use_intersection_area: false
         path_interpolation_ds: 0.1 # [m]
         consider_wrong_direction_vehicle: false
+        stop_velocity_threshold: 0.1388 # [m/s]
       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.
@@ -48,7 +49,6 @@
         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
-        first_stop_velocity_threshold: 0.2777 # [m/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

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -60,6 +60,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<double>(ns + ".common.path_interpolation_ds");
   ip.common.consider_wrong_direction_vehicle =
     node.declare_parameter<bool>(ns + ".common.consider_wrong_direction_vehicle");
+  ip.common.stop_velocity_threshold =
+    node.declare_parameter<double>(ns + ".common.stop_velocity_threshold");
 
   ip.stuck_vehicle.use_stuck_stopline =
     node.declare_parameter<bool>(ns + ".stuck_vehicle.use_stuck_stopline");
@@ -117,8 +119,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<std::vector<double>>(ns + ".occlusion.possible_object_bbox");
   ip.occlusion.ignore_parked_vehicle_speed_threshold =
     node.declare_parameter<double>(ns + ".occlusion.ignore_parked_vehicle_speed_threshold");
-  ip.occlusion.first_stop_velocity_threshold =
-    node.declare_parameter<double>(ns + ".occlusion.first_stop_velocity_threshold");
 }
 
 void IntersectionModuleManager::launchNewModules(

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -768,6 +768,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     const bool approached_stop_line =
       (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin);
     const bool is_stopped = planner_data_->isVehicleStopped();
+    // TODO(Mamoru Sobue): fix isVehicleStopped()
     if (is_stopped && approached_stop_line) {
       stuck_private_area_timeout_.setStateWithMarginTime(
         StateMachine::State::GO, logger_.get_child("stuck_private_area_timeout"), *clock_);
@@ -891,7 +892,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
       (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin);
     const bool over_stop_line = (dist_stopline < 0.0);
     const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x);
-    const bool is_stopped = (vel < planner_param_.occlusion.first_stop_vel_thr);
+    const bool is_stopped = (vel < planner_param_.common.stop_velocity_threshold);
     if (over_stop_line) {
       before_creep_state_machine_.setState(StateMachine::State::GO);
     }

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -60,6 +60,7 @@ class IntersectionModule : public SceneModuleInterface
       bool use_intersection_area;
       bool consider_wrong_direction_vehicle;
       double path_interpolation_ds;
+      double stop_velocity_threshold;
     } common;
     struct StuckVehicle
     {
@@ -109,7 +110,6 @@ class IntersectionModule : public SceneModuleInterface
       double denoise_kernel;
       std::vector<double> possible_object_bbox;
       double ignore_parked_vehicle_speed_threshold;
-      double first_stop_velocity_threshold;
     } occlusion;
   };