From a18a61ed7a4c843855b797f991b4d42a6e5022f1 Mon Sep 17 00:00:00 2001 From: asana17 <62983821+asana17@users.noreply.github.com> Date: Thu, 16 Feb 2023 17:15:32 +0900 Subject: [PATCH] feat(dummy diag publisher): change diag name specification method to YAML (#2840) * Signed-off-by: asana17 modified dummy_diag_publisher to use YAML for param * Signed-off-by: asana17 use YAML param for dummy_diag_publisher * fix empty param Signed-off-by: asana17 * fixed empty param Signed-off-by: asana17 * fix spelling Signed-off-by: asana17 * add pkg maintainer Signed-off-by: asana17 * launch dummy_diag_publisher by launch_dummy_diag_publisher param Signed-off-by: asana17 --------- Signed-off-by: asana17 --- .../launch/system.launch.xml | 19 ++ system/dummy_diag_publisher/CMakeLists.txt | 11 +- system/dummy_diag_publisher/README.md | 31 +- .../config/_empty.param.yaml | 15 + .../config/dummy_diag_publisher.param.yaml | 32 ++ .../config/extra.param.yaml | 16 + ...node.hpp => dummy_diag_publisher_core.hpp} | 38 ++- .../launch/dummy_diag_publisher.launch.xml | 47 +-- .../dummy_diag_publisher_node.launch.xml | 12 +- system/dummy_diag_publisher/package.xml | 3 + .../src/dummy_diag_publisher_core.cpp | 276 ++++++++++++++++++ .../src/dummy_diag_publisher_node.cpp | 26 ++ .../dummy_diag_publisher_node.cpp | 105 ------- 13 files changed, 455 insertions(+), 176 deletions(-) create mode 100644 system/dummy_diag_publisher/config/_empty.param.yaml create mode 100644 system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml create mode 100644 system/dummy_diag_publisher/config/extra.param.yaml rename system/dummy_diag_publisher/include/dummy_diag_publisher/{dummy_diag_publisher_node.hpp => dummy_diag_publisher_core.hpp} (54%) create mode 100644 system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp create mode 100644 system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp delete mode 100644 system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 49682a9daba54..2428a602ee31e 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -1,12 +1,19 @@ + + + + + + + @@ -69,4 +76,16 @@ + + + + + + + + + + + + diff --git a/system/dummy_diag_publisher/CMakeLists.txt b/system/dummy_diag_publisher/CMakeLists.txt index 936d6bc1e4708..cecb317bf9c34 100644 --- a/system/dummy_diag_publisher/CMakeLists.txt +++ b/system/dummy_diag_publisher/CMakeLists.txt @@ -4,15 +4,12 @@ project(dummy_diag_publisher) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(dummy_diag_publisher SHARED - src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp -) - -rclcpp_components_register_node(dummy_diag_publisher - PLUGIN "DummyDiagPublisherNode" - EXECUTABLE dummy_diag_publisher_node +ament_auto_add_executable(${PROJECT_NAME} + src/dummy_diag_publisher_node.cpp + src/dummy_diag_publisher_core.cpp ) ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/system/dummy_diag_publisher/README.md b/system/dummy_diag_publisher/README.md index 0c424b9d0699d..9704cc4af9bc5 100644 --- a/system/dummy_diag_publisher/README.md +++ b/system/dummy_diag_publisher/README.md @@ -16,12 +16,22 @@ This package outputs a dummy diagnostic data for debugging and developing. ### Node Parameters -| Name | Type | Default Value | Explanation | Reconfigurable | -| ------------- | ------------------------------ | ------------- | --------------------------------------- | -------------- | -| `update_rate` | int | `10` | Timer callback period [Hz] | false | -| `diag_name` | string | `diag_name` | Diag_name set by dummy diag publisher | false | -| `is_active` | bool | `true` | Force update or not | true | -| `status` | DummyDiagPublisherNode::Status | `0 (OK)` | diag status set by dummy diag publisher | true | +The parameter `DIAGNOSTIC_NAME` must be a name that exists in the parameter YAML file. If the parameter `status` is given from a command line, the parameter `is_active` is automatically set to `true`. + +| Name | Type | Default Value | Explanation | Reconfigurable | +| --------------------------- | ------ | ------------- | --------------------------------------- | -------------- | +| `update_rate` | int | `10` | Timer callback period [Hz] | false | +| `DIAGNOSTIC_NAME.is_active` | bool | `true` | Force update or not | true | +| `DIAGNOSTIC_NAME.status` | string | `"OK"` | diag status set by dummy diag publisher | true | + +### YAML format for dummy_diag_publisher + +If the value is `default`, the default value will be set. + +| Key | Type | Default Value | Explanation | +| ------------------------------------------ | ------ | ------------- | --------------------------------------- | +| `required_diags.DIAGNOSTIC_NAME.is_active` | bool | `true` | Force update or not | +| `required_diags.DIAGNOSTIC_NAME.status` | string | `"OK"` | diag status set by dummy diag publisher | ## Assumptions / Known limits @@ -29,6 +39,15 @@ TBD. ## Usage +### launch + ```sh ros2 launch dummy_diag_publisher dummy_diag_publisher.launch.xml ``` + +### reconfigure + +```sh +ros2 param set /dummy_diag_publisher velodyne_connection.status "Warn" +ros2 param set /dummy_diag_publisher velodyne_connection.is_active true +``` diff --git a/system/dummy_diag_publisher/config/_empty.param.yaml b/system/dummy_diag_publisher/config/_empty.param.yaml new file mode 100644 index 0000000000000..cf288a4874224 --- /dev/null +++ b/system/dummy_diag_publisher/config/_empty.param.yaml @@ -0,0 +1,15 @@ +# Description: +# name: diag name +# is_active: Force update or not +# status: diag status set by dummy diag publisher "OK, Warn, Error, Stale" +# +# Note: +# +# default values are: +# is_active: "true" +# status: "OK" +--- +/**: + ros__parameters: + required_diags: + dummy_diag_empty: default diff --git a/system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml b/system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml new file mode 100644 index 0000000000000..80861af7fc90e --- /dev/null +++ b/system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml @@ -0,0 +1,32 @@ +# Description: +# name: diag name +# is_active: Force update or not +# status: diag status set by dummy diag publisher "OK, Warn, Error, Stale" +# +# Note: +# +# default values are: +# is_active: "true" +# status: "OK" +--- +/**: + ros__parameters: + required_diags: + velodyne_connection: {is_active: "false", status: "Error"} + + velodyne_temperature: {is_active: "true", status: "Warn"} + + livox_ptp_signal: default + + camera_connection: {is_active: "true", status: "Stale"} + + + lane_departure: default + + obstacle_crash: default + + vehicle_errors: default + + can_bus_connection: default + + speaker: default diff --git a/system/dummy_diag_publisher/config/extra.param.yaml b/system/dummy_diag_publisher/config/extra.param.yaml new file mode 100644 index 0000000000000..1b4ca3ec6322a --- /dev/null +++ b/system/dummy_diag_publisher/config/extra.param.yaml @@ -0,0 +1,16 @@ +# Description: +# name: diag name +# is_active: Force update or not +# status: diag status set by dummy diag publisher "OK, Warn, Error, Stale" +# +# Note: +# +# default values are: +# is_active: "true" +# status: "OK" +--- +/**: + ros__parameters: + required_diags: + camera_connection: {is_active: "true", status: "Warn"} + extra: default diff --git a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_node.hpp b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp similarity index 54% rename from system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_node.hpp rename to system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp index 402efd6856e50..8398c15b8e8f6 100644 --- a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_node.hpp +++ b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_ -#define DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_ +#ifndef DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ +#define DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ #include +#include #include +#include #include #include struct DiagConfig { - std::string name; std::string hardware_id; std::string msg_ok; std::string msg_warn; @@ -31,10 +32,10 @@ struct DiagConfig std::string msg_stale; }; -class DummyDiagPublisherNode : public rclcpp::Node +class DummyDiagPublisher : public rclcpp::Node { public: - explicit DummyDiagPublisherNode(const rclcpp::NodeOptions & node_options); + DummyDiagPublisher(); private: enum Status { @@ -44,31 +45,46 @@ class DummyDiagPublisherNode : public rclcpp::Node STALE, }; - struct DummyDiagPublisherConfig + struct DummyDiagConfig { - Status status; + std::string name; bool is_active; + Status status; }; + using RequiredDiags = std::vector; + // Parameter double update_rate_; DiagConfig diag_config_; - DummyDiagPublisherConfig config_; + DummyDiagConfig config_; + + RequiredDiags required_diags_; + void loadRequiredDiags(); + + std::optional convertStrToStatus(std::string & status_str); + std::string convertStatusToStr(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 produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); - + 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_; }; -#endif // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_NODE_HPP_ +#endif // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml index ee42dded6792d..ae4436fdc9a6d 100644 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml @@ -1,4 +1,8 @@ + + + + @@ -22,46 +26,9 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml index d795f7b191517..8e71ce37543c2 100644 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml @@ -1,15 +1,13 @@ - + + - - - - + + + - - diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml index e466727a583d9..191facf6e3777 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/dummy_diag_publisher/package.xml @@ -5,6 +5,8 @@ 0.1.0 The dummy_diag_publisher ROS2 package Kenji Miyake + Akihiro Sakurai + Fumihito Ito Apache License 2.0 ament_cmake_auto @@ -12,6 +14,7 @@ autoware_cmake diagnostic_updater + fmt rclcpp rclcpp_components tier4_autoware_utils diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp new file mode 100644 index 0000000000000..21e0aa3283a26 --- /dev/null +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -0,0 +1,276 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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) +{ + std::vector elems; + std::stringstream ss(str); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + return elems; +} +} // 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 { + 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::convertStrToStatus( + std::string & status_str) +{ + static std::unordered_map const table = { + {"OK", Status::OK}, {"Warn", Status::WARN}, {"Error", Status::ERROR}, {"Stale", Status::STALE}}; + + auto it = table.find(status_str); + Status status; + if (it != table.end()) { + status = it->second; + return status; + } + return {}; +} +std::string DummyDiagPublisher::convertStatusToStr(const Status & status) +{ + if (status == Status::OK) { + return std::string("OK"); + } else if (status == Status::WARN) { + return std::string("Warn"); + } else if (status == Status::ERROR) { + return std::string("Error"); + } else { + return std::string("Stale"); + } +} + +void DummyDiagPublisher::loadRequiredDiags() +{ + const auto param_key = std::string("required_diags"); + const uint64_t depth = 3; + const auto param_names = this->list_parameters({param_key}, depth).names; + + if (param_names.empty()) { + throw std::runtime_error(fmt::format("no parameter found: {}", param_key)); + } + + std::set diag_names; + + for (const auto & param_name : param_names) { + const auto split_names = split(param_name, '.'); + const auto & param_required_diags = split_names.at(0); + const auto & param_diag = split_names.at(1); + + const auto diag_name_with_prefix = fmt::format("{0}.{1}", param_required_diags, param_diag); + + if (diag_names.count(diag_name_with_prefix) != 0) { + continue; + } + + diag_names.insert(diag_name_with_prefix); + + const auto is_active_key = diag_name_with_prefix + std::string(".is_active"); + std::string is_active_str; + this->get_parameter_or(is_active_key, is_active_str, std::string("true")); + const auto status_key = diag_name_with_prefix + std::string(".status"); + std::string status_str; + this->get_parameter_or(status_key, status_str, std::string("OK")); + + bool is_active{}; + std::istringstream(is_active_str) >> std::boolalpha >> is_active; + const auto status = convertStrToStatus(status_str); + if (!status) { + throw std::runtime_error(fmt::format("invalid status found: {}", status_str)); + } + required_diags_.push_back({param_diag, is_active, *status}); + } +} + +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(); } + +DummyDiagPublisher::DummyDiagPublisher() +: Node( + "dummy_diag_publisher", rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)), + updater_(this) + +{ + // 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)); +} diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp new file mode 100644 index 0000000000000..a532e8d1c6d01 --- /dev/null +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp @@ -0,0 +1,26 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "dummy_diag_publisher/dummy_diag_publisher_core.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp deleted file mode 100644 index d0750fd44c7a5..0000000000000 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "dummy_diag_publisher/dummy_diag_publisher_node.hpp" - -#include -#include - -#include -#include -#include -#include - -rcl_interfaces::msg::SetParametersResult DummyDiagPublisherNode::paramCallback( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - DummyDiagPublisherConfig config = config_; - try { - int status = static_cast(config.status); - tier4_autoware_utils::updateParam(parameters, "status", status); - config.status = Status(status); - tier4_autoware_utils::updateParam(parameters, "is_active", config.is_active); - config_ = config; - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - result.successful = false; - result.reason = e.what(); - } - - return result; -} - -void DummyDiagPublisherNode::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - diagnostic_msgs::msg::DiagnosticStatus status; - - if (config_.status == Status::OK) { - status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - status.message = diag_config_.msg_ok; - } else if (config_.status == Status::WARN) { - status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - status.message = diag_config_.msg_warn; - } else if (config_.status == Status::ERROR) { - status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - status.message = diag_config_.msg_error; - } else if (config_.status == Status::STALE) { - status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE; - status.message = diag_config_.msg_stale; - } else { - throw std::runtime_error("invalid status"); - } - - stat.summary(status.level, status.message); -} - -void DummyDiagPublisherNode::onTimer() -{ - if (config_.is_active) { - updater_.force_update(); - } -} - -DummyDiagPublisherNode::DummyDiagPublisherNode(const rclcpp::NodeOptions & node_options) -: Node("dummy_diag_publisher", node_options) -{ - // Parameter - update_rate_ = declare_parameter("update_rate", 10.0); - const std::string diag_name = this->declare_parameter("diag_name"); - const std::string hardware_id = "dummy_diag_" + diag_name; - diag_config_ = DiagConfig{ - diag_name, hardware_id, "OK", "Warn", "Error", "Stale", - }; - - // Set parameter callback - config_.status = static_cast(this->declare_parameter("status", 0)); - config_.is_active = this->declare_parameter("is_active", true); - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&DummyDiagPublisherNode::paramCallback, this, std::placeholders::_1)); - - // Diagnostic Updater - updater_.setHardwareID(diag_config_.hardware_id); - updater_.add(diag_config_.name, this, &DummyDiagPublisherNode::produceDiagnostics); - - // Timer - const auto period_ns = rclcpp::Rate(update_rate_).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&DummyDiagPublisherNode::onTimer, this)); -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(DummyDiagPublisherNode)