Skip to content

Commit c7164f9

Browse files
soblinHansRobo
authored andcommitted
fix(start_planner): start prepare blinker when autonomous mode is available (#6470)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent d5ccf97 commit c7164f9

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
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
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
@@ -176,8 +176,10 @@ void StartPlannerModule::updateData()
176176
DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
177177
}
178178

179-
if (!status_.first_approved_time && isActivated()) {
180-
status_.first_approved_time = clock_->now();
179+
if (
180+
planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS &&
181+
!status_.first_engaged_time) {
182+
status_.first_engaged_time = clock_->now();
181183
}
182184

183185
if (hasFinishedBackwardDriving()) {
@@ -1084,11 +1086,11 @@ bool StartPlannerModule::hasFinishedPullOut() const
10841086

10851087
bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const
10861088
{
1087-
if (!status_.first_approved_time) {
1089+
if (!status_.first_engaged_time) {
10881090
return true;
10891091
}
1090-
const auto first_approved_time = status_.first_approved_time.value();
1091-
const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds();
1092+
const auto first_engaged_time = status_.first_engaged_time.value();
1093+
const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds();
10921094
return elapsed < parameters_->prepare_time_before_start;
10931095
}
10941096

0 commit comments

Comments
 (0)