From 7d325311f90a1f49c9046b3f0d7ff599282755d7 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Mon, 6 Oct 2025 08:33:23 +0000 Subject: [PATCH] Use the new declare_or_get_parameter API for nav2_behaviors. Signed-off-by: Leander Stephen D'Souza --- .../plugins/drive_on_heading.hpp | 26 ++++------ nav2_behaviors/plugins/assisted_teleop.cpp | 21 ++------- nav2_behaviors/plugins/spin.cpp | 23 ++------- nav2_behaviors/src/behavior_server.cpp | 47 +++++-------------- nav2_behaviors/test/test_behaviors.cpp | 29 ++++-------- 5 files changed, 38 insertions(+), 108 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 936221d1d2b..c3c6fa6cce4 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -246,23 +246,15 @@ class DriveOnHeading : public TimedBehavior 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 " diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 255f891e552..457956e1fe2 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/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( node, diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index bf400361b0c..f51eabce990 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/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 command) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 0f4db4b3bf6..e4e7fa218d4 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/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; diff --git a/nav2_behaviors/test/test_behaviors.cpp b/nav2_behaviors/test/test_behaviors.cpp index 35df094b5f5..8a67cf794ff 100644 --- a/nav2_behaviors/test/test_behaviors.cpp +++ b/nav2_behaviors/test/test_behaviors.cpp @@ -111,19 +111,15 @@ class BehaviorTest : public ::testing::Test node_lifecycle_ = std::make_shared( "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(node_lifecycle_->get_clock()); auto timer_interface = std::make_shared( @@ -132,13 +128,6 @@ class BehaviorTest : public ::testing::Test tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*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 local_costmap_sub_ = std::make_shared( node_lifecycle_, global_costmap_topic);