add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -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