feat: modify param setting

Signed-off-by: TetsuKawa <[email protected]>
tier4 · Dec 19, 2024 · be10038 · be10038
1 parent 90001d7
commit be10038
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 2 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
@@ -60,9 +60,12 @@ class DummyDiagPublisher : public rclcpp::Node
   DummyDiagConfig config_;
 
   RequiredDiags required_diags_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
+
   void loadRequiredDiags();
+  void handleParameterSetting();
 
-  std::optional<Status> convertStrToStatus(std::string & status_str);
+  std::optional<Status> 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/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp
@@ -32,7 +32,7 @@ std::vector<std::string> split(const std::string & str, const char delim)
 }  // namespace
 
 std::optional<DummyDiagPublisher::Status> DummyDiagPublisher::convertStrToStatus(
-  std::string & status_str)
+  const std::string & status_str)
 {
   static std::unordered_map<std::string, Status> const table = {
     {"OK", Status::OK}, {"Warn", Status::WARN}, {"Error", Status::ERROR}, {"Stale", Status::STALE}};
@@ -139,6 +139,53 @@ rclcpp::NodeOptions override_options(rclcpp::NodeOptions options)
     true);
 }
 
+void DummyDiagPublisher::handleParameterSetting()
+{
+
+  param_callback_handle_ = this->add_on_set_parameters_callback(
+    [this](const std::vector<rclcpp::Parameter> & 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 + ".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: " + param_name;
+              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 + ".is_active") {
+            param_found = true;
+            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");
+          }
+        }
+
+        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 +206,9 @@ DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options)
 
   // Publisher
   pub_ = create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", rclcpp::QoS(1));
+
+  // Parameter Callback
+  handleParameterSetting();
 }
 
 #include <rclcpp_components/register_node_macro.hpp>