fix(stop_line): fix stop_line state (tier4#1058)

* fix(stop_line): fix stop_line state Signed-off-by: k-obitsu <koichi.obitsu@tier4.jp> * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali · Oct 3, 2022 · 2db6c40 · 2db6c40
1 parent c20d35d
commit 2db6c40
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 2 deletions.
diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml
@@ -4,5 +4,6 @@
       stop_margin: 0.0
       stop_check_dist: 2.0
       stop_duration_sec: 1.0
+      use_initialization_stop_line_state: true
       debug:
         show_stopline_collision_check: false            # [-] whether to show stopline collision
diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp
@@ -72,6 +72,7 @@ class StopLineModule : public SceneModuleInterface
     double stop_margin;
     double stop_check_dist;
     double stop_duration_sec;
+    bool use_initialization_stop_line_state;
     bool show_stopline_collision_check;
   };
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp
@@ -93,6 +93,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node)
   p.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0);
   p.stop_check_dist = node.declare_parameter(ns + ".stop_check_dist", 2.0);
   p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0);
+  p.use_initialization_stop_line_state =
+    node.declare_parameter(ns + ".use_initialization_stop_line_state", false);
   // debug
   p.show_stopline_collision_check =
     node.declare_parameter(ns + ".debug.show_stopline_collision_check", false);

diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp
@@ -278,8 +278,7 @@ bool StopLineModule::modifyPathVelocity(
     }
   } else if (state_ == State::START) {
     // Initialize if vehicle is far from stop_line
-    constexpr bool use_initialization_after_start = false;
-    if (use_initialization_after_start) {
+    if (planner_param_.use_initialization_stop_line_state) {
       if (signed_arc_dist_to_stop_point > planner_param_.stop_check_dist) {
         RCLCPP_INFO(logger_, "START -> APPROACH");
         state_ = State::APPROACH;