Skip to content

Commit 955c353

Browse files
committed
feat(behavior_velocity_traffic_light): ensure stopping if a signal, once received, is not received again (autowarefoundation#6468) (#1156)
feat(behavior_velocity_traffic_light): ensure stopping if a signal, once received, is not received again (autowarefoundation#6468) * feat(behavior_velocity_traffic_light): ensure stopping if a signal, once received, is not received again." * add comments for clarity * fix comments for clarity --------- Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
1 parent 202a178 commit 955c353

File tree

3 files changed

+16
-2
lines changed
  • planning
    • behavior_velocity_planner/src
    • behavior_velocity_planner_common/include/behavior_velocity_planner_common
    • behavior_velocity_traffic_light_module/src

3 files changed

+16
-2
lines changed

planning/behavior_velocity_planner/src/node.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -298,6 +298,8 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
298298
{
299299
std::lock_guard<std::mutex> lock(mutex_);
300300

301+
planner_data_.has_received_signal_ = true;
302+
301303
for (const auto & signal : msg->signals) {
302304
TrafficSignalStamped traffic_signal;
303305
traffic_signal.stamp = msg->stamp;

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,9 @@ struct PlannerData
8989
boost::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
9090
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
9191

92+
// this value becomes true once the signal message is received
93+
bool has_received_signal_ = false;
94+
9295
// velocity smoother
9396
std::shared_ptr<motion_velocity_smoother::SmootherBase> velocity_smoother_;
9497
// route handler

planning/behavior_velocity_traffic_light_module/src/scene.cpp

+11-2
Original file line numberDiff line numberDiff line change
@@ -302,11 +302,20 @@ bool TrafficLightModule::isStopSignal()
302302
{
303303
updateTrafficSignal();
304304

305-
// If it never receives traffic signal, it will PASS.
306-
if (!traffic_signal_stamp_) {
305+
// Pass through if no traffic signal information has been received yet
306+
// This is to prevent stopping on the planning simulator
307+
if (!planner_data_->has_received_signal_) {
307308
return false;
308309
}
309310

311+
// Stop if there is no upcoming traffic signal information
312+
// This is to safely stop in cases such that traffic light recognition is not working properly or
313+
// the map is incorrect
314+
if (!traffic_signal_stamp_) {
315+
return true;
316+
}
317+
318+
// Stop if the traffic signal information has timed out
310319
if (isTrafficSignalTimedOut()) {
311320
return true;
312321
}

0 commit comments

Comments
 (0)