feat(object_merger): add diagnostics warning for timeout when merge messages are incomplete

Author: lei.gu

perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp

@@ -23,7 +23,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
-
+#include <autoware_utils/ros/diagnostics_interface.hpp>
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/synchronizer.h>
@@ -57,6 +57,8 @@ class ObjectAssociationMergerNode : public rclcpp::Node
   void objectsCallback(
     const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects0_msg,
     const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects1_msg);
+  
+  void timeoutCallback();
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
@@ -73,6 +75,14 @@ class ObjectAssociationMergerNode : public rclcpp::Node
   std::unique_ptr<DataAssociation> data_association_;
   std::string base_link_frame_id_;  // associated with the base_link frame
 
+  // Timeout Related
+  double message_timeout_sec_;
+  rclcpp::Time last_sync_time_;
+  bool timeout_sent_;
+  rclcpp::TimerBase::SharedPtr timeout_timer_;
+  std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
+
+
   PriorityMode priority_mode_;
   bool remove_overlapped_unknown_objects_;
   struct

perception/autoware_object_merger/src/object_association_merger_node.cpp

@@ -130,6 +130,16 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(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);
+  // Timeout process initialization 
+  message_timeout_sec_ = this->declare_parameter<double>("message_timeout_sec", 0.001);
+  last_sync_time_ = this->now();
+  timeout_sent_ = false;
+  timeout_timer_ = this->create_wall_timer(
+    std::chrono::duration<double>(message_timeout_sec_),
+    std::bind(&ObjectAssociationMergerNode::timeoutCallback, this));
+  diagnostics_interface_ptr_ =
+    std::make_unique<autoware_utils::DiagnosticsInterface>(this, "object_association_merger");
+
 }
 
 void ObjectAssociationMergerNode::objectsCallback(
@@ -142,6 +152,15 @@ void ObjectAssociationMergerNode::objectsCallback(
   }
   stop_watch_ptr_->toc("processing_time", true);
 
+
+  // If messages normally synced
+  diagnostics_interface_ptr_->clear();
+  diagnostics_interface_ptr_->add_key_value("are_merge_messages_synced", true);
+  diagnostics_interface_ptr_->add_key_value("messages_interval", (this->now() - last_sync_time_).seconds() );
+  diagnostics_interface_ptr_->publish(input_objects0_msg->header.stamp);
+  last_sync_time_ = this->now();
+  timeout_sent_ = false;
+
   /* transform to base_link coordinate */
   autoware_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1;
   if (
@@ -238,6 +257,27 @@ void ObjectAssociationMergerNode::objectsCallback(
   processing_time_publisher_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
     "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
 }
+
+
+void ObjectAssociationMergerNode::timeoutCallback()
+{
+  rclcpp::Time now = this->now();
+  if ((now - last_sync_time_).seconds() >= message_timeout_sec_ && !timeout_sent_) {
+    diagnostics_interface_ptr_->clear();
+    diagnostics_interface_ptr_->add_key_value("are_merge_messages_synced", false);
+    diagnostics_interface_ptr_->add_key_value("messages_interval", (now - last_sync_time_).seconds() );
+    
+    
+    std::stringstream message;
+    message << "ObjectAssociationMergerNode Timeout: Merging messages not received";
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+    diagnostics_interface_ptr_->publish(now);
+    timeout_sent_ = true;
+  }
+}
+
+
 }  // namespace autoware::object_merger
 
 #include <rclcpp_components/register_node_macro.hpp>