diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 4d5f3c717b4..3b6a6987bb5 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -450,8 +450,14 @@ ObstacleLayer::updateBounds( const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_); - double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_; - double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_; + const unsigned int max_range_cells = cellDistance(obs.obstacle_max_range_); + const unsigned int min_range_cells = cellDistance(obs.obstacle_min_range_); + + unsigned int x0, y0; + if (!worldToMap(obs.origin_.x, obs.origin_.y, x0, y0)) { + RCLCPP_DEBUG(logger_, "Sensor origin is out of map bounds"); + continue; + } sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); @@ -472,31 +478,33 @@ ObstacleLayer::updateBounds( continue; } - // compute the squared distance from the hitpoint to the pointcloud's origin - double sq_dist = - (px - - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + - (pz - obs.origin_.z) * (pz - obs.origin_.z); + // now we need to compute the map coordinates for the observation + unsigned int mx, my; + if (!worldToMap(px, py, mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + + // compute the distance from the hitpoint to the pointcloud's origin + // Calculate the distance in cell space to match the ray trace algorithm + // used for clearing obstacles (see Costmap2D::raytraceLine). + const int dx = static_cast(mx) - static_cast(x0); + const int dy = static_cast(my) - static_cast(y0); + const unsigned int dist = static_cast( + std::hypot(static_cast(dx), static_cast(dy))); // if the point is far enough away... we won't consider it - if (sq_dist >= sq_obstacle_max_range) { + if (dist > max_range_cells) { RCLCPP_DEBUG(logger_, "The point is too far away"); continue; } - // if the point is too close, do not conisder it - if (sq_dist < sq_obstacle_min_range) { + // if the point is too close, do not consider it + if (dist < min_range_cells) { RCLCPP_DEBUG(logger_, "The point is too close"); continue; } - // now we need to compute the map coordinates for the observation - unsigned int mx, my; - if (!worldToMap(px, py, mx, my)) { - RCLCPP_DEBUG(logger_, "Computing map coords failed"); - continue; - } - unsigned int index = getIndex(mx, my); costmap_[index] = LETHAL_OBSTACLE; touch(px, py, min_x, min_y, max_x, max_y); diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 3f2f100d7a2..15fccfd32f4 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -56,3 +56,9 @@ target_link_libraries(denoise_layer_test ${PROJECT_NAME}::nav2_costmap_2d_core ${PROJECT_NAME}::layers ) + +ament_add_gtest(obstacle_layer_test obstacle_layer_test.cpp) +target_link_libraries(obstacle_layer_test + ${PROJECT_NAME}::nav2_costmap_2d_core + layers +) diff --git a/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp b/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp new file mode 100644 index 00000000000..376455e2086 --- /dev/null +++ b/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp @@ -0,0 +1,410 @@ +// Copyright 2025 Kudan Ltd + +// 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 "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/obstacle_layer.hpp" +#include "../testing_helper.hpp" +#include "tf2_ros/buffer.hpp" + + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const std::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 ObstacleLayerTest : public ::testing::Test +{ +public: + explicit ObstacleLayerTest(double resolution = 0.1) + : layers_("frame", false, false) + { + node_ = std::make_shared("obstacle_cell_distance_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(""))); + + // 20x20 cells with origin at (0, 0) + layers_.resizeMap(20, 20, resolution, 0, 0); + tf2_ros::Buffer tf(node_->get_clock()); + addObstacleLayer(layers_, tf, node_, obstacle_layer_); + } + + ~ObstacleLayerTest() {} + + void update() + { + double min_x, min_y, max_x, max_y; + obstacle_layer_->updateBounds(0.0, 0.0, 0.0, &min_x, &min_y, &max_x, &max_y); + } + + void printMap() + { + printf("Printing map:\n"); + for (unsigned int y = 0; y < obstacle_layer_->getSizeInCellsY(); y++) { + for (unsigned int x = 0; x < obstacle_layer_->getSizeInCellsX(); x++) { + printf("%4d", static_cast(obstacle_layer_->getCost(x, y))); + } + printf("\n"); + } + } + +protected: + std::shared_ptr obstacle_layer_; + +private: + std::shared_ptr node_; + nav2_costmap_2d::LayeredCostmap layers_; +}; + +class ObstacleLayerFineResolutionTest : public ObstacleLayerTest +{ +public: + ObstacleLayerFineResolutionTest() + : ObstacleLayerTest(0.05) {} +}; + +/** + * Test that a point within cell_max_range is marked as obstacle + */ +TEST_F(ObstacleLayerTest, testPointWithinCellMaxRange) +{ + // Add observation: point at (0.55, 0.0) + // obstacle_max_range = 1.0m + addObservation( + obstacle_layer_, 0.55, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.55, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that a point beyond cell_max_range is NOT marked as obstacle + */ +TEST_F(ObstacleLayerTest, testPointBeyondCellMaxRange) +{ + // Add observation: point at (1.55, 0.0) + // obstacle_max_range = 1.0m + addObservation( + obstacle_layer_, 1.55, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(1.55, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_NE(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that a point below cell_min_range is NOT marked as obstacle + */ +TEST_F(ObstacleLayerTest, testPointWithinCellMinRange) +{ + // Add observation: point at (0.35, 0.0) + // obstacle_min_range = 0.5m, obstacle_max_range = 10.0m + addObservation( + obstacle_layer_, 0.35, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 10.0, 0.5); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.35, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_NE(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that points within diagonal distance are marked as obstacles + */ +TEST_F(ObstacleLayerTest, testDiagonalDistanceWithinRange) +{ + // Add observation: point at (0.701, 0.701) + // obstacle_max_range = 1.0m + // obstacle range = sqrt(7^2 + 7^2) = 9.9 cells + addObservation( + obstacle_layer_, 0.701, 0.701, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + unsigned int mx, my; + obstacle_layer_->worldToMap(0.701, 0.701, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that point beyond obstacle max range but in same cell as max range is + * marked as obstacle. + */ +TEST_F(ObstacleLayerTest, testPointBeyondRangeButInSameCellAsMaxRange) { + // Observation at (0.79, 0.0) + // obstacle_max_range = 0.72m + // obstacle range = 0.79m, > 0.72m but at same cell distance as max range so + // should be marked as obstacle, since it could be cleared by raytracing with + // the same max range. + addObservation( + obstacle_layer_, 0.79, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 0.72, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.79, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test diagonal distance beyond range + */ +TEST_F(ObstacleLayerTest, testDiagonalDistanceBeyondRange) +{ + // Add observation: point at (1.0, 1.0) + // obstacle_max_range = 1.2m + // Distance: sqrt(10^2 + 10^2) = 14.14 cells, 14 cells > 12 cells, so should + // NOT be marked + addObservation( + obstacle_layer_, 1.0, 1.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.2, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(1.0, 1.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_NE(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test edge case: point exactly at cell_max_range boundary + */ +TEST_F(ObstacleLayerTest, testPointAtCellMaxRangeBoundary) +{ + // Add observation: point at (1.0, 0.0) + // obstacle_max_range = 1.0m + addObservation( + obstacle_layer_, 1.0, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + unsigned int mx, my; + obstacle_layer_->worldToMap(1.0, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test edge case: point exactly at cell_min_range boundary + */ +TEST_F(ObstacleLayerTest, testPointAtCellMinRangeBoundary) +{ + // Add observation: point at (0.5, 0.0) + // obstacle_min_range = 0.5m + addObservation( + obstacle_layer_, 0.5, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 10.0, 0.5); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.5, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test with different resolution + */ +TEST_F(ObstacleLayerFineResolutionTest, testDifferentResolution) +{ + // Add observation: point at (0.52, 0.28) + // Resolution: 0.05m/cell + // Cell index: (10, 5) + addObservation( + obstacle_layer_, 0.52, 0.28, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + + unsigned char cost = obstacle_layer_->getCost(10, 5); + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test with origin offset + */ +TEST_F(ObstacleLayerTest, testOriginOffset) +{ + // Add observation: point at (1.75, 1.85) with origin at (0.5, 1.5) + // obstacle_max_range = 2.0m + addObservation( + obstacle_layer_, 1.75, 1.85, MAX_Z / 2, 0.5, 1.5, MAX_Z / 2, + true, true, 100.0, 0.0, 2.0, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(1.75, 1.85, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that raytracing clears cells along the path and the endpoint is marked + * as obstacle. + */ +TEST_F(ObstacleLayerTest, testRaytraceClearsPathAndMarksEndpoint) +{ + // Mark a few cells along the path as obstacles, excluding the endpoint + for (unsigned int x = 0; x < 4; ++x) { + obstacle_layer_->setCost(x, 0, nav2_costmap_2d::LETHAL_OBSTACLE); + } + + // Add observation at (0.85, 0.0) + addObservation( + obstacle_layer_, 0.85, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 2.0, 0.0, 2.0, 0.0); + update(); + + unsigned int mx_end; + unsigned int my_end; + obstacle_layer_->worldToMap(0.85, 0.0, mx_end, my_end); + + // Check that cells before endpoint are cleared (FREE_SPACE) + for (unsigned int x = 0; x < mx_end; ++x) { + unsigned char cost = obstacle_layer_->getCost(x, 0); + ASSERT_EQ(cost, nav2_costmap_2d::FREE_SPACE); + } + + // Check that the endpoint cell is LETHAL_OBSTACLE + unsigned char cost_endpoint = obstacle_layer_->getCost(mx_end, my_end); + ASSERT_EQ(cost_endpoint, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test diagonal raytracing does not clear endpoint + */ +TEST_F(ObstacleLayerTest, testDiagonalRaytraceDoesNotClearEndpoint) +{ + // Mark the endpoint as obstacle + unsigned int mx_end, my_end; + obstacle_layer_->worldToMap(0.75, 0.75, mx_end, my_end); + obstacle_layer_->setCost(mx_end, my_end, nav2_costmap_2d::LETHAL_OBSTACLE); + // Mark a cell along the diagonal path + unsigned int mx_mid, my_mid; + obstacle_layer_->worldToMap(0.45, 0.45, mx_mid, my_mid); + obstacle_layer_->setCost(mx_mid, my_mid, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Add observation at (0.75, 0.75) + addObservation( + obstacle_layer_, 0.75, 0.75, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 2.0, 0.0, 100.0, 0.0); + update(); + + // The endpoint cell should still be LETHAL_OBSTACLE + unsigned char cost_endpoint = obstacle_layer_->getCost(mx_end, my_end); + ASSERT_EQ(cost_endpoint, nav2_costmap_2d::LETHAL_OBSTACLE); + + // The intermediate cell should be cleared + unsigned char cost_mid = obstacle_layer_->getCost(mx_mid, my_mid); + ASSERT_EQ(cost_mid, nav2_costmap_2d::FREE_SPACE); +} + +/** + * Test diagonal clearing up to max range + */ +TEST_F(ObstacleLayerTest, testClearDiagonalDistance) { + // Mark all points as obstacles + for (unsigned int x = 0; x < obstacle_layer_->getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < obstacle_layer_->getSizeInCellsY(); y++) { + obstacle_layer_->setCost(x, y, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + // Add observation at (1.55, 1.55) with max clearing range of 1.0m + // This should clear the diagonal distance up to range of 1.0m, cells (0, 0) + // to (7, 7) + addObservation( + obstacle_layer_, 1.55, 1.55, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 1.0, 0.0, 100.0, 0.0); + update(); + + for (unsigned int i = 0; i < 8; i++) { + ASSERT_EQ(obstacle_layer_->getCost(i, i), nav2_costmap_2d::FREE_SPACE); + } + ASSERT_EQ(countValues(*obstacle_layer_, nav2_costmap_2d::FREE_SPACE), 8); + ASSERT_EQ( + countValues(*obstacle_layer_, nav2_costmap_2d::LETHAL_OBSTACLE), + 20 * 20 - 8); +}