feat: add diagnostics handler to warn when output pointcloud is empty

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

Author: ktro2828

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp

@@ -79,6 +79,7 @@
 #include <autoware_utils/ros/managed_transform_buffer.hpp>
 #include <autoware_utils/ros/published_time_publisher.hpp>
 #include <autoware_utils/system/stop_watch.hpp>
+#include <autoware_utils_diagnostics/diagnostics_interface.hpp>
 
 namespace autoware::pointcloud_preprocessor
 {
@@ -180,6 +181,9 @@ class Filter : public rclcpp::Node
   std::unique_ptr<autoware_utils::DebugPublisher> debug_publisher_;
   std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
+  /** \brief Diagnostics handler. **/
+  std::unique_ptr<autoware_utils_diagnostics::DiagnosticsInterface> diagnostics_interface_ptr_;
+
   /** \brief Virtual abstract filter method. To be implemented by every child.
    * \param input the input point cloud dataset.
    * \param indices a pointer to the vector of point indices to use.

sensing/autoware_pointcloud_preprocessor/src/filter.cpp

@@ -55,6 +55,8 @@
 
 #include <pcl_ros/transforms.hpp>
 
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
 #include <pcl/io/io.h>
 
 #include <memory>
@@ -107,6 +109,10 @@ autoware::pointcloud_preprocessor::Filter::Filter(
   set_param_res_filter_ = this->add_on_set_parameters_callback(
     std::bind(&Filter::filterParamCallback, this, std::placeholders::_1));
 
+  // Set diagnostics
+  diagnostics_interface_ptr_ =
+    std::make_unique<autoware_utils_diagnostics::DiagnosticsInterface>(this, filter_name);
+
   published_time_publisher_ = std::make_unique<autoware_utils::PublishedTimePublisher>(this);
   RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created.");
 }
@@ -205,6 +211,14 @@ void autoware::pointcloud_preprocessor::Filter::computePublish(
   // Copy timestamp to keep it
   output->header.stamp = input->header.stamp;
 
+  diagnostics_interface_ptr_->clear();
+  if (bool is_output_emtpy = output->data.empty(); is_output_emtpy) {
+    diagnostics_interface_ptr_->add_key_value("is_pointcloud_empty", is_output_emtpy);
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, "Output pointcloud is empty.");
+    diagnostics_interface_ptr_->publish(output->header.stamp);
+  }
+
   // Publish a boost shared ptr
   pub_output_->publish(std::move(output));
   published_time_publisher_->publish_if_subscribed(pub_output_, input->header.stamp);
@@ -440,6 +454,14 @@ void autoware::pointcloud_preprocessor::Filter::faster_input_indices_callback(
 
   if (!convert_output_costly(output)) return;
 
+  diagnostics_interface_ptr_->clear();
+  if (bool is_output_emtpy = output->data.empty(); is_output_emtpy) {
+    diagnostics_interface_ptr_->add_key_value("is_pointcloud_empty", is_output_emtpy);
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, "Output pointcloud is empty.");
+    diagnostics_interface_ptr_->publish(output->header.stamp);
+  }
+
   output->header.stamp = cloud->header.stamp;
   pub_output_->publish(std::move(output));
   published_time_publisher_->publish_if_subscribed(pub_output_, cloud->header.stamp);