Merge pull request #1272 from tier4/fix/dummy_diag

fix(dummy_diag_publisher): not use diagnostic_updater and param callback

Author: ito-san

system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp

@@ -15,10 +15,10 @@
 #ifndef DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_
 #define DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_
 
-#include <diagnostic_updater/diagnostic_updater.hpp>
-#include <rcl_interfaces/msg/detail/set_parameters_result__struct.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
 #include <optional>
 #include <string>
 #include <vector>
@@ -64,27 +64,12 @@ class DummyDiagPublisher : public rclcpp::Node
 
   std::optional<Status> convertStrToStatus(std::string & status_str);
   std::string convertStatusToStr(const Status & status);
+  diagnostic_msgs::msg::DiagnosticStatus::_level_type convertStatusToLevel(const Status & status);
 
-  // Dynamic Reconfigure
-  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
-  rcl_interfaces::msg::SetParametersResult paramCallback(
-    const std::vector<rclcpp::Parameter> & parameters);
-
-  rcl_interfaces::msg::SetParametersResult updateDiag(
-    const std::string diag_name, DummyDiagConfig & config, bool is_active, const Status status);
-
-  // Diagnostic Updater
-  // Set long period to reduce automatic update
-  diagnostic_updater::Updater updater_{this, 1000.0 /* sec */};
-
-  void produceOKDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  void produceErrorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  void produceWarnDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  void produceStaleDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  void addDiagByStatus(const std::string & diag_name, const Status status);
   // Timer
   void onTimer();
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr pub_;
 };
 
 #endif  // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

@@ -14,22 +14,9 @@
 
 #include "dummy_diag_publisher/dummy_diag_publisher_core.hpp"
 
-#include <rcl_interfaces/msg/detail/set_parameters_result__struct.hpp>
-
 #define FMT_HEADER_ONLY
-#include <diagnostic_updater/diagnostic_updater.hpp>
-#include <rcl_interfaces/msg/set_parameters_result.hpp>
-#include <rclcpp/create_timer.hpp>
-#include <tier4_autoware_utils/ros/update_param.hpp>
-
 #include <fmt/format.h>
 
-#include <memory>
-#include <stdexcept>
-#include <string>
-#include <utility>
-#include <vector>
-
 namespace
 {
 std::vector<std::string> split(const std::string & str, const char delim)
@@ -44,82 +31,6 @@ std::vector<std::string> split(const std::string & str, const char delim)
 }
 }  // namespace
 
-rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::paramCallback(
-  const std::vector<rclcpp::Parameter> & parameters)
-{
-  rcl_interfaces::msg::SetParametersResult result;
-
-  for (const auto & param : parameters) {
-    const auto param_name = param.get_name();
-    const auto split_names = split(param_name, '.');
-    const auto & diag_name = split_names.at(0);
-    auto it = std::find_if(
-      std::begin(required_diags_), std::end(required_diags_),
-      [&diag_name](DummyDiagConfig config) { return config.name == diag_name; });
-    if (it == std::end(required_diags_)) {  // diag name not found
-      result.successful = false;
-      result.reason = "no matching diag name";
-    } else {  // diag name found
-      const auto status_prefix_str = diag_name + std::string(".status");
-      const auto is_active_prefix_str = diag_name + std::string(".is_active");
-      auto status_str = convertStatusToStr(it->status);
-      auto prev_status_str = status_str;
-      auto is_active = true;
-      try {
-        tier4_autoware_utils::updateParam(parameters, status_prefix_str, status_str);
-        tier4_autoware_utils::updateParam(parameters, is_active_prefix_str, is_active);
-      } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
-        result.successful = false;
-        result.reason = e.what();
-        return result;
-      }
-      const auto status = convertStrToStatus(status_str);
-      if (!status) {
-        result.successful = false;
-        result.reason = "invalid status";
-        return result;
-      }
-      result = updateDiag(diag_name, *it, is_active, *status);
-    }  // end diag name found
-  }
-  return result;
-}
-
-// update diag with new param
-rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::updateDiag(
-  const std::string diag_name, DummyDiagConfig & config, bool is_active, const Status status)
-{
-  rcl_interfaces::msg::SetParametersResult result;
-  result.successful = true;
-  result.reason = "success";
-
-  if (is_active == config.is_active && config.status == status) {  // diag config not changed
-    result.successful = true;
-    result.reason = "config not changed";
-  } else if (is_active == true && config.is_active == false) {  // newly activated
-    config.is_active = true;
-    if (config.status == status) {  // status not changed
-      addDiagByStatus(diag_name, config.status);
-    } else {  // status changed
-      config.status = status;
-      addDiagByStatus(diag_name, status);
-    }
-  } else {  // deactivated or status changed
-    if (!updater_.removeByName(diag_name)) {
-      result.successful = false;
-      result.reason = "updater removal failed";
-      return result;
-    }
-    if (is_active == false) {  // deactivated
-      config.is_active = false;
-    } else {  // status changed
-      config.status = status;
-      addDiagByStatus(diag_name, status);
-    }
-  }
-  return result;
-}
-
 std::optional<DummyDiagPublisher::Status> DummyDiagPublisher::convertStrToStatus(
   std::string & status_str)
 {
@@ -147,6 +58,21 @@ std::string DummyDiagPublisher::convertStatusToStr(const Status & status)
   }
 }
 
+diagnostic_msgs::msg::DiagnosticStatus::_level_type DummyDiagPublisher::convertStatusToLevel(
+  const Status & status)
+{
+  switch (status) {
+    case Status::OK:
+      return diagnostic_msgs::msg::DiagnosticStatus::OK;
+    case Status::WARN:
+      return diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    case Status::ERROR:
+      return diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    default:
+      return diagnostic_msgs::msg::DiagnosticStatus::STALE;
+  }
+}
+
 void DummyDiagPublisher::loadRequiredDiags()
 {
   const auto param_key = std::string("required_diags");
@@ -189,91 +115,44 @@ void DummyDiagPublisher::loadRequiredDiags()
   }
 }
 
-void DummyDiagPublisher::produceOKDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
-{
-  diagnostic_msgs::msg::DiagnosticStatus status;
-
-  status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  status.message = diag_config_.msg_ok;
-
-  stat.summary(status.level, status.message);
-}
-void DummyDiagPublisher::produceWarnDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
-{
-  diagnostic_msgs::msg::DiagnosticStatus status;
-
-  status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-  status.message = diag_config_.msg_warn;
-
-  stat.summary(status.level, status.message);
-}
-void DummyDiagPublisher::produceErrorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
-{
-  diagnostic_msgs::msg::DiagnosticStatus status;
-
-  status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
-  status.message = diag_config_.msg_error;
-
-  stat.summary(status.level, status.message);
-}
-void DummyDiagPublisher::produceStaleDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
-{
-  diagnostic_msgs::msg::DiagnosticStatus status;
-
-  status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
-  status.message = diag_config_.msg_stale;
-
-  stat.summary(status.level, status.message);
-}
-void DummyDiagPublisher::addDiagByStatus(const std::string & diag_name, const Status status)
-{
-  if (status == Status::OK) {
-    updater_.add(diag_name, this, &DummyDiagPublisher::produceOKDiagnostics);
-  } else if (status == Status::WARN) {
-    updater_.add(diag_name, this, &DummyDiagPublisher::produceWarnDiagnostics);
-  } else if (status == Status::ERROR) {
-    updater_.add(diag_name, this, &DummyDiagPublisher::produceErrorDiagnostics);
-  } else if (status == Status::STALE) {
-    updater_.add(diag_name, this, &DummyDiagPublisher::produceStaleDiagnostics);
-  } else {
-    throw std::runtime_error("invalid status");
-  }
-}
-
 void DummyDiagPublisher::onTimer()
 {
-  updater_.force_update();
+  diagnostic_msgs::msg::DiagnosticArray array;
+
+  for (const auto & e : required_diags_) {
+    if (e.is_active) {
+      diagnostic_msgs::msg::DiagnosticStatus status;
+      status.hardware_id = diag_config_.hardware_id;
+      status.name = e.name;
+      status.message = convertStatusToStr(e.status);
+      status.level = convertStatusToLevel(e.status);
+      array.status.push_back(status);
+    }
+  }
+  array.header.stamp = this->now();
+  pub_->publish(array);
 }
 
 DummyDiagPublisher::DummyDiagPublisher()
 : Node(
     "dummy_diag_publisher", rclcpp::NodeOptions()
                               .allow_undeclared_parameters(true)
-                              .automatically_declare_parameters_from_overrides(true)),
-  updater_(this)
-
+                              .automatically_declare_parameters_from_overrides(true))
 {
   // Parameter
   update_rate_ = this->get_parameter_or("update_rate", 10.0);
 
-  // Set parameter callback
-  set_param_res_ = this->add_on_set_parameters_callback(
-    std::bind(&DummyDiagPublisher::paramCallback, this, std::placeholders::_1));
-
   // Diagnostic Updater
   loadRequiredDiags();
 
   const std::string hardware_id = "dummy_diag";
-  updater_.setHardwareID(hardware_id);
   diag_config_ = DiagConfig{hardware_id, "OK", "Warn", "Error", "Stale"};
-  for (const auto & config : required_diags_) {
-    if (config.is_active) {
-      addDiagByStatus(config.name, config.status);
-    }
-  }
 
   // Timer
   const auto period_ns = rclcpp::Rate(update_rate_).period();
   timer_ = rclcpp::create_timer(
     this, get_clock(), period_ns, std::bind(&DummyDiagPublisher::onTimer, this));
+
+  // Publisher
+  pub_ = create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", rclcpp::QoS(1));
 }