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..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 @@ -60,9 +60,13 @@ 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..5d0e4ee0316cf 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -22,6 +22,8 @@ #define FMT_HEADER_ONLY #include +#include + namespace { std::vector split(const std::string & str, const char delim) @@ -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,61 @@ 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 +221,10 @@ 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