Update new_declare_or_get_parameter API for collision_monitor and map_server

nav2_collision_monitor/src/circle.cpp

@@ -95,12 +95,10 @@ bool Circle::getParameters(
   bool use_dynamic_sub = true;  // if getting parameter radius fails, use dynamic subscription
   try {
     // Leave it not initialized: the will cause an error if it will not set
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE);
-    radius_ = node->get_parameter(polygon_name_ + ".radius").as_double();
+    radius_ = node->declare_or_get_parameter<double>(polygon_name_ + ".radius");
     radius_squared_ = radius_ * radius_;
     use_dynamic_sub = false;
-  } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
+  } catch (const rclcpp::exceptions::InvalidParameterValueException &) {
     RCLCPP_INFO(
       logger_,
       "[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.",

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -147,27 +147,14 @@ bool CollisionDetector::getParameters()
 
   auto node = shared_from_this();
 
-  nav2::declare_parameter_if_not_declared(
-    node, "frequency", rclcpp::ParameterValue(10.0));
-  frequency_ = get_parameter("frequency").as_double();
-  nav2::declare_parameter_if_not_declared(
-    node, "base_frame_id", rclcpp::ParameterValue("base_footprint"));
-  base_frame_id = get_parameter("base_frame_id").as_string();
-  nav2::declare_parameter_if_not_declared(
-    node, "odom_frame_id", rclcpp::ParameterValue("odom"));
-  odom_frame_id = get_parameter("odom_frame_id").as_string();
-  nav2::declare_parameter_if_not_declared(
-    node, "transform_tolerance", rclcpp::ParameterValue(0.1));
-  transform_tolerance =
-    tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
-  nav2::declare_parameter_if_not_declared(
-    node, "source_timeout", rclcpp::ParameterValue(2.0));
-  source_timeout =
-    rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double());
-  nav2::declare_parameter_if_not_declared(
-    node, "base_shift_correction", rclcpp::ParameterValue(true));
-  const bool base_shift_correction =
-    get_parameter("base_shift_correction").as_bool();
+  frequency_ = node->declare_or_get_parameter("frequency", 10.0);
+  base_frame_id = node->declare_or_get_parameter("base_frame_id", std::string("base_footprint"));
+  odom_frame_id = node->declare_or_get_parameter("odom_frame_id", std::string("odom"));
+  transform_tolerance = tf2::durationFromSec(
+    node->declare_or_get_parameter("transform_tolerance", 0.1));
+  source_timeout = rclcpp::Duration::from_seconds(
+    node->declare_or_get_parameter("source_timeout", 2.0));
+  const bool base_shift_correction = node->declare_or_get_parameter("base_shift_correction", true);
 
   if (!configureSources(
       base_frame_id, odom_frame_id, transform_tolerance, source_timeout,
@@ -191,14 +178,12 @@ bool CollisionDetector::configurePolygons(
     auto node = shared_from_this();
 
     // Leave it to be not initialized: to intentionally cause an error if it will not set
-    nav2::declare_parameter_if_not_declared(
-      node, "polygons", rclcpp::PARAMETER_STRING_ARRAY);
-    std::vector<std::string> polygon_names = get_parameter("polygons").as_string_array();
+    std::vector<std::string> polygon_names =
+      node->declare_or_get_parameter<std::vector<std::string>>("polygons");
     for (std::string polygon_name : polygon_names) {
       // Leave it not initialized: the will cause an error if it will not set
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name + ".type", rclcpp::PARAMETER_STRING);
-      const std::string polygon_type = get_parameter(polygon_name + ".type").as_string();
+      const std::string polygon_type =
+        node->declare_or_get_parameter<std::string>(polygon_name + ".type");
 
       if (polygon_type == "polygon") {
         polygons_.push_back(
@@ -255,14 +240,11 @@ bool CollisionDetector::configureSources(
     auto node = shared_from_this();
 
     // Leave it to be not initialized to intentionally cause an error if it will not set
-    nav2::declare_parameter_if_not_declared(
-      node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY);
-    std::vector<std::string> source_names = get_parameter("observation_sources").as_string_array();
+    std::vector<std::string> source_names =
+      node->declare_or_get_parameter<std::vector<std::string>>("observation_sources");
     for (std::string source_name : source_names) {
-      nav2::declare_parameter_if_not_declared(
-        node, source_name + ".type",
-        rclcpp::ParameterValue("scan"));  // Laser scanner by default
-      const std::string source_type = get_parameter(source_name + ".type").as_string();
+      const std::string source_type = node->declare_or_get_parameter(
+        source_name + ".type", std::string("scan"));  // Laser scanner by default
 
       if (source_type == "scan") {
         std::shared_ptr<Scan> s = std::make_shared<Scan>(

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -88,10 +88,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
     "~/toggle",
     std::bind(&CollisionMonitor::toggleCMServiceCallback, this, _1, _2, _3));
 
-  nav2::declare_parameter_if_not_declared(
-    node, "use_realtime_priority", rclcpp::ParameterValue(false));
-  bool use_realtime_priority = false;
-  node->get_parameter("use_realtime_priority", use_realtime_priority);
+  bool use_realtime_priority = node->declare_or_get_parameter("use_realtime_priority", false);
   if (use_realtime_priority) {
     try {
       nav2::setSoftRealTimePriority();
@@ -102,8 +99,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
     }
   }
 
-  nav2::declare_parameter_if_not_declared(node, "enabled", rclcpp::ParameterValue(true));
-  node->get_parameter("enabled", enabled_);
+  enabled_ = node->declare_or_get_parameter("enabled", true);
 
   if (!enabled_) {
     RCLCPP_WARN(get_logger(), "Collision monitor is disabled at startup.");
@@ -253,39 +249,23 @@ bool CollisionMonitor::getParameters(
 
   auto node = shared_from_this();
 
-  nav2::declare_parameter_if_not_declared(
-    node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_smoothed"));
-  cmd_vel_in_topic = get_parameter("cmd_vel_in_topic").as_string();
-  nav2::declare_parameter_if_not_declared(
-    node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel"));
-  cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string();
-  nav2::declare_parameter_if_not_declared(
-    node, "state_topic", rclcpp::ParameterValue(""));
-  state_topic = get_parameter("state_topic").as_string();
-
-  nav2::declare_parameter_if_not_declared(
-    node, "base_frame_id", rclcpp::ParameterValue("base_footprint"));
-  base_frame_id = get_parameter("base_frame_id").as_string();
-  nav2::declare_parameter_if_not_declared(
-    node, "odom_frame_id", rclcpp::ParameterValue("odom"));
-  odom_frame_id = get_parameter("odom_frame_id").as_string();
-  nav2::declare_parameter_if_not_declared(
-    node, "transform_tolerance", rclcpp::ParameterValue(0.1));
-  transform_tolerance =
-    tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
-  nav2::declare_parameter_if_not_declared(
-    node, "source_timeout", rclcpp::ParameterValue(2.0));
-  source_timeout =
-    rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double());
-  nav2::declare_parameter_if_not_declared(
-    node, "base_shift_correction", rclcpp::ParameterValue(true));
-  const bool base_shift_correction =
-    get_parameter("base_shift_correction").as_bool();
-
-  nav2::declare_parameter_if_not_declared(
-    node, "stop_pub_timeout", rclcpp::ParameterValue(1.0));
-  stop_pub_timeout_ =
-    rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double());
+  cmd_vel_in_topic = node->declare_or_get_parameter(
+    "cmd_vel_in_topic", std::string("cmd_vel_smoothed"));
+  cmd_vel_out_topic = node->declare_or_get_parameter(
+    "cmd_vel_out_topic", std::string("cmd_vel"));
+  state_topic = node->declare_or_get_parameter("state_topic", std::string(""));
+
+  base_frame_id = node->declare_or_get_parameter(
+    "base_frame_id", std::string("base_footprint"));
+  odom_frame_id = node->declare_or_get_parameter("odom_frame_id", std::string("odom"));
+  transform_tolerance = tf2::durationFromSec(
+    node->declare_or_get_parameter("transform_tolerance", 0.1));
+  source_timeout = rclcpp::Duration::from_seconds(
+    node->declare_or_get_parameter("source_timeout", 2.0));
+  const bool base_shift_correction = node->declare_or_get_parameter("base_shift_correction", true);
+
+  stop_pub_timeout_ = rclcpp::Duration::from_seconds(
+    node->declare_or_get_parameter("stop_pub_timeout", 1.0));
 
   if (
     !configureSources(
@@ -309,14 +289,12 @@ bool CollisionMonitor::configurePolygons(
     auto node = shared_from_this();
 
     // Leave it to be not initialized: to intentionally cause an error if it will not set
-    nav2::declare_parameter_if_not_declared(
-      node, "polygons", rclcpp::PARAMETER_STRING_ARRAY);
-    std::vector<std::string> polygon_names = get_parameter("polygons").as_string_array();
+    std::vector<std::string> polygon_names =
+      node->declare_or_get_parameter<std::vector<std::string>>("polygons");
     for (std::string polygon_name : polygon_names) {
       // Leave it not initialized: the will cause an error if it will not set
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name + ".type", rclcpp::PARAMETER_STRING);
-      const std::string polygon_type = get_parameter(polygon_name + ".type").as_string();
+      const std::string polygon_type =
+        node->declare_or_get_parameter<std::string>(polygon_name + ".type");
 
       if (polygon_type == "polygon") {
         polygons_.push_back(
@@ -362,14 +340,11 @@ bool CollisionMonitor::configureSources(
     auto node = shared_from_this();
 
     // Leave it to be not initialized: to intentionally cause an error if it will not set
-    nav2::declare_parameter_if_not_declared(
-      node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY);
-    std::vector<std::string> source_names = get_parameter("observation_sources").as_string_array();
+    std::vector<std::string> source_names =
+      node->declare_or_get_parameter<std::vector<std::string>>("observation_sources");
     for (std::string source_name : source_names) {
-      nav2::declare_parameter_if_not_declared(
-        node, source_name + ".type",
-        rclcpp::ParameterValue("scan"));  // Laser scanner by default
-      const std::string source_type = get_parameter(source_name + ".type").as_string();
+      const std::string source_type = node->declare_or_get_parameter(
+        source_name + ".type", std::string("scan"));  // Laser scanner by default
 
       if (source_type == "scan") {
         std::shared_ptr<Scan> s = std::make_shared<Scan>(

nav2_collision_monitor/src/pointcloud.cpp

@@ -136,18 +136,11 @@ void PointCloud::getParameters(std::string & source_topic)
 
   getCommonParameters(source_topic);
 
-  nav2::declare_parameter_if_not_declared(
-    node, source_name_ + ".min_height", rclcpp::ParameterValue(0.05));
-  min_height_ = node->get_parameter(source_name_ + ".min_height").as_double();
-  nav2::declare_parameter_if_not_declared(
-    node, source_name_ + ".max_height", rclcpp::ParameterValue(0.5));
-  max_height_ = node->get_parameter(source_name_ + ".max_height").as_double();
-  nav2::declare_parameter_if_not_declared(
-    node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0));
-  min_range_ = node->get_parameter(source_name_ + ".min_range").as_double();
-  nav2::declare_parameter_if_not_declared(
-    node, source_name_ + ".transport_type", rclcpp::ParameterValue(std::string("raw")));
-  transport_type_ = node->get_parameter(source_name_ + ".transport_type").as_string();
+  min_height_ = node->declare_or_get_parameter(source_name_ + ".min_height", 0.05);
+  max_height_ = node->declare_or_get_parameter(source_name_ + ".max_height", 0.5);
+  min_range_ = node->declare_or_get_parameter(source_name_ + ".min_range", 0.0);
+  transport_type_ = node->declare_or_get_parameter(
+    source_name_ + ".transport_type", std::string("raw"));
 }
 
 void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)

nav2_collision_monitor/src/polygon.cpp

@@ -332,10 +332,8 @@ bool Polygon::getCommonParameters(
   try {
     // Get action type.
     // Leave it not initialized: the will cause an error if it will not set.
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".action_type", rclcpp::PARAMETER_STRING);
-    const std::string at_str =
-      node->get_parameter(polygon_name_ + ".action_type").as_string();
+    const std::string at_str = node->declare_or_get_parameter<std::string>(
+      polygon_name_ + ".action_type");
     if (at_str == "stop") {
       action_type_ = STOP;
     } else if (at_str == "slowdown") {
@@ -351,18 +349,11 @@ bool Polygon::getCommonParameters(
       return false;
     }
 
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true));
-    enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool();
-
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4));
-    min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int();
+    enabled_ = node->declare_or_get_parameter(polygon_name_ + ".enabled", true);
+    min_points_ = node->declare_or_get_parameter(polygon_name_ + ".min_points", 4);
 
     try {
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".max_points", rclcpp::PARAMETER_INTEGER);
-      min_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int() + 1;
+      min_points_ = node->declare_or_get_parameter<int>(polygon_name_ + ".max_points") + 1;
       RCLCPP_WARN(
         logger_,
         "[%s]: \"max_points\" parameter was deprecated. Use \"min_points\" instead to specify "
@@ -373,71 +364,49 @@ bool Polygon::getCommonParameters(
     }
 
     if (action_type_ == SLOWDOWN) {
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".slowdown_ratio", rclcpp::ParameterValue(0.5));
-      slowdown_ratio_ = node->get_parameter(polygon_name_ + ".slowdown_ratio").as_double();
+      slowdown_ratio_ = node->declare_or_get_parameter(polygon_name_ + ".slowdown_ratio", 0.5);
     }
 
     if (action_type_ == LIMIT) {
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".linear_limit", rclcpp::ParameterValue(0.5));
-      linear_limit_ = node->get_parameter(polygon_name_ + ".linear_limit").as_double();
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".angular_limit", rclcpp::ParameterValue(0.5));
-      angular_limit_ = node->get_parameter(polygon_name_ + ".angular_limit").as_double();
+      linear_limit_ = node->declare_or_get_parameter(polygon_name_ + ".linear_limit", 0.5);
+      angular_limit_ = node->declare_or_get_parameter(polygon_name_ + ".angular_limit", 0.5);
     }
 
     if (action_type_ == APPROACH) {
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".time_before_collision", rclcpp::ParameterValue(2.0));
-      time_before_collision_ =
-        node->get_parameter(polygon_name_ + ".time_before_collision").as_double();
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".simulation_time_step", rclcpp::ParameterValue(0.1));
-      simulation_time_step_ =
-        node->get_parameter(polygon_name_ + ".simulation_time_step").as_double();
+      time_before_collision_ = node->declare_or_get_parameter(
+        polygon_name_ + ".time_before_collision", 2.0);
+      simulation_time_step_ = node->declare_or_get_parameter(
+        polygon_name_ + ".simulation_time_step", 0.1);
     }
 
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".visualize", rclcpp::ParameterValue(false));
-    visualize_ = node->get_parameter(polygon_name_ + ".visualize").as_bool();
+    visualize_ = node->declare_or_get_parameter(polygon_name_ + ".visualize", false);
     if (visualize_) {
       // Get polygon topic parameter in case if it is going to be published
-      nav2::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_));
-      polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string();
+      polygon_pub_topic = node->declare_or_get_parameter(
+        polygon_name_ + ".polygon_pub_topic", polygon_name_);
     }
 
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false));
-    polygon_subscribe_transient_local_ =
-      node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool();
+    polygon_subscribe_transient_local_ = node->declare_or_get_parameter(
+      polygon_name_ + ".polygon_subscribe_transient_local", false);
 
     if (use_dynamic_sub_topic) {
       if (action_type_ != APPROACH) {
         // Get polygon sub topic
-        nav2::declare_parameter_if_not_declared(
-          node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING);
-        polygon_sub_topic =
-          node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string();
+        polygon_sub_topic = node->declare_or_get_parameter<std::string>(
+          polygon_name_ + ".polygon_sub_topic");
       } else {
         // Obtain the footprint topic to make a footprint subscription for approach polygon
-        nav2::declare_parameter_if_not_declared(
-          node, polygon_name_ + ".footprint_topic",
-          rclcpp::ParameterValue("local_costmap/published_footprint"));
-        footprint_topic =
-          node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
+        footprint_topic = node->declare_or_get_parameter(
+          polygon_name_ + ".footprint_topic",
+          std::string("local_costmap/published_footprint"));
       }
     }
 
     // By default, use all observation sources for polygon
-    nav2::declare_parameter_if_not_declared(
-      node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY);
     const std::vector<std::string> observation_sources =
-      node->get_parameter("observation_sources").as_string_array();
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(observation_sources));
-    sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array();
+      node->declare_or_get_parameter<std::vector<std::string>>("observation_sources");
+    sources_names_ = node->declare_or_get_parameter(
+      polygon_name_ + ".sources_names", observation_sources);
 
     // Check the observation sources configured for polygon are defined
     for (auto source_name : sources_names_) {
@@ -480,13 +449,11 @@ bool Polygon::getParameters(
   bool use_dynamic_sub = true;  // if getting parameter points fails, use dynamic subscription
   try {
     // Leave it uninitialized: it will throw an inner exception if the parameter is not set
-    nav2::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING);
-    std::string poly_string =
-      node->get_parameter(polygon_name_ + ".points").as_string();
+    std::string poly_string = node->declare_or_get_parameter<std::string>(
+      polygon_name_ + ".points");
 
     use_dynamic_sub = !getPolygonFromString(poly_string, poly_);
-  } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
+  } catch (const rclcpp::exceptions::InvalidParameterValueException &) {
     RCLCPP_INFO(
       logger_,
       "[%s]: Polygon points are not defined. Using dynamic subscription instead.",

nav2_collision_monitor/src/polygon_source.cpp

@@ -160,9 +160,8 @@ void PolygonSource::getParameters(std::string & source_topic)
 
   getCommonParameters(source_topic);
 
-  nav2::declare_parameter_if_not_declared(
-    node, source_name_ + ".sampling_distance", rclcpp::ParameterValue(0.1));
-  sampling_distance_ = node->get_parameter(source_name_ + ".sampling_distance").as_double();
+  sampling_distance_ = node->declare_or_get_parameter(
+    source_name_ + ".sampling_distance", 0.1);
 }
 
 void PolygonSource::dataCallback(geometry_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg)