From c306953bd89fd32b09819fc6b314f9cd99c84832 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Mon, 27 Oct 2025 11:16:33 +0000 Subject: [PATCH 1/2] Migrate the parameter API usage in nav2_costmap_2d package to the new style. Signed-off-by: Leander Stephen D'Souza --- .../plugins/plugin_container_layer.cpp | 17 +++------ nav2_costmap_2d/src/costmap_2d_ros.cpp | 36 +++++++------------ 2 files changed, 18 insertions(+), 35 deletions(-) diff --git a/nav2_costmap_2d/plugins/plugin_container_layer.cpp b/nav2_costmap_2d/plugins/plugin_container_layer.cpp index 76f58b003ce..9d7f2dc5ce8 100644 --- a/nav2_costmap_2d/plugins/plugin_container_layer.cpp +++ b/nav2_costmap_2d/plugins/plugin_container_layer.cpp @@ -35,18 +35,11 @@ void PluginContainerLayer::onInitialize() throw std::runtime_error{"Failed to lock node"}; } - nav2::declare_parameter_if_not_declared(node, name_ + "." + "enabled", - rclcpp::ParameterValue(true)); - nav2::declare_parameter_if_not_declared(node, name_ + "." + "plugins", - rclcpp::ParameterValue(std::vector{})); - nav2::declare_parameter_if_not_declared(node, name_ + "." + "combination_method", - rclcpp::ParameterValue(1)); - - node->get_parameter(name_ + "." + "enabled", enabled_); - node->get_parameter(name_ + "." + "plugins", plugin_names_); - - int combination_method_param{}; - node->get_parameter(name_ + "." + "combination_method", combination_method_param); + enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true); + plugin_names_ = node->declare_or_get_parameter( + name_ + "." + "plugins", std::vector{}); + int combination_method_param = node->declare_or_get_parameter( + name_ + "." + "combination_method", 1); combination_method_ = combination_method_from_int(combination_method_param); dyn_params_handler_ = node->add_on_set_parameters_callback( diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index f43373d286b..06809e1ab7f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -108,10 +108,6 @@ void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); - declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); - declare_parameter("map_vis_z", rclcpp::ParameterValue(0.0)); - declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); - declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map"))); declare_parameter("height", rclcpp::ParameterValue(5)); declare_parameter("width", rclcpp::ParameterValue(5)); @@ -120,20 +116,12 @@ void Costmap2DROS::init() declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); declare_parameter("origin_y", rclcpp::ParameterValue(0.0)); declare_parameter("plugins", rclcpp::ParameterValue(default_plugins_)); - declare_parameter("filters", rclcpp::ParameterValue(std::vector())); - declare_parameter("publish_frequency", rclcpp::ParameterValue(1.0)); declare_parameter("resolution", rclcpp::ParameterValue(0.1)); declare_parameter("robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); declare_parameter("robot_radius", rclcpp::ParameterValue(0.1)); - declare_parameter("rolling_window", rclcpp::ParameterValue(false)); - declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); - declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); - declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); - declare_parameter("use_maximum", rclcpp::ParameterValue(false)); - declare_parameter("subscribe_to_stamped_footprint", rclcpp::ParameterValue(false)); } Costmap2DROS::~Costmap2DROS() @@ -402,27 +390,29 @@ Costmap2DROS::getParameters() RCLCPP_DEBUG(get_logger(), " getParameters"); // Get all of the required parameters - get_parameter("always_send_full_costmap", always_send_full_costmap_); - get_parameter("map_vis_z", map_vis_z_); - get_parameter("footprint", footprint_); - get_parameter("footprint_padding", footprint_padding_); + always_send_full_costmap_ = declare_or_get_parameter("always_send_full_costmap", false); + map_vis_z_ = declare_or_get_parameter("map_vis_z", 0.0); + footprint_ = declare_or_get_parameter("footprint", std::string("[]")); + footprint_padding_ = declare_or_get_parameter("footprint_padding", 0.01f); + map_publish_frequency_ = declare_or_get_parameter("publish_frequency", 1.0); + rolling_window_ = declare_or_get_parameter("rolling_window", false); + track_unknown_space_ = declare_or_get_parameter("track_unknown_space", false); + transform_tolerance_ = declare_or_get_parameter("transform_tolerance", 0.3); + map_update_frequency_ = declare_or_get_parameter("update_frequency", 5.0); + filter_names_ = declare_or_get_parameter("filters", std::vector()); + subscribe_to_stamped_footprint_ = declare_or_get_parameter( + "subscribe_to_stamped_footprint", false); + get_parameter("global_frame", global_frame_); get_parameter("height", map_height_meters_); get_parameter("origin_x", origin_x_); get_parameter("origin_y", origin_y_); - get_parameter("publish_frequency", map_publish_frequency_); get_parameter("resolution", resolution_); get_parameter("robot_base_frame", robot_base_frame_); get_parameter("robot_radius", robot_radius_); - get_parameter("rolling_window", rolling_window_); - get_parameter("track_unknown_space", track_unknown_space_); - get_parameter("transform_tolerance", transform_tolerance_); get_parameter("initial_transform_timeout", initial_transform_timeout_); - get_parameter("update_frequency", map_update_frequency_); get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); - get_parameter("filters", filter_names_); - get_parameter("subscribe_to_stamped_footprint", subscribe_to_stamped_footprint_); auto node = shared_from_this(); From aabe5bc23825a449f1a11442af30d11571a2e6ee Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Mon, 27 Oct 2025 11:17:18 +0000 Subject: [PATCH 2/2] Remove redundant set_parameter calls. Signed-off-by: Leander Stephen D'Souza --- nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp | 2 -- nav2_costmap_2d/test/unit/keepout_filter_test.cpp | 4 ---- nav2_costmap_2d/test/unit/speed_filter_test.cpp | 6 ------ 3 files changed, 12 deletions(-) diff --git a/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp index 25731d8462c..8075e5064f7 100644 --- a/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp @@ -70,8 +70,6 @@ class TestNode : public ::testing::Test // Set CostmapFilter ROS-parameters node_->declare_parameter( std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue("filter_info")); - node_->set_parameter( - rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", "filter_info")); } ~TestNode() diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index f6078c52cc0..569c899c58e 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -219,12 +219,8 @@ void TestNode::createKeepoutFilter(const std::string & global_frame) node_->declare_parameter( std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); - node_->set_parameter( - rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); node_->declare_parameter( std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); - node_->set_parameter( - rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); keepout_filter_ = std::make_shared(); keepout_filter_->initialize(&layers, std::string(FILTER_NAME), tf_buffer_.get(), node_, nullptr); diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index aabe7ce03b7..7e84ea30dce 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -352,16 +352,10 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) node_->declare_parameter( std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); - node_->set_parameter( - rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); node_->declare_parameter( std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); - node_->set_parameter( - rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); node_->declare_parameter( std::string(FILTER_NAME) + ".speed_limit_topic", rclcpp::ParameterValue(SPEED_LIMIT_TOPIC)); - node_->set_parameter( - rclcpp::Parameter(std::string(FILTER_NAME) + ".speed_limit_topic", SPEED_LIMIT_TOPIC)); speed_filter_ = std::make_shared(); speed_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr);