diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml index 6d723c510cce9..936bfc76cc98d 100644 --- 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 index 371873eab83cb..d109398e33d56 100644 --- 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 index 6be80eb381c9d..627a814f89de3 100644 --- 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 index cc3ed5a3a030b..a2bc53a617bcd 100644 --- 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;