diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 932c062a2df434d7177ce5b9f31e0a34d3fb8053..347a139bb9c30b39dea6a94e06f42692505fcb6a 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -75,11 +75,11 @@ void ObstacleLayer::onInitialize() std::string topics_string; // TODO(mjeronimo): these four are candidates for dynamic update - node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true)); - node_->declare_parameter(name_ + "." + "footprint_clearing_enabled", + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); - node_->declare_parameter(name_ + "." + "max_obstacle_height", rclcpp::ParameterValue(2.0)); - node_->declare_parameter(name_ + "." + "combination_method", rclcpp::ParameterValue(1)); + declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); + declareParameter("combination_method", rclcpp::ParameterValue(1)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); @@ -114,31 +114,31 @@ void ObstacleLayer::onInitialize() std::string topic, sensor_frame, data_type; bool inf_is_valid, clearing, marking; - node_->declare_parameter(source + "." + "topic", rclcpp::ParameterValue(source)); - node_->declare_parameter(source + "." + "sensor_frame", - rclcpp::ParameterValue(std::string(""))); - node_->declare_parameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0)); - node_->declare_parameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0)); - node_->declare_parameter(source + "." + "data_type", - rclcpp::ParameterValue(std::string("LaserScan"))); - node_->declare_parameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0)); - node_->declare_parameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0)); - node_->declare_parameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false)); - node_->declare_parameter(source + "." + "marking", rclcpp::ParameterValue(true)); - node_->declare_parameter(source + "." + "clearing", rclcpp::ParameterValue(false)); - node_->declare_parameter(source + "." + "obstacle_range", rclcpp::ParameterValue(2.5)); - node_->declare_parameter(source + "." + "raytrace_range", rclcpp::ParameterValue(3.0)); - - node_->get_parameter(source + "." + "topic", topic); - node_->get_parameter(source + "." + "sensor_frame", sensor_frame); - node_->get_parameter(source + "." + "observation_persistence", observation_keep_time); - node_->get_parameter(source + "." + "expected_update_rate", expected_update_rate); - node_->get_parameter(source + "." + "data_type", data_type); - node_->get_parameter(source + "." + "min_obstacle_height", min_obstacle_height); - node_->get_parameter(source + "." + "max_obstacle_height", max_obstacle_height); - node_->get_parameter(source + "." + "inf_is_valid", inf_is_valid); - node_->get_parameter(source + "." + "marking", marking); - node_->get_parameter(source + "." + "clearing", clearing); + 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_range", rclcpp::ParameterValue(2.5)); + declareParameter(source + "." + "raytrace_range", rclcpp::ParameterValue(3.0)); + + 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); if (!(data_type == "PointCloud2" || data_type == "LaserScan")) { RCLCPP_FATAL(node_->get_logger(), @@ -149,11 +149,11 @@ void ObstacleLayer::onInitialize() // get the obstacle range for the sensor double obstacle_range; - node_->get_parameter(source + "." + "obstacle_range", obstacle_range); + node_->get_parameter(name_ + "." + source + "." + "obstacle_range", obstacle_range); // get the raytrace range for the sensor double raytrace_range; - node_->get_parameter(source + "." + "raytrace_range", raytrace_range); + node_->get_parameter(name_ + "." + source + "." + "raytrace_range", raytrace_range); RCLCPP_DEBUG(node_->get_logger(), "Creating an observation buffer for source %s, topic %s, frame %s", diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 8738e50c54ca9bb7d37d8096edf8084ccb048d9a..dfe74f96b8f632c21eaf22f22092c92b63c08248 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -121,8 +121,7 @@ StaticLayer::getParameters() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); - declareParameter("map_subscribe_transient_local", - rclcpp::ParameterValue(true)); + declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 919dc03af359eaa9e975bebde37026efe1f9f523..105b88a1492b500b3ab43a811c819cee6b1cd89e 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -59,17 +59,16 @@ void VoxelLayer::onInitialize() { ObstacleLayer::onInitialize(); - node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true)); - node_->declare_parameter(name_ + "." + "footprint_clearing_enabled", - rclcpp::ParameterValue(true)); - node_->declare_parameter(name_ + "." + "max_obstacle_height", rclcpp::ParameterValue(2.0)); - node_->declare_parameter(name_ + "." + "z_voxels", rclcpp::ParameterValue(10)); - node_->declare_parameter(name_ + "." + "origin_z", rclcpp::ParameterValue(0.0)); - node_->declare_parameter(name_ + "." + "z_resolution", rclcpp::ParameterValue(0.2)); - node_->declare_parameter(name_ + "." + "unknown_threshold", rclcpp::ParameterValue(15)); - node_->declare_parameter(name_ + "." + "mark_threshold", rclcpp::ParameterValue(0)); - node_->declare_parameter(name_ + "." + "combination_method", rclcpp::ParameterValue(1)); - node_->declare_parameter(name_ + "." + "publish_voxel_map", rclcpp::ParameterValue(false)); + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); + declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); + declareParameter("z_voxels", rclcpp::ParameterValue(10)); + declareParameter("origin_z", rclcpp::ParameterValue(0.0)); + declareParameter("z_resolution", rclcpp::ParameterValue(0.2)); + declareParameter("unknown_threshold", rclcpp::ParameterValue(15)); + declareParameter("mark_threshold", rclcpp::ParameterValue(0)); + declareParameter("combination_method", rclcpp::ParameterValue(1)); + declareParameter("publish_voxel_map", rclcpp::ParameterValue(false)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index e995846c7197aab81d8c2c3d9eb69e43413b535d..228c060f70d10ac93b04e73c824cc22269567004 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -31,6 +31,7 @@ #include <string> #include <vector> +#include "nav2_util/node_utils.hpp" namespace nav2_costmap_2d { @@ -71,7 +72,7 @@ Layer::declareParameter( const std::string & param_name, const rclcpp::ParameterValue & value) { local_params_.insert(param_name); - node_->declare_parameter(getFullName(param_name), value); + nav2_util::declare_parameter_if_not_declared(node_, getFullName(param_name), value); } bool diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 5bb76e56267bbcaf7ba190b1003defa780de6051..a409846ce6eea5e05851aa34195c92ad89a678be 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -80,6 +80,19 @@ std::string time_to_string(size_t len); rclcpp::NodeOptions get_node_options_default(bool allow_undeclared = true, bool declare_initial_params = true); +template<typename NodeT> +void declare_parameter_if_not_declared( + NodeT node, + const std::string & param_name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()) +{ + if(!node->has_parameter(param_name)) { + node->declare_parameter(param_name, default_value, parameter_descriptor); + } +} + } // namespace nav2_util #endif // NAV2_UTIL__NODE_UTILS_HPP_