From e18b65fe60beb4acee3bb753c8e9410026ca29e0 Mon Sep 17 00:00:00 2001 From: k-obitsu Date: Wed, 8 Jun 2022 15:41:21 +0900 Subject: [PATCH 1/3] fix(stop_line): fix stop_line state Signed-off-by: k-obitsu --- 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 | 1 + .../src/scene_module/stop_line/scene.cpp | 3 +-- 4 files changed, 4 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..0fe15f8b6fc2c 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: false 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..70fa0511d6151 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,7 @@ 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; From 058b041ff99e9af831cf763499bb548637ef5855 Mon Sep 17 00:00:00 2001 From: k-obitsu Date: Thu, 9 Jun 2022 11:57:10 +0900 Subject: [PATCH 2/3] fix(stop_line): fix stop_line state Signed-off-by: k-obitsu --- planning/behavior_velocity_planner/config/stop_line.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml index 0fe15f8b6fc2c..936bfc76cc98d 100644 --- a/planning/behavior_velocity_planner/config/stop_line.param.yaml +++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml @@ -4,6 +4,6 @@ stop_margin: 0.0 stop_check_dist: 2.0 stop_duration_sec: 1.0 - use_initialization_stop_line_state: false + use_initialization_stop_line_state: true debug: show_stopline_collision_check: false # [-] whether to show stopline collision From b320bdbd17711b3e30151752be86ceb4fee81b12 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 9 Jun 2022 03:04:15 +0000 Subject: [PATCH 3/3] ci(pre-commit): autofix --- .../src/scene_module/stop_line/manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 70fa0511d6151..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,7 +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); + 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);