fix(traffic_light_classifier): fix zero size roi bug (autowarefoundation#7608)

* fix: continue to process when input roi size is zero

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: consider when roi size is zero, rois is empty

fix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: use emplace_back instead of push_back for adding images and backlight indices

The code changes in `traffic_light_classifier_node.cpp` modify the way images and backlight indices are added to the respective vectors. Instead of using `push_back`, the code now uses `emplace_back`. This change improves performance and ensures proper object construction.

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor: bring back for loop skim and output_msg filling

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* chore: refactor code to handle empty input ROIs in traffic_light_classifier_node.cpp

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor: using index instead of vector length

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

perception/traffic_light_classifier/src/nodelet.cpp

@@ -84,6 +84,9 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
   if (classifier_ptr_.use_count() == 0) {
     return;
   }
+  if (input_rois_msg->rois.empty()) {
+    return;
+  }
 
   cv_bridge::CvImagePtr cv_ptr;
   try {
@@ -95,41 +98,49 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
   }
 
   tier4_perception_msgs::msg::TrafficLightArray output_msg;
-
   output_msg.signals.resize(input_rois_msg->rois.size());
 
   std::vector<cv::Mat> images;
   std::vector<size_t> backlight_indices;
-  for (size_t i = 0; i < input_rois_msg->rois.size(); i++) {
-    // skip if the roi is not detected
-    if (input_rois_msg->rois.at(i).roi.height == 0) {
-      break;
+  size_t idx_valid_roi = 0;
+  for (const auto & input_roi : input_rois_msg->rois) {
+    // ignore if the roi is not the type to be classified
+    if (input_roi.traffic_light_type != classify_traffic_light_type_) {
+      continue;
     }
-    if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) {
+    // skip if the roi size is zero
+    if (input_roi.roi.height == 0 || input_roi.roi.width == 0) {
       continue;
     }
-    output_msg.signals[images.size()].traffic_light_id =
-      input_rois_msg->rois.at(i).traffic_light_id;
-    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;
 
+    // set traffic light id and type
+    output_msg.signals[idx_valid_roi].traffic_light_id = input_roi.traffic_light_id;
+    output_msg.signals[idx_valid_roi].traffic_light_type = input_roi.traffic_light_type;
+
+    const sensor_msgs::msg::RegionOfInterest & roi = input_roi.roi;
     auto roi_img = cv_ptr->image(cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height));
     if (is_harsh_backlight(roi_img)) {
-      backlight_indices.emplace_back(i);
+      backlight_indices.emplace_back(idx_valid_roi);
     }
     images.emplace_back(roi_img);
+    idx_valid_roi++;
   }
 
+  // classify the images
   output_msg.signals.resize(images.size());
-  if (!classifier_ptr_->getTrafficSignals(images, output_msg)) {
-    RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback");
-    return;
+  if (!images.empty()) {
+    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_) {
+    // if the type is the target type but the roi size is zero, the roi is undetected
+    if (
+      (input_roi.roi.height == 0 || input_roi.roi.width == 0) &&
+      input_roi.traffic_light_type == classify_traffic_light_type_) {
       tier4_perception_msgs::msg::TrafficLight tlr_sig;
       tlr_sig.traffic_light_id = input_roi.traffic_light_id;
       tlr_sig.traffic_light_type = input_roi.traffic_light_type;