feat(traffic_light_classifier): update diagnostics when harsh backlight is detected (#10218)

feat: update diagnostics when harsh backlight is detected

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

Author: ktro2828

perception/autoware_traffic_light_classifier/package.xml

@@ -19,6 +19,7 @@
   <depend>autoware_cuda_utils</depend>
   <depend>autoware_tensorrt_classifier</depend>
   <depend>autoware_tensorrt_common</depend>
+  <depend>autoware_utils</depend>
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>libboost-filesystem-dev</depend>

perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 #include "traffic_light_classifier_node.hpp"
 
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <tier4_perception_msgs/msg/traffic_light_element.hpp>
 
 #include <iostream>
@@ -60,6 +61,9 @@ TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeO
       this->get_logger(), "please install CUDA, CUDNN and TensorRT to use cnn classifier");
 #endif
   }
+
+  diagnostics_interface_ptr_ =
+    std::make_unique<autoware_utils::DiagnosticsInterface>(this, "traffic_light_classifier");
 }
 
 void TrafficLightClassifierNodelet::connectCb()
@@ -165,6 +169,17 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
 
   output_msg.header = input_image_msg->header;
   traffic_signal_array_pub_->publish(output_msg);
+
+  // publish diagnostics
+  diagnostics_interface_ptr_->clear();
+  bool found_harsh_backlight = backlight_indices.size() != 0;
+  diagnostics_interface_ptr_->add_key_value("found_harsh_backlight", found_harsh_backlight);
+  if (found_harsh_backlight) {
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN,
+      "Found harsh backlight in ROI(s) and corresponding ROI(s) were overwritten by UNKNOWN");
+  }
+  diagnostics_interface_ptr_->publish(output_msg.header.stamp);
 }
 
 bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) const

perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.hpp

@@ -48,6 +48,7 @@
 
 #include "classifier/color_classifier.hpp"
 
+#include <autoware_utils/ros/diagnostics_interface.hpp>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 
@@ -89,6 +90,9 @@ class TrafficLightClassifierNodelet : public rclcpp::Node
     traffic_signal_array_pub_;
   std::shared_ptr<ClassifierInterface> classifier_ptr_;
 
+  std::unique_ptr<autoware_utils::DiagnosticsInterface>
+    diagnostics_interface_ptr_;  //!< Diagnostic handler.
+
   double backlight_threshold_;
   bool is_harsh_backlight(const cv::Mat & img) const;
 };