Dima/update origin

costmap_2d/CMakeLists.txt

@@ -185,6 +185,9 @@ if(CATKIN_ENABLE_TESTING)
 
   catkin_add_gtest(coordinates_test test/coordinates_test.cpp)
   target_link_libraries(coordinates_test costmap_2d)
+
+  catkin_add_gtest(update_origin_test test/update_origin_test.cpp)
+  target_link_libraries(update_origin_test costmap_2d)
 endif()
 
 install( TARGETS
@@ -215,3 +218,21 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   PATTERN ".svn" EXCLUDE
 )
+
+###############
+## Benchmark ##
+###############
+
+set(${PROJECT_NAME}_BENCHMARK OFF)
+if(${PROJECT_NAME}_BENCHMARK)
+  # you may install the benchmark library via sudo apt install libbenchmark-dev
+  find_package(benchmark)
+  if(benchmark_FOUND)
+    message("building benchmark target")
+    add_executable(update_origin_perf perf/update_origin_perf.cpp)
+    target_link_libraries(update_origin_perf benchmark::benchmark ${PROJECT_NAME} ${catkin_LIBRARIES})
+    target_include_directories(update_origin_perf PRIVATE ${catkin_INCLUDE_DIRS})
+  else()
+    message("benchmark library not found")
+  endif()
+endif()

costmap_2d/perf/update_origin_perf.cpp

@@ -0,0 +1,87 @@
+#include <benchmark/benchmark.h>
+#include <costmap_2d/costmap_2d.h>
+
+#include <cmath>
+
+/// @brief Legacy version of the updateOrigin function used for comparing it with
+/// newer implementations.
+struct Costmap2DLegacy : public costmap_2d::Costmap2D
+{
+
+  Costmap2DLegacy(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
+                  double origin_x, double origin_y) : costmap_2d::Costmap2D(cells_size_x, cells_size_y, resolution, origin_x, origin_y) {}
+
+  void updateOrigin(double new_origin_x, double new_origin_y) override
+  {
+    int cell_ox, cell_oy;
+    cell_ox = int((new_origin_x - origin_x_) / resolution_);
+    cell_oy = int((new_origin_y - origin_y_) / resolution_);
+
+    if (cell_ox == 0 && cell_oy == 0)
+      return;
+
+    double new_grid_ox, new_grid_oy;
+    new_grid_ox = origin_x_ + cell_ox * resolution_;
+    new_grid_oy = origin_y_ + cell_oy * resolution_;
+
+    int size_x = size_x_;
+    int size_y = size_y_;
+
+    int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+    lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+    lower_left_y = std::min(std::max(cell_oy, 0), size_y);
+    upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
+    upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
+
+    unsigned int cell_size_x = upper_right_x - lower_left_x;
+    unsigned int cell_size_y = upper_right_y - lower_left_y;
+    unsigned char *local_map = new unsigned char[cell_size_x * cell_size_y];
+
+    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+
+    resetMaps();
+
+    origin_x_ = new_grid_ox;
+    origin_y_ = new_grid_oy;
+
+    int start_x = lower_left_x - cell_ox;
+    int start_y = lower_left_y - cell_oy;
+
+    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+    delete[] local_map;
+  }
+};
+
+static void
+perf_legacy(benchmark::State &_state)
+{
+  // Benchmarks for the current updateOrigin implementation
+  Costmap2DLegacy map(1000, 1000, 0.01, 0, 0);
+  for (auto _ : _state)
+  {
+    for (int ii = 0; ii != 10; ++ii)
+    {
+      map.updateOrigin(map.getOriginX() + ii * 0.2, ii * 0.05);
+    }
+  }
+}
+
+static void
+perf_current(benchmark::State &_state)
+{
+  // Benchmarks for the current updateOrigin implementation
+  costmap_2d::Costmap2D map(1000, 1000, 0.01, 0, 0);
+  for (auto _ : _state)
+  {
+    for (int ii = 0; ii != 10; ++ii)
+    {
+      map.updateOrigin(map.getOriginX() + ii * 0.2, ii * 0.1);
+    }
+  }
+}
+
+// benchmarks comparing the tf product of transform with the eigen-based.
+BENCHMARK(perf_legacy);
+BENCHMARK(perf_current);
+
+BENCHMARK_MAIN();

costmap_2d/src/costmap_2d.cpp

@@ -36,7 +36,15 @@
  *         David V. Lu!!
  *********************************************************************/
 #include <costmap_2d/costmap_2d.h>
+
+#include <ros/console.h>
+
+#include <algorithm>
+#include <cassert>
+#include <cstddef>
 #include <cstdio>
+#include <cmath>
+#include <iostream>
 
 using namespace std;
 
@@ -148,6 +156,7 @@ Costmap2D& Costmap2D::operator=(const Costmap2D& map)
   resolution_ = map.resolution_;
   origin_x_ = map.origin_x_;
   origin_y_ = map.origin_y_;
+  default_value_ = map.default_value_;
 
   // initialize our various maps
   initMaps(size_x_, size_y_);
@@ -268,48 +277,77 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
   if (cell_ox == 0 && cell_oy == 0)
     return;
 
-  // compute the associated world coordinates for the origin cell
-  // because we want to keep things grid-aligned
-  double new_grid_ox, new_grid_oy;
-  new_grid_ox = origin_x_ + cell_ox * resolution_;
-  new_grid_oy = origin_y_ + cell_oy * resolution_;
+  // update the origin with the appropriate world coordinates
+  origin_x_ += cell_ox * resolution_;
+  origin_y_ += cell_oy * resolution_;
 
-  // To save casting from unsigned int to int a bunch of times
-  int size_x = size_x_;
-  int size_y = size_y_;
+  if (std::abs(cell_ox) >= size_x_ || std::abs(cell_oy) >= size_y_)
+  {
+    // If the new and old maps don't overlap, we can just reset the costmap.
+    ROS_DEBUG("Maps don't overlap. Dropping entire data");
+    resetMaps();
+    return;
+  }
 
-  // we need to compute the overlap of the new and existing windows
-  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
-  lower_left_x = min(max(cell_ox, 0), size_x);
-  lower_left_y = min(max(cell_oy, 0), size_y);
-  upper_right_x = min(max(cell_ox + size_x, 0), size_x);
-  upper_right_y = min(max(cell_oy + size_y, 0), size_y);
+  // The size of the windows to copy.
+  const unsigned int window_width = size_x_ - std::abs(cell_ox);
+  const unsigned int window_height = size_y_ - std::abs(cell_oy);
 
-  unsigned int cell_size_x = upper_right_x - lower_left_x;
-  unsigned int cell_size_y = upper_right_y - lower_left_y;
+  assert(window_width <= size_x_ && "window_width out of bounds");
+  assert(window_height <= size_y_ && "window_height out of bounds");
 
-  // we need a map to store the obstacles in the window temporarily
-  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
+  // The stride has the sign of the cell_oy offset; if we move the costmap up,
+  // we have to copy from the bottom up; if we move it down, we copy from the
+  // top down.
+  const std::ptrdiff_t stride = std::copysign(size_x_, cell_oy);
 
-  // copy the local window in the costmap to the local map
-  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+  // The rows will underflow in case of an error.
+  const unsigned int start_target_row = cell_oy < 0 ? size_y_ - 1 : 0;
+  const unsigned int start_source_row = cell_oy < 0 ? size_y_ - 1 + cell_oy : cell_oy;
+  const unsigned int start_target_col = std::max(0, -cell_ox);
+  const unsigned int start_source_col = std::max(0, cell_ox);
 
-  // now we'll set the costmap to be completely unknown if we track unknown space
-  resetMaps();
+  assert(start_target_row <= size_y_ && "start_target_row out of bounds");
+  assert(start_source_row <= size_y_ && "start_source_row out of bounds");
+  assert(start_target_col <= size_x_ && "start_target_col out of bounds");
+  assert(start_source_col <= size_x_ && "start_source_col out of bounds");
 
-  // update the origin with the appropriate world coordinates
-  origin_x_ = new_grid_ox;
-  origin_y_ = new_grid_oy;
+  unsigned char *source_data = costmap_ + (start_source_row * size_x_ + start_source_col);
+  unsigned char *target_data = costmap_ + (start_target_row * size_x_ + start_target_col);
 
-  // compute the starting cell location for copying data back in
-  int start_x = lower_left_x - cell_ox;
-  int start_y = lower_left_y - cell_oy;
+  for (unsigned int row = 0; row != window_height; ++row)
+  {
+    std::copy_n(source_data, window_width, target_data);
+    source_data += stride;
+    target_data += stride;
+  }
+
+  // Now we have to clear the remaining two rectangles.
+  {
+    // Horizontal rectangle.
+    const unsigned int start_row = cell_oy < 0 ? 0 : size_y_ - cell_oy;
+    const unsigned int end_row = cell_oy < 0 ? -cell_oy : size_y_;
 
-  // now we want to copy the overlapping information back into the map, but in its new location
-  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+    assert(start_row <= end_row && "start_row must be smaller than end_row");
+    assert(end_row <= size_y_ && "end_row out of bounds");
 
-  // make sure to clean up
-  delete[] local_map;
+    resetMap(0, start_row, size_x_, end_row);
+  }
+
+  {
+    // Vertical rectangle (smaller one since the map is row-major).
+    const unsigned int start_col = cell_ox < 0 ? 0 : size_x_ - cell_ox;
+    const unsigned int start_row = std::max(-cell_oy, 0);
+    const unsigned int end_col = cell_ox < 0 ? -cell_ox : size_x_;
+    const unsigned int end_row = start_row + window_height;
+
+    assert(start_col <= end_col && "start_col must be smaller than end_col");
+    assert(start_row <= end_row && "start_row must be smaller than end_row");
+    assert(end_col <= size_x_ && "end_col out of bounds");
+    assert(end_row <= size_y_ && "end_row out of bounds");
+
+    resetMap(start_col, start_row, end_col, end_row);
+  }
 }
 
 bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)

costmap_2d/test/update_origin_test.cpp

@@ -0,0 +1,121 @@
+#include <costmap_2d/costmap_2d.h>
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <cmath>
+#include <tuple>
+
+/// @brief Helper class which contains the legacy implementation of updateOrigin.
+struct Costmap2DLegacy : public costmap_2d::Costmap2D
+{
+
+  Costmap2DLegacy(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
+                  double origin_x, double origin_y) : costmap_2d::Costmap2D(cells_size_x, cells_size_y, resolution, origin_x, origin_y) {}
+
+  // This code is the legacy version of the updateOrigin method. It's well proven
+  // and requires a temporal buffer for updating the costmap. Our tests will use
+  // this method as ground-truth.
+  void updateOrigin(double new_origin_x, double new_origin_y) override
+  {
+    int cell_ox, cell_oy;
+    cell_ox = int((new_origin_x - origin_x_) / resolution_);
+    cell_oy = int((new_origin_y - origin_y_) / resolution_);
+
+    if (cell_ox == 0 && cell_oy == 0)
+      return;
+
+    double new_grid_ox, new_grid_oy;
+    new_grid_ox = origin_x_ + cell_ox * resolution_;
+    new_grid_oy = origin_y_ + cell_oy * resolution_;
+
+    int size_x = size_x_;
+    int size_y = size_y_;
+
+    int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+    lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+    lower_left_y = std::min(std::max(cell_oy, 0), size_y);
+    upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
+    upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
+
+    unsigned int cell_size_x = upper_right_x - lower_left_x;
+    unsigned int cell_size_y = upper_right_y - lower_left_y;
+    unsigned char *local_map = new unsigned char[cell_size_x * cell_size_y];
+
+    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+
+    resetMaps();
+
+    origin_x_ = new_grid_ox;
+    origin_y_ = new_grid_oy;
+
+    int start_x = lower_left_x - cell_ox;
+    int start_y = lower_left_y - cell_oy;
+
+    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+    delete[] local_map;
+  }
+
+  bool operator==(const costmap_2d::Costmap2D &other) const
+  {
+    if (size_x_ != other.getSizeInCellsX() || size_y_ != other.getSizeInCellsY())
+      return false;
+    const auto size = size_x_ * size_y_;
+    return std::equal(costmap_, costmap_ + size, other.getCharMap());
+  }
+};
+
+/// @brief Fixture which will provide a costmap to test.
+struct Costmap2DFixture : public testing::Test
+{
+  Costmap2DLegacy map_;
+  Costmap2DFixture() : map_(10, 20, 0.1, 1, 2)
+  {
+    // Fill the map with some data.
+    const size_t size = map_.getSizeInCellsX() * map_.getSizeInCellsY();
+    auto data = map_.getCharMap();
+    for (size_t ii = 0; ii != size; ++ii, ++data)
+    {
+      // this will overflow - but its fine.
+      *data = ii;
+    }
+  }
+};
+
+TEST_F(Costmap2DFixture, no_update)
+{
+  // Verify that if there is no update, the data does not change.
+  costmap_2d::Costmap2D copy_map(map_);
+
+  map_.updateOrigin(map_.getOriginX(), map_.getOriginY());
+  ASSERT_EQ(map_, copy_map);
+}
+
+using coordinate = std::tuple<double, double>;
+struct ParamCostmap2DFixture : public Costmap2DFixture,
+                               public testing::WithParamInterface<coordinate>
+{
+};
+
+INSTANTIATE_TEST_CASE_P(/**/,
+                        ParamCostmap2DFixture,
+                        testing::Combine(testing::Range(-0.1, 2.1, 0.55),
+                                         testing::Range(-0.1, 4.1, 0.44)));
+
+TEST_P(ParamCostmap2DFixture, regression)
+{
+  // Verfies that the updateOrigin produces the same result as the legacy version.
+  const auto param = GetParam();
+  const auto x = std::get<0>(param);
+  const auto y = std::get<1>(param);
+  costmap_2d::Costmap2D copy_map(map_);
+  map_.updateOrigin(x, y);
+  copy_map.updateOrigin(x, y);
+
+  ASSERT_EQ(map_, copy_map);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}