use isVehicleStopped(before_creep_stop_time)

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

Hide file tree

Showing 4 changed files with 2 additions and 6 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -12,7 +12,6 @@
         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.

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -60,8 +60,6 @@ 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");

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -891,8 +891,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     const bool approached_stop_line =
       (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_.common.stop_velocity_threshold);
+    const bool is_stopped =
+      planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time);
     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,7 +60,6 @@ class IntersectionModule : public SceneModuleInterface
       bool use_intersection_area;
       bool consider_wrong_direction_vehicle;
       double path_interpolation_ds;
-      double stop_velocity_threshold;
     } common;
     struct StuckVehicle
     {