From 2c8e282b350a89f1652ca284a4482ae0c5b40174 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 19 Dec 2024 21:35:02 +0900 Subject: [PATCH 1/3] feat: update param setting Signed-off-by: TetsuKawa --- .../dummy_diag_publisher_core.hpp | 5 +- system/dummy_diag_publisher/package.xml | 3 +- .../src/dummy_diag_publisher_core.cpp | 59 ++++++++++++++++++- 3 files changed, 63 insertions(+), 4 deletions(-) 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 bbca397fb47ec..97a7a9efdd7f4 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 @@ -60,9 +60,12 @@ class DummyDiagPublisher : public rclcpp::Node DummyDiagConfig config_; RequiredDiags required_diags_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + void loadRequiredDiags(); + rcl_interfaces::msg::SetParametersResult onSetParams(const std::vector & parameters); - std::optional convertStrToStatus(std::string & status_str); + std::optional convertStrToStatus(const std::string & status_str); std::string convertStatusToStr(const Status & status); diagnostic_msgs::msg::DiagnosticStatus::_level_type convertStatusToLevel(const Status & status); diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml index 56faf31413e77..e6b0302216e14 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/dummy_diag_publisher/package.xml @@ -11,8 +11,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils - diagnostic_updater + diagnostic_msgs fmt rclcpp rclcpp_components 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 6bc1378507a37..736650511ea93 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -21,6 +21,8 @@ #define FMT_HEADER_ONLY #include +#include +#include namespace { @@ -37,7 +39,7 @@ std::vector split(const std::string & str, const char delim) } // namespace std::optional DummyDiagPublisher::convertStrToStatus( - std::string & status_str) + const std::string & status_str) { static std::unordered_map const table = { {"OK", Status::OK}, {"Warn", Status::WARN}, {"Error", Status::ERROR}, {"Stale", Status::STALE}}; @@ -144,6 +146,56 @@ rclcpp::NodeOptions override_options(rclcpp::NodeOptions options) true); } +rcl_interfaces::msg::SetParametersResult + DummyDiagPublisher::onSetParams(const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & parameter : parameters) { + bool param_found = false; + const auto & param_name = parameter.get_name(); + + for (auto & diag : required_diags_) { + if (param_name == diag.name + std::string(".status")) { + param_found = true; + auto new_status = convertStrToStatus(parameter.as_string()); + if (new_status) { + diag.status = *new_status; + RCLCPP_INFO(this->get_logger(), "Updated %s status to: %s", + diag.name.c_str(), parameter.as_string().c_str()); + } else { + result.successful = false; + result.reason = "Invalid status value for: " + parameter.as_string(); + RCLCPP_WARN(this->get_logger(), "Invalid status value for %s: %s", + diag.name.c_str(), parameter.as_string().c_str()); + } + } else if (param_name == diag.name + std::string(".is_active")) { + param_found = true; + try { + diag.is_active = parameter.as_bool(); + RCLCPP_INFO(this->get_logger(), "Updated %s is_active to: %s", + diag.name.c_str(), diag.is_active ? "true" : "false"); + } catch (const rclcpp::ParameterTypeException & e) { + result.successful = false; + result.reason = "Invalid is_active value for: " + parameter.as_string(); + RCLCPP_WARN(this->get_logger(), "Invalid is_active value for %s: %s", + diag.name.c_str(), parameter.as_string().c_str()); + } + } + } + + if (!param_found) { + result.successful = false; + result.reason = "Parameter not registered: " + parameter.get_name(); + RCLCPP_WARN(this->get_logger(), "Attempted to set unregistered parameter: %s", + parameter.get_name().c_str()); + } + } + + return result; +} + DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) : Node("dummy_diag_publisher", override_options(options)) @@ -164,6 +216,11 @@ DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) // Publisher pub_ = create_publisher("/diagnostics", rclcpp::QoS(1)); + + // Parameter Callback Handle + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&DummyDiagPublisher::onSetParams, this, std::placeholders::_1) + ); } #include From d1eb9d6371f3acfbc5ce33d7fb19edbc209e88b5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 16 Jan 2025 15:32:29 +0000 Subject: [PATCH 2/3] style(pre-commit): autofix --- .../dummy_diag_publisher_core.hpp | 3 +- .../src/dummy_diag_publisher_core.cpp | 33 +++++++++++-------- 2 files changed, 21 insertions(+), 15 deletions(-) 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 97a7a9efdd7f4..64e2e0426046d 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 @@ -63,7 +63,8 @@ class DummyDiagPublisher : public rclcpp::Node rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; void loadRequiredDiags(); - rcl_interfaces::msg::SetParametersResult onSetParams(const std::vector & parameters); + rcl_interfaces::msg::SetParametersResult onSetParams( + const std::vector & parameters); std::optional convertStrToStatus(const std::string & status_str); std::string convertStatusToStr(const Status & status); 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 736650511ea93..b418220831684 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -21,6 +21,7 @@ #define FMT_HEADER_ONLY #include + #include #include @@ -146,8 +147,8 @@ rclcpp::NodeOptions override_options(rclcpp::NodeOptions options) true); } -rcl_interfaces::msg::SetParametersResult - DummyDiagPublisher::onSetParams(const std::vector & parameters) +rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::onSetParams( + const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -162,25 +163,29 @@ rcl_interfaces::msg::SetParametersResult auto new_status = convertStrToStatus(parameter.as_string()); if (new_status) { diag.status = *new_status; - RCLCPP_INFO(this->get_logger(), "Updated %s status to: %s", - diag.name.c_str(), parameter.as_string().c_str()); + RCLCPP_INFO( + this->get_logger(), "Updated %s status to: %s", diag.name.c_str(), + parameter.as_string().c_str()); } else { result.successful = false; result.reason = "Invalid status value for: " + parameter.as_string(); - RCLCPP_WARN(this->get_logger(), "Invalid status value for %s: %s", - diag.name.c_str(), parameter.as_string().c_str()); + RCLCPP_WARN( + this->get_logger(), "Invalid status value for %s: %s", diag.name.c_str(), + parameter.as_string().c_str()); } } else if (param_name == diag.name + std::string(".is_active")) { param_found = true; try { diag.is_active = parameter.as_bool(); - RCLCPP_INFO(this->get_logger(), "Updated %s is_active to: %s", - diag.name.c_str(), diag.is_active ? "true" : "false"); + RCLCPP_INFO( + this->get_logger(), "Updated %s is_active to: %s", diag.name.c_str(), + diag.is_active ? "true" : "false"); } catch (const rclcpp::ParameterTypeException & e) { result.successful = false; result.reason = "Invalid is_active value for: " + parameter.as_string(); - RCLCPP_WARN(this->get_logger(), "Invalid is_active value for %s: %s", - diag.name.c_str(), parameter.as_string().c_str()); + RCLCPP_WARN( + this->get_logger(), "Invalid is_active value for %s: %s", diag.name.c_str(), + parameter.as_string().c_str()); } } } @@ -188,8 +193,9 @@ rcl_interfaces::msg::SetParametersResult if (!param_found) { result.successful = false; result.reason = "Parameter not registered: " + parameter.get_name(); - RCLCPP_WARN(this->get_logger(), "Attempted to set unregistered parameter: %s", - parameter.get_name().c_str()); + RCLCPP_WARN( + this->get_logger(), "Attempted to set unregistered parameter: %s", + parameter.get_name().c_str()); } } @@ -219,8 +225,7 @@ DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) // Parameter Callback Handle param_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&DummyDiagPublisher::onSetParams, this, std::placeholders::_1) - ); + std::bind(&DummyDiagPublisher::onSetParams, this, std::placeholders::_1)); } #include From e1416418f1ab43e282b1159b7dfa73875b687edf Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 17 Jan 2025 00:36:54 +0900 Subject: [PATCH 3/3] modify: delete unnecessary include Signed-off-by: TetsuKawa --- system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp | 1 - 1 file changed, 1 deletion(-) 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 b418220831684..5d0e4ee0316cf 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -23,7 +23,6 @@ #include #include -#include namespace {