Apply raytraceLine 3D fixes to its 2D version

(back-port of ros-navigation#2460 and ros-navigation#2781)
AlexeyMerzlyakov · Mar 21, 2022 · f0bb1f1 · f0bb1f1
1 parent 8190716
commit f0bb1f1
Show file tree

Hide file tree

Showing 4 changed files with 192 additions and 20 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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 <nav2_costmap_2d/costmap_2d.hpp>
+#include <gtest/gtest.h>
+
+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();
+}