feat(intersection): add param for stuck stopline overshoot margin (#2756

) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
autowarefoundation · Jan 27, 2023 · 0911600 · 0911600
1 parent 7b16fb8
commit 0911600
Show file tree

Hide file tree

Showing 4 changed files with 4 additions and 2 deletions.
diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml
@@ -20,6 +20,7 @@
       use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
       assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
       enable_front_car_decel_prediction: false # By default this feature is disabled
+      stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collsion detection
 
     merge_from_private_road:
       stop_duration_sec: 1.0
diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
@@ -93,6 +93,7 @@ class IntersectionModule : public SceneModuleInterface
     double
       assumed_front_car_decel;  //! the expected deceleration of front car when front car as well
     bool enable_front_car_decel_prediction;  //! flag for using above feature
+    double stop_overshoot_margin;            //! overshoot margin for stuck, collsion detection
   };
 
   IntersectionModule(

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
@@ -69,6 +69,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.assumed_front_car_decel = node.declare_parameter(ns + ".assumed_front_car_decel", 1.0);
   ip.enable_front_car_decel_prediction =
     node.declare_parameter(ns + ".enable_front_car_decel_prediction", false);
+  ip.stop_overshoot_margin = node.declare_parameter(ns + ".stop_overshoot_margin", 0.5);
 }
 
 MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node)

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -202,10 +202,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     const double dist_stuck_stopline = motion_utils::calcSignedArcLength(
       path->points, path->points.at(stuck_line_idx).point.pose.position,
       path->points.at(closest_idx).point.pose.position);
-    const double eps = 1e-1;  // NOTE: check if sufficiently over the stuck stopline
     const bool is_over_stuck_stopline =
       util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) &&
-      dist_stuck_stopline > eps;
+      dist_stuck_stopline > planner_param_.stop_overshoot_margin;
     if (is_stuck && !is_over_stuck_stopline) {
       stop_line_idx_final = stuck_line_idx;
       pass_judge_line_idx_final = stuck_line_idx;