add diagnostics

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

Author: MasatoSaeki

perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp

@@ -18,6 +18,7 @@
 #include "autoware/tracking_object_merger/association/data_association.hpp"
 #include "autoware/tracking_object_merger/utils/tracker_state.hpp"
 #include "autoware/tracking_object_merger/utils/utils.hpp"
+#include "autoware/universe_utils/ros/diagnostics_interface.hpp"
 #include "autoware_utils/ros/debug_publisher.hpp"
 #include "autoware_utils/ros/published_time_publisher.hpp"
 #include "autoware_utils/system/stop_watch.hpp"
@@ -129,6 +130,9 @@ class DecorativeTrackerMergerNode : public rclcpp::Node
     bool enable = false;
     std::string path;
   } logging_;
+
+  // diagnostics
+  std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
 };
 
 }  // namespace autoware::tracking_object_merger

perception/autoware_tracking_object_merger/package.xml

@@ -15,7 +15,7 @@
 
   <depend>autoware_object_recognition_utils</depend>
   <depend>autoware_perception_msgs</depend>
-  <depend>autoware_utils</depend>
+  <depend>autoware_universe_utils</depend>
   <depend>eigen</depend>
   <depend>glog</depend>
   <depend>mussp</depend>

perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp

@@ -20,6 +20,8 @@
 #include "autoware/tracking_object_merger/association/solver/ssp.hpp"
 #include "autoware/tracking_object_merger/utils/utils.hpp"
 
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
@@ -168,6 +170,10 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio
   stop_watch_ptr_->tic("cyclic_time");
   stop_watch_ptr_->tic("processing_time");
   published_time_publisher_ = std::make_unique<autoware_utils::PublishedTimePublisher>(this);
+
+  // diagnostics
+  diagnostics_interface_ptr_ =
+    std::make_unique<autoware::universe_utils::DiagnosticsInterface>(this, "decorative_object_merger_node");
 }
 
 void DecorativeTrackerMergerNode::set3dDataAssociation(
@@ -200,6 +206,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
   const TrackedObjects::ConstSharedPtr & main_objects)
 {
   stop_watch_ptr_->toc("processing_time", true);
+  diagnostics_interface_ptr_->clear();
 
   /* transform to target merge coordinate */
   TrackedObjects transformed_objects;
@@ -210,6 +217,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
   TrackedObjects::ConstSharedPtr transformed_main_objects =
     std::make_shared<TrackedObjects>(transformed_objects);
 
+  bool is_existing_sub_objects = true;
   // try to merge sub object
   if (!sub_objects_buffer_.empty()) {
     // get interpolated sub objects
@@ -236,6 +244,16 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
     } else {
       RCLCPP_DEBUG(this->get_logger(), "interpolated_sub_objects is null");
     }
+  } else {
+    is_existing_sub_objects = false;
+  }
+
+  diagnostics_interface_ptr_->add_key_value("is_existing_sub_objects", is_existing_sub_objects);
+  if (!is_existing_sub_objects) {
+    std::stringstream message;
+    message << "Sub object is empty, so only main object is published";
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
   }
 
   // try to merge main object
@@ -249,6 +267,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
     "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
   processing_time_publisher_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
     "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
+  diagnostics_interface_ptr_->publish(tracked_objects.header.stamp);
 }
 
 /**
@@ -259,6 +278,10 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
  */
 void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::ConstSharedPtr & msg)
 {
+  if (msg->objects.empty()) {
+    return;
+  }
+
   /* transform to target merge coordinate */
   TrackedObjects transformed_objects;
   if (!autoware::object_recognition_utils::transformObjects(