Use the new declare_or_get_parameter API for nav2_controller. (#5624)

* Migrate majority of parameter declarations to use the new parameter API.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Migrate controller_server.cpp to use declare_or_get_parameter API.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

---------

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

Author: leander-dsouza

nav2_controller/plugins/pose_progress_checker.cpp

@@ -39,9 +39,8 @@ void PoseProgressChecker::initialize(
   SimpleProgressChecker::initialize(parent, plugin_name);
   auto node = parent.lock();
 
-  nav2::declare_parameter_if_not_declared(
-    node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5));
-  node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5);
+  required_movement_angle_ = node->declare_or_get_parameter(
+    plugin_name + ".required_movement_angle", 0.5);
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(

nav2_controller/plugins/position_goal_checker.cpp

@@ -41,15 +41,8 @@ void PositionGoalChecker::initialize(
   plugin_name_ = plugin_name;
   auto node = parent.lock();
 
-  nav2::declare_parameter_if_not_declared(
-    node,
-    plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
-  nav2::declare_parameter_if_not_declared(
-    node,
-    plugin_name + ".stateful", rclcpp::ParameterValue(true));
-
-  node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
-  node->get_parameter(plugin_name + ".stateful", stateful_);
+  xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25);
+  stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true);
 
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
 

nav2_controller/plugins/simple_goal_checker.cpp

@@ -69,19 +69,9 @@ void SimpleGoalChecker::initialize(
   plugin_name_ = plugin_name;
   auto node = parent.lock();
 
-  nav2::declare_parameter_if_not_declared(
-    node,
-    plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
-  nav2::declare_parameter_if_not_declared(
-    node,
-    plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25));
-  nav2::declare_parameter_if_not_declared(
-    node,
-    plugin_name + ".stateful", rclcpp::ParameterValue(true));
-
-  node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
-  node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_);
-  node->get_parameter(plugin_name + ".stateful", stateful_);
+  xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25);
+  yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25);
+  stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true);
 
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
 

nav2_controller/plugins/simple_progress_checker.cpp

@@ -36,14 +36,9 @@ void SimpleProgressChecker::initialize(
 
   clock_ = node->get_clock();
 
-  nav2::declare_parameter_if_not_declared(
-    node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5));
-  nav2::declare_parameter_if_not_declared(
-    node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0));
-  // Scale is set to 0 by default, so if it was not set otherwise, set to 0
-  node->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5);
-  double time_allowance_param = 0.0;
-  node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0);
+  radius_ = node->declare_or_get_parameter(plugin_name + ".required_movement_radius", 0.5);
+  double time_allowance_param = node->declare_or_get_parameter(
+    plugin_name + ".movement_time_allowance", 10.0);
   time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param);
 
   // Add callback for dynamic parameters

nav2_controller/plugins/stopped_goal_checker.cpp

@@ -65,15 +65,10 @@ void StoppedGoalChecker::initialize(
 
   auto node = parent.lock();
 
-  nav2::declare_parameter_if_not_declared(
-    node,
-    plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue(0.25));
-  nav2::declare_parameter_if_not_declared(
-    node,
-    plugin_name + ".trans_stopped_velocity", rclcpp::ParameterValue(0.25));
-
-  node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_);
-  node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_);
+  rot_stopped_velocity_ = node->declare_or_get_parameter(
+    plugin_name + ".rot_stopped_velocity", 0.25);
+  trans_stopped_velocity_ = node->declare_or_get_parameter(
+    plugin_name + ".trans_stopped_velocity", 0.25);
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(

nav2_controller/src/controller_server.cpp

@@ -49,26 +49,6 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
 {
   RCLCPP_INFO(get_logger(), "Creating controller server");
 
-  declare_parameter("controller_frequency", 20.0);
-
-  declare_parameter("progress_checker_plugins", default_progress_checker_ids_);
-  declare_parameter("goal_checker_plugins", default_goal_checker_ids_);
-  declare_parameter("controller_plugins", default_ids_);
-  declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
-  declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
-  declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
-
-  declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
-
-  declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0));
-  declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false));
-  declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true));
-  declare_parameter("costmap_update_timeout", 0.30);  // 300ms
-
-  declare_parameter("odom_topic", rclcpp::ParameterValue("odom"));
-  declare_parameter("odom_duration", rclcpp::ParameterValue(0.3));
-  declare_parameter("search_window", rclcpp::ParameterValue(2.0));
-
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()},
@@ -91,7 +71,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   RCLCPP_INFO(get_logger(), "Configuring controller interface");
 
   RCLCPP_INFO(get_logger(), "getting progress checker plugins..");
-  get_parameter("progress_checker_plugins", progress_checker_ids_);
+  progress_checker_ids_ = declare_or_get_parameter(
+    "progress_checker_plugins", default_progress_checker_ids_);
   if (progress_checker_ids_ == default_progress_checker_ids_) {
     for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) {
       nav2::declare_parameter_if_not_declared(
@@ -101,7 +82,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   }
 
   RCLCPP_INFO(get_logger(), "getting goal checker plugins..");
-  get_parameter("goal_checker_plugins", goal_checker_ids_);
+  goal_checker_ids_ = declare_or_get_parameter("goal_checker_plugins", default_goal_checker_ids_);
   if (goal_checker_ids_ == default_goal_checker_ids_) {
     for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) {
       nav2::declare_parameter_if_not_declared(
@@ -110,7 +91,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     }
   }
 
-  get_parameter("controller_plugins", controller_ids_);
+  controller_ids_ = declare_or_get_parameter("controller_plugins", default_ids_);
   if (controller_ids_ == default_ids_) {
     for (size_t i = 0; i < default_ids_.size(); ++i) {
       nav2::declare_parameter_if_not_declared(
@@ -123,21 +104,20 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   goal_checker_types_.resize(goal_checker_ids_.size());
   progress_checker_types_.resize(progress_checker_ids_.size());
 
-  get_parameter("controller_frequency", controller_frequency_);
-  get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_);
-  get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_);
-  get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
+  controller_frequency_ = declare_or_get_parameter("controller_frequency", 20.0);
+  min_x_velocity_threshold_ = declare_or_get_parameter("min_x_velocity_threshold", 0.0001);
+  min_y_velocity_threshold_ = declare_or_get_parameter("min_y_velocity_threshold", 0.0001);
+  min_theta_velocity_threshold_ = declare_or_get_parameter("min_theta_velocity_threshold", 0.0001);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
-  std::string speed_limit_topic, odom_topic;
-  get_parameter("speed_limit_topic", speed_limit_topic);
-  get_parameter("odom_topic", odom_topic);
-  double odom_duration;
-  get_parameter("odom_duration", odom_duration);
-  get_parameter("failure_tolerance", failure_tolerance_);
-  get_parameter("use_realtime_priority", use_realtime_priority_);
-  get_parameter("publish_zero_velocity", publish_zero_velocity_);
-  get_parameter("search_window", search_window_);
+  std::string speed_limit_topic = declare_or_get_parameter(
+    "speed_limit_topic", std::string("speed_limit"));
+  std::string odom_topic = declare_or_get_parameter("odom_topic", std::string("odom"));
+  double odom_duration = declare_or_get_parameter("odom_duration", 0.3);
+  failure_tolerance_ = declare_or_get_parameter("failure_tolerance", 0.0);
+  use_realtime_priority_ = declare_or_get_parameter("use_realtime_priority", false);
+  publish_zero_velocity_ = declare_or_get_parameter("publish_zero_velocity", true);
+  search_window_ = declare_or_get_parameter("search_window", 2.0);
 
   costmap_ros_->configure();
   // Launch a thread to run the costmap node
@@ -231,8 +211,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel");
   tracking_feedback_pub_ = create_publisher<nav2_msgs::msg::TrackingFeedback>("tracking_feedback");
 
-  double costmap_update_timeout_dbl;
-  get_parameter("costmap_update_timeout", costmap_update_timeout_dbl);
+  double costmap_update_timeout_dbl = declare_or_get_parameter("costmap_update_timeout", 0.30);
   costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl);
 
   // Create the action server that we implement with our followPath method

nav2_controller/test/test_dynamic_parameters.cpp

@@ -31,10 +31,26 @@ class ControllerShim : public nav2_controller::ControllerServer
   {
   }
 
+  /**
+   * @brief Declare parameters needed for dynamic parameter testing.
+   *
+   * This method mirrors the parameter declarations in the
+   * on_configure method of ControllerServer.
+   */
+  void declareTestParameters()
+  {
+    declare_parameter("min_x_velocity_threshold", 0.0001);
+    declare_parameter("min_y_velocity_threshold", 0.0001);
+    declare_parameter("min_theta_velocity_threshold", 0.0001);
+    declare_parameter("failure_tolerance", 0.0);
+  }
+
   // Since we cannot call configure/activate due to costmaps
   // requiring TF
   void setDynamicCallback()
   {
+    declareTestParameters();
+
     auto node = shared_from_this();
     // Add callback for dynamic parameters
     dyn_params_handler_ = node->add_on_set_parameters_callback(