feat(object_merger): add parameters loading

Author: lei.gu

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/src/object_association_merger_node.cpp

@@ -12,6 +12,7 @@
 // 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"
@@ -131,7 +132,7 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio
   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);
+  message_timeout_sec_ = this->declare_parameter<double>("message_timeout_sec", 0.15);
   last_sync_time_ = this->now();
   timeout_sent_ = false;
   timeout_timer_ = this->create_wall_timer(
@@ -152,11 +153,13 @@ 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_->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;
@@ -264,17 +267,16 @@ 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("timeout_occurred", true);
     diagnostics_interface_ptr_->add_key_value("messages_interval", (now - last_sync_time_).seconds() );
-    
-    
     std::stringstream message;
-    message << "ObjectAssociationMergerNode Timeout: Merging messages not received";
+    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;
-  }
+  } 
 }