Skip to content

Commit

Permalink
Apply raytraceLine 3D fixes to its 2D version (ros-navigation#2857)
Browse files Browse the repository at this point in the history
  • Loading branch information
AlexeyMerzlyakov authored and jonipol committed Apr 1, 2022
1 parent 28dbc94 commit bc52808
Show file tree
Hide file tree
Showing 4 changed files with 192 additions and 20 deletions.
45 changes: 25 additions & 20 deletions nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
1 change: 1 addition & 0 deletions nav2_costmap_2d/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -3,3 +3,4 @@ set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files)

add_subdirectory(unit)
add_subdirectory(integration)
add_subdirectory(regression)
4 changes: 4 additions & 0 deletions nav2_costmap_2d/test/regression/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp)
target_link_libraries(costmap_bresenham_2d
nav2_costmap_2d_core
)
162 changes: 162 additions & 0 deletions nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp
Original file line number Diff line number Diff line change
@@ -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();
}

0 comments on commit bc52808

Please sign in to comment.