Change dynamic parameters method (#2585)

* dyn param method

* dont add to reset for checkers

* fix test

Author: doisyg

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -37,10 +37,12 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "nav2_core/goal_checker.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 namespace nav2_controller
 {
@@ -73,16 +75,16 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
   bool stateful_, check_xy_;
   // Cached squared xy_goal_tolerance_
   double xy_goal_tolerance_sq_;
-  // Subscription for parameter change
-  rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
-  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
   std::string plugin_name_;
 
   /**
    * @brief Callback executed when a paramter change is detected
-   * @param event ParameterEvent message
+   * @param parameters list of changed parameters
    */
-  void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 };
 
 }  // namespace nav2_controller

nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp

@@ -16,6 +16,7 @@
 #define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
 
 #include <string>
+#include <vector>
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "nav2_core/progress_checker.hpp"
@@ -61,16 +62,16 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker
   rclcpp::Time baseline_time_;
 
   bool baseline_pose_set_{false};
-  // Subscription for parameter change
-  rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
-  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
   std::string plugin_name_;
 
   /**
    * @brief Callback executed when a paramter change is detected
-   * @param event ParameterEvent message
+   * @param parameters list of changed parameters
    */
-  void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 };
 }  // namespace nav2_controller
 

nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

@@ -37,6 +37,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -66,16 +67,16 @@ class StoppedGoalChecker : public SimpleGoalChecker
 
 protected:
   double rot_stopped_velocity_, trans_stopped_velocity_;
-  // Subscription for parameter change
-  rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
-  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
   std::string plugin_name_;
 
   /**
    * @brief Callback executed when a paramter change is detected
-   * @param event ParameterEvent message
+   * @param parameters list of changed parameters
    */
-  void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 };
 
 }  // namespace nav2_controller

nav2_controller/plugins/simple_goal_checker.cpp

@@ -35,6 +35,7 @@
 #include <memory>
 #include <string>
 #include <limits>
+#include <vector>
 #include "nav2_controller/plugins/simple_goal_checker.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "angles/angles.h"
@@ -83,15 +84,9 @@ void SimpleGoalChecker::initialize(
 
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
 
-  // Setup callback for changes to parameters.
-  parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface());
-
-  parameter_event_sub_ = parameters_client_->on_parameter_event(
-    std::bind(&SimpleGoalChecker::on_parameter_event_callback, this, _1));
+  // Add callback for dynamic parameters
+  dyn_params_handler = node->add_on_set_parameters_callback(
+    std::bind(&SimpleGoalChecker::dynamicParametersCallback, this, _1));
 }
 
 void SimpleGoalChecker::reset()
@@ -144,28 +139,29 @@ bool SimpleGoalChecker::getTolerances(
   return true;
 }
 
-void
-SimpleGoalChecker::on_parameter_event_callback(
-  const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+rcl_interfaces::msg::SetParametersResult
+SimpleGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
-  for (auto & changed_parameter : event->changed_parameters) {
-    const auto & type = changed_parameter.value.type;
-    const auto & name = changed_parameter.name;
-    const auto & value = changed_parameter.value;
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto & parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
 
     if (type == ParameterType::PARAMETER_DOUBLE) {
       if (name == plugin_name_ + ".xy_goal_tolerance") {
-        xy_goal_tolerance_ = value.double_value;
+        xy_goal_tolerance_ = parameter.as_double();
         xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
       } else if (name == plugin_name_ + ".yaw_goal_tolerance") {
-        yaw_goal_tolerance_ = value.double_value;
+        yaw_goal_tolerance_ = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == plugin_name_ + ".stateful") {
-        stateful_ = value.bool_value;
+        stateful_ = parameter.as_bool();
       }
     }
   }
+  result.successful = true;
+  return result;
 }
 
 }  // namespace nav2_controller

nav2_controller/plugins/simple_progress_checker.cpp

@@ -16,6 +16,7 @@
 #include <cmath>
 #include <string>
 #include <memory>
+#include <vector>
 #include "nav2_core/exceptions.hpp"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -49,15 +50,9 @@ void SimpleProgressChecker::initialize(
   node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0);
   time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param);
 
-  // Setup callback for changes to parameters.
-  parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface());
-
-  parameter_event_sub_ = parameters_client_->on_parameter_event(
-    std::bind(&SimpleProgressChecker::on_parameter_event_callback, this, _1));
+  // Add callback for dynamic parameters
+  dyn_params_handler = node->add_on_set_parameters_callback(
+    std::bind(&SimpleProgressChecker::dynamicParametersCallback, this, _1));
 }
 
 bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
@@ -101,23 +96,24 @@ static double pose_distance(
   return std::hypot(dx, dy);
 }
 
-void
-SimpleProgressChecker::on_parameter_event_callback(
-  const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+rcl_interfaces::msg::SetParametersResult
+SimpleProgressChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
-  for (auto & changed_parameter : event->changed_parameters) {
-    const auto & type = changed_parameter.value.type;
-    const auto & name = changed_parameter.name;
-    const auto & value = changed_parameter.value;
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
 
     if (type == ParameterType::PARAMETER_DOUBLE) {
       if (name == plugin_name_ + ".required_movement_radius") {
-        radius_ = value.double_value;
+        radius_ = parameter.as_double();
       } else if (name == plugin_name_ + ".movement_time_allowance") {
-        time_allowance_ = rclcpp::Duration::from_seconds(value.double_value);
+        time_allowance_ = rclcpp::Duration::from_seconds(parameter.as_double());
       }
     }
   }
+  result.successful = true;
+  return result;
 }
 
 }  // namespace nav2_controller

nav2_controller/plugins/stopped_goal_checker.cpp

@@ -36,6 +36,7 @@
 #include <string>
 #include <memory>
 #include <limits>
+#include <vector>
 #include "nav2_controller/plugins/stopped_goal_checker.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -73,15 +74,9 @@ void StoppedGoalChecker::initialize(
   node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_);
   node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_);
 
-  // Setup callback for changes to parameters.
-  parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface());
-
-  parameter_event_sub_ = parameters_client_->on_parameter_event(
-    std::bind(&StoppedGoalChecker::on_parameter_event_callback, this, _1));
+  // Add callback for dynamic parameters
+  dyn_params_handler = node->add_on_set_parameters_callback(
+    std::bind(&StoppedGoalChecker::dynamicParametersCallback, this, _1));
 }
 
 bool StoppedGoalChecker::isGoalReached(
@@ -118,23 +113,24 @@ bool StoppedGoalChecker::getTolerances(
   return true && rtn;
 }
 
-void
-StoppedGoalChecker::on_parameter_event_callback(
-  const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+rcl_interfaces::msg::SetParametersResult
+StoppedGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
-  for (auto & changed_parameter : event->changed_parameters) {
-    const auto & type = changed_parameter.value.type;
-    const auto & name = changed_parameter.name;
-    const auto & value = changed_parameter.value;
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
 
     if (type == ParameterType::PARAMETER_DOUBLE) {
       if (name == plugin_name_ + ".rot_stopped_velocity") {
-        rot_stopped_velocity_ = value.double_value;
+        rot_stopped_velocity_ = parameter.as_double();
       } else if (name == plugin_name_ + ".trans_stopped_velocity") {
-        trans_stopped_velocity_ = value.double_value;
+        trans_stopped_velocity_ = parameter.as_double();
       }
     }
   }
+  result.successful = true;
+  return result;
 }
 
 }  // namespace nav2_controller

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp

@@ -37,6 +37,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
@@ -120,10 +121,14 @@ class KinematicsHandler
 protected:
   std::atomic<KinematicParameters *> kinematics_;
 
-  // Subscription for parameter change
-  rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
-  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
-  void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
   void update_kinematics(KinematicParameters kinematics);
   std::string plugin_name_;
 };