diff --git a/nav2_controller/plugins/test/path_handler.cpp b/nav2_controller/plugins/test/path_handler.cpp index dc2b0cdca01..070a766302e 100644 --- a/nav2_controller/plugins/test/path_handler.cpp +++ b/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( "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( "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()); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 571df68c9d3..f929f93778e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/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 & 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 local_params_; - private: std::vector footprint_spec_; }; diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index cfcac48a004..c319df7e005 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/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 diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 74fbf046fec..fb79dc74839 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/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(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 diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index a72d0ab00fa..bb21956ad13 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/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_ = \ diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index 92882a49734..8495413ea9b 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/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); diff --git a/nav2_costmap_2d/plugins/denoise_layer.cpp b/nav2_costmap_2d/plugins/denoise_layer.cpp index e38c61a9e03..f17cd0f70ad 100644 --- a/nav2_costmap_2d/plugins/denoise_layer.cpp +++ b/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(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 { diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 90dfbab319e..89c1c57ac8b 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/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( diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 7cf7cbbba9b..bcfdab605a0 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -79,18 +79,11 @@ 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"}; @@ -98,19 +91,22 @@ void ObstacleLayer::onInitialize() 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); diff --git a/nav2_costmap_2d/plugins/plugin_container_layer.cpp b/nav2_costmap_2d/plugins/plugin_container_layer.cpp index 7352ab07df4..0a61a41dbdd 100644 --- a/nav2_costmap_2d/plugins/plugin_container_layer.cpp +++ b/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{})); - 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/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index 275403a8283..3230cf6d936 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -73,33 +73,28 @@ void RangeSensorLayer::onInitialize() throw std::runtime_error{"Failed to lock node"}; } - declareParameter("enabled", rclcpp::ParameterValue(true)); - node->get_parameter(name_ + "." + "enabled", enabled_); - declareParameter("phi", rclcpp::ParameterValue(1.2)); - node->get_parameter(name_ + "." + "phi", phi_v_); - declareParameter("inflate_cone", rclcpp::ParameterValue(1.0)); - node->get_parameter(name_ + "." + "inflate_cone", inflate_cone_); - declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0)); - node->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); - declareParameter("clear_threshold", rclcpp::ParameterValue(0.2)); - node->get_parameter(name_ + "." + "clear_threshold", clear_threshold_); - declareParameter("mark_threshold", rclcpp::ParameterValue(0.8)); - node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); - declareParameter("clear_on_max_reading", rclcpp::ParameterValue(false)); - node->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_); + enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true); + phi_v_ = node->declare_or_get_parameter(name_ + "." + "phi", 1.2); + inflate_cone_ = node->declare_or_get_parameter(name_ + "." + "inflate_cone", 1.0); + no_readings_timeout_ = node->declare_or_get_parameter( + name_ + "." + "no_readings_timeout", 0.0); + clear_threshold_ = node->declare_or_get_parameter( + name_ + "." + "clear_threshold", 0.2); + mark_threshold_ = node->declare_or_get_parameter( + name_ + "." + "mark_threshold", 0.8); + clear_on_max_reading_ = node->declare_or_get_parameter( + name_ + "." + "clear_on_max_reading", false); double temp_tf_tol = 0.0; node->get_parameter("transform_tolerance", temp_tf_tol); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); - std::vector topic_names{}; - declareParameter("topics", rclcpp::ParameterValue(topic_names)); - node->get_parameter(name_ + "." + "topics", topic_names); + std::vector topic_names = node->declare_or_get_parameter( + name_ + "." + "topics", std::vector{}); InputSensorType input_sensor_type = InputSensorType::ALL; - std::string sensor_type_name; - declareParameter("input_sensor_type", rclcpp::ParameterValue("ALL")); - node->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name); + std::string sensor_type_name = node->declare_or_get_parameter( + name_ + "." + "input_sensor_type", std::string("ALL")); std::transform( sensor_type_name.begin(), sensor_type_name.end(), diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index a23fc71c1b5..3d568b79022 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -133,28 +133,23 @@ StaticLayer::getParameters() int temp_lethal_threshold = 0; double temp_tf_tol = 0.0; - declareParameter("enabled", rclcpp::ParameterValue(true)); - declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); - declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); - declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); - declareParameter("map_topic", rclcpp::ParameterValue("map")); - declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); - declareParameter("restore_cleared_footprint", rclcpp::ParameterValue(true)); - auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; } - node->get_parameter(name_ + "." + "enabled", enabled_); - node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); - node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); - node->get_parameter(name_ + "." + "restore_cleared_footprint", restore_cleared_footprint_); - node->get_parameter(name_ + "." + "map_topic", map_topic_); + enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true); + subscribe_to_updates_ = node->declare_or_get_parameter( + name_ + "." + "subscribe_to_updates", false); + footprint_clearing_enabled_ = node->declare_or_get_parameter( + name_ + "." + "footprint_clearing_enabled", false); + restore_cleared_footprint_ = node->declare_or_get_parameter( + name_ + "." + "restore_cleared_footprint", true); + map_topic_ = node->declare_or_get_parameter( + name_ + "." + "map_topic", std::string("map")); map_topic_ = joinWithParentNamespace(map_topic_); - node->get_parameter( - name_ + "." + "map_subscribe_transient_local", - map_subscribe_transient_local_); + map_subscribe_transient_local_ = node->declare_or_get_parameter( + name_ + "." + "map_subscribe_transient_local", true); node->get_parameter("track_unknown_space", track_unknown_space_); node->get_parameter("use_maximum", use_maximum_); node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index c90e7781cce..f20f01f34ef 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -62,36 +62,28 @@ void VoxelLayer::onInitialize() { ObstacleLayer::onInitialize(); - 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("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)); - auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; } - 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_); - node->get_parameter(name_ + "." + "z_voxels", size_z_); - node->get_parameter(name_ + "." + "origin_z", origin_z_); - node->get_parameter(name_ + "." + "z_resolution", z_resolution_); - node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); - node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); - node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); - - int combination_method_param{}; - node->get_parameter(name_ + "." + "combination_method", combination_method_param); + 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); + size_z_ = node->declare_or_get_parameter(name_ + "." + "z_voxels", 10); + origin_z_ = node->declare_or_get_parameter(name_ + "." + "origin_z", 0.0); + z_resolution_ = node->declare_or_get_parameter(name_ + "." + "z_resolution", 0.2); + unknown_threshold_ = node->declare_or_get_parameter( + name_ + "." + "unknown_threshold", 15); + mark_threshold_ = node->declare_or_get_parameter(name_ + "." + "mark_threshold", 0); + int combination_method_param = node->declare_or_get_parameter( + name_ + "." + "combination_method", 1); + publish_voxel_ = node->declare_or_get_parameter( + name_ + "." + "publish_voxel_map", false); combination_method_ = combination_method_from_int(combination_method_param); if (publish_voxel_) { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5ea1d20682a..f20b67d9d6e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -111,34 +111,11 @@ Costmap2DROS::Costmap2DROS( 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)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); - declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); - 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("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); - 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() @@ -410,27 +387,46 @@ 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_); - 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_); + 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_padding_ = declare_or_get_parameter("footprint_padding", 0.01f); + footprint_ = declare_or_get_parameter( + "footprint", std::string("[]")); + global_frame_ = declare_or_get_parameter( + "global_frame", std::string("map")); + map_height_meters_ = declare_or_get_parameter( + "height", 5); + map_width_meters_ = declare_or_get_parameter( + "width", 5); + origin_x_ = declare_or_get_parameter( + "origin_x", 0.0); + origin_y_ = declare_or_get_parameter( + "origin_y", 0.0); + plugin_names_ = declare_or_get_parameter( + "plugins", default_plugins_); + filter_names_ = declare_or_get_parameter( + "filters", std::vector()); + map_publish_frequency_ = declare_or_get_parameter( + "publish_frequency", 1.0); + resolution_ = declare_or_get_parameter( + "resolution", 0.1); + robot_base_frame_ = declare_or_get_parameter( + "robot_base_frame", std::string("base_link")); + robot_radius_ = declare_or_get_parameter( + "robot_radius", 0.1); + 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); + initial_transform_timeout_ = declare_or_get_parameter( + "initial_transform_timeout", 60.0); + map_update_frequency_ = declare_or_get_parameter( + "update_frequency", 5.0); + subscribe_to_stamped_footprint_ = declare_or_get_parameter( + "subscribe_to_stamped_footprint", false); auto node = shared_from_this(); diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index c0437974fe7..c64ff6f1603 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -72,44 +72,6 @@ Layer::getFootprint() const return layered_costmap_->getFootprint(); } -void -Layer::declareParameter( - const std::string & param_name, - const rclcpp::ParameterValue & value) -{ - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - local_params_.insert(param_name); - nav2::declare_parameter_if_not_declared( - node, getFullName(param_name), value); -} - -void -Layer::declareParameter( - const std::string & param_name, - const rclcpp::ParameterType & param_type) -{ - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - local_params_.insert(param_name); - nav2::declare_parameter_if_not_declared( - node, getFullName(param_name), param_type); -} - -bool -Layer::hasParameter(const std::string & param_name) -{ - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - return node->has_parameter(getFullName(param_name)); -} - std::string Layer::getFullName(const std::string & param_name) { diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 35b0169a0b7..5da52c8278a 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -337,11 +337,10 @@ TEST_F(TestNode, testRepeatedResets) { // Set parameters auto plugins = layers.getPlugins(); for_each( - begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) { + begin(*plugins), end(*plugins), [this, &layer_dummy](const auto & plugin) { string layer_param = layer_dummy.first + "_" + plugin->getName(); - // Notice we are using Layer::declareParameter - plugin->declareParameter(layer_param, rclcpp::ParameterValue(layer_dummy.second)); + node_->declare_parameter(layer_param, rclcpp::ParameterValue(layer_dummy.second)); }); // Check that all parameters have been set @@ -351,9 +350,9 @@ TEST_F(TestNode, testRepeatedResets) { // layer-level param ASSERT_TRUE( all_of( - begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) { + begin(*plugins), end(*plugins), [this, &layer_dummy](const auto & plugin) { string layer_param = layer_dummy.first + "_" + plugin->getName(); - return plugin->hasParameter(layer_param); + return node_->has_parameter(layer_param); })); // Reset all layers. Parameters should be declared if not declared, otherwise skipped. diff --git a/nav2_costmap_2d/test/regression/plugin_api_order.cpp b/nav2_costmap_2d/test/regression/plugin_api_order.cpp index 318ded02bf1..a457b111e6c 100644 --- a/nav2_costmap_2d/test/regression/plugin_api_order.cpp +++ b/nav2_costmap_2d/test/regression/plugin_api_order.cpp @@ -25,10 +25,14 @@ TEST(CostmapPluginsTester, checkPluginAPIOrder) std::make_shared("costmap_ros"); // Workaround to avoid setting base_link->map transform + costmap_ros->declare_parameter("robot_base_frame", "map"); costmap_ros->set_parameter(rclcpp::Parameter("robot_base_frame", "map")); // Specifying order verification plugin in the parameters std::vector plugins_str; plugins_str.push_back("order_layer"); + costmap_ros->declare_parameter( + "plugins", + rclcpp::ParameterValue(plugins_str)); costmap_ros->set_parameter(rclcpp::Parameter("plugins", plugins_str)); costmap_ros->declare_parameter( "order_layer.plugin", diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp index a426dd5a8a9..d757a605750 100644 --- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -38,7 +38,7 @@ TEST(DeclareParameter, useValidParameter) layer.initialize(&layers, "test_layer", &tf, node, nullptr); - layer.declareParameter("test1", rclcpp::ParameterValue("test_val1")); + node->declare_parameter("test_layer.test1", rclcpp::ParameterValue("test_val1")); try { std::string val = node->get_parameter("test_layer.test1").as_string(); EXPECT_EQ(val, "test_val1"); @@ -57,7 +57,7 @@ TEST(DeclareParameter, useInvalidParameter) layer.initialize(&layers, "test_layer", &tf, node, nullptr); - layer.declareParameter("test2", rclcpp::PARAMETER_STRING); + node->declare_parameter("test_layer.test2", rclcpp::PARAMETER_STRING); try { std::string val = node->get_parameter("test_layer.test2").as_string(); FAIL() << "Incorrectly handling test_layer.test2 parameter which was not set"; diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index e54264ebeae..ed81acc36e7 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -25,6 +25,7 @@ TEST(LifecylceTest, CheckInitialTfTimeout) { rclcpp::init(0, nullptr); auto costmap = std::make_shared(rclcpp::NodeOptions()); + costmap->declare_parameter("initial_transform_timeout", 0.0); costmap->set_parameter({"initial_transform_timeout", 0.0}); std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 85057dae1c3..c716107eb90 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -470,15 +470,11 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) { // Create a costmap of 10x10 meters auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } + costmap_ros->declare_parameter("global_frame", rclcpp::ParameterValue("test_global_frame")); + costmap_ros->declare_parameter("robot_base_frame", rclcpp::ParameterValue("test_robot_frame")); + costmap_ros->declare_parameter("width", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("height", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("resolution", rclcpp::ParameterValue(0.1)); costmap_ros->on_configure(rclcpp_lifecycle::State()); // Create controller @@ -556,15 +552,11 @@ TEST(GracefulControllerTest, computeVelocityCommandRegular) { // Create a costmap of 10x10 meters auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } + costmap_ros->declare_parameter("global_frame", rclcpp::ParameterValue("test_global_frame")); + costmap_ros->declare_parameter("robot_base_frame", rclcpp::ParameterValue("test_robot_frame")); + costmap_ros->declare_parameter("width", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("height", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("resolution", rclcpp::ParameterValue(0.1)); costmap_ros->on_configure(rclcpp_lifecycle::State()); // Create controller @@ -645,15 +637,11 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { // Create a costmap of 10x10 meters auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } + costmap_ros->declare_parameter("global_frame", rclcpp::ParameterValue("test_global_frame")); + costmap_ros->declare_parameter("robot_base_frame", rclcpp::ParameterValue("test_robot_frame")); + costmap_ros->declare_parameter("width", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("height", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("resolution", rclcpp::ParameterValue(0.1)); costmap_ros->on_configure(rclcpp_lifecycle::State()); // Create controller @@ -729,15 +717,11 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) { // Create a costmap of 10x10 meters auto costmap_ros = std::make_shared("test_costmap"); - auto results = costmap_ros->set_parameters( - {rclcpp::Parameter("global_frame", "test_global_frame"), - rclcpp::Parameter("robot_base_frame", "test_robot_frame"), - rclcpp::Parameter("width", 10), - rclcpp::Parameter("height", 10), - rclcpp::Parameter("resolution", 0.1)}); - for (const auto & result : results) { - EXPECT_TRUE(result.successful) << result.reason; - } + costmap_ros->declare_parameter("global_frame", rclcpp::ParameterValue("test_global_frame")); + costmap_ros->declare_parameter("robot_base_frame", rclcpp::ParameterValue("test_robot_frame")); + costmap_ros->declare_parameter("width", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("height", rclcpp::ParameterValue(10)); + costmap_ros->declare_parameter("resolution", rclcpp::ParameterValue(0.1)); costmap_ros->on_configure(rclcpp_lifecycle::State()); // Create controller diff --git a/nav2_planner/test/test_plan_through_poses.cpp b/nav2_planner/test/test_plan_through_poses.cpp index f0aa854c07c..029d980b84f 100644 --- a/nav2_planner/test/test_plan_through_poses.cpp +++ b/nav2_planner/test/test_plan_through_poses.cpp @@ -163,10 +163,10 @@ void Tester::setParameters() PLANNER_PLUGINS.front() + ".tolerance", rclcpp::ParameterValue(PLANNER_TOLERANCE)); auto costmap = planner_->getCostmapROS(); - costmap->set_parameter(rclcpp::Parameter("height", COSTMAP_HEIGHT_METERS)); - costmap->set_parameter(rclcpp::Parameter("width", COSTMAP_WIDTH_METERS)); - costmap->set_parameter(rclcpp::Parameter("resolution", COSTMAP_RESOLUTION)); - costmap->set_parameter(rclcpp::Parameter("plugins", COSTMAP_PLUGINS)); + costmap->declare_parameter("height", COSTMAP_HEIGHT_METERS); + costmap->declare_parameter("width", COSTMAP_WIDTH_METERS); + costmap->declare_parameter("resolution", COSTMAP_RESOLUTION); + costmap->declare_parameter("plugins", COSTMAP_PLUGINS); costmap->declare_parameter("static_layer.plugin", rclcpp::ParameterValue(COSTMAP_LAYER_NAME)); costmap->declare_parameter("static_layer.lethal_cost_threshold", rclcpp::ParameterValue(100)); diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index b2fbedd498a..9791b84fede 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -219,8 +219,8 @@ TEST(RotationShimControllerTest, computeVelocityTests) auto tf = std::make_shared(node->get_clock()); auto listener = std::make_shared(*tf, node, true); auto costmap = std::make_shared("fake_costmap"); - costmap->set_parameter(rclcpp::Parameter("origin_x", 0.0)); - costmap->set_parameter(rclcpp::Parameter("origin_y", 0.0)); + costmap->declare_parameter("origin_x", 0.0); + costmap->declare_parameter("origin_y", 0.0); costmap->configure(); auto tf_broadcaster = std::make_shared(node);