diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 66f6163b3c..5f68dcaed1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -238,6 +238,13 @@ class InflationLayer : public Layer unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + double inflation_radius_, inscribed_radius_, cost_scaling_factor_; bool inflate_unknown_, inflate_around_unknown_; unsigned int cell_inflation_radius_; @@ -257,6 +264,8 @@ class InflationLayer : public Layer // Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; mutex_t * access_; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index bf047d7f38..62ccadb2bc 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -152,6 +152,13 @@ class StaticLayer : public CostmapLayer */ unsigned char interpretValue(unsigned char value); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + std::string global_frame_; ///< @brief The global frame for the costmap std::string map_frame_; /// @brief frame that map is located in @@ -178,6 +185,9 @@ class StaticLayer : public CostmapLayer tf2::Duration transform_tolerance_; std::atomic update_in_progress_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dyn_param_mutex_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 4e91c940b0..340f071a8f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -51,6 +51,7 @@ #include #include #include +#include namespace nav2_costmap_2d { @@ -220,6 +221,18 @@ class VoxelLayer : public ObstacleLayer { return (size_z_ - 1 + 0.5) * z_resolution_; } + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index daff9ccd08..4cbc2ce0d3 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::InflationLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::LETHAL_OBSTACLE; using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; using nav2_costmap_2d::NO_INFORMATION; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { @@ -77,6 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { + dyn_params_handler_.reset(); delete access_; } @@ -99,6 +101,11 @@ InflationLayer::onInitialize() 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_); + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &InflationLayer::dynamicParametersCallback, + this, std::placeholders::_1)); } current_ = true; @@ -108,6 +115,7 @@ InflationLayer::onInitialize() need_reinflation_ = false; cell_inflation_radius_ = cellDistance(inflation_radius_); matchSize(); + } void @@ -407,4 +415,77 @@ InflationLayer::generateIntegerDistances() return level; } +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +InflationLayer::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard guard(*getMutex()); + + struct paramChange + { + bool inflation_radius : 1; + bool cost_scaling_factor : 1; + bool enabled : 1; + bool inflate_unknown : 1; + bool inflate_around_unknown : 1; + } paramChange_s{0, 0, 0, 0, 0}; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "inflation_radius" && + inflation_radius_ != parameter.as_double()) + { + inflation_radius_ = parameter.as_double(); + paramChange_s.inflation_radius = true; + cell_inflation_radius_ = cellDistance(inflation_radius_); + + } else if (param_name == name_ + "." + "cost_scaling_factor" && + cost_scaling_factor_ != parameter.as_double()) + { + cost_scaling_factor_ = parameter.as_double(); + paramChange_s.cost_scaling_factor = true; + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { + enabled_ = parameter.as_bool(); + paramChange_s.enabled = true; + } else if (param_name == name_ + "." + "inflate_unknown" && + inflate_unknown_ != parameter.as_bool()) + { + inflate_unknown_ = parameter.as_bool(); + paramChange_s.inflate_unknown = true; + } else if (param_name == name_ + "." + "inflate_around_unknown" && + inflate_around_unknown_ != parameter.as_bool()) + { + inflate_around_unknown_ = parameter.as_bool(); + paramChange_s.inflate_around_unknown = true; + } + } + + } + + if (paramChange_s.enabled || paramChange_s.inflate_unknown || + paramChange_s.inflate_around_unknown || paramChange_s.inflation_radius || + paramChange_s.cost_scaling_factor) + { + need_reinflation_ = true; + } + + if (paramChange_s.inflation_radius || paramChange_s.cost_scaling_factor) { + computeCaches(); + } + + result.successful = true; + return result; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 75956d6462..81718ddd04 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -51,6 +51,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::NO_INFORMATION; using nav2_costmap_2d::LETHAL_OBSTACLE; using nav2_costmap_2d::FREE_SPACE; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { @@ -110,6 +111,7 @@ StaticLayer::activate() void StaticLayer::deactivate() { + dyn_params_handler_.reset(); } void @@ -162,6 +164,12 @@ StaticLayer::getParameters() update_in_progress_.store(false); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &StaticLayer::dynamicParametersCallback, + this, std::placeholders::_1)); } void @@ -372,6 +380,7 @@ StaticLayer::updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { + std::lock_guard lock_reinit(dyn_param_mutex_); if (!enabled_) { update_in_progress_.store(false); return; @@ -435,4 +444,46 @@ StaticLayer::updateCosts( current_ = true; } +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +StaticLayer::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(dyn_param_mutex_); + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_name == name_ + "." + "map_subscribe_transient_local" || + param_name == name_ + "." + "map_topic" || + param_name == name_ + "." + "subscribe_to_updates") + { + RCLCPP_WARN( + logger_, "%s is not a dynamic parameter " + "cannot be changed while running. Rejecting parameter update.", param_name.c_str()); + } else if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "transform_tolerance") { + transform_tolerance_ = tf2::durationFromSec(parameter.as_double()); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { + enabled_ = parameter.as_bool(); + + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + has_updated_data_ = true; + } + } + + } + result.successful = true; + return result; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 5887ef467e..8f07d1eb45 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::VoxelLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::NO_INFORMATION; using nav2_costmap_2d::LETHAL_OBSTACLE; using nav2_costmap_2d::FREE_SPACE; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { @@ -102,10 +103,17 @@ void VoxelLayer::onInitialize() unknown_threshold_ += (VOXEL_BITS - size_z_); matchSize(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &VoxelLayer::dynamicParametersCallback, + this, std::placeholders::_1)); } VoxelLayer::~VoxelLayer() { + dyn_params_handler_.reset(); } void VoxelLayer::matchSize() @@ -137,6 +145,8 @@ void VoxelLayer::updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) { + std::lock_guard lock_reinit(mutex_); + if (rolling_window_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } @@ -458,4 +468,60 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) delete[] local_voxel_map; } +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +VoxelLayer::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + + if (param_name == name_ + "." + "max_obstacle_height") { + max_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "origin_z") { + origin_z_ = parameter.as_double(); + } else if (param_name == name_ + "." + "z_resolution") { + z_resolution_ = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } else if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } else if (param_name == name_ + "." + "publish_voxel_map") { + RCLCPP_WARN( + logger_, "publish voxel map is not a dynamic parameter " + "cannot be changed while running. Rejecting parameter update."); + continue; + } + + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + "." + "z_voxels") { + size_z_ = parameter.as_int(); + } else if (param_name == name_ + "." + "unknown_threshold") { + unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_); + } else if (param_name == name_ + "." + "mark_threshold") { + mark_threshold_ = parameter.as_int(); + } else if (param_name == name_ + "." + "combination_method") { + combination_method_ = parameter.as_int(); + } + } + + } + + matchSize(); + + result.successful = true; + return result; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index e3619c070d..2aec40f8f3 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -46,6 +46,7 @@ #include "nav2_costmap_2d/observation_buffer.hpp" #include "../testing_helper.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" using geometry_msgs::msg::Point; using nav2_costmap_2d::CellData; @@ -600,3 +601,48 @@ TEST_F(TestNode, testInflation3) ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1u); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4u); } + + +/** + * Test dynamic parameter setting of inflation layer + */ +TEST_F(TestNode, testDynParamsSet) +{ + auto node = std::make_shared("inflation_dyn_param_test"); + + auto costmap = std::make_shared("test_costmap"); + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link"))); + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("inflation_layer.inflation_radius", 0.0), + rclcpp::Parameter("inflation_layer.cost_scaling_factor", 0.0), + rclcpp::Parameter("inflation_layer.inflate_unknown", true), + rclcpp::Parameter("inflation_layer.inflate_around_unknown", true), + rclcpp::Parameter("inflation_layer.enabled", false) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("inflation_layer.inflation_radius").as_double(), 0.0); + EXPECT_EQ(costmap->get_parameter("inflation_layer.cost_scaling_factor").as_double(), 0.0); + EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_unknown").as_bool(), true); + EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_around_unknown").as_bool(), true); + EXPECT_EQ(costmap->get_parameter("inflation_layer.enabled").as_bool(), false); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); + +} diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 643a3007fe..70528b9ad8 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -42,6 +42,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" #include "../testing_helper.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" using std::begin; using std::end; @@ -405,5 +406,103 @@ TEST_F(TestNode, testRaytracing) { lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE); ASSERT_EQ(lethal_count, 1); +} + +/** + * Test dynamic parameter setting of voxel layer + */ +TEST_F(TestNode, testDynParamsSetVoxel) +{ + auto node = std::make_shared("voxel_dyn_param_test"); + + auto costmap = std::make_shared("test_costmap"); + + // Add voxel layer + std::vector plugins_str; + plugins_str.push_back("voxel_layer"); + costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + costmap->declare_parameter( + "voxel_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::VoxelLayer"))); + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link"))); + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("voxel_layer.combination_method", 0), + rclcpp::Parameter("voxel_layer.mark_threshold", 1), + rclcpp::Parameter("voxel_layer.unknown_threshold", 10), + rclcpp::Parameter("voxel_layer.z_resolution", 0.4), + rclcpp::Parameter("voxel_layer.origin_z", 1.0), + rclcpp::Parameter("voxel_layer.z_voxels", 14), + rclcpp::Parameter("voxel_layer.max_obstacle_height", 4.0), + rclcpp::Parameter("voxel_layer.footprint_clearing_enabled", false), + rclcpp::Parameter("voxel_layer.enabled", false) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("voxel_layer.combination_method").as_int(), 0); + EXPECT_EQ(costmap->get_parameter("voxel_layer.mark_threshold").as_int(), 1); + EXPECT_EQ(costmap->get_parameter("voxel_layer.unknown_threshold").as_int(), 10); + EXPECT_EQ(costmap->get_parameter("voxel_layer.z_resolution").as_double(), 0.4); + EXPECT_EQ(costmap->get_parameter("voxel_layer.origin_z").as_double(), 1.0); + EXPECT_EQ(costmap->get_parameter("voxel_layer.z_voxels").as_int(), 14); + EXPECT_EQ(costmap->get_parameter("voxel_layer.max_obstacle_height").as_double(), 4.0); + EXPECT_EQ(costmap->get_parameter("voxel_layer.footprint_clearing_enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("voxel_layer.enabled").as_bool(), false); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); + +} + + +/** + * Test dynamic parameter setting of static layer + */ +TEST_F(TestNode, testDynParamsSetStatic) +{ + auto node = std::make_shared("static_dyn_param_test"); + + auto costmap = std::make_shared("test_costmap"); + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link"))); + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("static_layer.transform_tolerance", 1.0), + rclcpp::Parameter("static_layer.enabled", false) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("static_layer.transform_tolerance").as_double(), 1.0); + EXPECT_EQ(costmap->get_parameter("static_layer.enabled").as_bool(), false); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); }