diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 11f571eef1a..fc8b8071d8d 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -67,6 +67,7 @@ add_library(layers SHARED plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp + plugins/plugin_container_layer.cpp ) target_include_directories(layers PUBLIC diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 6748cd5fcae..16e2f4ba61a 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -18,6 +18,9 @@ Filters noise-induced freestanding obstacles or small obstacles groups + + Plugin to group different layers within the same costmap + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp new file mode 100644 index 00000000000..2b34d409a50 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp @@ -0,0 +1,128 @@ +// Copyright (c) 2024 Polymath Robotics, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_ +#define NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/observation_buffer.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "tf2_ros/message_filter.h" +#include "message_filters/subscriber.hpp" +#include "pluginlib/class_loader.hpp" + +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 +{ +/** + * @class PluginContainerLayer + * @brief Holds a list of plugins and applies them only to the specific layer + */ +class PluginContainerLayer : public CostmapLayer +{ +public: + /** + * @brief Initialization process of layer on startup + */ + virtual void onInitialize(); + /** + * @brief Update the bounds of the master costmap by this layer's update + *dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateBounds( + double robot_x, + double robot_y, + double robot_yaw, + double * min_x, + double * min_y, + double * max_x, + double * max_y); + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, + int min_j, + int max_i, + int max_j); + virtual void onFootprintChanged(); + /** @brief Update the footprint to match size of the parent costmap. */ + virtual void matchSize(); + /** + * @brief Deactivate the layer + */ + virtual void deactivate(); + /** + * @brief Activate the layer + */ + virtual void activate(); + /** + * @brief Reset this costmap + */ + virtual void reset(); + /** + * @brief If clearing operations should be processed on this layer or not + */ + virtual bool isClearable(); + /** + * @brief Clear an area in the constituent costmaps with the given dimension + * if invert, then clear everything except these dimensions + */ + void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert) override; + + void addPlugin(std::shared_ptr plugin, std::string layer_name); + pluginlib::ClassLoader plugin_loader_{"nav2_costmap_2d", "nav2_costmap_2d::Layer"}; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + +private: + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + dyn_params_handler_; + + nav2_costmap_2d::CombinationMethod combination_method_; + std::vector> plugins_; + std::vector plugin_names_; + std::vector plugin_types_; +}; +} // namespace nav2_costmap_2d +#endif // NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_ diff --git a/nav2_costmap_2d/plugins/plugin_container_layer.cpp b/nav2_costmap_2d/plugins/plugin_container_layer.cpp new file mode 100644 index 00000000000..7d171f6b6a1 --- /dev/null +++ b/nav2_costmap_2d/plugins/plugin_container_layer.cpp @@ -0,0 +1,234 @@ +// Copyright (c) 2024 Polymath Robotics, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_costmap_2d/plugin_container_layer.hpp" + +#include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/node_utils.hpp" +#include "rclcpp/parameter_events_filter.hpp" +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::PluginContainerLayer, nav2_costmap_2d::Layer) + +using std::vector; + +namespace nav2_costmap_2d +{ + +void PluginContainerLayer::onInitialize() +{ + auto node = node_.lock(); + + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "enabled", + rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "plugins", + rclcpp::ParameterValue(std::vector{})); + nav2_util::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); + combination_method_ = combination_method_from_int(combination_method_param); + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &PluginContainerLayer::dynamicParametersCallback, + this, + std::placeholders::_1)); + + plugin_types_.resize(plugin_names_.size()); + + for (unsigned int i = 0; i < plugin_names_.size(); ++i) { + plugin_types_[i] = nav2_util::get_plugin_type_param(node, name_ + "." + plugin_names_[i]); + std::shared_ptr plugin = plugin_loader_.createSharedInstance(plugin_types_[i]); + addPlugin(plugin, plugin_names_[i]); + } + + default_value_ = nav2_costmap_2d::NO_INFORMATION; + + PluginContainerLayer::matchSize(); + current_ = true; +} + +void PluginContainerLayer::addPlugin(std::shared_ptr plugin, std::string layer_name) +{ + plugins_.push_back(plugin); + auto node = node_.lock(); + plugin->initialize(layered_costmap_, name_ + "." + layer_name, tf_, node, callback_group_); +} + +void PluginContainerLayer::updateBounds( + double robot_x, + double robot_y, + double robot_yaw, + double * min_x, + double * min_y, + double * max_x, + double * max_y) +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); + } +} + +void PluginContainerLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, + int min_j, + int max_i, + int max_j) +{ + std::lock_guard guard(*getMutex()); + if (!enabled_) { + return; + } + + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->updateCosts(*this, min_i, min_j, max_i, max_j); + } + + switch (combination_method_) { + case CombinationMethod::Overwrite: + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + case CombinationMethod::Max: + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + break; + case CombinationMethod::MaxWithoutUnknownOverwrite: + updateWithMaxWithoutUnknownOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Nothing + break; + } + + current_ = true; +} + +void PluginContainerLayer::activate() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->activate(); + } +} + +void PluginContainerLayer::deactivate() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->deactivate(); + } +} + +void PluginContainerLayer::reset() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->reset(); + } + resetMaps(); + current_ = false; +} + +void PluginContainerLayer::onFootprintChanged() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->onFootprintChanged(); + } +} + +void PluginContainerLayer::matchSize() +{ + std::lock_guard guard(*getMutex()); + Costmap2D * master = layered_costmap_->getCostmap(); + resizeMap( + master->getSizeInCellsX(), master->getSizeInCellsY(), + master->getResolution(), master->getOriginX(), master->getOriginY()); + + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->matchSize(); + } +} + +bool PluginContainerLayer::isClearable() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + if((*plugin)->isClearable()) { + return true; + } + } + return false; +} + +void PluginContainerLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert) +{ + CostmapLayer::clearArea(start_x, start_y, end_x, end_y, invert); + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + auto costmap_layer = std::dynamic_pointer_cast(*plugin); + if ((*plugin)->isClearable() && costmap_layer != nullptr) { + costmap_layer->clearArea(start_x, start_y, end_x, end_y, invert); + } + } +} + +rcl_interfaces::msg::SetParametersResult PluginContainerLayer::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_INTEGER) { + if (param_name == name_ + "." + "combination_method") { + combination_method_ = combination_method_from_int(parameter.as_int()); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { + enabled_ = parameter.as_bool(); + current_ = false; + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5413fd3d2c1..6e354b6785f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -177,9 +177,14 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->addPlugin(plugin); // TODO(mjeronimo): instead of get(), use a shared ptr - plugin->initialize( - layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), - shared_from_this(), callback_group_); + try { + plugin->initialize(layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), + shared_from_this(), callback_group_); + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to initialize costmap plugin %s! %s.", + plugin_names_[i].c_str(), e.what()); + return nav2_util::CallbackReturn::FAILURE; + } lock.unlock(); diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 65667847538..dfa32e637d7 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -39,6 +39,15 @@ target_link_libraries(range_tests_exec layers ) +ament_add_gtest_executable(plugin_container_tests_exec + plugin_container_tests.cpp +) +target_link_libraries(plugin_container_tests_exec + nav2_costmap_2d_core + layers +) + + ament_add_gtest(dyn_params_tests dyn_params_tests.cpp ) @@ -113,6 +122,16 @@ ament_add_test(range_tests TEST_EXECUTABLE=$ ) +ament_add_test(plugin_container_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ament_add_test(test_costmap_publisher_exec GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp new file mode 100644 index 00000000000..9a7bf160388 --- /dev/null +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -0,0 +1,688 @@ +// Copyright (c) 2024 Polymath Robotics, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d.hpp" +#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" +#include "nav2_costmap_2d/plugin_container_layer.hpp" + +using std::begin; +using std::end; +using std::for_each; +using std::all_of; +using std::none_of; +using std::pair; +using std::string; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + { + node_ = std::make_shared("plugin_container_test_node"); + node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); + node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("unknown_cost_value", + rclcpp::ParameterValue(static_cast(0xff))); + node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); + } + + ~TestNode() {} + + void waitForMap(std::shared_ptr & slayer) + { + while (!slayer->isCurrent()) { + rclcpp::spin_some(node_->get_node_base_interface()); + } + } + + std::vector setRadii( + nav2_costmap_2d::LayeredCostmap & layers, + double length, double width) + { + std::vector polygon; + geometry_msgs::msg::Point p; + p.x = width; + p.y = length; + polygon.push_back(p); + p.x = width; + p.y = -length; + polygon.push_back(p); + p.x = -width; + p.y = -length; + polygon.push_back(p); + p.x = -width; + p.y = length; + polygon.push_back(p); + layers.setFootprint(polygon); + + return polygon; + } + +protected: + std::shared_ptr node_; +}; + +/* + * For reference, the static map looks like this: + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 254 254 254 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 254 0 0 254 254 254 + * + * 0 0 0 0 254 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * upper left is 0,0, lower right is 9,9 + */ + +/** + * Test if combining layers different plugin container layers works + */ + +TEST_F(TestNode, testObstacleLayers) { + tf2_ros::Buffer tf(node_->get_clock()); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr olayer_a = + std::make_shared(); + pclayer_a->addPlugin(olayer_a, "pclayer_a.obstacles"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + addObservation(olayer_a, 5.0, 5.0); + addObservation(olayer_b, 3.0, 8.0); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 2); +} + +TEST_F(TestNode, testObstacleAndStaticLayers) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + addObservation(olayer_b, 3.0, 8.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); +} + +TEST_F(TestNode, testDifferentInflationLayers) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_b.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 5.0, 4.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4); +} + +TEST_F(TestNode, testDifferentInflationLayers2) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 28); +} + +TEST_F(TestNode, testResetting) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + ASSERT_EQ(pclayer_a->isCurrent(), true); + ASSERT_EQ(pclayer_b->isCurrent(), true); + + ASSERT_EQ(slayer->isCurrent(), true); + ASSERT_EQ(ilayer_a->isCurrent(), true); + ASSERT_EQ(olayer_b->isCurrent(), true); + ASSERT_EQ(ilayer_b->isCurrent(), true); + + pclayer_a->reset(); + pclayer_b->reset(); + + ASSERT_EQ(pclayer_a->isCurrent(), false); + ASSERT_EQ(pclayer_b->isCurrent(), false); + + ASSERT_EQ(slayer->isCurrent(), false); + ASSERT_EQ(ilayer_a->isCurrent(), false); + ASSERT_EQ(olayer_b->isCurrent(), false); + ASSERT_EQ(ilayer_b->isCurrent(), false); +} + +TEST_F(TestNode, testClearing) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 29); + ASSERT_EQ(olayer_b->getCost(9, 9), nav2_costmap_2d::LETHAL_OBSTACLE); + + pclayer_a->clearArea(-1, -1, 10, 10, false); + pclayer_b->clearArea(-1, -1, 10, 10, false); + + ASSERT_EQ(slayer->getCost(9, 0), nav2_costmap_2d::LETHAL_OBSTACLE); + ASSERT_EQ(olayer_b->getCost(9, 9), nav2_costmap_2d::NO_INFORMATION); +} + +TEST_F(TestNode, testOverwriteCombinationMethods) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + node_->set_parameter(rclcpp::Parameter("pclayer_b.combination_method", 0)); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 2); +} + +TEST_F(TestNode, testWithoutUnknownOverwriteCombinationMethods) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + node_->set_parameter(rclcpp::Parameter("pclayer_a.combination_method", 2)); + node_->set_parameter(rclcpp::Parameter("pclayer_b.combination_method", 2)); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 100); +} + +TEST_F(TestNode, testClearable) { + tf2_ros::Buffer tf(node_->get_clock()); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr olayer_a = + std::make_shared(); + pclayer_a->addPlugin(olayer_a, "pclayer_a.obstacles"); + + std::shared_ptr slayer_b = + std::make_shared(); + pclayer_b->addPlugin(slayer_b, "pclayer_b.obstacles"); + + waitForMap(slayer); + + ASSERT_EQ(pclayer_a->isClearable(), true); + ASSERT_EQ(pclayer_b->isClearable(), false); +} + +TEST_F(TestNode, testDynParamsSetPluginContainerLayer) +{ + auto costmap = std::make_shared("test_costmap"); + + // Add obstacle layer + std::vector plugins_str; + plugins_str.push_back("plugin_container_layer_a"); + plugins_str.push_back("plugin_container_layer_b"); + costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + + costmap->declare_parameter("plugin_container_layer_a.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::PluginContainerLayer"))); + costmap->declare_parameter("plugin_container_layer_b.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::PluginContainerLayer"))); + + std::vector plugins_str_a; + plugins_str_a.push_back("obstacle_layer"); + plugins_str_a.push_back("inflation_layer"); + costmap->declare_parameter("plugin_container_layer_a.plugins", + rclcpp::ParameterValue(plugins_str_a)); + + costmap->declare_parameter("plugin_container_layer_a.obstacle_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer"))); + costmap->declare_parameter("plugin_container_layer_a.inflation_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::InflationLayer"))); + + std::vector plugins_str_b; + plugins_str_b.push_back("static_layer"); + plugins_str_b.push_back("inflation_layer"); + costmap->declare_parameter("plugin_container_layer_b.plugins", + rclcpp::ParameterValue(plugins_str_b)); + + costmap->declare_parameter("plugin_container_layer_b.static_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::StaticLayer"))); + costmap->declare_parameter("plugin_container_layer_b.inflation_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::InflationLayer"))); + + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("map"))); + costmap->set_parameter(rclcpp::Parameter("robot_base_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("plugin_container_layer_a.combination_method", 2), + rclcpp::Parameter("plugin_container_layer_b.combination_method", 1), + rclcpp::Parameter("plugin_container_layer_a.enabled", false), + rclcpp::Parameter("plugin_container_layer_b.enabled", true) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_a.combination_method").as_int(), 2); + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_b.combination_method").as_int(), 1); + + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_a.enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_b.enabled").as_bool(), true); + + 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/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index 8ab95e6d444..03e1d6a0bd8 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -17,6 +17,7 @@ #define TESTING_HELPER_HPP_ #include +#include #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -26,6 +27,7 @@ #include "nav2_costmap_2d/range_sensor_layer.hpp" #include "nav2_costmap_2d/obstacle_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_costmap_2d/plugin_container_layer.hpp" #include "nav2_util/lifecycle_node.hpp" const double MAX_Z(1.0); @@ -142,5 +144,16 @@ void addInflationLayer( layers.addPlugin(ipointer); } +void addPluginContainerLayer( + nav2_costmap_2d::LayeredCostmap & layers, + tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr & pclayer, + std::string name, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) +{ + pclayer = std::make_shared(); + pclayer->initialize(&layers, name, &tf, node, callback_group); + layers.addPlugin(std::shared_ptr(pclayer)); +} #endif // TESTING_HELPER_HPP_