feat(traffic_light): output undetected traffic light as unknown (auto…

…warefoundation#5934)

* fix: output undetected traffic light as unknown

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

* Update perception/traffic_light_classifier/src/nodelet.cpp

Co-authored-by: Kotaro Uetake <[email protected]>

* Update perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp

Co-authored-by: Kotaro Uetake <[email protected]>

* style(pre-commit): autofix

* fix: unnecessary count

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

* style(pre-commit): autofix

* fix: improve readability

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

* fix: index error

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

* fix: append the undetected rois at the end so that the order can be easily kept

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

* fix: false occlusion ratio for undetected signals

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

---------

Signed-off-by: tzhong518 <[email protected]>
Co-authored-by: Kotaro Uetake <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/traffic_light_classifier/src/nodelet.cpp

@@ -13,6 +13,8 @@
 // limitations under the License.
 #include "traffic_light_classifier/nodelet.hpp"
 
+#include <tier4_perception_msgs/msg/traffic_light_element.hpp>
+
 #include <iostream>
 #include <memory>
 #include <utility>
@@ -98,16 +100,18 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
   output_msg.signals.resize(input_rois_msg->rois.size());
 
   std::vector<cv::Mat> images;
-  size_t num_valid_roi = 0;
   std::vector<size_t> backlight_indices;
   for (size_t i = 0; i < input_rois_msg->rois.size(); i++) {
-    // skip if not the expected type of roi
+    // skip if the roi is not detected
+    if (input_rois_msg->rois.at(i).roi.height == 0) {
+      break;
+    }
     if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) {
       continue;
     }
-    output_msg.signals[num_valid_roi].traffic_light_id =
+    output_msg.signals[images.size()].traffic_light_id =
       input_rois_msg->rois.at(i).traffic_light_id;
-    output_msg.signals[num_valid_roi].traffic_light_type =
+    output_msg.signals[images.size()].traffic_light_type =
       input_rois_msg->rois.at(i).traffic_light_type;
     const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi;
 
@@ -116,15 +120,29 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
       backlight_indices.emplace_back(i);
     }
     images.emplace_back(roi_img);
-    num_valid_roi++;
   }
-  output_msg.signals.resize(num_valid_roi);
 
+  output_msg.signals.resize(images.size());
   if (!classifier_ptr_->getTrafficSignals(images, output_msg)) {
     RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback");
     return;
   }
 
+  // append the undetected rois as unknown
+  for (const auto & input_roi : input_rois_msg->rois) {
+    if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) {
+      tier4_perception_msgs::msg::TrafficSignal tlr_sig;
+      tlr_sig.traffic_light_id = input_roi.traffic_light_id;
+      tlr_sig.traffic_light_type = input_roi.traffic_light_type;
+      tier4_perception_msgs::msg::TrafficLightElement element;
+      element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
+      element.shape = tier4_perception_msgs::msg::TrafficLightElement::CIRCLE;
+      element.confidence = 0.0;
+      tlr_sig.elements.push_back(element);
+      output_msg.signals.push_back(tlr_sig);
+    }
+  }
+
   for (const auto & idx : backlight_indices) {
     auto & elements = output_msg.signals.at(idx).elements;
     for (auto & element : elements) {

perception/traffic_light_fine_detector/src/nodelet.cpp

@@ -272,14 +272,28 @@ void TrafficLightFineDetectorNodelet::detectionMatch(
   }
 
   out_rois.rois.clear();
-  for (const auto & p : bestDetections) {
+  std::vector<size_t> invalid_roi_id;
+  for (const auto & [tlr_id, roi] : id2expectRoi) {
+    // if matches, update the roi info
+    if (!bestDetections.count(tlr_id)) {
+      invalid_roi_id.emplace_back(tlr_id);
+      continue;
+    }
+    TrafficLightRoi tlr;
+    tlr.traffic_light_id = tlr_id;
+    const auto & object = bestDetections.at(tlr_id);
+    tlr.traffic_light_type = roi.traffic_light_type;
+    tlr.roi.x_offset = object.x_offset;
+    tlr.roi.y_offset = object.y_offset;
+    tlr.roi.width = object.width;
+    tlr.roi.height = object.height;
+    out_rois.rois.push_back(tlr);
+  }
+  // append undetected rois at the end
+  for (const auto & id : invalid_roi_id) {
     TrafficLightRoi tlr;
-    tlr.traffic_light_id = p.first;
-    tlr.traffic_light_type = id2expectRoi[p.first].traffic_light_type;
-    tlr.roi.x_offset = p.second.x_offset;
-    tlr.roi.y_offset = p.second.y_offset;
-    tlr.roi.width = p.second.width;
-    tlr.roi.height = p.second.height;
+    tlr.traffic_light_id = id;
+    tlr.traffic_light_type = id2expectRoi[id].traffic_light_type;
     out_rois.rois.push_back(tlr);
   }
 }

perception/traffic_light_occlusion_predictor/src/nodelet.cpp

@@ -129,20 +129,26 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback(
   const uint8_t traffic_light_type)
 {
   std::vector<int> occlusion_ratios;
+  size_t not_detected_roi = 0;
   if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) {
     occlusion_ratios.resize(in_signal_msg->signals.size(), 0);
   } else {
     tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg;
     selected_roi_msg.rois.reserve(in_roi_msg->rois.size());
     for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) {
+      // not detected roi
+      if (in_roi_msg->rois[i].roi.height == 0) {
+        not_detected_roi++;
+        continue;
+      }
       if (in_roi_msg->rois.at(i).traffic_light_type == traffic_light_type) {
         selected_roi_msg.rois.push_back(in_roi_msg->rois.at(i));
       }
     }
 
     tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg;
 
-    if (selected_roi_msg.rois.size() != in_signal_msg->signals.size()) {
+    if (selected_roi_msg.rois.size() != in_signal_msg->signals.size() - not_detected_roi) {
       occlusion_ratios.resize(in_signal_msg->signals.size(), 0);
     } else {
       auto selected_roi_msg_ptr =
@@ -162,6 +168,12 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback(
       traffic_light_utils::setSignalUnknown(out_msg_.signals.at(predicted_num + i), 0.0);
     }
   }
+
+  // push back not detected rois
+  for (size_t i = occlusion_ratios.size(); i < in_signal_msg->signals.size(); ++i) {
+    out_msg_.signals.push_back(in_signal_msg->signals[i]);
+  }
+
   subscribed_.at(traffic_light_type) = true;
 
   if (std::all_of(subscribed_.begin(), subscribed_.end(), [](bool v) { return v; })) {

perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp

@@ -88,6 +88,10 @@ void CloudOcclusionPredictor::predict(
   image_geometry::PinholeCameraModel pinhole_model;
   pinhole_model.fromCameraInfo(*camera_info_msg);
   for (size_t i = 0; i < rois_msg->rois.size(); i++) {
+    // skip if no detection
+    if (rois_msg->rois[i].roi.height == 0) {
+      continue;
+    }
     calcRoiVector3D(
       rois_msg->rois[i], pinhole_model, traffic_light_position_map, tf_camera2map, roi_tls[i],
       roi_brs[i]);
@@ -107,7 +111,7 @@ void CloudOcclusionPredictor::predict(
     lidar_rays_[static_cast<int>(ray.azimuth)][static_cast<int>(ray.elevation)].push_back(ray);
   }
   for (size_t i = 0; i < roi_tls.size(); i++) {
-    occlusion_ratios[i] = predict(roi_tls[i], roi_brs[i]);
+    occlusion_ratios[i] = rois_msg->rois[i].roi.height == 0 ? 0 : predict(roi_tls[i], roi_brs[i]);
   }
 }