feat(pointcloud preprocessor): add hysteresis to concat diag (#1655)

* 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 <[email protected]> * fix(pointcloud_preprocessor): typo consecutive_concatenate_failures (#1310) --------- Signed-off-by: kotaro-hihara <[email protected]> Co-authored-by: Shinnosuke Hirakawa <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <[email protected]>
tier4 · Dec 1, 2024 · f03e037 · f03e037
1 parent 3803cfc
commit f03e037
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 18 deletions.
diff --git a/...or/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/...or/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);
 

diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
@@ -218,9 +218,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);
   }
 }
 
@@ -462,7 +461,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) {
@@ -712,21 +711,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