feat(pointcloud preprocessor): add hysteresis to diag (#1309)

* remove diagnostics_updater

* add counter

* change node name

* rename

* style(pre-commit): autofix

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: kotaro-hihara <kotaro.hihara@tier4.jp>

Author: 2 people

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

@@ -65,9 +65,9 @@
 
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
-#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.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,9 @@ 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();
+  int consecutive_concatenate_failures{0};
+
   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);
   }
 }
 
@@ -455,7 +454,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) {
@@ -676,21 +675,45 @@ 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);
+  }
+
+  if (not_subscribed_topic_names_.size() > 0) {
+    consecutive_concatenate_failures += 1;
+  } else {
+    consecutive_concatenate_failures = 0;
   }
 
-  const int8_t level = not_subscribed_topic_names_.empty()
-                         ? diagnostic_msgs::msg::DiagnosticStatus::OK
-                         : diagnostic_msgs::msg::DiagnosticStatus::WARN;
-  const std::string message = not_subscribed_topic_names_.empty()
-                                ? "Concatenate all topics"
-                                : "Some topics are not concatenated";
-  stat.summary(level, message);
+  {
+    diagnostic_msgs::msg::KeyValue key_value_msg;
+    key_value_msg.key = "consecutiveConcatenateFailures";
+    key_value_msg.value = std::to_string(consecutive_concatenate_failures_);
+    diag_status_msg.values.push_back(key_value_msg);
+  }
+
+  if (consecutive_concatenate_failures > 1) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message = "Some topics are not concatenated";
+  } else {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    diag_status_msg.message = "Concatenate all topics";
+  }
+
+  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