Use the new declare_or_get_parameter API for nav2_behaviors. (#5583)

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

Author: leander-dsouza

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -246,23 +246,15 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       throw std::runtime_error{"Failed to lock node"};
     }
 
-    nav2::declare_parameter_if_not_declared(
-      node,
-      "simulate_ahead_time", rclcpp::ParameterValue(2.0));
-    node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
-
-    nav2::declare_parameter_if_not_declared(
-      node, this->behavior_name_ + ".acceleration_limit",
-      rclcpp::ParameterValue(2.5));
-    nav2::declare_parameter_if_not_declared(
-      node, this->behavior_name_ + ".deceleration_limit",
-      rclcpp::ParameterValue(-2.5));
-    nav2::declare_parameter_if_not_declared(
-      node, this->behavior_name_ + ".minimum_speed",
-      rclcpp::ParameterValue(0.10));
-    node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_);
-    node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_);
-    node->get_parameter(this->behavior_name_ + ".minimum_speed", minimum_speed_);
+    simulate_ahead_time_ = node->declare_or_get_parameter(
+      "simulate_ahead_time", 2.0);
+    acceleration_limit_ = node->declare_or_get_parameter(
+      this->behavior_name_ + ".acceleration_limit", 2.5);
+    deceleration_limit_ = node->declare_or_get_parameter(
+      this->behavior_name_ + ".deceleration_limit", -2.5);
+    minimum_speed_ = node->declare_or_get_parameter(
+      this->behavior_name_ + ".minimum_speed", 0.10);
+
     if (acceleration_limit_ <= 0.0 || deceleration_limit_ >= 0.0) {
       RCLCPP_ERROR(this->logger_,
         "DriveOnHeading: acceleration_limit and deceleration_limit must be "

nav2_behaviors/plugins/assisted_teleop.cpp

@@ -33,23 +33,10 @@ void AssistedTeleop::onConfigure()
   }
 
   // set up parameters
-  nav2::declare_parameter_if_not_declared(
-    node,
-    "projection_time", rclcpp::ParameterValue(1.0));
-
-  nav2::declare_parameter_if_not_declared(
-    node,
-    "simulation_time_step", rclcpp::ParameterValue(0.1));
-
-  nav2::declare_parameter_if_not_declared(
-    node,
-    "cmd_vel_teleop", rclcpp::ParameterValue(std::string("cmd_vel_teleop")));
-
-  node->get_parameter("projection_time", projection_time_);
-  node->get_parameter("simulation_time_step", simulation_time_step_);
-
-  std::string cmd_vel_teleop;
-  node->get_parameter("cmd_vel_teleop", cmd_vel_teleop);
+  projection_time_ = node->declare_or_get_parameter("projection_time", 1.0);
+  simulation_time_step_ = node->declare_or_get_parameter("simulation_time_step", 0.1);
+  std::string cmd_vel_teleop = node->declare_or_get_parameter(
+    "cmd_vel_teleop", std::string("cmd_vel_teleop"));
 
   vel_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
     node,

nav2_behaviors/plugins/spin.cpp

@@ -49,25 +49,10 @@ void Spin::onConfigure()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  nav2::declare_parameter_if_not_declared(
-    node,
-    "simulate_ahead_time", rclcpp::ParameterValue(2.0));
-  node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
-
-  nav2::declare_parameter_if_not_declared(
-    node,
-    "max_rotational_vel", rclcpp::ParameterValue(1.0));
-  node->get_parameter("max_rotational_vel", max_rotational_vel_);
-
-  nav2::declare_parameter_if_not_declared(
-    node,
-    "min_rotational_vel", rclcpp::ParameterValue(0.4));
-  node->get_parameter("min_rotational_vel", min_rotational_vel_);
-
-  nav2::declare_parameter_if_not_declared(
-    node,
-    "rotational_acc_lim", rclcpp::ParameterValue(3.2));
-  node->get_parameter("rotational_acc_lim", rotational_acc_lim_);
+  simulate_ahead_time_ = node->declare_or_get_parameter("simulate_ahead_time", 2.0);
+  max_rotational_vel_ = node->declare_or_get_parameter("max_rotational_vel", 1.0);
+  min_rotational_vel_ = node->declare_or_get_parameter("min_rotational_vel", 0.4);
+  rotational_acc_lim_ = node->declare_or_get_parameter("rotational_acc_lim", 3.2);
 }
 
 ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)

nav2_behaviors/src/behavior_server.cpp

@@ -31,26 +31,8 @@ BehaviorServer::BehaviorServer(const rclcpp::NodeOptions & options)
     "nav2_behaviors::DriveOnHeading",
     "nav2_behaviors::Wait"}
 {
-  declare_parameter(
-    "local_costmap_topic",
-    rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
-
-  declare_parameter(
-    "global_costmap_topic",
-    rclcpp::ParameterValue(std::string("global_costmap/costmap_raw")));
-
-  declare_parameter(
-    "local_footprint_topic",
-    rclcpp::ParameterValue(std::string("local_costmap/published_footprint")));
-
-  declare_parameter(
-    "global_footprint_topic",
-    rclcpp::ParameterValue(std::string("global_costmap/published_footprint")));
-
   declare_parameter("cycle_frequency", rclcpp::ParameterValue(10.0));
-  declare_parameter("behavior_plugins", default_ids_);
-
-  get_parameter("behavior_plugins", behavior_ids_);
+  behavior_ids_ = declare_or_get_parameter("behavior_plugins", default_ids_);
   if (behavior_ids_ == default_ids_) {
     for (size_t i = 0; i < default_ids_.size(); ++i) {
       declare_parameter(default_ids_[i] + ".plugin", default_types_[i]);
@@ -63,12 +45,6 @@ BehaviorServer::BehaviorServer(const rclcpp::NodeOptions & options)
   declare_parameter(
     "global_frame",
     rclcpp::ParameterValue(std::string("map")));
-  declare_parameter(
-    "robot_base_frame",
-    rclcpp::ParameterValue(std::string("base_link")));
-  declare_parameter(
-    "transform_tolerance",
-    rclcpp::ParameterValue(0.1));
 }
 
 
@@ -140,16 +116,17 @@ void BehaviorServer::configureBehaviorPlugins()
 
 void BehaviorServer::setupResourcesForBehaviorPlugins()
 {
-  std::string local_costmap_topic, global_costmap_topic;
-  std::string local_footprint_topic, global_footprint_topic;
-  std::string robot_base_frame;
-  double transform_tolerance;
-  get_parameter("local_costmap_topic", local_costmap_topic);
-  get_parameter("global_costmap_topic", global_costmap_topic);
-  get_parameter("local_footprint_topic", local_footprint_topic);
-  get_parameter("global_footprint_topic", global_footprint_topic);
-  get_parameter("robot_base_frame", robot_base_frame);
-  transform_tolerance = get_parameter("transform_tolerance").as_double();
+  std::string local_costmap_topic = declare_or_get_parameter(
+    "local_costmap_topic", std::string("local_costmap/costmap_raw"));
+  std::string global_costmap_topic = declare_or_get_parameter(
+    "global_costmap_topic", std::string("global_costmap/costmap_raw"));
+  std::string local_footprint_topic = declare_or_get_parameter(
+    "local_footprint_topic", std::string("local_costmap/published_footprint"));
+  std::string global_footprint_topic = declare_or_get_parameter(
+    "global_footprint_topic", std::string("global_costmap/published_footprint"));
+  std::string robot_base_frame = declare_or_get_parameter(
+    "robot_base_frame", std::string("base_link"));
+  double transform_tolerance = declare_or_get_parameter("transform_tolerance", 0.1);
 
   bool need_local_costmap = false;
   bool need_global_costmap = false;

nav2_behaviors/test/test_behaviors.cpp

@@ -111,19 +111,15 @@ class BehaviorTest : public ::testing::Test
     node_lifecycle_ =
       std::make_shared<nav2::LifecycleNode>(
       "LifecycleBehaviorTestNode", rclcpp::NodeOptions());
-    node_lifecycle_->declare_parameter(
-      "local_costmap_topic",
-      rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
-    node_lifecycle_->declare_parameter(
-      "local_footprint_topic",
-      rclcpp::ParameterValue(std::string("local_costmap/published_footprint")));
-
-    node_lifecycle_->declare_parameter(
-      "global_costmap_topic",
-      rclcpp::ParameterValue(std::string("global_costmap/costmap_raw")));
-    node_lifecycle_->declare_parameter(
-      "global_footprint_topic",
-      rclcpp::ParameterValue(std::string("global_costmap/published_footprint")));
+
+    std::string local_costmap_topic = node_lifecycle_->declare_or_get_parameter(
+      "local_costmap_topic", std::string("local_costmap/costmap_raw"));
+    std::string local_footprint_topic = node_lifecycle_->declare_or_get_parameter(
+      "local_footprint_topic", std::string("local_costmap/published_footprint"));
+    std::string global_costmap_topic = node_lifecycle_->declare_or_get_parameter(
+      "global_costmap_topic", std::string("global_costmap/costmap_raw"));
+    std::string global_footprint_topic = node_lifecycle_->declare_or_get_parameter(
+      "global_footprint_topic", std::string("global_costmap/published_footprint"));
 
     tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_lifecycle_->get_clock());
     auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -132,13 +128,6 @@ class BehaviorTest : public ::testing::Test
     tf_buffer_->setCreateTimerInterface(timer_interface);
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
-    std::string local_costmap_topic, global_costmap_topic;
-    std::string local_footprint_topic, global_footprint_topic;
-    node_lifecycle_->get_parameter("local_costmap_topic", local_costmap_topic);
-    node_lifecycle_->get_parameter("global_costmap_topic", global_costmap_topic);
-    node_lifecycle_->get_parameter("local_footprint_topic", local_footprint_topic);
-    node_lifecycle_->get_parameter("global_footprint_topic", global_footprint_topic);
-
     std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> local_costmap_sub_ =
       std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
       node_lifecycle_, global_costmap_topic);