add humble-navigation2

navigation2/nav2_dwb_controller/costmap_queue/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.5)
+project(costmap_queue)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(rclcpp REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/costmap_queue.cpp
+  src/limited_costmap_queue.cpp
+)
+
+set(dependencies
+  rclcpp
+  nav2_costmap_2d
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  ${dependencies}
+)
+
+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)
+
+  ament_add_gtest(mbq_test test/mbq_test.cpp)
+  ament_target_dependencies(mbq_test ${dependencies})
+
+  ament_add_gtest(utest test/utest.cpp)
+  ament_target_dependencies(utest ${dependencies})
+  target_link_libraries(utest ${PROJECT_NAME})
+endif()
+
+install(TARGETS ${PROJECT_NAME}
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+install(DIRECTORY include/
+        DESTINATION include/
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_package()

navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp

@@ -0,0 +1,189 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+
+#ifndef COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
+#define COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
+
+#include <cmath>
+#include <vector>
+#include <limits>
+#include <memory>
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "costmap_queue/map_based_queue.hpp"
+
+namespace costmap_queue
+{
+/**
+ * @class CellData
+ * @brief Storage for cell information used during queue expansion
+ */
+class CellData
+{
+public:
+  /**
+   * @brief Real Constructor
+   * @param d The distance to the nearest obstacle
+   * @param i The index of the cell in the costmap. Redundant with the following two parameters.
+   * @param x The x coordinate of the cell in the cost map
+   * @param y The y coordinate of the cell in the cost map
+   * @param sx The x coordinate of the closest source cell in the costmap
+   * @param sy The y coordinate of the closest source cell in the costmap
+   */
+  CellData(
+    const double d, const unsigned int i, const unsigned int x, const unsigned int y,
+    const unsigned int sx, const unsigned int sy)
+  : distance_(d), index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
+  {
+  }
+
+  /**
+   * @brief Default Constructor - Should be used sparingly
+   */
+  CellData()
+  : distance_(std::numeric_limits<double>::max()), index_(0), x_(0), y_(0), src_x_(0), src_y_(0)
+  {
+  }
+
+  static unsigned absolute_difference(const unsigned x, const unsigned y)
+  {
+    return (x > y) ? (x - y) : (y - x);
+  }
+
+  double distance_;
+  unsigned int index_;
+  unsigned int x_, y_;
+  unsigned int src_x_, src_y_;
+};
+
+/**
+ * @class CostmaQueue
+ * @brief A tool for finding the cells closest to some set of originating cells.
+ *
+ * A common operation with costmaps is to define a set of cells in the costmap, and then
+ * perform some operation on all the other cells based on which cell in the original set
+ * the other cells are closest to. This operation is done in the inflation layer to figure out
+ * how far each cell is from an obstacle, and is also used in a number of Trajectory cost functions.
+ *
+ * It is implemented with a queue. The standard operation is to enqueueCell the original set, and then
+ * retreive the other cells with the isEmpty/getNextCell iterator-like functionality. getNextCell
+ * returns an object that contains the coordinates of this cell and the origin cell, as well as
+ * the distance between them. By default, the Euclidean distance is used for ordering, but passing in
+ * manhattan=true to the constructor will use the Manhattan distance.
+ *
+ * The validCellToQueue overridable-function allows for deriving classes to limit the queue traversal
+ * to a subset of all costmap cells. LimitedCostmapQueue does this by ignoring distances above a limit.
+ *
+ */
+class CostmapQueue : public MapBasedQueue<CellData>
+{
+public:
+  /**
+   * @brief constructor
+   * @param costmap Costmap which defines the size/number of cells
+   * @param manhattan If true, sort cells by Manhattan distance, otherwise use Euclidean distance
+   */
+  explicit CostmapQueue(nav2_costmap_2d::Costmap2D & costmap, bool manhattan = false);
+
+  /**
+   * @brief Clear the queue
+   */
+  void reset() override;
+
+  /**
+   * @brief Add a cell the queue
+   * @param x X coordinate of the cell
+   * @param y Y coordinate of the cell
+   */
+  void enqueueCell(unsigned int x, unsigned int y);
+
+  /**
+   * @brief Get the next cell to examine, and enqueue its neighbors as needed
+   * @return The next cell
+   *
+   * NB: Assumes that isEmpty has been called before this call and returned false
+   */
+  CellData getNextCell();
+
+  /**
+   * @brief Check to see if we should add this cell to the queue. Always true unless overridden.
+   * @param cell The cell to check
+   * @return True, unless overriden
+   */
+  virtual bool validCellToQueue(const CellData & /*cell*/) {return true;}
+  /**
+   * @brief convenience typedef for a pointer
+   */
+  typedef std::shared_ptr<CostmapQueue> Ptr;
+
+protected:
+  /**
+   * @brief Enqueue a cell with the given coordinates and the given source cell
+   */
+  void enqueueCell(
+    unsigned int index, unsigned int cur_x, unsigned int cur_y, unsigned int src_x,
+    unsigned int src_y);
+
+  /**
+   * @brief Compute the cached distances
+   */
+  void computeCache();
+
+  nav2_costmap_2d::Costmap2D & costmap_;
+  std::vector<bool> seen_;
+  int max_distance_;
+  bool manhattan_;
+
+protected:
+  /**
+   * @brief  Lookup pre-computed distances
+   * @param cur_x The x coordinate of the current cell
+   * @param cur_y The y coordinate of the current cell
+   * @param src_x The x coordinate of the source cell
+   * @param src_y The y coordinate of the source cell
+   * @return
+   */
+  inline double distanceLookup(
+    const unsigned int cur_x, const unsigned int cur_y,
+    const unsigned int src_x, const unsigned int src_y)
+  {
+    unsigned int dx = CellData::absolute_difference(cur_x, src_x);
+    unsigned int dy = CellData::absolute_difference(cur_y, src_y);
+    return cached_distances_[dx][dy];
+  }
+  std::vector<std::vector<double>> cached_distances_;
+  int cached_max_distance_;
+};
+}  // namespace costmap_queue
+
+#endif  // COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_

navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/limited_costmap_queue.hpp

@@ -0,0 +1,58 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+
+#ifndef COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
+#define COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
+
+#include "costmap_queue/costmap_queue.hpp"
+
+namespace costmap_queue
+{
+
+/**
+ * @class LimitedCostmapQueue
+ * @brief Extension of Costmap Queue where distances are limited to a given distance from source cells.
+ */
+class LimitedCostmapQueue : public CostmapQueue
+{
+public:
+  /**
+   * @brief Constructor with limit as an integer number of cells.
+   */
+  LimitedCostmapQueue(nav2_costmap_2d::Costmap2D & costmap, const int cell_distance_limit);
+  bool validCellToQueue(const CellData & cell) override;
+};
+}  // namespace costmap_queue
+
+#endif  // COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_

navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/map_based_queue.hpp

@@ -0,0 +1,172 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+
+#ifndef COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
+#define COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
+
+#include <algorithm>
+#include <map>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+namespace costmap_queue
+{
+/**
+ * @brief Templatized interface for a priority queue
+ *
+ * This is faster than the std::priority_queue implementation in certain cases because iterating does
+ * not require resorting after every element is examined.
+ * Based on https://github.com/ros-planning/navigation/pull/525
+ * The relative speed of this against the priority queue depends how many items with each
+ * priority are inserted into the queue.
+ *
+ * One additional speed up depends on the patterns of priorities during each iteration of the queue.
+ * If the same priorities are inserted into the queue on every iteration, then it is quicker to
+ * set reset_bins = false, such that the priority bins are not reset and will not have to be recreated
+ * on each iteration.
+ */
+template<class item_t>
+class MapBasedQueue
+{
+public:
+  /**
+   * @brief Default Constructor
+   */
+  explicit MapBasedQueue(bool reset_bins = true)
+  : reset_bins_(reset_bins), item_count_(0)
+  {
+    reset();
+  }
+
+  /**
+   * @brief Clear the queue
+   */
+  virtual void reset()
+  {
+    if (reset_bins_ || item_count_ > 0) {
+      item_bins_.clear();
+      item_count_ = 0;
+    }
+    iter_ = last_insert_iter_ = item_bins_.end();
+  }
+
+  /**
+   * @brief Add a new item to the queue with a set priority
+   * @param priority Priority of the item
+   * @param item Payload item
+   */
+  void enqueue(const double priority, item_t item)
+  {
+    // We keep track of the last priority we inserted. If this items priority
+    // matches the previous insertion we can avoid searching through all the
+    // bins.
+    if (last_insert_iter_ == item_bins_.end() || last_insert_iter_->first != priority) {
+      last_insert_iter_ = item_bins_.find(priority);
+
+      // If not found, create a new bin
+      if (last_insert_iter_ == item_bins_.end()) {
+        auto map_item = std::make_pair(priority, std::move(std::vector<item_t>()));
+
+        // Inserts an item if it doesn't exist. Returns an iterator to the item
+        // whether it existed or was inserted.
+        std::pair<ItemMapIterator, bool> insert_result = item_bins_.insert(std::move(map_item));
+        last_insert_iter_ = insert_result.first;
+      }
+    }
+
+    // Add the item to the vector for this map key
+    last_insert_iter_->second.push_back(item);
+    item_count_++;
+
+    // Use short circuiting to check if we want to update the iterator
+    if (iter_ == item_bins_.end() || priority < iter_->first) {
+      iter_ = last_insert_iter_;
+    }
+  }
+
+  /**
+   * @brief Check to see if there is anything in the queue
+   * @return True if there is nothing in the queue
+   *
+   * Must be called prior to front/pop.
+   */
+  bool isEmpty()
+  {
+    return item_count_ == 0;
+  }
+
+  /**
+   * @brief Return the item at the front of the queue
+   * @return The item at the front of the queue
+   */
+  item_t & front()
+  {
+    if (iter_ == item_bins_.end()) {
+      throw std::out_of_range("front() called on empty costmap_queue::MapBasedQueue!");
+    }
+
+    return iter_->second.back();
+  }
+
+  /**
+   * @brief Remove (and destroy) the item at the front of the queue
+   */
+  void pop()
+  {
+    if (iter_ != item_bins_.end() && !iter_->second.empty()) {
+      iter_->second.pop_back();
+      item_count_--;
+    }
+
+    auto not_empty = [](const typename ItemMap::value_type & key_val) {
+        return !key_val.second.empty();
+      };
+    iter_ = std::find_if(iter_, item_bins_.end(), not_empty);
+  }
+
+protected:
+  using ItemMap = std::map<double, std::vector<item_t>>;
+  using ItemMapIterator = typename ItemMap::iterator;
+
+  bool reset_bins_;
+
+  ItemMap item_bins_;
+  unsigned int item_count_;
+  ItemMapIterator iter_;
+  ItemMapIterator last_insert_iter_;
+};
+}  // namespace costmap_queue
+
+#endif  // COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_

navigation2/nav2_dwb_controller/costmap_queue/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>costmap_queue</name>
+  <version>1.1.18</version>
+  <description>The costmap_queue package</description>
+  <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
+
+  <depend>nav2_costmap_2d</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_dwb_controller/costmap_queue/src/costmap_queue.cpp

@@ -0,0 +1,137 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+#include "costmap_queue/costmap_queue.hpp"
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+using std::hypot;
+
+namespace costmap_queue
+{
+
+CostmapQueue::CostmapQueue(nav2_costmap_2d::Costmap2D & costmap, bool manhattan)
+: MapBasedQueue(), costmap_(costmap), max_distance_(-1), manhattan_(manhattan),
+  cached_max_distance_(-1)
+{
+  reset();
+}
+
+void CostmapQueue::reset()
+{
+  unsigned int size_x = costmap_.getSizeInCellsX(), size_y = costmap_.getSizeInCellsY();
+  if (seen_.size() != size_x * size_y) {
+    seen_.resize(size_x * size_y);
+  }
+  std::fill(seen_.begin(), seen_.end(), false);
+  computeCache();
+  MapBasedQueue::reset();
+}
+
+void CostmapQueue::enqueueCell(unsigned int x, unsigned int y)
+{
+  unsigned int index = costmap_.getIndex(x, y);
+  enqueueCell(index, x, y, x, y);
+}
+
+void CostmapQueue::enqueueCell(
+  unsigned int index, unsigned int cur_x, unsigned int cur_y,
+  unsigned int src_x, unsigned int src_y)
+{
+  if (seen_[index]) {return;}
+
+  // we compute our distance table one cell further than the inflation radius
+  // dictates so we can make the check below
+  double distance = distanceLookup(cur_x, cur_y, src_x, src_y);
+  CellData data(distance, index, cur_x, cur_y, src_x, src_y);
+  if (validCellToQueue(data)) {
+    seen_[index] = true;
+    enqueue(distance, data);
+  }
+}
+
+CellData CostmapQueue::getNextCell()
+{
+  // get the highest priority cell and pop it off the priority queue
+  CellData current_cell = front();
+  pop();
+
+  unsigned int index = current_cell.index_;
+  unsigned int mx = current_cell.x_;
+  unsigned int my = current_cell.y_;
+  unsigned int sx = current_cell.src_x_;
+  unsigned int sy = current_cell.src_y_;
+
+  // attempt to put the neighbors of the current cell onto the queue
+  unsigned int size_x = costmap_.getSizeInCellsX();
+  if (mx > 0) {
+    enqueueCell(index - 1, mx - 1, my, sx, sy);
+  }
+  if (my > 0) {
+    enqueueCell(index - size_x, mx, my - 1, sx, sy);
+  }
+  if (mx < size_x - 1) {
+    enqueueCell(index + 1, mx + 1, my, sx, sy);
+  }
+  if (my < costmap_.getSizeInCellsY() - 1) {
+    enqueueCell(index + size_x, mx, my + 1, sx, sy);
+  }
+
+  return current_cell;
+}
+
+void CostmapQueue::computeCache()
+{
+  if (max_distance_ == -1) {
+    max_distance_ = std::max(costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY());
+  }
+  if (max_distance_ == cached_max_distance_) {return;}
+  cached_distances_.clear();
+
+  cached_distances_.resize(max_distance_ + 2);
+
+  for (unsigned int i = 0; i < cached_distances_.size(); ++i) {
+    cached_distances_[i].resize(max_distance_ + 2);
+    for (unsigned int j = 0; j < cached_distances_[i].size(); ++j) {
+      if (manhattan_) {
+        cached_distances_[i][j] = i + j;
+      } else {
+        cached_distances_[i][j] = hypot(i, j);
+      }
+    }
+  }
+  cached_max_distance_ = max_distance_;
+}
+
+}  // namespace costmap_queue

navigation2/nav2_dwb_controller/costmap_queue/src/limited_costmap_queue.cpp

@@ -0,0 +1,54 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+
+#include "costmap_queue/limited_costmap_queue.hpp"
+
+namespace costmap_queue
+{
+
+LimitedCostmapQueue::LimitedCostmapQueue(
+  nav2_costmap_2d::Costmap2D & costmap,
+  const int distance_limit)
+: CostmapQueue(costmap)
+{
+  max_distance_ = distance_limit;
+  reset();
+}
+
+bool LimitedCostmapQueue::validCellToQueue(const CellData & cell)
+{
+  return cell.distance_ <= max_distance_;
+}
+
+}  // namespace costmap_queue

navigation2/nav2_dwb_controller/costmap_queue/test/mbq_test.cpp

@@ -0,0 +1,117 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+
+#include <string>
+#include "gtest/gtest.h"
+#include "costmap_queue/map_based_queue.hpp"
+
+using costmap_queue::MapBasedQueue;
+
+void letter_test(MapBasedQueue<char> & q, const char test_letter)
+{
+  ASSERT_FALSE(q.isEmpty());
+  char c = q.front();
+  EXPECT_EQ(c, test_letter);
+  q.pop();
+}
+
+TEST(MapBasedQueue, emptyQueue)
+{
+  MapBasedQueue<char> q;
+  EXPECT_TRUE(q.isEmpty());
+  q.enqueue(1.0, 'A');
+  EXPECT_FALSE(q.isEmpty());
+}
+
+TEST(MapBasedQueue, checkOrdering)
+{
+  MapBasedQueue<char> q;
+  q.enqueue(1.0, 'A');
+  q.enqueue(3.0, 'B');
+  q.enqueue(2.0, 'C');
+  q.enqueue(5.0, 'D');
+  q.enqueue(0.0, 'E');
+
+  std::string expected = "EACBD";
+  for (unsigned int i = 0; i < expected.size(); i++) {
+    letter_test(q, expected[i]);
+  }
+  EXPECT_TRUE(q.isEmpty());
+}
+
+TEST(MapBasedQueue, checkDynamicOrdering)
+{
+  MapBasedQueue<char> q;
+  q.enqueue(1.0, 'A');
+  q.enqueue(3.0, 'B');
+  q.enqueue(2.0, 'C');
+  q.enqueue(5.0, 'D');
+
+  std::string expected = "ACB";
+  for (unsigned int i = 0; i < expected.size(); i++) {
+    letter_test(q, expected[i]);
+  }
+
+  q.enqueue(0.0, 'E');
+  letter_test(q, 'E');
+}
+
+TEST(MapBasedQueue, checkDynamicOrdering2)
+{
+  MapBasedQueue<char> q;
+  q.enqueue(1.0, 'A');
+  q.enqueue(2.0, 'B');
+  letter_test(q, 'A');
+  q.enqueue(3.0, 'C');
+  letter_test(q, 'B');
+}
+
+TEST(MapBasedQueue, checkDynamicOrdering3)
+{
+  MapBasedQueue<char> q;
+  q.enqueue(1.0, 'A');
+  q.enqueue(2.0, 'B');
+  q.enqueue(5.0, 'D');
+  letter_test(q, 'A');
+  letter_test(q, 'B');
+  q.enqueue(1.0, 'C');
+  letter_test(q, 'C');
+  letter_test(q, 'D');
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

navigation2/nav2_dwb_controller/costmap_queue/test/utest.cpp

@@ -0,0 +1,142 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+
+#include <cmath>
+#include <memory>
+#include <algorithm>
+#include "gtest/gtest.h"
+#include "costmap_queue/costmap_queue.hpp"
+#include "costmap_queue/limited_costmap_queue.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using std::hypot;
+
+nav2_costmap_2d::Costmap2D costmap(5, 5, 1.0, 0.0, 0.0);
+
+TEST(CostmapQueue, basicQueue)
+{
+  costmap_queue::CostmapQueue q(costmap);
+  int count = 0;
+  q.enqueueCell(0, 0);
+  while (!q.isEmpty()) {
+    costmap_queue::CellData cell = q.getNextCell();
+    EXPECT_EQ(cell.distance_, hypot(cell.x_, cell.y_));
+    count++;
+  }
+  EXPECT_EQ(count, 25);
+}
+
+TEST(CostmapQueue, bigTest)
+{
+  nav2_costmap_2d::Costmap2D big_map(500, 500, 1.0, 0.0, 0.0);
+  costmap_queue::CostmapQueue q(big_map);
+  int count = 0;
+  q.enqueueCell(0, 0);
+  while (!q.isEmpty()) {
+    costmap_queue::CellData cell = q.getNextCell();
+    EXPECT_EQ(cell.distance_, hypot(cell.x_, cell.y_));
+    count++;
+  }
+  EXPECT_EQ(count, 500 * 500);
+}
+
+TEST(CostmapQueue, linearQueue)
+{
+  costmap_queue::CostmapQueue q(costmap);
+  int count = 0;
+  q.enqueueCell(0, 0);
+  q.enqueueCell(0, 1);
+  q.enqueueCell(0, 2);
+  q.enqueueCell(0, 3);
+  q.enqueueCell(0, 4);
+  while (!q.isEmpty()) {
+    costmap_queue::CellData cell = q.getNextCell();
+    EXPECT_EQ(cell.distance_, cell.x_);
+    count++;
+  }
+  EXPECT_EQ(count, 25);
+}
+
+TEST(CostmapQueue, crossQueue)
+{
+  costmap_queue::CostmapQueue q(costmap);
+  int count = 0;
+  int xs[] = {1, 2, 2, 3};
+  int ys[] = {2, 1, 3, 2};
+  int N = 4;
+  for (int i = 0; i < N; i++) {
+    q.enqueueCell(xs[i], ys[i]);
+  }
+
+  while (!q.isEmpty()) {
+    costmap_queue::CellData cell = q.getNextCell();
+    double min_d = 1000;
+
+    for (int i = 0; i < N; i++) {
+      double dd = hypot(xs[i] - static_cast<float>(cell.x_), ys[i] - static_cast<float>(cell.y_));
+      min_d = std::min(min_d, dd);
+    }
+    EXPECT_NEAR(cell.distance_, min_d, 0.00001);
+    count++;
+  }
+  EXPECT_EQ(count, 25);
+}
+
+TEST(CostmapQueue, limitedQueue)
+{
+  costmap_queue::LimitedCostmapQueue q(costmap, 5);
+  int count = 0;
+  q.enqueueCell(0, 0);
+  while (!q.isEmpty()) {
+    costmap_queue::CellData cell = q.getNextCell();
+    EXPECT_EQ(cell.distance_, hypot(cell.x_, cell.y_));
+    count++;
+  }
+  EXPECT_EQ(count, 24);
+
+  costmap_queue::LimitedCostmapQueue q2(costmap, 3);
+  count = 0;
+  q2.enqueueCell(0, 0);
+  while (!q2.isEmpty()) {
+    q2.getNextCell();
+    count++;
+  }
+  EXPECT_EQ(count, 11);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}