136 lines
3.6 KiB
C++
136 lines
3.6 KiB
C++
#ifndef COSTMAP_TOOLS_H_
|
|
#define COSTMAP_TOOLS_H_
|
|
|
|
#include <geometry_msgs/msg/point_stamped.hpp>
|
|
#include <geometry_msgs/msg/polygon_stamped.hpp>
|
|
#include <rclcpp/rclcpp.hpp>
|
|
|
|
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
|
|
|
namespace frontier_exploration
|
|
{
|
|
/**
|
|
* @brief Determine 4-connected neighbourhood of an input cell, checking for map
|
|
* edges
|
|
* @param idx input cell index
|
|
* @param costmap Reference to map data
|
|
* @return neighbour cell indexes
|
|
*/
|
|
std::vector<unsigned int> nhood4(unsigned int idx,
|
|
const nav2_costmap_2d::Costmap2D& costmap)
|
|
{
|
|
// get 4-connected neighbourhood indexes, check for edge of map
|
|
std::vector<unsigned int> out;
|
|
|
|
unsigned int size_x_ = costmap.getSizeInCellsX(),
|
|
size_y_ = costmap.getSizeInCellsY();
|
|
|
|
if (idx > size_x_ * size_y_ - 1) {
|
|
RCLCPP_WARN(rclcpp::get_logger("FrontierExploration"), "Evaluating nhood "
|
|
"for offmap point");
|
|
return out;
|
|
}
|
|
|
|
if (idx % size_x_ > 0) {
|
|
out.push_back(idx - 1);
|
|
}
|
|
if (idx % size_x_ < size_x_ - 1) {
|
|
out.push_back(idx + 1);
|
|
}
|
|
if (idx >= size_x_) {
|
|
out.push_back(idx - size_x_);
|
|
}
|
|
if (idx < size_x_ * (size_y_ - 1)) {
|
|
out.push_back(idx + size_x_);
|
|
}
|
|
return out;
|
|
}
|
|
|
|
/**
|
|
* @brief Determine 8-connected neighbourhood of an input cell, checking for map
|
|
* edges
|
|
* @param idx input cell index
|
|
* @param costmap Reference to map data
|
|
* @return neighbour cell indexes
|
|
*/
|
|
std::vector<unsigned int> nhood8(unsigned int idx,
|
|
const nav2_costmap_2d::Costmap2D& costmap)
|
|
{
|
|
// get 8-connected neighbourhood indexes, check for edge of map
|
|
std::vector<unsigned int> out = nhood4(idx, costmap);
|
|
|
|
unsigned int size_x_ = costmap.getSizeInCellsX(),
|
|
size_y_ = costmap.getSizeInCellsY();
|
|
|
|
if (idx > size_x_ * size_y_ - 1) {
|
|
return out;
|
|
}
|
|
|
|
if (idx % size_x_ > 0 && idx >= size_x_) {
|
|
out.push_back(idx - 1 - size_x_);
|
|
}
|
|
if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
|
|
out.push_back(idx - 1 + size_x_);
|
|
}
|
|
if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
|
|
out.push_back(idx + 1 - size_x_);
|
|
}
|
|
if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
|
|
out.push_back(idx + 1 + size_x_);
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
/**
|
|
* @brief Find nearest cell of a specified value
|
|
* @param result Index of located cell
|
|
* @param start Index initial cell to search from
|
|
* @param val Specified value to search for
|
|
* @param costmap Reference to map data
|
|
* @return True if a cell with the requested value was found
|
|
*/
|
|
bool nearestCell(unsigned int& result, unsigned int start, unsigned char val,
|
|
const nav2_costmap_2d::Costmap2D& costmap)
|
|
{
|
|
const unsigned char* map = costmap.getCharMap();
|
|
const unsigned int size_x = costmap.getSizeInCellsX(),
|
|
size_y = costmap.getSizeInCellsY();
|
|
|
|
if (start >= size_x * size_y) {
|
|
return false;
|
|
}
|
|
|
|
// initialize breadth first search
|
|
std::queue<unsigned int> bfs;
|
|
std::vector<bool> visited_flag(size_x * size_y, false);
|
|
|
|
// push initial cell
|
|
bfs.push(start);
|
|
visited_flag[start] = true;
|
|
|
|
// search for neighbouring cell matching value
|
|
while (!bfs.empty()) {
|
|
unsigned int idx = bfs.front();
|
|
bfs.pop();
|
|
|
|
// return if cell of correct value is found
|
|
if (map[idx] == val) {
|
|
result = idx;
|
|
return true;
|
|
}
|
|
|
|
// iterate over all adjacent unvisited cells
|
|
for (unsigned nbr : nhood8(idx, costmap)) {
|
|
if (!visited_flag[nbr]) {
|
|
bfs.push(nbr);
|
|
visited_flag[nbr] = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
} // namespace frontier_exploration
|
|
#endif
|