feat(object_merger): add diagnostics warning for timeout when merging

perception/autoware_object_merger/config/object_association_merger.param.yaml

@@ -6,3 +6,4 @@
     remove_overlapped_unknown_objects: true
     base_link_frame_id: base_link
     priority_mode: 2  # PriorityMode::Confidence
+    message_timeout_sec: 0.15

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

@@ -20,6 +20,7 @@
 #include "autoware_utils/ros/published_time_publisher.hpp"
 #include "autoware_utils/system/stop_watch.hpp"
 
+#include <autoware_utils/ros/diagnostics_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
@@ -58,6 +59,8 @@ class ObjectAssociationMergerNode : public rclcpp::Node
     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_;
   rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr merged_object_pub_;
@@ -73,6 +76,13 @@ 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/schema/object_association_merger.schema.json

@@ -36,6 +36,11 @@
           "description": "Index for the priority_mode.",
           "default": 3,
           "enum": [0, 1, 2, 3]
+        },
+        "message_timeout_sec": {
+          "type": "number",
+          "description": "The warning timeout threshold for no synchronized messages received.",
+          "default": 0.15
         }
       },
       "required": [

perception/autoware_object_merger/src/object_association_merger_node.cpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <type_traits>
 #define EIGEN_MPL2_ONLY
 
 #include "autoware/object_merger/object_association_merger_node.hpp"
-
 #include "autoware/object_recognition_utils/object_recognition_utils.hpp"
 #include "autoware_utils/geometry/geometry.hpp"
 
@@ -130,6 +130,15 @@
   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");
+  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 +151,18 @@
   }
   stop_watch_ptr_->toc("processing_time", true);
 
+  // If messages normally synced
+  diagnostics_interface_ptr_->clear();
+  diagnostics_interface_ptr_->add_key_value("timeout_occurred", false);
+  diagnostics_interface_ptr_->add_key_value(
+    "messages_interval", (this->now() - last_sync_time_).seconds());
+  diagnostics_interface_ptr_->update_level_and_message(
+    diagnostic_msgs::msg::DiagnosticStatus::OK,
+    "ObjectAssociationMergerNode OK: Messages merged successfully");
+  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 +259,25 @@
   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("timeout_occurred", true);
+    diagnostics_interface_ptr_->add_key_value(
+      "messages_interval", (now - last_sync_time_).seconds());
+    std::stringstream message;
+    message << "Timeout occurred: No synchronized messages received within " << message_timeout_sec_
+            << "s.";
+    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>