void ExploreFrontier::findFrontiers() {
frontiers_.clear();
const size_t w = costmap_->getSizeInCellsX();
const size_t h = costmap_->getSizeInCellsY();
const size_t size = w * h;
map_.info.width = w;
map_.info.height = h;
map_.data.resize(size);
map_.info.resolution = costmap_->getResolution();
map_.info.origin.position.x = costmap_->getOriginX();
map_.info.origin.position.y = costmap_->getOriginY();
// lock as we are accessing raw underlying map
auto *mutex = costmap_->getMutex();
std::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap_->getCharMap();
ROS_DEBUG_COND(!map, "no map available");
for (size_t i = 0; i < size; ++i) {
// //get the world point for the index
// unsigned int mx, my;
// costmap.indexToCells(i, mx, my);
// geometry_msgs::Point p;
// costmap.mapToWorld(mx, my, p.x, p.y);
//
//check if the point has valid potential and is next to unknown space
// bool valid_point = planner_->validPointPotential(p);
bool valid_point = (map[i] < costmap_2d::LETHAL_OBSTACLE);
// ROS_DEBUG_COND(!valid_point, "invalid point %u", map[i]);
if ((valid_point && map) &&
(((i+1 < size) && (map[i+1] == costmap_2d::NO_INFORMATION)) ||
((i-1 < size) && (map[i-1] == costmap_2d::NO_INFORMATION)) ||
((i+w < size) && (map[i+w] == costmap_2d::NO_INFORMATION)) ||
((i-w < size) && (map[i-w] == costmap_2d::NO_INFORMATION))))
{
ROS_DEBUG_THROTTLE(30, "found suitable point");
map_.data[i] = -128;
} else {
map_.data[i] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
for (size_t y = 0, i = h-1; y < w; ++y) {
ROS_DEBUG_THROTTLE(30, "cleaning cell %lu", i);
map_.data[i] = -127;
i += h;
}
// Group adjoining map_ pixels
signed char segment_id = 127;
std::vector<std::vector<FrontierPoint>> segments;
for (size_t i = 0; i < size; i++) {
if (map_.data[i] == -128) {
ROS_DEBUG_THROTTLE(30, "adjoining on %lu", i);
std::vector<size_t> neighbors;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
// claim all neighbors
while (!neighbors.empty()) {
ROS_DEBUG_THROTTLE(30, "got %lu neighbors", neighbors.size());
size_t idx = neighbors.back();
neighbors.pop_back();
map_.data[idx] = segment_id;
tf::Vector3 tot(0,0,0);
size_t c = 0;
if (idx+1 < size && map[idx+1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(1,0,0);
++c;
}
if (idx-1 < size && map[idx-1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(-1,0,0);
++c;
}
if (idx+w < size && map[idx+w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,1,0);
++c;
}
if (idx-w < size && map[idx-w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,-1,0);
++c;
}
if(!(c > 0)) {
ROS_ERROR("assertion failed. corrupted costmap?");
ROS_DEBUG("c is %lu", c);
continue;
}
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (idx-1 < size && map_.data[idx-1] == -128)
neighbors.push_back(idx-1);
if (idx+1 < size && map_.data[idx+1] == -128)
neighbors.push_back(idx+1);
if (idx-w < size && map_.data[idx-w] == -128)
neighbors.push_back(idx-w);
if (idx-w+1 < size && map_.data[idx-w+1] == -128)
neighbors.push_back(idx-w+1);
if (idx-w-1 < size && map_.data[idx-w-1] == -128)
neighbors.push_back(idx-w-1);
if (idx+w < size && map_.data[idx+w] == -128)
neighbors.push_back(idx+w);
if (idx+w+1 < size && map_.data[idx+w+1] == -128)
neighbors.push_back(idx+w+1);
if (idx+w-1 < size && map_.data[idx+w-1] == -128)
neighbors.push_back(idx+w-1);
}
segments.push_back(std::move(segment));
segment_id--;
if (segment_id < -127)
break;
}
}
// no longer accessing map
lock.unlock();
int num_segments = 127 - segment_id;
if (num_segments <= 0) {
ROS_DEBUG("#segments is <0, no frontiers found");
return;
}
for (auto& segment : segments) {
Frontier frontier;
size_t segment_size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
if (segment_size * costmap_->getResolution() < costmap_client_->getInscribedRadius()) {
ROS_DEBUG_THROTTLE(30, "some frontiers were small");
continue;
}
float x = 0, y = 0;
tf::Vector3 d(0,0,0);
for (const auto& frontier_point : segment) {
d += frontier_point.d;
size_t idx = frontier_point.idx;
x += (idx % w);
y += (idx / w);
}
d = d / segment_size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / segment_size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / segment_size);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(atan2(d.y(), d.x()));
frontier.size = segment_size;
frontiers_.push_back(std::move(frontier));
}
}
void ExploreFrontier::findFrontiers(Costmap2DROS& costmap_) {
frontiers_.clear();
Costmap2D costmap;
costmap_.getCostmapCopy(costmap);
int idx;
int w = costmap.getSizeInCellsX();
int h = costmap.getSizeInCellsY();
int size = (w * h);
int pts = 0;
map_.info.width = w;
map_.info.height = h;
map_.set_data_size(size);
map_.info.resolution = costmap.getResolution();
map_.info.origin.position.x = costmap.getOriginX();
map_.info.origin.position.y = costmap.getOriginY();
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap.getCharMap();
for (idx = 0; idx < size; idx++) {
//get the world point for the index
unsigned int mx, my;
costmap.indexToCells(idx, mx, my);
geometry_msgs::Point p;
costmap.mapToWorld(mx, my, p.x, p.y);
//check if the point has valid potential and is next to unknown space
//bool valid_point = planner_->validPointPotential(p);
//bool valid_point = planner_->computePotential(p) && planner_->validPointPotential(p);
//bool valid_point = (map[idx] < LETHAL_OBSTACLE);
//bool valid_point = (map[idx] < INSCRIBED_INFLATED_OBSTACLE );
bool valid_point = (map[idx] == FREE_SPACE);
if ((valid_point && map) &&
(((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) ||
((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) ||
((idx+w < size) && (map[idx+w] == NO_INFORMATION)) ||
((idx-w >= 0) && (map[idx-w] == NO_INFORMATION))))
{
map_.data[idx] = -128;
//ROS_INFO("Candidate Point %d.", pts++ );
} else {
map_.data[idx] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
// I don't know what this means -- Travis.
idx = map_.info.height - 1;
for (unsigned int y=0; y < map_.info.width; y++) {
map_.data[idx] = -127;
idx += map_.info.height;
}
// Group adjoining map_ pixels
int segment_id = 127;
std::vector< std::vector<FrontierPoint> > segments;
for (int i = 0; i < size; i++) {
if (map_.data[i] == -128) {
std::vector<int> neighbors, candidates;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
int points_in_seg = 0;
unsigned int mx, my;
geometry_msgs::Point p_init, p;
costmap.indexToCells(i, mx, my);
costmap.mapToWorld(mx, my, p_init.x, p_init.y);
// claim all neighbors
while (neighbors.size() > 0) {
int idx = neighbors.back();
neighbors.pop_back();
btVector3 tot(0,0,0);
int c = 0;
if ((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) {
tot += btVector3(1,0,0);
c++;
}
if ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) {
tot += btVector3(-1,0,0);
c++;
}
if ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) {
tot += btVector3(0,1,0);
c++;
}
if ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION)) {
tot += btVector3(0,-1,0);
c++;
}
assert(c > 0);
// Only adjust the map and add idx to segment when its near enough to start point.
// This should prevent greedy approach where all cells belong to one frontier!
costmap.indexToCells(idx, mx, my);
costmap.mapToWorld(mx, my, p.x, p.y);
float dist;
dist = sqrt( pow( p.x - p_init.x, 2 ) + pow( p.y - p_init.y, 2 ));
if ( dist <= 0.8 ){
map_.data[idx] = segment_id; // This used to be up before "assert" block above.
points_in_seg += 1;
//ROS_INFO( "Dist to start cell: %3.2f", dist );
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (((idx-1)>0) && (map_.data[idx-1] == -128))
neighbors.push_back(idx-1);
if (((idx+1)<size) && (map_.data[idx+1] == -128))
neighbors.push_back(idx+1);
if (((idx-map_.info.width)>0) && (map_.data[idx-map_.info.width] == -128))
neighbors.push_back(idx-map_.info.width);
if (((idx-map_.info.width+1)>0) && (map_.data[idx-map_.info.width+1] == -128))
neighbors.push_back(idx-map_.info.width+1);
if (((idx-map_.info.width-1)>0) && (map_.data[idx-map_.info.width-1] == -128))
neighbors.push_back(idx-map_.info.width-1);
if (((idx+(int)map_.info.width)<size) && (map_.data[idx+map_.info.width] == -128))
neighbors.push_back(idx+map_.info.width);
if (((idx+(int)map_.info.width+1)<size) && (map_.data[idx+map_.info.width+1] == -128))
neighbors.push_back(idx+map_.info.width+1);
if (((idx+(int)map_.info.width-1)<size) && (map_.data[idx+map_.info.width-1] == -128))
neighbors.push_back(idx+map_.info.width-1);
}
}
segments.push_back(segment);
ROS_INFO( "Segment %d has %d costmap cells", segment_id, points_in_seg );
segment_id--;
if (segment_id < -127)
break;
}
}
int num_segments = 127 - segment_id;
ROS_INFO("Segments: %d", num_segments );
if (num_segments <= 0)
return;
for (unsigned int i=0; i < segments.size(); i++) {
Frontier frontier;
std::vector<FrontierPoint>& segment = segments[i];
uint size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
// This seems like a goofy heuristic rather than fact. What happens if we don't do this...?
if (size * costmap.getResolution() < costmap.getInscribedRadius()){
ROS_INFO( "Discarding segment... too small?" );
//continue;
}
float x = 0, y = 0;
btVector3 d(0,0,0);
for (uint j=0; j<size; j++) {
d += segment[j].d;
int idx = segment[j].idx;
x += (idx % map_.info.width);
y += (idx / map_.info.width);
// x = (idx % map_.info.width);
// y = (idx / map_.info.width);
}
d = d / size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / size);
// frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x);
// frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(btAtan2(d.y(), d.x()));
frontier.size = size;
geometry_msgs::Point p;
p.x = map_.info.origin.position.x + map_.info.resolution * (x); // frontier.pose.position
p.y = map_.info.origin.position.y + map_.info.resolution * (y);
if (!planner_->validPointPotential(p)){
ROS_INFO( "Discarding segment... can't reach?" );
//continue;
}
frontiers_.push_back(frontier);
}
}
