add humble-navigation2

navigation2/nav2_voxel_grid/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_voxel_grid)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include)
+
+add_library(voxel_grid SHARED
+  src/voxel_grid.cpp
+)
+
+set(dependencies
+  rclcpp
+)
+
+ament_target_dependencies(voxel_grid
+  ${dependencies}
+)
+
+install(TARGETS voxel_grid
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+ament_export_dependencies(rclcpp)
+ament_export_include_directories(include)
+ament_export_libraries(voxel_grid)
+
+ament_package()

navigation2/nav2_voxel_grid/README.md

@@ -0,0 +1,9 @@
+# Nav2 Voxel Grid
+
+The `nav2_voxel_grid` package contains the VoxelGrid used by the `Voxel Layer` inside of `nav2_costmap_2d`. The voxel grid itself is simply a 2D char pointer array of the map size with bit locations corresponding to voxel values (free, unknown, occupied , etc). 
+
+It is branched out as a separate package for use in other applications where a dense voxel grid representation may be useful. It also contains implementations of 3D raycasting. 
+
+## ROS1 Comparison
+
+This package is a direct port to ROS2 for use in the voxel layer. 

navigation2/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp

@@ -0,0 +1,456 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  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 the Willow Garage 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: Eitan Marder-Eppstein
+ *********************************************************************/
+#ifndef NAV2_VOXEL_GRID__VOXEL_GRID_HPP_
+#define NAV2_VOXEL_GRID__VOXEL_GRID_HPP_
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+#include <limits.h>
+#include <algorithm>
+#include "rclcpp/rclcpp.hpp"
+
+/**
+ * @class VoxelGrid
+ * @brief A 3D grid structure that stores points as an integer array.
+ *        X and Y index the array and Z selects which bit of the integer
+ *        is used giving a limit of 16 vertical cells.
+ */
+namespace nav2_voxel_grid
+{
+
+enum VoxelStatus
+{
+  FREE = 0,
+  UNKNOWN = 1,
+  MARKED = 2,
+};
+
+class VoxelGrid
+{
+public:
+  /**
+   * @brief  Constructor for a voxel grid
+   * @param size_x The x size of the grid
+   * @param size_y The y size of the grid
+   * @param size_z The z size of the grid, only sizes <= 16 are supported
+   */
+  VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z);
+
+  ~VoxelGrid();
+
+  /**
+   * @brief  Resizes a voxel grid to the desired size
+   * @param size_x The x size of the grid
+   * @param size_y The y size of the grid
+   * @param size_z The z size of the grid, only sizes <= 16 are supported
+   */
+  void resize(unsigned int size_x, unsigned int size_y, unsigned int size_z);
+
+  void reset();
+  uint32_t * getData() {return data_;}
+
+  inline void markVoxel(unsigned int x, unsigned int y, unsigned int z)
+  {
+    if (x >= size_x_ || y >= size_y_ || z >= size_z_) {
+      RCLCPP_DEBUG(logger, "Error, voxel out of bounds.\n");
+      return;
+    }
+    uint32_t full_mask = ((uint32_t)1 << z << 16) | (1 << z);
+    data_[y * size_x_ + x] |= full_mask;  // clear unknown and mark cell
+  }
+
+  inline bool markVoxelInMap(
+    unsigned int x, unsigned int y, unsigned int z,
+    unsigned int marked_threshold)
+  {
+    if (x >= size_x_ || y >= size_y_ || z >= size_z_) {
+      RCLCPP_DEBUG(logger, "Error, voxel out of bounds.\n");
+      return false;
+    }
+
+    int index = y * size_x_ + x;
+    uint32_t * col = &data_[index];
+    uint32_t full_mask = ((uint32_t)1 << z << 16) | (1 << z);
+    *col |= full_mask;  // clear unknown and mark cell
+
+    unsigned int marked_bits = *col >> 16;
+
+    // make sure the number of bits in each is below our thresholds
+    return !bitsBelowThreshold(marked_bits, marked_threshold);
+  }
+
+  inline void clearVoxel(unsigned int x, unsigned int y, unsigned int z)
+  {
+    if (x >= size_x_ || y >= size_y_ || z >= size_z_) {
+      RCLCPP_DEBUG(logger, "Error, voxel out of bounds.\n");
+      return;
+    }
+    uint32_t full_mask = ((uint32_t)1 << z << 16) | (1 << z);
+    data_[y * size_x_ + x] &= ~(full_mask);  // clear unknown and clear cell
+  }
+
+  inline void clearVoxelColumn(unsigned int index)
+  {
+    assert(index < size_x_ * size_y_);
+    data_[index] = 0;
+  }
+
+  inline void clearVoxelInMap(unsigned int x, unsigned int y, unsigned int z)
+  {
+    if (x >= size_x_ || y >= size_y_ || z >= size_z_) {
+      RCLCPP_DEBUG(logger, "Error, voxel out of bounds.\n");
+      return;
+    }
+    int index = y * size_x_ + x;
+    uint32_t * col = &data_[index];
+    uint32_t full_mask = ((uint32_t)1 << z << 16) | (1 << z);
+    *col &= ~(full_mask);  // clear unknown and clear cell
+
+    unsigned int unknown_bits = uint16_t(*col >> 16) ^ uint16_t(*col);
+    unsigned int marked_bits = *col >> 16;
+
+    // make sure the number of bits in each is below our thresholds
+    if (bitsBelowThreshold(unknown_bits, 1) && bitsBelowThreshold(marked_bits, 1)) {
+      costmap[index] = 0;
+    }
+  }
+
+  inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
+  {
+    unsigned int bit_count;
+    for (bit_count = 0; n; ) {
+      ++bit_count;
+      if (bit_count > bit_threshold) {
+        return false;
+      }
+      n &= n - 1;  // clear the least significant bit set
+    }
+    return true;
+  }
+
+  static inline unsigned int numBits(unsigned int n)
+  {
+    unsigned int bit_count;
+    for (bit_count = 0; n; ++bit_count) {
+      n &= n - 1;  // clear the least significant bit set
+    }
+    return bit_count;
+  }
+
+  static VoxelStatus getVoxel(
+    unsigned int x, unsigned int y, unsigned int z,
+    unsigned int size_x, unsigned int size_y, unsigned int size_z, const uint32_t * data)
+  {
+    if (x >= size_x || y >= size_y || z >= size_z) {
+      return UNKNOWN;
+    }
+    uint32_t full_mask = ((uint32_t)1 << z << 16) | (1 << z);
+    uint32_t result = data[y * size_x + x] & full_mask;
+    unsigned int bits = numBits(result);
+
+    // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
+    if (bits < 2) {
+      if (bits < 1) {
+        return FREE;
+      }
+      return UNKNOWN;
+    }
+    return MARKED;
+  }
+
+  void markVoxelLine(
+    double x0, double y0, double z0, double x1, double y1, double z1,
+    unsigned int max_length = UINT_MAX);
+  void clearVoxelLine(
+    double x0, double y0, double z0, double x1, double y1, double z1,
+    unsigned int max_length = UINT_MAX, unsigned int min_length = 0);
+  void clearVoxelLineInMap(
+    double x0, double y0, double z0, double x1, double y1, double z1, unsigned char * map_2d,
+    unsigned int unknown_threshold, unsigned int mark_threshold,
+    unsigned char free_cost = 0, unsigned char unknown_cost = 255,
+    unsigned int max_length = UINT_MAX, unsigned int min_length = 0);
+
+  VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z);
+
+  // Are there any obstacles at that (x, y) location in the grid?
+  VoxelStatus getVoxelColumn(
+    unsigned int x, unsigned int y,
+    unsigned int unknown_threshold = 0, unsigned int marked_threshold = 0);
+
+  void printVoxelGrid();
+  void printColumnGrid();
+  unsigned int sizeX();
+  unsigned int sizeY();
+  unsigned int sizeZ();
+
+  template<class ActionType>
+  inline void raytraceLine(
+    ActionType at, double x0, double y0, double z0,
+    double x1, double y1, double z1, unsigned int max_length = UINT_MAX,
+    unsigned int min_length = 0)
+  {
+    // we need to chose how much to scale our dominant dimension, based on the
+    // maximum length of the line
+    double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1));
+    if ((unsigned int)(dist) < min_length) {
+      return;
+    }
+    double scale, min_x0, min_y0, min_z0;
+    if (dist > 0.0) {
+      scale = std::min(1.0, max_length / dist);
+
+      // Updating starting point to the point at distance min_length from the initial point
+      min_x0 = x0 + (x1 - x0) / dist * min_length;
+      min_y0 = y0 + (y1 - y0) / dist * min_length;
+      min_z0 = z0 + (z1 - z0) / dist * min_length;
+    } else {
+      // dist can be 0 if [x0, y0, z0]==[x1, y1, z1].
+      // In this case only this voxel should be processed.
+      scale = 1.0;
+      min_x0 = x0;
+      min_y0 = y0;
+      min_z0 = z0;
+    }
+
+    int dx = int(x1) - int(min_x0);  // NOLINT
+    int dy = int(y1) - int(min_y0);  // NOLINT
+    int dz = int(z1) - int(min_z0);  // NOLINT
+
+    unsigned int abs_dx = abs(dx);
+    unsigned int abs_dy = abs(dy);
+    unsigned int abs_dz = abs(dz);
+
+    int offset_dx = sign(dx);
+    int offset_dy = sign(dy) * size_x_;
+    int offset_dz = sign(dz);
+
+    unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)min_z0;
+    unsigned int offset = (unsigned int)min_y0 * size_x_ + (unsigned int)min_x0;
+
+    GridOffset grid_off(offset);
+    ZOffset z_off(z_mask);
+
+    // is x dominant
+    if (abs_dx >= max(abs_dy, abs_dz)) {
+      int error_y = abs_dx / 2;
+      int error_z = abs_dx / 2;
+
+      bresenham3D(
+        at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z,
+        offset_dx, offset_dy, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dx));
+      return;
+    }
+
+    // y is dominant
+    if (abs_dy >= abs_dz) {
+      int error_x = abs_dy / 2;
+      int error_z = abs_dy / 2;
+
+      bresenham3D(
+        at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z,
+        offset_dy, offset_dx, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dy));
+      return;
+    }
+
+    // otherwise, z is dominant
+    int error_x = abs_dz / 2;
+    int error_y = abs_dz / 2;
+
+    bresenham3D(
+      at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz,
+      offset_dx, offset_dy, offset, z_mask, (unsigned int)(scale * abs_dz));
+  }
+
+private:
+  // the real work is done here... 3D bresenham implementation
+  template<class ActionType, class OffA, class OffB, class OffC>
+  inline void bresenham3D(
+    ActionType at, OffA off_a, OffB off_b, OffC off_c,
+    unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc,
+    int error_b, int error_c, int offset_a, int offset_b, int offset_c, unsigned int & offset,
+    unsigned int & z_mask, unsigned int max_length = UINT_MAX)
+  {
+    unsigned int end = std::min(max_length, abs_da);
+    for (unsigned int i = 0; i < end; ++i) {
+      at(offset, z_mask);
+      off_a(offset_a);
+      error_b += abs_db;
+      error_c += abs_dc;
+      if ((unsigned int)error_b >= abs_da) {
+        off_b(offset_b);
+        error_b -= abs_da;
+      }
+      if ((unsigned int)error_c >= abs_da) {
+        off_c(offset_c);
+        error_c -= abs_da;
+      }
+    }
+    at(offset, z_mask);
+  }
+
+  inline int sign(int i)
+  {
+    return i > 0 ? 1 : -1;
+  }
+
+  inline unsigned int max(unsigned int x, unsigned int y)
+  {
+    return x > y ? x : y;
+  }
+
+  unsigned int size_x_, size_y_, size_z_;
+  uint32_t * data_;
+  unsigned char * costmap;
+  rclcpp::Logger logger;
+
+  // Aren't functors so much fun... used to recreate the Bresenham macro Eric
+  // wrote in the original version, but in "proper" c++
+  class MarkVoxel
+  {
+public:
+    explicit MarkVoxel(uint32_t * data)
+    : data_(data) {}
+    inline void operator()(unsigned int offset, unsigned int z_mask)
+    {
+      data_[offset] |= z_mask;  // clear unknown and mark cell
+    }
+
+private:
+    uint32_t * data_;
+  };
+
+  class ClearVoxel
+  {
+public:
+    explicit ClearVoxel(uint32_t * data)
+    : data_(data) {}
+    inline void operator()(unsigned int offset, unsigned int z_mask)
+    {
+      data_[offset] &= ~(z_mask);  // clear unknown and clear cell
+    }
+
+private:
+    uint32_t * data_;
+  };
+
+  class ClearVoxelInMap
+  {
+public:
+    ClearVoxelInMap(
+      uint32_t * data, unsigned char * costmap,
+      unsigned int unknown_clear_threshold, unsigned int marked_clear_threshold,
+      unsigned char free_cost = 0, unsigned char unknown_cost = 255)
+    : data_(data), costmap_(costmap),
+      unknown_clear_threshold_(unknown_clear_threshold), marked_clear_threshold_(
+        marked_clear_threshold),
+      free_cost_(free_cost), unknown_cost_(unknown_cost)
+    {
+    }
+
+    inline void operator()(unsigned int offset, unsigned int z_mask)
+    {
+      uint32_t * col = &data_[offset];
+      *col &= ~(z_mask);  // clear unknown and clear cell
+
+      unsigned int unknown_bits = uint16_t(*col >> 16) ^ uint16_t(*col);
+      unsigned int marked_bits = *col >> 16;
+
+      // make sure the number of bits in each is below our thresholds
+      if (bitsBelowThreshold(marked_bits, marked_clear_threshold_)) {
+        if (bitsBelowThreshold(unknown_bits, unknown_clear_threshold_)) {
+          costmap_[offset] = free_cost_;
+        } else {
+          costmap_[offset] = unknown_cost_;
+        }
+      }
+    }
+
+private:
+    inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
+    {
+      unsigned int bit_count;
+      for (bit_count = 0; n; ) {
+        ++bit_count;
+        if (bit_count > bit_threshold) {
+          return false;
+        }
+        n &= n - 1;  // clear the least significant bit set
+      }
+      return true;
+    }
+
+    uint32_t * data_;
+    unsigned char * costmap_;
+    unsigned int unknown_clear_threshold_, marked_clear_threshold_;
+    unsigned char free_cost_, unknown_cost_;
+  };
+
+  class GridOffset
+  {
+public:
+    explicit GridOffset(unsigned int & offset)
+    : offset_(offset) {}
+    inline void operator()(int offset_val)
+    {
+      offset_ += offset_val;
+    }
+
+private:
+    unsigned int & offset_;
+  };
+
+  class ZOffset
+  {
+public:
+    explicit ZOffset(unsigned int & z_mask)
+    : z_mask_(z_mask) {}
+    inline void operator()(int offset_val)
+    {
+      offset_val > 0 ? z_mask_ <<= 1 : z_mask_ >>= 1;
+    }
+
+private:
+    unsigned int & z_mask_;
+  };
+};
+
+}  // namespace nav2_voxel_grid
+
+#endif  // NAV2_VOXEL_GRID__VOXEL_GRID_HPP_

navigation2/nav2_voxel_grid/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_voxel_grid</name>
+  <version>1.1.18</version>
+  <description>
+      voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures.
+  </description>
+  <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
+
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navigation2/nav2_voxel_grid/src/voxel_grid.cpp

@@ -0,0 +1,259 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  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 the Willow Garage 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: Eitan Marder-Eppstein
+*********************************************************************/
+#include <nav2_voxel_grid/voxel_grid.hpp>
+
+namespace nav2_voxel_grid
+{
+VoxelGrid::VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z)
+: logger(rclcpp::get_logger("voxel_grid"))
+{
+  size_x_ = size_x;
+  size_y_ = size_y;
+  size_z_ = size_z;
+
+  if (size_z_ > 16) {
+    RCLCPP_INFO(
+      logger, "Error, this implementation can only support up to 16 z values (%d)",
+      size_z_);
+    size_z_ = 16;
+  }
+
+  data_ = new uint32_t[size_x_ * size_y_];
+  uint32_t unknown_col = ~((uint32_t)0) >> 16;
+  uint32_t * col = data_;
+  for (unsigned int i = 0; i < size_x_ * size_y_; ++i) {
+    *col = unknown_col;
+    ++col;
+  }
+}
+
+void VoxelGrid::resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
+{
+  // if we're not actually changing the size, we can just reset things
+  if (size_x == size_x_ && size_y == size_y_ && size_z == size_z_) {
+    reset();
+    return;
+  }
+
+  delete[] data_;
+  size_x_ = size_x;
+  size_y_ = size_y;
+  size_z_ = size_z;
+
+  if (size_z_ > 16) {
+    RCLCPP_INFO(
+      logger, "Error, this implementation can only support up to 16 z values (%d)",
+      size_z);
+    size_z_ = 16;
+  }
+
+  data_ = new uint32_t[size_x_ * size_y_];
+  uint32_t unknown_col = ~((uint32_t)0) >> 16;
+  uint32_t * col = data_;
+  for (unsigned int i = 0; i < size_x_ * size_y_; ++i) {
+    *col = unknown_col;
+    ++col;
+  }
+}
+
+VoxelGrid::~VoxelGrid()
+{
+  delete[] data_;
+}
+
+void VoxelGrid::reset()
+{
+  uint32_t unknown_col = ~((uint32_t)0) >> 16;
+  uint32_t * col = data_;
+  for (unsigned int i = 0; i < size_x_ * size_y_; ++i) {
+    *col = unknown_col;
+    ++col;
+  }
+}
+
+void VoxelGrid::markVoxelLine(
+  double x0, double y0, double z0, double x1, double y1, double z1,
+  unsigned int max_length)
+{
+  if (x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
+    z1 >= size_z_)
+  {
+    RCLCPP_DEBUG(
+      logger,
+      "Error, line endpoint out of bounds. "
+      "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f),  size: (%d, %d, %d)",
+      x0, y0, z0, x1, y1, z1, size_x_, size_y_, size_z_);
+    return;
+  }
+
+  MarkVoxel mv(data_);
+  raytraceLine(mv, x0, y0, z0, x1, y1, z1, max_length);
+}
+
+void VoxelGrid::clearVoxelLine(
+  double x0, double y0, double z0, double x1, double y1, double z1,
+  unsigned int max_length, unsigned int min_length)
+{
+  if (x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
+    z1 >= size_z_)
+  {
+    RCLCPP_DEBUG(
+      logger,
+      "Error, line endpoint out of bounds. "
+      "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f),  size: (%d, %d, %d)",
+      x0, y0, z0, x1, y1, z1, size_x_, size_y_, size_z_);
+    return;
+  }
+
+  ClearVoxel cv(data_);
+  raytraceLine(cv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+}
+
+void VoxelGrid::clearVoxelLineInMap(
+  double x0, double y0, double z0, double x1, double y1, double z1, unsigned char * map_2d,
+  unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost,
+  unsigned char unknown_cost, unsigned int max_length, unsigned int min_length)
+{
+  costmap = map_2d;
+  if (map_2d == NULL) {
+    clearVoxelLine(x0, y0, z0, x1, y1, z1, max_length, min_length);
+    return;
+  }
+
+  if (x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
+    z1 >= size_z_)
+  {
+    RCLCPP_DEBUG(
+      logger,
+      "Error, line endpoint out of bounds. "
+      "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f),  size: (%d, %d, %d)",
+      x0, y0, z0, x1, y1, z1, size_x_, size_y_, size_z_);
+    return;
+  }
+
+  ClearVoxelInMap cvm(data_, costmap, unknown_threshold, mark_threshold, free_cost, unknown_cost);
+  raytraceLine(cvm, x0, y0, z0, x1, y1, z1, max_length, min_length);
+}
+
+VoxelStatus VoxelGrid::getVoxel(unsigned int x, unsigned int y, unsigned int z)
+{
+  if (x >= size_x_ || y >= size_y_ || z >= size_z_) {
+    RCLCPP_DEBUG(logger, "Error, voxel out of bounds. (%d, %d, %d)\n", x, y, z);
+    return UNKNOWN;
+  }
+  uint32_t full_mask = ((uint32_t)1 << z << 16) | (1 << z);
+  uint32_t result = data_[y * size_x_ + x] & full_mask;
+  unsigned int bits = numBits(result);
+
+  // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
+  if (bits < 2) {
+    if (bits < 1) {
+      return FREE;
+    }
+
+    return UNKNOWN;
+  }
+
+  return MARKED;
+}
+
+VoxelStatus VoxelGrid::getVoxelColumn(
+  unsigned int x, unsigned int y,
+  unsigned int unknown_threshold, unsigned int marked_threshold)
+{
+  if (x >= size_x_ || y >= size_y_) {
+    RCLCPP_DEBUG(logger, "Error, voxel out of bounds. (%d, %d)\n", x, y);
+    return UNKNOWN;
+  }
+
+  uint32_t * col = &data_[y * size_x_ + x];
+
+  unsigned int unknown_bits = uint16_t(*col >> 16) ^ uint16_t(*col);
+  unsigned int marked_bits = *col >> 16;
+
+  // check if the number of marked bits qualifies the col as marked
+  if (!bitsBelowThreshold(marked_bits, marked_threshold)) {
+    return MARKED;
+  }
+
+  // check if the number of unkown bits qualifies the col as unknown
+  if (!bitsBelowThreshold(unknown_bits, unknown_threshold)) {
+    return UNKNOWN;
+  }
+
+  return FREE;
+}
+
+unsigned int VoxelGrid::sizeX()
+{
+  return size_x_;
+}
+
+unsigned int VoxelGrid::sizeY()
+{
+  return size_y_;
+}
+
+unsigned int VoxelGrid::sizeZ()
+{
+  return size_z_;
+}
+
+void VoxelGrid::printVoxelGrid()
+{
+  for (unsigned int z = 0; z < size_z_; z++) {
+    printf("Layer z = %u:\n", z);
+    for (unsigned int y = 0; y < size_y_; y++) {
+      for (unsigned int x = 0; x < size_x_; x++) {
+        printf((getVoxel(x, y, z)) == nav2_voxel_grid::MARKED ? "#" : " ");
+      }
+      printf("|\n");
+    }
+  }
+}
+
+void VoxelGrid::printColumnGrid()
+{
+  printf("Column view:\n");
+  for (unsigned int y = 0; y < size_y_; y++) {
+    for (unsigned int x = 0; x < size_x_; x++) {
+      printf((getVoxelColumn(x, y, 16, 0) == nav2_voxel_grid::MARKED) ? "#" : " ");
+    }
+    printf("|\n");
+  }
+}
+}  // namespace nav2_voxel_grid

navigation2/nav2_voxel_grid/test/CMakeLists.txt

@@ -0,0 +1,5 @@
+ament_add_gtest(voxel_grid_tests voxel_grid_tests.cpp)
+target_link_libraries(voxel_grid_tests voxel_grid)
+
+ament_add_gtest(voxel_grid_bresenham_3d voxel_grid_bresenham_3d.cpp)
+target_link_libraries(voxel_grid_bresenham_3d voxel_grid)

navigation2/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp

@@ -0,0 +1,156 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2021 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_voxel_grid/voxel_grid.hpp>
+#include <gtest/gtest.h>
+
+class TestVoxel
+{
+public:
+  explicit TestVoxel(uint32_t * data, int sz_x, int sz_y)
+  : data_(data)
+  {
+    size_ = sz_x * sz_y;
+  }
+  inline void operator()(unsigned int off, unsigned int val)
+  {
+    ASSERT_TRUE(off < size_);
+    data_[off] = val;
+  }
+  inline unsigned int operator()(unsigned int off)
+  {
+    return data_[off];
+  }
+
+private:
+  uint32_t * data_;
+  unsigned int size_;
+};
+
+TEST(voxel_grid, bresenham3DBoundariesCheck)
+{
+  const int sz_x = 60;
+  const int sz_y = 60;
+  const int sz_z = 2;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 6;
+  nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z);
+  TestVoxel tv(vg.getData(), sz_x, sz_y);
+
+  // Initial point - some assymetrically standing point in order to cover most corner cases
+  const double x0 = 2.2;
+  const double y0 = 3.8;
+  const double z0 = 0.4;
+  // z-axis won't be domimant
+  const double z1 = 0.5;
+  // (x1, y1) point will move
+  double x1, y1;
+
+  // Epsilon for outer boundaries of voxel grid array
+  const double epsilon = 0.02;
+
+  // Running on (x, 0) edge
+  y1 = 0.0;
+  for (int i = 0; i <= sz_x; i++) {
+    if (i != sz_x) {
+      x1 = i;
+    } else {
+      x1 = i - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+
+  // Running on (x, sz_y) edge
+  y1 = sz_y - epsilon;
+  for (int i = 0; i <= sz_x; i++) {
+    if (i != sz_x) {
+      x1 = i;
+    } else {
+      x1 = i - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+
+  // Running on (0, y) edge
+  x1 = 0.0;
+  for (int j = 0; j <= sz_y; j++) {
+    if (j != sz_y) {
+      y1 = j;
+    } else {
+      y1 = j - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+
+  // Running on (sz_x, y) edge
+  x1 = sz_x - epsilon;
+  for (int j = 0; j <= sz_y; j++) {
+    if (j != sz_y) {
+      y1 = j;
+    } else {
+      y1 = j - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+}
+
+TEST(voxel_grid, bresenham3DSamePoint)
+{
+  const int sz_x = 60;
+  const int sz_y = 60;
+  const int sz_z = 2;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 0;
+  nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z);
+  TestVoxel tv(vg.getData(), sz_x, sz_y);
+
+  // Initial point
+  const double x0 = 2.2;
+  const double y0 = 3.8;
+  const double z0 = 0.4;
+
+  unsigned int offset = static_cast<int>(y0) * sz_x + static_cast<int>(x0);
+  unsigned int val_before = tv(offset);
+  // Same point to check
+  vg.raytraceLine(tv, x0, y0, z0, x0, y0, z0, max_length, min_length);
+  unsigned int val_after = tv(offset);
+  ASSERT_FALSE(val_before == val_after);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

navigation2/nav2_voxel_grid/test/voxel_grid_tests.cpp

@@ -0,0 +1,204 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  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: Eitan Marder-Eppstein
+*********************************************************************/
+#include <nav2_voxel_grid/voxel_grid.hpp>
+#include <gtest/gtest.h>
+
+TEST(voxel_grid, basicMarkingAndClearing) {
+  int size_x = 50, size_y = 10, size_z = 16;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+
+  // Put a "tabletop" into the scene.  A flat rectangle of set voxels at z = 12.
+  int table_z = 12;
+  int table_x_min = 5, table_x_max = 15;
+  int table_y_min = 0, table_y_max = 3;
+  for (int x = table_x_min; x <= table_x_max; x++) {
+    vg.markVoxelLine(x, table_y_min, table_z, x, table_y_max, table_z);
+  }
+
+  for (int i = table_x_min; i <= table_x_max; ++i) {
+    for (int j = table_y_min; j <= table_y_max; ++j) {
+      // check that each cell of the table is marked
+      ASSERT_EQ(nav2_voxel_grid::MARKED, vg.getVoxel(i, j, table_z));
+    }
+  }
+
+  int mark_count = 0;
+  unsigned int unknown_count = 0;
+  // go through each cell in the voxel grid and make sure that only 44 are filled in
+  for (unsigned int i = 0; i < vg.sizeX(); ++i) {
+    for (unsigned int j = 0; j < vg.sizeY(); ++j) {
+      for (unsigned int k = 0; k < vg.sizeZ(); ++k) {
+        if (vg.getVoxel(i, j, k) == nav2_voxel_grid::MARKED) {
+          mark_count++;
+        } else if (vg.getVoxel(i, j, k) == nav2_voxel_grid::UNKNOWN) {
+          unknown_count++;
+        }
+      }
+    }
+  }
+  ASSERT_EQ(mark_count, 44);
+
+  // the rest of the cells should be unknown
+  ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);
+
+  // now, let's clear one of the rows of the table
+  vg.clearVoxelLine(table_x_min, table_y_min, table_z, table_x_max, table_y_min, table_z);
+
+  mark_count = 0;
+  unknown_count = 0;
+  int free_count = 0;
+  // go through each cell in the voxel grid and make sure that only 33 are now filled in
+  for (unsigned int i = 0; i < vg.sizeX(); ++i) {
+    for (unsigned int j = 0; j < vg.sizeY(); ++j) {
+      for (unsigned int k = 0; k < vg.sizeZ(); ++k) {
+        if (vg.getVoxel(i, j, k) == nav2_voxel_grid::MARKED) {
+          mark_count++;
+        } else if (vg.getVoxel(i, j, k) == nav2_voxel_grid::FREE) {
+          free_count++;
+        } else if (vg.getVoxel(i, j, k) == nav2_voxel_grid::UNKNOWN) {
+          unknown_count++;
+        }
+      }
+    }
+  }
+
+  // we've switched 11 cells from marked to free
+  ASSERT_EQ(mark_count, 33);
+
+  // we've just explicitly seen through 11 cells
+  ASSERT_EQ(free_count, 11);
+
+  // the rest of the cells should still be unknown
+  ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);
+
+  // now let's put in a vertical column manually to test markVoxel
+  for (unsigned int i = 0; i < vg.sizeZ(); ++i) {
+    vg.markVoxel(0, 0, i);
+    ASSERT_EQ(vg.getVoxel(0, 0, i), nav2_voxel_grid::MARKED);
+  }
+
+  vg.printColumnGrid();
+  vg.printVoxelGrid();
+
+  // now, let's clear that line of voxels and make sure that they clear out OK
+  vg.clearVoxelLine(0, 0, 0, 0, 0, vg.sizeZ() - 1);
+
+  for (unsigned int i = 0; i < vg.sizeZ(); ++i) {
+    ASSERT_EQ(vg.getVoxel(0, 0, i), nav2_voxel_grid::FREE);
+  }
+
+  mark_count = 0;
+
+  // Visualize the output
+  /*
+     v->printVoxelGrid();
+     v->printColumnGrid();
+
+     printf("CostMap:\n===========\n");
+     for(int y = 0; y < size_y; y++){
+     for(int x = 0; x < size_x; x++){
+     printf((costMap[y * size_x + x] > 0 ? "#" : " "));
+     }printf("|\n");
+     }
+     */
+}
+
+TEST(voxel_grid, InvalidSize) {
+  int size_x = 50, size_y = 10, size_z = 17;
+  int test_z = 16;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+  vg.resize(size_x, size_y, test_z);
+  vg.resize(size_x, size_y, size_z);
+  EXPECT_TRUE(vg.getVoxelColumn(51, 10, 0, 0) == nav2_voxel_grid::VoxelStatus::UNKNOWN);
+  EXPECT_TRUE(vg.getVoxelColumn(50, 11, 0, 0) == nav2_voxel_grid::VoxelStatus::UNKNOWN);
+}
+
+TEST(voxel_grid, MarkAndClear) {
+  int size_x = 10, size_y = 10, size_z = 10;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+  vg.markVoxelInMap(5, 5, 5, 0);
+  EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::MARKED);
+  vg.clearVoxelColumn(55);
+  EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::FREE);
+}
+
+TEST(voxel_grid, clearVoxelLineInMap) {
+  int size_x = 10, size_y = 10, size_z = 10;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+  vg.markVoxelInMap(0, 0, 5, 0);
+  EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED);
+
+  unsigned char * map_2d = new unsigned char[100];
+  map_2d[0] = 254;
+
+  vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, map_2d, 16, 0);
+
+  EXPECT_EQ(map_2d[0], 0);
+
+  vg.markVoxelInMap(0, 0, 5, 0);
+  vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, nullptr, 16, 0);
+  EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::FREE);
+
+  // Testing for min range for raytrace clearing
+  vg.markVoxelInMap(0, 0, 5, 0);
+  vg.markVoxelInMap(0, 0, 7, 0);
+  vg.clearVoxelLineInMap(
+    0, 0, 0, 0, 0, 9, nullptr, 16, 0, (unsigned char)'\000',
+    (unsigned char)'\377', UINT_MAX, 6);
+  EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED);
+  EXPECT_EQ(vg.getVoxel(0, 0, 7), nav2_voxel_grid::FREE);
+
+  delete[] map_2d;
+}
+
+TEST(voxel_grid, GetVoxelData) {
+  uint32_t * data = new uint32_t[9];
+  data[4] = 255;
+  data[0] = 0;
+  EXPECT_EQ(
+    nav2_voxel_grid::VoxelGrid::getVoxel(1, 1, 1, 3, 3, 3, data), nav2_voxel_grid::UNKNOWN);
+
+  EXPECT_EQ(
+    nav2_voxel_grid::VoxelGrid::getVoxel(0, 0, 0, 3, 3, 3, data), nav2_voxel_grid::FREE);
+  delete[] data;
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}