Enable dynamic server parameter change

CMakeLists.txt

@@ -33,6 +33,7 @@ find_package(rcutils REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(rmw REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 
 find_package(Boost REQUIRED COMPONENTS program_options filesystem)
 find_package(yaml_cpp_vendor REQUIRED)
@@ -148,7 +149,27 @@ target_include_directories(client_save_on_update
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test/include>
 )
 
-install(TARGETS client_default client_with_node_options client_save_on_update DESTINATION lib/${PROJECT_NAME})
+add_executable(client_dynamic_param_change
+  test/src/test_dynamic_param_change.cpp
+  test/src/persist_parameter_client.cpp
+)
+
+target_link_libraries(client_dynamic_param_change
+  PUBLIC
+  rclcpp::rclcpp
+  rclcpp_components::component
+  rcutils::rcutils
+  ${rcl_interfaces_TARGETS}
+  ${std_msgs_TARGETS}
+  ${std_srvs_TARGETS}
+)
+
+target_include_directories(client_dynamic_param_change
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test/include>
+)
+
+install(TARGETS client_default client_with_node_options client_save_on_update client_dynamic_param_change DESTINATION lib/${PROJECT_NAME})
 
 # Install launch files.
 install(DIRECTORY

server/include/parameter_server.h

@@ -38,7 +38,15 @@ class ParameterServer : public rclcpp::Node
   ~ParameterServer();
 
 private:
+#if RCLCPP_VERSION_MAJOR < 17
+  std::shared_ptr<rclcpp::ParameterEventHandler> server_param_subscriber_;
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> storing_period_callback_handle_;
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> dynamic_typing_callback_handle_;
+#endif
+  // Initialize parameters value
   bool must_save_on_update_ = false;
+  bool allow_dynamic_typing_ = false;
+  int64_t storing_period_ = 0;
   // Using custom yaml file same as yaml format of ros2 parameter as much as possible,
   // so use rcl_yaml_param_parser functions directly to load custom persistent yaml file.
   void LoadYamlFile();
@@ -52,6 +60,10 @@ class ParameterServer : public rclcpp::Node
   void CheckYamlFile(const std::string& file);
   void ValidateYamlFile(YAML::Node node, const std::string& key = "");
   void SaveNode(YAML::Emitter& out, YAML::Node node, const std::string& key = "");
+#if RCLCPP_VERSION_MAJOR >= 17
+  void updateStoringTimer(const rclcpp::Parameter & param);
+  void updateDynamicTyping(const rclcpp::Parameter & param);
+#endif
 
   // Check whether parameter name contains "persistent." in the parameter list
   bool CheckPersistentParam(const std::vector<rclcpp::Parameter> & parameters);
@@ -80,8 +92,8 @@ class ParameterServer : public rclcpp::Node
 
   // for periodic storing to the file system
   rclcpp::TimerBase::SharedPtr timer_;
+  void TimerCallback();
 
-  bool allow_dynamic_typing_ = false;
   // For manual triggering of save
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr save_trigger_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reload_trigger_;

server/src/parameter_server.cpp

@@ -75,7 +75,10 @@ ParameterServer::ParameterServer(
   RCLCPP_DEBUG(
     this->get_logger(), "%s yaml:%s", PARAMETER_SERVER_FUNCTION, persistent_yaml_file_.c_str());
 
-  int storing_period = 0;
+#if RCLCPP_VERSION_MAJOR < 17
+  server_param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
+#endif
+
   // if automatically_declare_parameters_from_overrides is false, then the parameter_overrides will not be declared.
   // So it is safer to fetch the passed parameters directly from options.parameter_overrides()
   const std::vector<rclcpp::Parameter> & parameter_overrides = options.parameter_overrides();
@@ -84,7 +87,7 @@ ParameterServer::ParameterServer(
       allow_dynamic_typing_ = param.as_bool();
     }
     if (param.get_name() == "storing_period") {
-      storing_period = param.as_int();
+      storing_period_ = param.as_int();
     }
     if (param.get_name() == "must_save_on_update") {
       this->must_save_on_update_ = param.as_bool();
@@ -103,34 +106,94 @@ ParameterServer::ParameterServer(
       "Save on update enabled. Parameters will be saved when set.");
   }
 
-  if (storing_period < 0) {
+  if (storing_period_ < 0) {
     RCLCPP_WARN(
       this->get_logger(),
-      "storing_period parameter value (%d) is not valid, treating as 0", storing_period);
-    storing_period = 0;
+      "storing_period parameter value (%ld) is not valid, treating as 0", storing_period_);
+    storing_period_ = 0;
   }
 
-  if (!storing_period) {
+  if (!storing_period_) {
     RCLCPP_INFO(
       this->get_logger(), "Period is 0. Will not perform periodic persistent parameter storing");
   } else {
     timer_ = this->create_wall_timer(
-      std::chrono::seconds(storing_period), 
-      [this]{ 
-        StoreYamlFile(); 
-      }
+      std::chrono::seconds(storing_period_), 
+      std::bind(&ParameterServer::TimerCallback, this)
     );
 
     RCLCPP_INFO(
-      this->get_logger(), "Will perform periodic persistent parameter storing every %ds",
-      storing_period);
+      this->get_logger(), "Will perform periodic persistent parameter storing every %lds",
+      storing_period_);
   }
 
+#if RCLCPP_VERSION_MAJOR < 17
+  auto allow_dynamic_typing_param_change_callback =
+    [this](const rclcpp::Parameter & p) {
+      RCLCPP_INFO(
+        this->get_logger(), "Allow dynamic typing param value changed to %s",
+        p.as_bool() ? "true" : "false");
+      allow_dynamic_typing_ = p.as_bool();
+      RCLCPP_WARN(
+        this->get_logger(),
+        "Changing allow_dynamic_typing at runtime only affects parameters declared after "
+        "this change; already-declared parameter descriptors are not updated automatically.");
+    };
+  dynamic_typing_callback_handle_ = server_param_subscriber_->add_parameter_callback(
+    "allow_dynamic_typing",
+    allow_dynamic_typing_param_change_callback
+  );
+
+  auto storing_period_param_change_callback =
+    [this](const rclcpp::Parameter & p) {
+
+      const int64_t new_storing_period = p.as_int();
+
+      RCLCPP_INFO(
+        this->get_logger(), "Storing period param value changed to %lld",
+        static_cast<long long>(new_storing_period));
+
+      if (timer_) {
+        timer_->cancel();
+        timer_.reset();
+      }
+
+      storing_period_ = new_storing_period;
+      if (storing_period_ > 0) {
+        timer_ = this->create_wall_timer(
+          std::chrono::seconds(storing_period_),
+          std::bind(&ParameterServer::TimerCallback, this)
+        );
+      }
+    };
+  storing_period_callback_handle_ = server_param_subscriber_->add_parameter_callback(
+    "storing_period",
+    storing_period_param_change_callback
+  );
+#endif
+
   // Declare a parameter change request callback
   auto param_change_callback =
     [this](const std::vector<rclcpp::Parameter> & parameters)
     {
       auto result = rcl_interfaces::msg::SetParametersResult();
+
+      // Check internal param values are valid
+      for (const rclcpp::Parameter & param : parameters) {
+        if(param.get_name() == "storing_period" && param.as_int() < 0) {
+          result.successful = false;
+          result.reason = "Storing period cannot be a negative value.";
+          return result;
+        }
+#if RCLCPP_VERSION_MAJOR < 17
+        if(param.get_name() == "must_save_on_update") {
+          RCLCPP_INFO(
+            this->get_logger(), "Save on update parameter value changed to %s",
+            param.as_bool() ? "true" : "false");
+          must_save_on_update_ = param.as_bool();
+        }
+#endif
+      }
       result.successful = true;
 
       if (CheckPersistentParam(parameters))
@@ -161,6 +224,20 @@ ParameterServer::ParameterServer(
   auto post_param_change_callback =
     [this](const std::vector<rclcpp::Parameter> & parameters)
     {
+      // Update behavior based on the updated parameter
+      for (const rclcpp::Parameter & param : parameters) {
+        if(param.get_name() == "must_save_on_update") {
+          RCLCPP_INFO(
+            this->get_logger(), "Save on update parameter value changed to %s",
+            param.as_bool() ? "true" : "false");
+          must_save_on_update_ = param.as_bool();
+        } else if(param.get_name() == "storing_period") {
+          updateStoringTimer(param);
+        } else if(param.get_name() == "allow_dynamic_typing") {
+          updateDynamicTyping(param);
+        }
+      }
+
       if (CheckPersistentParam(parameters))
       {
         if(must_save_on_update_)
@@ -221,6 +298,46 @@ ParameterServer::~ParameterServer()
   StoreYamlFile();
 }
 
+void ParameterServer::TimerCallback() {
+  StoreYamlFile();
+}
+
+#if RCLCPP_VERSION_MAJOR >= 17
+void ParameterServer::updateStoringTimer(const rclcpp::Parameter & param)
+{
+  const int64_t new_storing_period = param.as_int();
+
+  RCLCPP_INFO(
+    this->get_logger(), "Storing period param value changed to %lld",
+    static_cast<long long>(new_storing_period));
+
+  if (timer_) {
+    timer_->cancel();
+    timer_.reset();
+  }
+
+  storing_period_ = new_storing_period;
+  if (storing_period_ > 0) {
+    timer_ = this->create_wall_timer(
+      std::chrono::seconds(storing_period_),
+      std::bind(&ParameterServer::TimerCallback, this)
+    );
+  }
+}
+
+void ParameterServer::updateDynamicTyping(const rclcpp::Parameter & param)
+{
+  RCLCPP_INFO(
+    this->get_logger(), "Allow dynamic typing param value changed to %s",
+    param.as_bool() ? "true" : "false");
+  allow_dynamic_typing_ = param.as_bool();
+  RCLCPP_WARN(
+    this->get_logger(),
+    "Changing allow_dynamic_typing at runtime only affects parameters declared after "
+    "this change; already-declared parameter descriptors are not updated automatically.");
+}
+#endif
+
 // Add a limitation that A node that is a map in custom YAML file can't contain '.' in the key name
 void ParameterServer::ValidateYamlFile(YAML::Node node, const std::string& key) {
   for (YAML::const_iterator it = node.begin(); it != node.end(); ++it)

test/include/persist_parameter_client.hpp

@@ -17,6 +17,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_srvs/srv/trigger.hpp"
+#include "rcl_interfaces/srv/get_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
 
 using namespace std::chrono_literals;
 
@@ -95,24 +97,89 @@ class PersistParametersClient : public rclcpp::Node
     return ret;
   }
 
+  rclcpp::Parameter read_server_parameter(const std::string & param_name)
+  {
+    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
+    request->names.push_back(param_name);
+
+    auto future = get_server_param_client_->async_send_request(request);
+    auto rc = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future, 5s);
+    if (rc != rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(),
+        "GET OPERATION : spin_until_future_complete failed (%s) for parameter: %s",
+        rclcpp::to_string(rc).c_str(), param_name.c_str());
+      return rclcpp::Parameter(param_name);
+    }
+
+    auto response = future.get();
+    if(response->values.empty()) {
+      RCLCPP_ERROR(this->get_logger(),
+      "GET OPERATION : No values in response for parameter %s", param_name.c_str());
+      return rclcpp::Parameter(param_name);
+    }
+    return rclcpp::Parameter(param_name, response->values[0]);
+  }
+
+  template <typename ValueType>
+  bool modify_server_parameter(const std::string & param_name, const ValueType & param_value)
+  {
+    auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
+    request->parameters.push_back(
+      rclcpp::Parameter(param_name, param_value).to_parameter_msg()
+    );
+
+    auto future = set_server_param_client_->async_send_request(request);
+    auto rc = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future, 5s);
+    if (rc != rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(),
+        "SET OPERATION : spin_until_future_complete failed (%s) for parameter: %s",
+        rclcpp::to_string(rc).c_str(), param_name.c_str());
+      return false;
+    }
+
+    auto response = future.get();
+    for (auto & result : response->results) {
+        if (!result.successful) {
+            RCLCPP_INFO(this->get_logger(),
+                "SET OPERATION : Failed to set server parameter: %s", result.reason.c_str());
+            return false;
+        }
+    }
+    RCLCPP_INFO(this->get_logger(),
+        "SET OPERATION : Set server parameter %s successfully.", param_name.c_str());
+    return true;
+  }
+
   inline std::shared_ptr<std_srvs::srv::Trigger::Response> trigger_save() {
     auto trigger = std::make_shared<std_srvs::srv::Trigger::Request>();
     auto fut = this->save_trigger_client_->async_send_request(trigger);
-    rclcpp::spin_until_future_complete(this->get_node_base_interface(), fut);
+    auto rc = rclcpp::spin_until_future_complete(this->get_node_base_interface(), fut, 5s);
+    if (rc != rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(),
+        "SAVE TRIGGER : spin_until_future_complete failed (%s)", rclcpp::to_string(rc).c_str());
+      return nullptr;
+    }
     return fut.get();
   }
 
   inline std::shared_ptr<std_srvs::srv::Trigger::Response> reload_yaml() {
     auto trigger = std::make_shared<std_srvs::srv::Trigger::Request>();
     auto fut = this->reload_trigger_client_->async_send_request(trigger);
-    rclcpp::spin_until_future_complete(this->get_node_base_interface(), fut);
+    auto rc = rclcpp::spin_until_future_complete(this->get_node_base_interface(), fut, 5s);
+    if (rc != rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(),
+        "RELOAD TRIGGER : spin_until_future_complete failed (%s)", rclcpp::to_string(rc).c_str());
+      return nullptr;
+    }
     return fut.get();
   }
 
 private:
    std::unique_ptr<rclcpp::SyncParametersClient> sync_param_client_;
    std::shared_ptr<rclcpp::Client<std_srvs::srv::Trigger>> save_trigger_client_;
    std::shared_ptr<rclcpp::Client<std_srvs::srv::Trigger>> reload_trigger_client_;
+   std::shared_ptr<rclcpp::Client<rcl_interfaces::srv::SetParameters>> set_server_param_client_;
+   std::shared_ptr<rclcpp::Client<rcl_interfaces::srv::GetParameters>> get_server_param_client_;
 };
 
 #endif