remove diagnostics_updater

Author: 0x126

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

@@ -63,11 +63,11 @@
 // ROS includes
 #include "autoware_point_types/types.hpp"
 
-#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
@@ -138,7 +138,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
 
   rclcpp::TimerBase::SharedPtr timer_;
-  diagnostic_updater::Updater updater_{this};
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
 
   const std::string input_twist_topic_type_;
 
@@ -180,7 +180,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input);
   void timer_callback();
 
-  void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void checkConcatStatus();
   std::string replaceSyncTopicNamePostfix(
     const std::string & original_topic_name, const std::string & postfix);
 

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

@@ -211,9 +211,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
 
   // Diagnostic Updater
   {
-    updater_.setHardwareID("concatenate_data_checker");
-    updater_.add(
-      "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus);
+    diagnostics_pub_ =
+      this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
   }
 }
 
@@ -463,7 +462,7 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
     }
   }
 
-  updater_.force_update();
+  checkConcatStatus();
 
   cloud_stdmap_ = cloud_stdmap_tmp_;
   std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) {
@@ -684,12 +683,17 @@ void PointCloudConcatenateDataSynchronizerComponent::odom_callback(
   twist_ptr_queue_.push_back(twist_ptr);
 }
 
-void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus(
-  diagnostic_updater::DiagnosticStatusWrapper & stat)
+void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus()
 {
+  diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
+  diag_status_msg.name = std::string(this->get_name()) + ": concat_status";
+  diag_status_msg.hardware_id = "concatenate_data_checker";
+
   for (const std::string & e : input_topics_) {
-    const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK";
-    stat.add(e, subscribe_status);
+    diagnostic_msgs::msg::KeyValue key_value_msg;
+    key_value_msg.key = e;
+    key_value_msg.value = not_subscribed_topic_names_.count(e) ? "NG" : "OK";
+    diag_status_msg.values.push_back(key_value_msg);
   }
 
   const int8_t level = not_subscribed_topic_names_.empty()
@@ -698,7 +702,15 @@ void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus(
   const std::string message = not_subscribed_topic_names_.empty()
                                 ? "Concatenate all topics"
                                 : "Some topics are not concatenated";
-  stat.summary(level, message);
+  diag_status_msg.level = level;
+  diag_status_msg.message = message;
+
+  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  diag_msg.header.stamp = this->now();
+  diag_msg.status.push_back(diag_status_msg);
+
+  diagnostics_pub_->publish(diag_msg);
+
 }
 }  // namespace pointcloud_preprocessor