feat(intersection): add flag to enable creep towards intersection occ…

…lusion (#3510) * feat(intersection): add flag to enable creep towards intersection occlusion Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Apr 24, 2023 · 2a0f760 · 2a0f760
1 parent aba954f
commit 2a0f760
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 2 deletions.
diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml
@@ -31,6 +31,7 @@
       occlusion:
         enable: true
         occlusion_detection_area_length: 70.0 # [m]
+        enable_creeping: false # flag to use the creep velocity when reaching occlusion limit stop line
         occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line
         free_space_max: 43
         occupied_min: 58

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
@@ -110,7 +110,8 @@ class IntersectionModule : public SceneModuleInterface
     {
       bool enable;
       double occlusion_detection_area_length;  //! used for occlusion detection
-      double occlusion_creep_velocity;         //! the creep velocity to occlusion limit stop lline
+      bool enable_creeping;
+      double occlusion_creep_velocity;  //! the creep velocity to occlusion limit stop lline
       int free_space_max;
       int occupied_min;
       bool do_dp;

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
@@ -81,6 +81,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.occlusion.enable = node.declare_parameter<bool>(ns + ".occlusion.enable");
   ip.occlusion.occlusion_detection_area_length =
     node.declare_parameter<double>(ns + ".occlusion.occlusion_detection_area_length");
+  ip.occlusion.enable_creeping = node.declare_parameter<bool>(ns + ".occlusion.enable_creeping");
   ip.occlusion.occlusion_creep_velocity =
     node.declare_parameter<double>(ns + ".occlusion.occlusion_creep_velocity");
   ip.occlusion.free_space_max = node.declare_parameter<int>(ns + ".occlusion.free_space_max");

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
@@ -405,7 +405,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   if (!occlusion_activated_) {
     is_go_out_ = false;
     /* in case of creeping */
-    if (insert_creep_during_occlusion) {
+    if (insert_creep_during_occlusion && planner_param_.occlusion.enable_creeping) {
       const auto [start, end] = insert_creep_during_occlusion.value();
       for (size_t i = start; i < end; ++i) {
         planning_utils::setVelocityFromIndex(