From f0bb1f1ad046cd3ec168c5fc69c8338a4bc072a6 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 21 Mar 2022 16:19:17 +0300 Subject: [PATCH] Apply raytraceLine 3D fixes to its 2D version (back-port of #2460 and #2781) --- .../include/nav2_costmap_2d/costmap_2d.hpp | 45 ++--- nav2_costmap_2d/test/CMakeLists.txt | 1 + .../test/regression/CMakeLists.txt | 4 + .../test/regression/costmap_bresenham_2d.cpp | 162 ++++++++++++++++++ 4 files changed, 192 insertions(+), 20 deletions(-) create mode 100644 nav2_costmap_2d/test/regression/CMakeLists.txt create mode 100644 nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index ad56711c3fb..ba8c701262f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -433,48 +433,53 @@ class Costmap2D unsigned int y1, unsigned int max_length = UINT_MAX, unsigned int min_length = 0) { - int dx = x1 - x0; - int dy = y1 - y0; - - unsigned int abs_dx = abs(dx); - unsigned int abs_dy = abs(dy); - - int offset_dx = sign(dx); - int offset_dy = sign(dy) * size_x_; + int dx_full = x1 - x0; + int dy_full = y1 - y0; // we need to chose how much to scale our dominant dimension, // based on the maximum length of the line - double dist = std::hypot(dx, dy); + double dist = std::hypot(dx_full, dy_full); if (dist < min_length) { return; } - // Adjust starting point and offset to start from min_length distance - unsigned int min_x0 = (unsigned int)(x0 + dx / dist * min_length); - unsigned int min_y0 = (unsigned int)(y0 + dy / dist * min_length); + unsigned int min_x0, min_y0; + if (dist > 0.0) { + // Adjust starting point and offset to start from min_length distance + min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); + min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); + } else { + // dist can be 0 if [x0, y0]==[x1, y1]. + // In this case only this cell should be processed. + min_x0 = x0; + min_y0 = y0; + } unsigned int offset = min_y0 * size_x_ + min_x0; + int dx = x1 - min_x0; + int dy = y1 - min_y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * size_x_; + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); - unsigned int length; // if x is dominant if (abs_dx >= abs_dy) { int error_y = abs_dx / 2; - // Subtract minlength from length since initial point (x0, y0)has been adjusted by min Z - length = (unsigned int)(scale * abs_dx) - min_length; - bresenham2D( - at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, length); + at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); return; } // otherwise y is dominant int error_x = abs_dy / 2; - // Subtract minlength from total length since initial point (x0, y0) has been adjusted by min Z - length = (unsigned int)(scale * abs_dy) - min_length; bresenham2D( - at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, length); + at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); } private: diff --git a/nav2_costmap_2d/test/CMakeLists.txt b/nav2_costmap_2d/test/CMakeLists.txt index 44b89f86933..ad7905ad1f5 100644 --- a/nav2_costmap_2d/test/CMakeLists.txt +++ b/nav2_costmap_2d/test/CMakeLists.txt @@ -3,3 +3,4 @@ set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) add_subdirectory(unit) add_subdirectory(integration) +add_subdirectory(regression) diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt new file mode 100644 index 00000000000..c492459d2ff --- /dev/null +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -0,0 +1,4 @@ +ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) +target_link_libraries(costmap_bresenham_2d + nav2_costmap_2d_core +) diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp new file mode 100644 index 00000000000..a5b67bb8a79 --- /dev/null +++ b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp @@ -0,0 +1,162 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2022 Samsung Research Russia +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Alexey Merzlyakov +*********************************************************************/ +#include +#include + +class CostmapAction +{ +public: + explicit CostmapAction( + unsigned char * costmap, unsigned int size, unsigned char mark_val = 128) + : costmap_(costmap), size_(size), mark_val_(mark_val) + { + } + + inline void operator()(unsigned int off) + { + ASSERT_TRUE(off < size_); + costmap_[off] = mark_val_; + } + inline unsigned int get(unsigned int off) + { + return costmap_[off]; + } + +private: + unsigned char * costmap_; + unsigned int size_; + unsigned char mark_val_; +}; + +class CostmapTest : public nav2_costmap_2d::Costmap2D +{ +public: + CostmapTest( + unsigned int size_x, unsigned int size_y, double resolution, + double origin_x, double origin_y, unsigned char default_val = 0) + : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val) + { + } + + unsigned char * getCostmap() + { + return costmap_; + } + + unsigned int getSize() + { + return size_x_ * size_y_; + } + + void raytraceLine( + CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) + { + nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } +}; + +TEST(costmap_2d, bresenham2DBoundariesCheck) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); + CostmapAction ca(ct.getCostmap(), ct.getSize()); + + // Initial point - some assymetrically standing point in order to cover most corner cases + const unsigned int x0 = 2; + const unsigned int y0 = 4; + // (x1, y1) point will move + unsigned int x1, y1; + + // Running on (x, 0) edge + y1 = 0; + for (unsigned int i = 0; i < sz_x; i++) { + x1 = i; + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - 1; + for (unsigned int i = 0; i < sz_x; i++) { + x1 = i; + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0; + for (unsigned int j = 0; j < sz_y; j++) { + y1 = j; + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x; + for (unsigned int j = 0; j < sz_y; j++) { + y1 = j; + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } +} + +TEST(costmap_2d, bresenham2DSamePoint) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); + CostmapAction ca(ct.getCostmap(), ct.getSize()); + + // Initial point + const double x0 = 2; + const double y0 = 4; + + unsigned int offset = y0 * sz_x + x0; + unsigned char val_before = ca.get(offset); + // Same point to check + ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length); + unsigned char val_after = ca.get(offset); + ASSERT_FALSE(val_before == val_after); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}