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 befe02ecb330e..90d015fbb07a3 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 51a0846b45179..a7144ee56227e 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -16,6 +16,8 @@ #define FMT_HEADER_ONLY #include +#include +#include namespace { @@ -32,7 +34,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}}; @@ -139,6 +141,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)) @@ -159,6 +211,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