Skip to content

Commit df33eb0

Browse files
soblin0x126
authored andcommitted
fix(start_planner): start prepare blinker when autonomous mode is available (autowarefoundation#6470)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent e6fbf9e commit df33eb0

File tree

2 files changed

+8
-6
lines changed

2 files changed

+8
-6
lines changed

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,7 @@ struct PullOutStatus
7171
bool has_stop_point{false};
7272
std::optional<Pose> stop_pose{std::nullopt};
7373
//! record the first time when the state changed from !isActivated() to isActivated()
74-
std::optional<rclcpp::Time> first_approved_time{std::nullopt};
74+
std::optional<rclcpp::Time> first_engaged_time{std::nullopt};
7575

7676
PullOutStatus() {}
7777
};

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+7-5
Original file line numberDiff line numberDiff line change
@@ -161,8 +161,10 @@ void StartPlannerModule::updateData()
161161
DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
162162
}
163163

164-
if (!status_.first_approved_time && isActivated()) {
165-
status_.first_approved_time = clock_->now();
164+
if (
165+
planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS &&
166+
!status_.first_engaged_time) {
167+
status_.first_engaged_time = clock_->now();
166168
}
167169

168170
if (hasFinishedBackwardDriving()) {
@@ -968,11 +970,11 @@ bool StartPlannerModule::hasFinishedPullOut() const
968970

969971
bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const
970972
{
971-
if (!status_.first_approved_time) {
973+
if (!status_.first_engaged_time) {
972974
return true;
973975
}
974-
const auto first_approved_time = status_.first_approved_time.value();
975-
const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds();
976+
const auto first_engaged_time = status_.first_engaged_time.value();
977+
const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds();
976978
return elapsed < parameters_->prepare_time_before_start;
977979
}
978980

0 commit comments

Comments
 (0)