From 2db6c4062de1c7ff0cd78d5fb88c42be3efcb020 Mon Sep 17 00:00:00 2001 From: k-obitsu <56008637+k-obitsu@users.noreply.github.com> Date: Fri, 10 Jun 2022 08:26:37 +0900 Subject: [PATCH] fix(stop_line): fix stop_line state (#1058) * fix(stop_line): fix stop_line state Signed-off-by: k-obitsu * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/behavior_velocity_planner/config/stop_line.param.yaml | 1 + .../include/scene_module/stop_line/scene.hpp | 1 + .../src/scene_module/stop_line/manager.cpp | 2 ++ .../src/scene_module/stop_line/scene.cpp | 3 +-- 4 files changed, 5 insertions(+), 2 deletions(-) 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;