HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
navigations/costmap_queue/CMakeLists.txt Executable file
View File

@@ -0,0 +1,46 @@
cmake_minimum_required(VERSION 3.0.2)
project(costmap_queue)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Werror")
find_package(catkin REQUIRED COMPONENTS
nav_core2
roscpp
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS nav_core2 roscpp
LIBRARIES ${PROJECT_NAME}
)
include_directories(
include ${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
src/costmap_queue.cpp
src/limited_costmap_queue.cpp
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
if (CATKIN_ENABLE_TESTING)
catkin_add_gtest(mbq_test test/mbq_test.cpp)
target_link_libraries(mbq_test ${catkin_LIBRARIES})
catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp)
target_link_libraries(${PROJECT_NAME}_utest ${PROJECT_NAME} ${catkin_LIBRARIES})
find_package(roslint REQUIRED)
roslint_cpp()
roslint_add_test()
endif (CATKIN_ENABLE_TESTING)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)

22
navigations/costmap_queue/README.md Executable file
View File

@@ -0,0 +1,22 @@
# costmap_queue
This package contains helper classes that implement one particular common costmap operation: figuring out the distance of every cell to a subset of those cells.
## Inflation Example
Let's assume you have a grid `G` and you know that there are lethal obstacles in a subset of those cells `O`. You want to mark all cells within distance `r` with one value, and all cells within a greater distance `r2` with another.
One costly way to do this would be to to iterate through all the cells `g` in `G`, calculate the distance from `g` to each cell in `O`, find the minimum distance, and compare to `r` and `r2`. This is roughly quadratic with the number of cells.
The more efficient way to do it is to start with all the cells in `O` in a queue. For each cell you dequeue, you calculate the distance to its origin cell from `O` and then enqueue all of its neighbors. This approach is roughly linear with the number of cells, although you need to be careful to not incur too many costs from resorting the queue.
## Map Based Queue
While this operation could be done with the standard priority queue implementation, it can be optimized by using a custom priority queue based on `std::map`. This relies on the fact that many of the items inserted into the queue will have the same priority. In the `costmap_queue`, the priorities used are distances on the grid, of which there are a finite number. Thus by grouping all of the elements of the same weight into a vector together, and storing them in the `std::map` with their priority, we can eliminate the need to resort after each individual item is popped. This is based on the [optimizations to the Inflation algorithm](https://github.com/ros-planning/navigation/pull/525).
Also, since the same (or similar) distances will be used in the `costmap_queue` from iteration to iteration, additional time can be saved by not destroying the vectors in the map, and reusing them iteration to iteration.
## Costmap Queue
The `CostmapQueue` class operates on a `nav_core2::Costmap`. First, you must enqueue all of the "subset" of cells (i.e. `O` in the above example) using `enqueueCell`. Then, while the queue is not empty (i.e. `isEmpty` is false), you can call `costmap_queue::CellData cell = q.getNextCell();` to get the next cell in the queue.
The `CellData` class contains 5 values: `x_` and `y_` are the coordinates for the current cell, `src_x_` and `src_y_` are the coordinates for the original cell, and `distance_` is the distance between them.
By default, `CostmapQueue` will iterate through all the cells in the `Costmap`. If you want to limit it to only cells within a certain distance, you can use `LimitedCostmapQueue` instead.

190
navigations/costmap_queue/include/costmap_queue/costmap_queue.h Executable file
View File

@@ -0,0 +1,190 @@
/*
* 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_H
#define COSTMAP_QUEUE_COSTMAP_QUEUE_H
#include <nav_core2/costmap.h>
#include <costmap_queue/map_based_queue.h>
#include <nav_grid/vector_nav_grid.h>
#include <algorithm>
#include <limits>
#include <memory>
#include <vector>
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 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 x, const unsigned int y, const unsigned int sx, const unsigned int sy) :
distance_(d), x_(x), y_(y), src_x_(sx), src_y_(sy)
{
}
/**
* @brief Default Constructor - Should be used sparingly
*/
CellData() :
distance_(std::numeric_limits<double>::max()), x_(0), y_(0), src_x_(0), src_y_(0)
{
}
double distance_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
/**
* @class CostmapQueue
* @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(nav_core2::Costmap& 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 Get the maximum x or y distance we'll need to calculate the distance between
*/
virtual int getMaxDistance() const { return std::max(costmap_.getWidth(), costmap_.getHeight()); }
/**
* @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 definition for a pointer
*/
using Ptr = std::shared_ptr<CostmapQueue>;
protected:
/**
* @brief Enqueue a cell with the given coordinates and the given source cell
*/
void enqueueCell(unsigned int cur_x, unsigned int cur_y, unsigned int src_x, unsigned int src_y);
/**
* @brief Compute the cached distances
*/
void computeCache();
nav_core2::Costmap& costmap_;
// This really should be VectorNavGrid<bool>, but since
// vector<bool> is wacky, it would result in compile errors.
nav_grid::VectorNavGrid<unsigned char> seen_;
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 Precomputed distance
*
* NB: Note that while abs() has the correct behavior i.e. abs(2 - 5) ==> 3.
* std::abs() without explicit casting does not have the correct behavior
* i.e. std::abs(2 - 5) ==> (the unsigned version of) -3.
* We're using explicit casting here to ensure the behavior is not compiler dependent.
* std::abs(static_cast<int>(2) - static_cast<int>(5)) ==> 3.
*/
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 = std::abs(static_cast<int>(cur_x) - static_cast<int>(src_x));
unsigned int dy = std::abs(static_cast<int>(cur_y) - static_cast<int>(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_H

61
navigations/costmap_queue/include/costmap_queue/limited_costmap_queue.h Executable file
View File

@@ -0,0 +1,61 @@
/*
* 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_H
#define COSTMAP_QUEUE_LIMITED_COSTMAP_QUEUE_H
#include <costmap_queue/costmap_queue.h>
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(nav_core2::Costmap& costmap, const int cell_distance_limit);
bool validCellToQueue(const CellData& cell) override;
int getMaxDistance() const override;
protected:
int max_distance_;
};
} // namespace costmap_queue
#endif // COSTMAP_QUEUE_LIMITED_COSTMAP_QUEUE_H

173
navigations/costmap_queue/include/costmap_queue/map_based_queue.h Executable file
View File

@@ -0,0 +1,173 @@
/*
* 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_H
#define COSTMAP_QUEUE_MAP_BASED_QUEUE_H
#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_H

17
navigations/costmap_queue/package.xml Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<package format="2">
<name>costmap_queue</name>
<version>0.3.0</version>
<description>Tool for iterating through the cells of a costmap to find the closest distance to a subset of cells.</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>nav_core2</depend>
<depend>roscpp</depend>
<test_depend>roslint</test_depend>
<test_depend>rosunit</test_depend>
</package>

125
navigations/costmap_queue/src/costmap_queue.cpp Executable file
View File

@@ -0,0 +1,125 @@
/*
* 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.h>
#include <algorithm>
#include <vector>
namespace costmap_queue
{
CostmapQueue::CostmapQueue(nav_core2::Costmap& costmap, bool manhattan) :
MapBasedQueue(false), costmap_(costmap), seen_(0), manhattan_(manhattan), cached_max_distance_(-1)
{
reset();
}
void CostmapQueue::reset()
{
seen_.setInfo(costmap_.getInfo());
seen_.reset();
computeCache();
MapBasedQueue::reset();
}
void CostmapQueue::enqueueCell(unsigned int x, unsigned int y)
{
enqueueCell(x, y, x, y);
}
void CostmapQueue::enqueueCell(unsigned int cur_x, unsigned int cur_y,
unsigned int src_x, unsigned int src_y)
{
if (seen_(cur_x, cur_y)) 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, cur_x, cur_y, src_x, src_y);
if (validCellToQueue(data))
{
seen_.setValue(cur_x, cur_y, 1);
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 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
if (mx > 0)
enqueueCell(mx - 1, my, sx, sy);
if (my > 0)
enqueueCell(mx, my - 1, sx, sy);
if (mx < costmap_.getWidth() - 1)
enqueueCell(mx + 1, my, sx, sy);
if (my < costmap_.getHeight() - 1)
enqueueCell(mx, my + 1, sx, sy);
return current_cell;
}
void CostmapQueue::computeCache()
{
int max_distance = getMaxDistance();
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

56
navigations/costmap_queue/src/limited_costmap_queue.cpp Executable file
View File

@@ -0,0 +1,56 @@
/*
* 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.h>
namespace costmap_queue
{
LimitedCostmapQueue::LimitedCostmapQueue(nav_core2::Costmap& costmap, const int distance_limit) :
CostmapQueue(costmap)
{
max_distance_ = distance_limit;
reset();
}
int LimitedCostmapQueue::getMaxDistance() const
{
return max_distance_;
}
bool LimitedCostmapQueue::validCellToQueue(const CellData& cell)
{
return cell.distance_ <= max_distance_;
}
} // namespace costmap_queue

118
navigations/costmap_queue/test/mbq_test.cpp Executable file
View File

@@ -0,0 +1,118 @@
/*
* 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 <gtest/gtest.h>
#include <costmap_queue/map_based_queue.h>
#include <string>
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();
}

208
navigations/costmap_queue/test/utest.cpp Executable file
View File

@@ -0,0 +1,208 @@
/*
* 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 <gtest/gtest.h>
#include <nav_core2/basic_costmap.h>
#include <costmap_queue/costmap_queue.h>
#include <costmap_queue/limited_costmap_queue.h>
#include <ros/ros.h>
#include <memory>
#include <algorithm>
nav_core2::BasicCostmap costmap;
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_FLOAT_EQ(cell.distance_, hypot(cell.x_, cell.y_));
count++;
}
EXPECT_EQ(count, 25);
}
TEST(CostmapQueue, reverseQueue)
{
costmap_queue::CostmapQueue q(costmap);
int count = 0;
q.enqueueCell(4, 4);
while (!q.isEmpty())
{
costmap_queue::CellData cell = q.getNextCell();
EXPECT_FLOAT_EQ(cell.distance_, hypot(4.0 - static_cast<double>(cell.x_),
4.0 - static_cast<double>(cell.y_)));
count++;
}
EXPECT_EQ(count, 25);
}
TEST(CostmapQueue, bigTest)
{
nav_grid::NavGridInfo big_info;
big_info.width = 500;
big_info.height = 500;
nav_core2::BasicCostmap big_map;
big_map.setInfo(big_info);
costmap_queue::CostmapQueue q(big_map);
int count = 0;
q.enqueueCell(0, 0);
while (!q.isEmpty())
{
costmap_queue::CellData cell = q.getNextCell();
EXPECT_FLOAT_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_FLOAT_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_FLOAT_EQ(cell.distance_, min_d);
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_FLOAT_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);
}
TEST(CostmapQueue, changingSize)
{
nav_grid::NavGridInfo info0;
info0.width = 2;
info0.height = 3;
nav_grid::NavGridInfo info1;
info1.width = 6;
info1.height = 7;
nav_core2::BasicCostmap size_map;
size_map.setInfo(info0);
costmap_queue::CostmapQueue q(size_map);
unsigned int count = 0;
q.enqueueCell(0, 0);
while (!q.isEmpty())
{
q.getNextCell();
count++;
}
EXPECT_EQ(count, info0.width * info0.height);
size_map.setInfo(info1);
q.reset();
count = 0;
q.enqueueCell(0, 0);
while (!q.isEmpty())
{
q.getNextCell();
count++;
}
EXPECT_EQ(count, info1.width * info1.height);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
nav_grid::NavGridInfo info;
info.width = 5;
info.height = 5;
costmap.setInfo(info);
return RUN_ALL_TESTS();
}