feat(dummy_diag_publisher): update param setting (#1716)

* feat: update param setting

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* style(pre-commit): autofix

---------

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: TetsuKawa

launch/tier4_control_launch/launch/control.launch.py

@@ -30,13 +30,15 @@
 from launch_ros.substitutions import FindPackageShare
 import yaml
 
+
 def get_control_cmd_topic(context):
-    is_redundant = LaunchConfiguration('launch_redundancy_system_components').perform(context)
-    system_run_mode = LaunchConfiguration('system_run_mode').perform(context)
+    is_redundant = LaunchConfiguration("launch_redundancy_system_components").perform(context)
+    system_run_mode = LaunchConfiguration("system_run_mode").perform(context)
+
+    if is_redundant.lower() == "true" and system_run_mode.lower() == "planning_simulation":
+        return "/main/control/command/control_cmd"
+    return "/control/command/control_cmd"
 
-    if is_redundant.lower() == 'true' and system_run_mode.lower() == 'planning_simulation':
-        return '/main/control/command/control_cmd'
-    return '/control/command/control_cmd'
 
 def launch_setup(context, *args, **kwargs):
     with open(LaunchConfiguration("vehicle_param_file").perform(context), "r") as f:

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<rclcpp::Parameter> & parameters);
 
-  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);
 

system/dummy_diag_publisher/package.xml

@@ -11,8 +11,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_universe_utils</depend>
-  <depend>diagnostic_updater</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp

@@ -17,6 +17,9 @@
 #define FMT_HEADER_ONLY
 #include <fmt/format.h>
 
+#include <sstream>
+#include <unordered_map>
+
 namespace
 {
 std::vector<std::string> split(const std::string & str, const char delim)
@@ -32,7 +35,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 +142,61 @@ rclcpp::NodeOptions override_options(rclcpp::NodeOptions options)
     true);
 }
 
+rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::onSetParams(
+  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 + 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 +217,10 @@ DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options)
 
   // Publisher
   pub_ = create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/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 <rclcpp_components/register_node_macro.hpp>