Use the new declare_or_get_parameter API for nav2_costmap_2d

nav2_controller/plugins/test/path_handler.cpp

@@ -109,11 +109,11 @@ TEST(PathHandlerTests, TestBounds)
   node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
+  rclcpp_lifecycle::State state;
+  costmap_ros->on_configure(state);
   auto results = costmap_ros->set_parameters_atomically(
     {rclcpp::Parameter("global_frame", "odom"),
       rclcpp::Parameter("robot_base_frame", "base_link")});
-  rclcpp_lifecycle::State state;
-  costmap_ros->on_configure(state);
 
   // Test initialization and getting costmap basic metadata
   handler.initialize(node, node->get_logger(), "dummy", costmap_ros, costmap_ros->getTfBuffer());
@@ -160,11 +160,11 @@ TEST(PathHandlerTests, TestBoundsWithConstraintCheck)
   node->declare_parameter("dummy.enforce_path_rotation", true);
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
+  rclcpp_lifecycle::State state;
+  costmap_ros->on_configure(state);
   auto results = costmap_ros->set_parameters_atomically(
     {rclcpp::Parameter("global_frame", "odom"),
       rclcpp::Parameter("robot_base_frame", "base_link")});
-  rclcpp_lifecycle::State state;
-  costmap_ros->on_configure(state);
 
   // Test initialization and getting costmap basic metadata
   handler.initialize(node, node->get_logger(), "dummy", costmap_ros, costmap_ros->getTfBuffer());

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -147,17 +147,6 @@ class Layer
 
   /** @brief Convenience function for layered_costmap_->getFootprint(). */
   const std::vector<geometry_msgs::msg::Point> & getFootprint() const;
-
-  /** @brief Convenience functions for declaring ROS parameters */
-  void declareParameter(
-    const std::string & param_name,
-    const rclcpp::ParameterValue & value);
-  /** @brief Convenience functions for declaring ROS parameters */
-  void declareParameter(
-    const std::string & param_name,
-    const rclcpp::ParameterType & param_type);
-  /** @brief Convenience functions for declaring ROS parameters */
-  bool hasParameter(const std::string & param_name);
   /** @brief Convenience functions for declaring ROS parameters */
   std::string getFullName(const std::string & param_name);
 
@@ -200,9 +189,6 @@ class Layer
   // TODO(bpwilcox): make this managed by this class and/or container class.
   bool enabled_;
 
-  // Names of the parameters declared on the ROS node
-  std::unordered_set<std::string> local_params_;
-
 private:
   std::vector<geometry_msgs::msg::Point> footprint_spec_;
 };

nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp

@@ -67,13 +67,10 @@ void BinaryFilter::initializeFilter(
   }
 
   // Declare parameters specific to BinaryFilter only
-  std::string binary_state_topic;
-  declareParameter("default_state", rclcpp::ParameterValue(false));
-  node->get_parameter(name_ + "." + "default_state", default_state_);
-  declareParameter("binary_state_topic", rclcpp::ParameterValue("binary_state"));
-  node->get_parameter(name_ + "." + "binary_state_topic", binary_state_topic);
-  declareParameter("flip_threshold", rclcpp::ParameterValue(50.0));
-  node->get_parameter(name_ + "." + "flip_threshold", flip_threshold_);
+  default_state_ = node->declare_or_get_parameter(name_ + "." + "default_state", false);
+  std::string binary_state_topic = node->declare_or_get_parameter(name_ + "." +
+    "binary_state_topic", std::string("binary_state"));
+  flip_threshold_ = node->declare_or_get_parameter(name_ + "." + "flip_threshold", 50.0);
 
   filter_info_topic_ = filter_info_topic;
   // Setting new costmap filter info subscriber

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -70,16 +70,12 @@ void CostmapFilter::onInitialize()
   }
 
   try {
-    // Declare common for all costmap filters parameters
-    declareParameter("enabled", rclcpp::ParameterValue(true));
-    declareParameter("filter_info_topic", rclcpp::PARAMETER_STRING);
-    declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1));
-
     // Get parameters
-    node->get_parameter(name_ + "." + "enabled", enabled_);
-    filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string();
-    double transform_tolerance {};
-    node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance);
+    enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
+    filter_info_topic_ = node->declare_or_get_parameter<std::string>(name_ + "." +
+      "filter_info_topic");
+    double transform_tolerance = node->declare_or_get_parameter(name_ + "." + "transform_tolerance",
+      0.1);
     transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
 
     // Costmap Filter enabling service

nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -79,10 +79,10 @@ void KeepoutFilter::initializeFilter(
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
 
-  declareParameter("override_lethal_cost", rclcpp::ParameterValue(false));
-  node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_);
-  declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE));
-  node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_);
+  override_lethal_cost_ = node->declare_or_get_parameter(name_ + "." + "override_lethal_cost",
+    false);
+  lethal_override_cost_ = node->declare_or_get_parameter(name_ + "." + "lethal_override_cost",
+    MAX_NON_OBSTACLE);
 
   // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given
   lethal_override_cost_ = \

nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

@@ -66,9 +66,8 @@ void SpeedFilter::initializeFilter(
   }
 
   // Declare "speed_limit_topic" parameter specific to SpeedFilter only
-  std::string speed_limit_topic;
-  declareParameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
-  node->get_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic);
+  std::string speed_limit_topic = node->declare_or_get_parameter(name_ + "." + "speed_limit_topic",
+    std::string("speed_limit"));
   speed_limit_topic = joinWithParentNamespace(speed_limit_topic);
 
   filter_info_topic_ = joinWithParentNamespace(filter_info_topic);

nav2_costmap_2d/plugins/denoise_layer.cpp

@@ -27,27 +27,18 @@ namespace nav2_costmap_2d
 void
 DenoiseLayer::onInitialize()
 {
-  // Enable/disable plugin
-  declareParameter("enabled", rclcpp::ParameterValue(true));
-  // Smaller groups should be filtered
-  declareParameter("minimal_group_size", rclcpp::ParameterValue(2));
-  // Pixels connectivity type
-  declareParameter("group_connectivity_type", rclcpp::ParameterValue(8));
-
   const auto node = node_.lock();
 
   if (!node) {
     throw std::runtime_error("DenoiseLayer::onInitialize: Failed to lock node");
   }
-  node->get_parameter(name_ + "." + "enabled", enabled_);
-
-  auto getInt = [&](const std::string & parameter_name) {
-      int param{};
-      node->get_parameter(name_ + "." + parameter_name, param);
-      return param;
-    };
-
-  const int minimal_group_size_param = getInt("minimal_group_size");
+  enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
+  // Smaller groups should be filtered
+  const int minimal_group_size_param = node->declare_or_get_parameter(
+    name_ + "." + "minimal_group_size", 2);
+  // Pixels connectivity type
+  const int group_connectivity_type_param = node->declare_or_get_parameter(
+    name_ + "." + "group_connectivity_type", 8);
 
   if (minimal_group_size_param <= 1) {
     RCLCPP_WARN(
@@ -60,8 +51,6 @@ DenoiseLayer::onInitialize()
     minimal_group_size_ = static_cast<size_t>(minimal_group_size_param);
   }
 
-  const int group_connectivity_type_param = getInt("group_connectivity_type");
-
   if (group_connectivity_type_param == 4) {
     group_connectivity_type_ = ConnectivityType::Way4;
   } else {

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -89,22 +89,18 @@ InflationLayer::~InflationLayer()
 void
 InflationLayer::onInitialize()
 {
-  declareParameter("enabled", rclcpp::ParameterValue(true));
-  declareParameter("inflation_radius", rclcpp::ParameterValue(0.55));
-  declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0));
-  declareParameter("inflate_unknown", rclcpp::ParameterValue(false));
-  declareParameter("inflate_around_unknown", rclcpp::ParameterValue(false));
-
   {
     auto node = node_.lock();
     if (!node) {
       throw std::runtime_error{"Failed to lock node"};
     }
-    node->get_parameter(name_ + "." + "enabled", enabled_);
-    node->get_parameter(name_ + "." + "inflation_radius", inflation_radius_);
-    node->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_);
-    node->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_);
-    node->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_);
+    enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
+    inflation_radius_ = node->declare_or_get_parameter(name_ + "." + "inflation_radius", 0.55);
+    cost_scaling_factor_ = node->declare_or_get_parameter(
+      name_ + "." + "cost_scaling_factor", 10.0);
+    inflate_unknown_ = node->declare_or_get_parameter(name_ + "." + "inflate_unknown", false);
+    inflate_around_unknown_ = node->declare_or_get_parameter(
+      name_ + "." + "inflate_around_unknown", false);
 
     dyn_params_handler_ = node->add_on_set_parameters_callback(
       std::bind(

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -79,38 +79,34 @@ ObstacleLayer::~ObstacleLayer()
 void ObstacleLayer::onInitialize()
 {
   bool track_unknown_space;
-  double transform_tolerance;
+  double transform_tolerance = 0.1;
 
   // The topics that we'll subscribe to from the parameter server
   std::string topics_string;
 
-  declareParameter("enabled", rclcpp::ParameterValue(true));
-  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
-  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
-  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
-  declareParameter("combination_method", rclcpp::ParameterValue(1));
-  declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));
-
   auto node = node_.lock();
   if (!node) {
     throw std::runtime_error{"Failed to lock node"};
   }
 
   allow_parameter_qos_overrides_ = nav2::declare_or_get_parameter(node,
     "allow_parameter_qos_overrides", true);
-  node->get_parameter(name_ + "." + "enabled", enabled_);
-  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
-  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
-  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
+  enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
+  footprint_clearing_enabled_ = node->declare_or_get_parameter(
+    name_ + "." + "footprint_clearing_enabled", true);
+  min_obstacle_height_ = node->declare_or_get_parameter(
+    name_ + "." + "min_obstacle_height", 0.0);
+  max_obstacle_height_ = node->declare_or_get_parameter(
+    name_ + "." + "max_obstacle_height", 2.0);
+  int combination_method_param = node->declare_or_get_parameter(
+    name_ + "." + "combination_method", 1);
+  topics_string = node->declare_or_get_parameter(
+    name_ + "." + "observation_sources", std::string(""));
   node->get_parameter("track_unknown_space", track_unknown_space);
   node->get_parameter("transform_tolerance", transform_tolerance);
-  node->get_parameter(name_ + "." + "observation_sources", topics_string);
   double tf_filter_tolerance = nav2::declare_or_get_parameter(
     node, name_ + "." +
     "tf_filter_tolerance", 0.05);
-
-  int combination_method_param{};
-  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
   combination_method_ = combination_method_from_int(combination_method_param);
 
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -147,37 +143,28 @@ void ObstacleLayer::onInitialize()
     std::string topic, sensor_frame, data_type, transport_type;
     bool inf_is_valid, clearing, marking;
 
-    declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));
-    declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
-    declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));
-    declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));
-    declareParameter(source + "." + "data_type", rclcpp::ParameterValue(std::string("LaserScan")));
-    declareParameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
-    declareParameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0));
-    declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
-    declareParameter(source + "." + "marking", rclcpp::ParameterValue(true));
-    declareParameter(source + "." + "clearing", rclcpp::ParameterValue(false));
-    declareParameter(source + "." + "obstacle_max_range", rclcpp::ParameterValue(2.5));
-    declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0));
-    declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0));
-    declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0));
-    declareParameter(source + "." + "transport_type", rclcpp::ParameterValue(std::string("raw")));
-
-    node->get_parameter(name_ + "." + source + "." + "topic", topic);
-    node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
-    node->get_parameter(
-      name_ + "." + source + "." + "observation_persistence",
-      observation_keep_time);
-    node->get_parameter(
-      name_ + "." + source + "." + "expected_update_rate",
-      expected_update_rate);
-    node->get_parameter(name_ + "." + source + "." + "data_type", data_type);
-    node->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height);
-    node->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height);
-    node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
-    node->get_parameter(name_ + "." + source + "." + "marking", marking);
-    node->get_parameter(name_ + "." + source + "." + "clearing", clearing);
-    node->get_parameter(name_ + "." + source + "." + "transport_type", transport_type);
+    topic = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "topic", source);
+    sensor_frame = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "sensor_frame", std::string(""));
+    observation_keep_time = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "observation_persistence", 0.0);
+    expected_update_rate = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "expected_update_rate", 0.0);
+    data_type = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "data_type", std::string("LaserScan"));
+    min_obstacle_height = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "min_obstacle_height", 0.0);
+    max_obstacle_height = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "max_obstacle_height", 0.0);
+    inf_is_valid = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "inf_is_valid", false);
+    marking = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "marking", true);
+    clearing = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "clearing", false);
+    transport_type = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "transport_type", std::string("raw"));
 
     if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
       RCLCPP_FATAL(
@@ -188,14 +175,16 @@ void ObstacleLayer::onInitialize()
     }
 
     // get the obstacle range for the sensor
-    double obstacle_max_range, obstacle_min_range;
-    node->get_parameter(name_ + "." + source + "." + "obstacle_max_range", obstacle_max_range);
-    node->get_parameter(name_ + "." + source + "." + "obstacle_min_range", obstacle_min_range);
+    double obstacle_max_range = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "obstacle_max_range", 2.5);
+    double obstacle_min_range = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "obstacle_min_range", 0.0);
 
     // get the raytrace ranges for the sensor
-    double raytrace_max_range, raytrace_min_range;
-    node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range);
-    node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range);
+    double raytrace_max_range = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "raytrace_max_range", 3.0);
+    double raytrace_min_range = node->declare_or_get_parameter(
+      name_ + "." + source + "." + "raytrace_min_range", 0.0);
 
     topic = joinWithParentNamespace(topic);
 

nav2_costmap_2d/plugins/plugin_container_layer.cpp

@@ -35,21 +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<std::string>{}));
-  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<std::string>{});
+  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(