diff --git a/.gitignore b/.gitignore index 7fb9789cb56..21696be8722 100644 --- a/.gitignore +++ b/.gitignore @@ -51,3 +51,6 @@ sphinx_doc/_build # CLion artifacts .idea cmake-build-debug/ + +# doxygen docs +doc/html/ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 2ba47090bf6..66558b26d25 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -131,6 +131,13 @@ class ObstacleLayer : public CostmapLayer */ virtual bool isClearable() {return true;} + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + /** * @brief triggers the update of observations buffer */ @@ -234,6 +241,9 @@ class ObstacleLayer : public CostmapLayer /// @brief Used to store observation buffers used for clearing obstacles std::vector> clearing_buffers_; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + // Used only for testing purposes std::vector static_clearing_observations_; std::vector static_marking_observations_; diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 7887456a923..f4883a2b135 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -55,12 +55,14 @@ using nav2_costmap_2d::FREE_SPACE; using nav2_costmap_2d::ObservationBuffer; using nav2_costmap_2d::Observation; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { ObstacleLayer::~ObstacleLayer() { + dyn_params_handler_.reset(); for (auto & notifier : observation_notifiers_) { notifier.reset(); } @@ -74,7 +76,6 @@ void ObstacleLayer::onInitialize() // The topics that we'll subscribe to from the parameter server std::string topics_string; - // TODO(mjeronimo): these four are candidates for dynamic update declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); @@ -94,6 +95,12 @@ void ObstacleLayer::onInitialize() node->get_parameter("transform_tolerance", transform_tolerance); node->get_parameter(name_ + "." + "observation_sources", topics_string); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ObstacleLayer::dynamicParametersCallback, + this, + std::placeholders::_1)); + RCLCPP_INFO( logger_, "Subscribed to Topics: %s", topics_string.c_str()); @@ -272,6 +279,38 @@ void ObstacleLayer::onInitialize() } } +rcl_interfaces::msg::SetParametersResult +ObstacleLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + + 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_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_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + "." + "combination_method") { + combination_method_ = parameter.as_int(); + } + } + } + + result.successful = true; + return result; +} + void ObstacleLayer::laserScanCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr message, @@ -365,6 +404,7 @@ ObstacleLayer::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 guard(*getMutex()); if (rolling_window_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } @@ -455,7 +495,6 @@ ObstacleLayer::updateFootprint( double * max_x, double * max_y) { - std::lock_guard guard(*getMutex()); if (!footprint_clearing_enabled_) {return;} transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index b6ec6bb2c43..707d795a34b 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -408,6 +408,53 @@ TEST_F(TestNode, testRaytracing) { ASSERT_EQ(lethal_count, 1); } +/** + * Test dynamic parameter setting of obstacle layer + */ +TEST_F(TestNode, testDynParamsSetObstacle) +{ + auto costmap = std::make_shared("test_costmap"); + + // Add obstacle layer + std::vector plugins_str; + plugins_str.push_back("obstacle_layer"); + costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + costmap->declare_parameter( + "obstacle_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer"))); + + 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("obstacle_layer.combination_method", 5), + rclcpp::Parameter("obstacle_layer.max_obstacle_height", 4.0), + rclcpp::Parameter("obstacle_layer.enabled", false), + rclcpp::Parameter("obstacle_layer.footprint_clearing_enabled", false) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("obstacle_layer.combination_method").as_int(), 5); + EXPECT_EQ(costmap->get_parameter("obstacle_layer.max_obstacle_height").as_double(), 4.0); + EXPECT_EQ(costmap->get_parameter("obstacle_layer.enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("obstacle_layer.footprint_clearing_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 voxel layer */