From eb72adbb70a5281a7bec485c6165ede63d5e23d5 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
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 <mamoru.sobue@tier4.jp>
---
.../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 <mamoru.sobue@tier4.jp>
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 <mamoru.sobue@tier4.jp>
---
.../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<PathWithLaneId> prev_stop_path_after_approval{nullptr};
std::optional<Pose> stop_pose{std::nullopt};
//! record the first time when the state changed from !isActivated() to isActivated()
- std::optional<rclcpp::Time> first_approved_time{std::nullopt};
+ std::optional<rclcpp::Time> 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;
}