File tree 2 files changed +8
-6
lines changed
planning/behavior_path_start_planner_module
include/behavior_path_start_planner_module
2 files changed +8
-6
lines changed Original file line number Diff line number Diff line change @@ -71,7 +71,7 @@ struct PullOutStatus
71
71
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr };
72
72
std::optional<Pose> stop_pose{std::nullopt};
73
73
// ! 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};
75
75
76
76
PullOutStatus () {}
77
77
};
Original file line number Diff line number Diff line change @@ -176,8 +176,10 @@ void StartPlannerModule::updateData()
176
176
DEBUG_PRINT (" StartPlannerModule::updateData() received new route, reset status" );
177
177
}
178
178
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 ();
181
183
}
182
184
183
185
if (hasFinishedBackwardDriving ()) {
@@ -1084,11 +1086,11 @@ bool StartPlannerModule::hasFinishedPullOut() const
1084
1086
1085
1087
bool StartPlannerModule::needToPrepareBlinkerBeforeStart () const
1086
1088
{
1087
- if (!status_.first_approved_time ) {
1089
+ if (!status_.first_engaged_time ) {
1088
1090
return true ;
1089
1091
}
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 ();
1092
1094
return elapsed < parameters_->prepare_time_before_start ;
1093
1095
}
1094
1096
You can’t perform that action at this time.
0 commit comments