From eb72adbb70a5281a7bec485c6165ede63d5e23d5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 21 Feb 2024 16:54:42 +0900 Subject: [PATCH 1/2] fix(start_planner): start prepare blinker when autonomous mode is available Signed-off-by: Mamoru Sobue --- .../src/start_planner_module.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index acccd5319bab0..12acdc6998270 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -176,7 +176,9 @@ void StartPlannerModule::updateData() DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); } - if (!status_.first_approved_time && isActivated()) { + if ( + planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && + !status_.first_approved_time && isActivated()) { status_.first_approved_time = clock_->now(); } From e0e29e4fce2273a3e7f36a9905e0e5514b6449c4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 21 Feb 2024 18:05:50 +0900 Subject: [PATCH 2/2] just count the elapsed time from engage Signed-off-by: Mamoru Sobue --- .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a9eb81c3ba235..067bbd5d74aee 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -71,7 +71,7 @@ struct PullOutStatus std::shared_ptr prev_stop_path_after_approval{nullptr}; std::optional stop_pose{std::nullopt}; //! record the first time when the state changed from !isActivated() to isActivated() - std::optional first_approved_time{std::nullopt}; + std::optional first_engaged_time{std::nullopt}; PullOutStatus() {} }; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 12acdc6998270..2171373cd85ff 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -178,8 +178,8 @@ void StartPlannerModule::updateData() if ( planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && - !status_.first_approved_time && isActivated()) { - status_.first_approved_time = clock_->now(); + !status_.first_engaged_time) { + status_.first_engaged_time = clock_->now(); } if (hasFinishedBackwardDriving()) { @@ -1086,11 +1086,11 @@ bool StartPlannerModule::hasFinishedPullOut() const bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const { - if (!status_.first_approved_time) { + if (!status_.first_engaged_time) { return true; } - const auto first_approved_time = status_.first_approved_time.value(); - const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds(); + const auto first_engaged_time = status_.first_engaged_time.value(); + const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds(); return elapsed < parameters_->prepare_time_before_start; }