File tree 3 files changed +17
-2
lines changed
behavior_velocity_planner/src
behavior_velocity_planner_common/include/behavior_velocity_planner_common
behavior_velocity_traffic_light_module/src
3 files changed +17
-2
lines changed Original file line number Diff line number Diff line change @@ -323,6 +323,8 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
323
323
{
324
324
std::lock_guard<std::mutex> lock (mutex_);
325
325
326
+ planner_data_.has_received_signal_ = true ;
327
+
326
328
// clear previous observation
327
329
planner_data_.traffic_light_id_map_raw_ .clear ();
328
330
const auto traffic_light_id_map_last_observed_old =
Original file line number Diff line number Diff line change @@ -83,6 +83,9 @@ struct PlannerData
83
83
std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
84
84
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
85
85
86
+ // this value becomes true once the signal message is received
87
+ bool has_received_signal_ = false ;
88
+
86
89
// velocity smoother
87
90
std::shared_ptr<motion_velocity_smoother::SmootherBase> velocity_smoother_;
88
91
// route handler
Original file line number Diff line number Diff line change @@ -281,15 +281,25 @@ bool TrafficLightModule::isStopSignal()
281
281
{
282
282
updateTrafficSignal ();
283
283
284
- // If it never receives traffic signal, it will PASS.
285
- if (!traffic_signal_stamp_) {
284
+ // Pass through if no traffic signal information has been received yet
285
+ // This is to prevent stopping on the planning simulator
286
+ if (!planner_data_->has_received_signal_ ) {
286
287
return false ;
287
288
}
288
289
290
+ // Stop if there is no upcoming traffic signal information
291
+ // This is to safely stop in cases such that traffic light recognition is not working properly or
292
+ // the map is incorrect
293
+ if (!traffic_signal_stamp_) {
294
+ return true ;
295
+ }
296
+
297
+ // Stop if the traffic signal information has timed out
289
298
if (isTrafficSignalTimedOut ()) {
290
299
return true ;
291
300
}
292
301
302
+ // Check if the current traffic signal state requires stopping
293
303
return traffic_light_utils::isTrafficSignalStop (lane_, looking_tl_state_);
294
304
}
295
305
You can’t perform that action at this time.
0 commit comments