From bb058713f61a7108c0fdf49c8e02a49eb3dfd44f Mon Sep 17 00:00:00 2001 From: RBT22 <43723477+RBT22@users.noreply.github.com> Date: Fri, 23 Jun 2023 10:39:21 +0200 Subject: [PATCH] Option for ObstacleLayer to not override StaticLayer's unknown parts (#3612) (#6) * Add updateWithMaxWithoutUnknownOverride * Add missing break to switch case * Add additional NO_INFORMATION check to make more robust * Add CombinationMethod enum with combination_method_from_int * Rename override to overwrite * Update docs of combination_method_from_int * Move definitions to costmap_layer and remove function_name param * Replace logger with node's logger * Fix linting errors * Add test * Add CombinationMethod::Max test as a counter-case --- .../include/nav2_costmap_2d/cost_values.hpp | 28 ++++++++ .../include/nav2_costmap_2d/costmap_layer.hpp | 20 ++++++ .../nav2_costmap_2d/obstacle_layer.hpp | 2 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 14 ++-- nav2_costmap_2d/plugins/voxel_layer.cpp | 7 +- nav2_costmap_2d/src/costmap_layer.cpp | 48 +++++++++++++ .../test/integration/obstacle_tests.cpp | 71 +++++++++++++++++++ 7 files changed, 183 insertions(+), 7 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index dca22c9f7ff..6fbb5352440 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -39,6 +39,34 @@ /** Provides a mapping for often used cost values */ namespace nav2_costmap_2d { + +/** + * @enum nav2_costmap_2d::CombinationMethod + * @brief Describes the method used to add data to master costmap, default to maximum. + */ +enum class CombinationMethod : int +{ + /** + * Overwrite means every valid value from this layer + * is written into the master grid (does not copy NO_INFORMATION) + */ + Overwrite = 0, + /** + * Sets the new value to the maximum of the master_grid's value + * and this layer's value. If the master value is NO_INFORMATION, + * it is overwritten. If the layer's value is NO_INFORMATION, + * the master value does not change + */ + Max = 1, + /** + * Sets the new value to the maximum of the master_grid's value + * and this layer's value. If the master value is NO_INFORMATION, + * it is NOT overwritten. If the layer's value is NO_INFORMATION, + * the master value does not change. + */ + MaxWithoutUnknownOverwrite = 2 +}; + static constexpr unsigned char NO_INFORMATION = 255; static constexpr unsigned char LETHAL_OBSTACLE = 254; static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp index 15285df5fe2..43a82e0cde1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp @@ -128,6 +128,19 @@ class CostmapLayer : public Layer, public Costmap2D nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * Sets the new value to the maximum of the master_grid's value + * and this layer's value. If the master value is NO_INFORMATION, + * it is NOT overwritten. If the layer's value is NO_INFORMATION, + * the master value does not change. + */ + void updateWithMaxWithoutUnknownOverwrite( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + int max_j); + /* * Updates the master_grid within the specified * bounding box using this layer's values. @@ -172,6 +185,13 @@ class CostmapLayer : public Layer, public Costmap2D void useExtraBounds(double * min_x, double * min_y, double * max_x, double * max_y); bool has_extra_bounds_; + /** + * @brief Converts an integer to a CombinationMethod enum and logs on failure + * @param value The integer to convert + * @param function_name The name of the function calling this conversion (for logging) + */ + CombinationMethod combination_method_from_int(const int value); + private: double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; }; 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 754b434930c..4b8409f4678 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -252,7 +252,7 @@ class ObstacleLayer : public CostmapLayer bool rolling_window_; bool was_reset_; - int combination_method_; + nav2_costmap_2d::CombinationMethod combination_method_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 653c0885cae..cccae4d3938 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -92,11 +92,14 @@ void ObstacleLayer::onInitialize() 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_ + "." + "combination_method", combination_method_); node->get_parameter("track_unknown_space", track_unknown_space); node->get_parameter("transform_tolerance", transform_tolerance); node->get_parameter(name_ + "." + "observation_sources", topics_string); + 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( &ObstacleLayer::dynamicParametersCallback, @@ -311,7 +314,7 @@ ObstacleLayer::dynamicParametersCallback( } } else if (param_type == ParameterType::PARAMETER_INTEGER) { if (param_name == name_ + "." + "combination_method") { - combination_method_ = parameter.as_int(); + combination_method_ = combination_method_from_int(parameter.as_int()); } } } @@ -540,12 +543,15 @@ ObstacleLayer::updateCosts( } switch (combination_method_) { - case 0: // Overwrite + case CombinationMethod::Overwrite: updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; - case 1: // Maximum + 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; } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 21d6e669011..fc411d9f42e 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -86,9 +86,12 @@ void VoxelLayer::onInitialize() 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_ + "." + "combination_method", combination_method_); node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); + int combination_method_param{}; + node->get_parameter(name_ + "." + "combination_method", combination_method_param); + combination_method_ = combination_method_from_int(combination_method_param); + auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); if (publish_voxel_) { @@ -522,7 +525,7 @@ VoxelLayer::dynamicParametersCallback( } else if (param_name == name_ + "." + "mark_threshold") { mark_threshold_ = parameter.as_int(); } else if (param_name == name_ + "." + "combination_method") { - combination_method_ = parameter.as_int(); + combination_method_ = combination_method_from_int(parameter.as_int()); } } } diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 3f9da4013d8..842876823df 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -135,6 +135,35 @@ void CostmapLayer::updateWithMax( } } +void CostmapLayer::updateWithMaxWithoutUnknownOverwrite( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, + int max_i, + int max_j) +{ + if (!enabled_) { + return; + } + + unsigned char * master_array = master_grid.getCharMap(); + unsigned int span = master_grid.getSizeInCellsX(); + + for (int j = min_j; j < max_j; j++) { + unsigned int it = j * span + min_i; + for (int i = min_i; i < max_i; i++) { + if (costmap_[it] == NO_INFORMATION) { + it++; + continue; + } + + unsigned char old_cost = master_array[it]; + if (old_cost != NO_INFORMATION && old_cost < costmap_[it]) { + master_array[it] = costmap_[it]; + } + it++; + } + } +} + void CostmapLayer::updateWithTrueOverwrite( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, @@ -215,4 +244,23 @@ void CostmapLayer::updateWithAddition( } } } + +CombinationMethod CostmapLayer::combination_method_from_int(const int value) +{ + switch (value) { + case 0: + return CombinationMethod::Overwrite; + case 1: + return CombinationMethod::Max; + case 2: + return CombinationMethod::MaxWithoutUnknownOverwrite; + default: + RCLCPP_WARN( + logger_, + "Param combination_method: %i. Possible values are 0 (Overwrite) or 1 (Maximum) or " + "2 (Maximum without overwriting the master's NO_INFORMATION values)." + "The default value 1 will be used", value); + return CombinationMethod::Max; + } +} } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 707d795a34b..ebd4c0c2d13 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -554,3 +554,74 @@ TEST_F(TestNode, testDynParamsSetStatic) costmap->on_cleanup(rclcpp_lifecycle::State()); costmap->on_shutdown(rclcpp_lifecycle::State()); } + + +/** + * Test CombinationMethod::Max overwrites unknown value in ObstacleLayer. + */ +TEST_F(TestNode, testMaxCombinationMethod) { + tf2_ros::Buffer tf(node_->get_clock()); + + // Create a costmap with full unknown space + nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2); + + // The observation sets the cost of the cell to 254 + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose + // printMap(*(layers.getCostmap())); + + int unknown_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::NO_INFORMATION); + + ASSERT_EQ(unknown_count, 99); +} + +class TestNodeWithoutUnknownOverwrite : public ::testing::Test +{ +public: + TestNodeWithoutUnknownOverwrite() + { + node_ = std::make_shared("obstacle_test_node"); + node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(true)); + node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + 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(""))); + // MaxWithoutUnknownOverwrite + node_->declare_parameter("obstacles.combination_method", rclcpp::ParameterValue(2)); + } + + ~TestNodeWithoutUnknownOverwrite() {} + +protected: + std::shared_ptr node_; +}; + +/** + * Test CombinationMethod::MaxWithoutUnknownOverwrite in ObstacleLayer. + */ +TEST_F(TestNodeWithoutUnknownOverwrite, testMaxWithoutUnknownOverwriteCombinationMethod) { + tf2_ros::Buffer tf(node_->get_clock()); + + // Create a costmap with full unknown space + nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2); + + // The observation tries to set the cost of the cell to 254, but since it is unknown, it should + // remain unknown. + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose + // printMap(*(layers.getCostmap())); + + int unknown_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::NO_INFORMATION); + + ASSERT_EQ(unknown_count, 100); +}