add code in order to check invalid detection

Signed-off-by: MasatoSaeki <[email protected]>

perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp

@@ -97,6 +97,8 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
 
   void removeDuplicateIds(TrafficSignalArray & signal_array) const;
 
+  bool isInvalidDetectionStatus(const TrafficSignal & signal) const;
+
   // Node param
   bool use_last_detect_color_;
   double last_detect_color_hold_time_;

perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp

@@ -295,6 +295,14 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
     if (valid_id2idx_map.count(id)) {
       size_t idx = valid_id2idx_map[id];
       auto signal = msg.traffic_light_groups[idx];
+      if (isInvalidDetectionStatus(signal)) {
+        TrafficSignalElement output_traffic_signal_element;
+        output_traffic_signal_element.color = color;
+        output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE;
+        output_traffic_signal_element.confidence = 1.0;
+        output.traffic_light_groups[idx].elements[0] = output_traffic_signal_element;
+        continue;
+      }
       updateFlashingState(signal);  // check if it is flashing
       // update output msg according to flashing and current state
       output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal);
@@ -311,6 +319,19 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
   }
 }
 
+bool CrosswalkTrafficLightEstimatorNode::isInvalidDetectionStatus(const TrafficSignal & signal) const
+{
+  // check occlusion, backlight(shape is unknown) and no detection(shape is circle)
+  if (
+    signal.elements.front().color == TrafficSignalElement::UNKNOWN &&
+    signal.elements.front().confidence == 0.0)
+  {
+    return true;
+  }
+
+  return false;
+}
+
 void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal)
 {
   const auto id = signal.traffic_light_group_id;