diff --git a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp index 071e665ece6ec..bbca397fb47ec 100644 --- a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp +++ b/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 -#include #include +#include + #include #include #include @@ -64,27 +64,12 @@ class DummyDiagPublisher : public rclcpp::Node std::optional 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 & 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::SharedPtr pub_; }; #endif // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index e46c6e4341284..3432aba0b486b 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -14,22 +14,9 @@ #include "dummy_diag_publisher/dummy_diag_publisher_core.hpp" -#include - #define FMT_HEADER_ONLY -#include -#include -#include -#include - #include -#include -#include -#include -#include -#include - namespace { std::vector split(const std::string & str, const char delim) @@ -44,82 +31,6 @@ std::vector split(const std::string & str, const char delim) } } // namespace -rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::paramCallback( - const std::vector & 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 { - autoware::universe_utils::updateParam(parameters, status_prefix_str, status_str); - autoware::universe_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::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,60 +115,22 @@ 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); } rclcpp::NodeOptions override_options(rclcpp::NodeOptions options) @@ -258,26 +146,19 @@ DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) // 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("/diagnostics", rclcpp::QoS(1)); } #include