fix(traffic_light): stop if the traffic light signal timed out (autow…

…arefoundation#5819)

* fix(traffic_light): stop if the traffic light signal timed out

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(traffic_light): fix README format

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_velocity_traffic_light_module/README.md

@@ -19,7 +19,13 @@ This module is activated when there is traffic light in ego lane.
 
 1. Obtains a traffic light mapped to the route and a stop line correspond to the traffic light from a map information.
 
-2. Uses the highest reliability one of the traffic light recognition result and if the color of that was red, generates a stop point.
+   - If a corresponding traffic light signal have never been found, it treats as a signal to pass.
+
+   - If a corresponding traffic light signal is found but timed out, it treats as a signal to stop.
+
+2. Uses the highest reliability one of the traffic light recognition result and if the color of that was not green or corresponding arrow signal, generates a stop point.
+
+   - If an elapsed time to receive stop signal is less than `stop_time_hysteresis`, it treats as a signal to pass. This feature is to prevent chattering.
 
 3. When vehicle current velocity is
 
@@ -63,12 +69,13 @@ This module is activated when there is traffic light in ego lane.
 
 #### Module Parameters
 
-| Parameter            | Type   | Description                                     |
-| -------------------- | ------ | ----------------------------------------------- |
-| `stop_margin`        | double | [m] margin before stop point                    |
-| `tl_state_timeout`   | double | [s] time out for detected traffic light result. |
-| `yellow_lamp_period` | double | [s] time for yellow lamp                        |
-| `enable_pass_judge`  | bool   | [-] whether to use pass judge                   |
+| Parameter              | Type   | Description                                                          |
+| ---------------------- | ------ | -------------------------------------------------------------------- |
+| `stop_margin`          | double | [m] margin before stop point                                         |
+| `tl_state_timeout`     | double | [s] time out for detected traffic light result.                      |
+| `stop_time_hysteresis` | double | [s] time threshold to decide stop planning for chattering prevention |
+| `yellow_lamp_period`   | double | [s] time for yellow lamp                                             |
+| `enable_pass_judge`    | bool   | [-] whether to use pass judge                                        |
 
 #### Flowchart
 
@@ -91,7 +98,7 @@ if (state is APPROACH) then (yes)
     :change state to GO_OUT;
     stop
   elseif (no stop signal) then (yes)
-    :change previous state to STOP;
+    :change previous state to PASS;
     stop
   elseif (not pass through) then (yes)
     :insert stop pose;

planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml

@@ -3,6 +3,7 @@
     traffic_light:
       stop_margin: 0.0
       tl_state_timeout: 1.0
+      stop_time_hysteresis: 0.1
       yellow_lamp_period: 2.75
       enable_pass_judge: true
       enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval

planning/behavior_velocity_traffic_light_module/src/manager.cpp

@@ -38,6 +38,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node)
   const std::string ns(getModuleName());
   planner_param_.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
   planner_param_.tl_state_timeout = getOrDeclareParameter<double>(node, ns + ".tl_state_timeout");
+  planner_param_.stop_time_hysteresis =
+    getOrDeclareParameter<double>(node, ns + ".stop_time_hysteresis");
   planner_param_.enable_pass_judge = getOrDeclareParameter<bool>(node, ns + ".enable_pass_judge");
   planner_param_.yellow_lamp_period =
     getOrDeclareParameter<double>(node, ns + ".yellow_lamp_period");

planning/behavior_velocity_traffic_light_module/src/scene.cpp

@@ -222,13 +222,33 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     if (signed_arc_length_to_stop_point < signed_deadline_length) {
       RCLCPP_INFO(logger_, "APPROACH -> GO_OUT");
       state_ = State::GO_OUT;
+      stop_signal_received_time_ptr_.reset();
       return true;
     }
 
     first_ref_stop_path_point_index_ = stop_line_point_idx;
 
     // Check if stop is coming.
-    setSafe(!isStopSignal());
+    const bool is_stop_signal = isStopSignal();
+
+    // Update stop signal received time
+    if (is_stop_signal) {
+      if (!stop_signal_received_time_ptr_) {
+        stop_signal_received_time_ptr_ = std::make_unique<Time>(clock_->now());
+      }
+    } else {
+      stop_signal_received_time_ptr_.reset();
+    }
+
+    // Check hysteresis
+    const double time_diff =
+      stop_signal_received_time_ptr_
+        ? std::max((clock_->now() - *stop_signal_received_time_ptr_).seconds(), 0.0)
+        : 0.0;
+    const bool to_be_stopped =
+      is_stop_signal && (is_prev_state_stop_ || time_diff > planner_param_.stop_time_hysteresis);
+
+    setSafe(!to_be_stopped);
     if (isActivated()) {
       is_prev_state_stop_ = false;
       return true;
@@ -250,6 +270,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
         state_ = State::APPROACH;
       }
     }
+    stop_signal_received_time_ptr_.reset();
     return true;
   }
 
@@ -258,27 +279,33 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
 bool TrafficLightModule::isStopSignal()
 {
-  if (!updateTrafficSignal()) {
+  updateTrafficSignal();
+
+  // If it never receives traffic signal, it will PASS.
+  if (!traffic_signal_stamp_) {
     return false;
   }
 
+  if (isTrafficSignalTimedOut()) {
+    return true;
+  }
+
   return isTrafficSignalStop(looking_tl_state_);
 }
 
-bool TrafficLightModule::updateTrafficSignal()
+void TrafficLightModule::updateTrafficSignal()
 {
-  TrafficSignal signal;
-  bool found_signal = findValidTrafficSignal(signal);
-
-  if (!found_signal) {
-    // Don't stop when UNKNOWN or TIMEOUT as discussed at #508
-    return false;
+  TrafficSignalStamped signal;
+  if (!findValidTrafficSignal(signal)) {
+    // Don't stop if it never receives traffic light topic.
+    return;
   }
 
-  // Found signal associated with the lanelet
-  looking_tl_state_ = signal;
+  traffic_signal_stamp_ = signal.stamp;
 
-  return true;
+  // Found signal associated with the lanelet
+  looking_tl_state_ = signal.signal;
+  return;
 }
 
 bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
@@ -355,7 +382,7 @@ bool TrafficLightModule::isTrafficSignalStop(
   return true;
 }
 
-bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_signal)
+bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const
 {
   // get traffic signal associated with the regulatory element id
   const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id());
@@ -366,20 +393,27 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_si
     return false;
   }
 
-  // check if the traffic signal data is outdated
+  valid_traffic_signal = *traffic_signal_stamped;
+  return true;
+}
+
+bool TrafficLightModule::isTrafficSignalTimedOut() const
+{
+  if (!traffic_signal_stamp_) {
+    return false;
+  }
+
   const auto is_traffic_signal_timeout =
-    (clock_->now() - traffic_signal_stamped->stamp).seconds() > planner_param_.tl_state_timeout;
+    (clock_->now() - *traffic_signal_stamp_).seconds() > planner_param_.tl_state_timeout;
   if (is_traffic_signal_timeout) {
     RCLCPP_WARN_THROTTLE(
       logger_, *clock_, 5000 /* ms */, "the received traffic signal data is outdated");
     RCLCPP_WARN_STREAM_THROTTLE(
       logger_, *clock_, 5000 /* ms */,
-      "time diff: " << (clock_->now() - traffic_signal_stamped->stamp).seconds());
-    return false;
+      "time diff: " << (clock_->now() - *traffic_signal_stamp_).seconds());
+    return true;
   }
-
-  valid_traffic_signal = traffic_signal_stamped->signal;
-  return true;
+  return false;
 }
 
 autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose(

planning/behavior_velocity_traffic_light_module/src/scene.hpp

@@ -39,6 +39,7 @@ class TrafficLightModule : public SceneModuleInterface
 public:
   using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
   using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
+  using Time = rclcpp::Time;
   enum class State { APPROACH, GO_OUT };
 
   struct DebugData
@@ -60,6 +61,7 @@ class TrafficLightModule : public SceneModuleInterface
     double stop_margin;
     double tl_state_timeout;
     double yellow_lamp_period;
+    double stop_time_hysteresis;
     bool enable_pass_judge;
   };
 
@@ -100,9 +102,11 @@ class TrafficLightModule : public SceneModuleInterface
 
   bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const;
 
-  bool findValidTrafficSignal(TrafficSignal & valid_traffic_signal);
+  bool findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const;
 
-  bool updateTrafficSignal();
+  bool isTrafficSignalTimedOut() const;
+
+  void updateTrafficSignal();
 
   // Lane id
   const int64_t lane_id_;
@@ -120,11 +124,16 @@ class TrafficLightModule : public SceneModuleInterface
   // Debug
   DebugData debug_data_;
 
-  // prevent paththrough chattering
+  // prevent pass through chattering
   bool is_prev_state_stop_;
 
+  // prevent stop chattering
+  std::unique_ptr<Time> stop_signal_received_time_ptr_{};
+
   boost::optional<int> first_ref_stop_path_point_index_;
 
+  boost::optional<Time> traffic_signal_stamp_;
+
   // Traffic Light State
   TrafficSignal looking_tl_state_;
 };