void map_update_cspace_changed(const int x, const int y, const int state, const double max_occ_dist, map_t *map)
{
int i, j;
int ni, nj;
int s;
double d;
map_cell_t *cell, *ncell;
map->max_occ_dist = max_occ_dist;
s = (int) ceil(map->max_occ_dist / map->scale);
// Reset the distance values in the ratios (x, y) +- 2.0m / map_scale
for (j = y-s; j < y+s; j++)
{
for (i = x-s; i < x+s; i++)
{
if(MAP_VALID(map, i, j)) {
cell = map->cells + MAP_INDEX(map, i, j);
cell->occ_dist = map->max_occ_dist;
}
}
}
// Find all the occupied cells and update their neighbours
for (j = y-s; j < y+s; j++)
{
for (i = x-s; i < y+s; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
if (cell->occ_state != +1)
continue;
cell->occ_dist = 0;
// Update adjacent cells
for (nj = -s; nj <= +s; nj++)
{
for (ni = -s; ni <= +s; ni++)
{
if (!MAP_VALID(map, i + ni, j + nj))
continue;
ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
d = map->scale * sqrt(ni * ni + nj * nj);
if (d < ncell->occ_dist)
ncell->occ_dist = d;
}
}
}
}
}
pf_vector_t AMCLocalizer::uniformPoseGenerator(void* arg) {
map_t* map = (map_t*)arg;
#if NEW_UNIFORM_SAMPLING
unsigned int rand_index = drand48() * free_space_indices.size();
std::pair<int,int> free_point = free_space_indices[rand_index];
pf_vector_t p;
p.v[0] = MAP_WXGX(map, free_point.first);
p.v[1] = MAP_WYGY(map, free_point.second);
p.v[2] = drand48() * 2 * M_PI - M_PI;
#else
double min_x, max_x, min_y, max_y;
min_x = (map->size_x * map->scale)/2.0 - map->origin_x;
max_x = (map->size_x * map->scale)/2.0 + map->origin_x;
min_y = (map->size_y * map->scale)/2.0 - map->origin_y;
max_y = (map->size_y * map->scale)/2.0 + map->origin_y;
pf_vector_t p;
ROS_DEBUG("Generating new uniform sample");
for(;;)
{
p.v[0] = min_x + drand48() * (max_x - min_x);
p.v[1] = min_y + drand48() * (max_y - min_y);
p.v[2] = drand48() * 2 * M_PI - M_PI;
// Check that it's a free cell
int i,j;
i = MAP_GXWX(map, p.v[0]);
j = MAP_GYWY(map, p.v[1]);
if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
break;
}
#endif
return p;
}
int coordIndex(double x, double y) const {
int xi = MAP_GXWX(map_, x);
int yi = MAP_GYWY(map_, y);
if (!MAP_VALID(map_, xi, yi)) {
return -1;
} else {
return MAP_INDEX(map_, xi, yi);
}
}
// Update the cspace distance values
void map_update_cspace(map_t *map, double max_occ_dist)
{
int i, j;
int ni, nj;
int s;
double d;
map_cell_t *cell, *ncell;
map->max_occ_dist = max_occ_dist;
s = (int) ceil(map->max_occ_dist / map->scale);
// Reset the distance values
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
cell->occ_dist = map->max_occ_dist;
}
}
// Find all the occupied cells and update their neighbours
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
if (cell->occ_state != +1)
continue;
cell->occ_dist = 0;
// Update adjacent cells
for (nj = -s; nj <= +s; nj++)
{
for (ni = -s; ni <= +s; ni++)
{
if (!MAP_VALID(map, i + ni, j + nj))
continue;
ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
d = map->scale * sqrt(ni * ni + nj * nj);
if (d < ncell->occ_dist)
ncell->occ_dist = d;
}
}
}
}
return;
}
void OccupancyMap::initializeSearch(double startx, double starty) {
starti_ = MAP_GXWX(map_, startx);
startj_ = MAP_GYWY(map_, starty);
if (!MAP_VALID(map_, starti_, startj_)) {
ROS_ERROR("OccupancyMap::initializeSearch() Invalid starting position");
ROS_BREAK();
}
int ncells = map_->size_x * map_->size_y;
if (ncells_ != ncells) {
ncells_ = ncells;
costs_.reset(new float[ncells]);
prev_i_.reset(new int[ncells]);
prev_j_.reset(new int[ncells]);
}
// TODO: Return to more efficient lazy-initialization
// // Map is large and initializing costs_ takes a while. To speedup,
// // partially initialize costs_ in a rectangle surrounding start and stop
// // positions + margin. If you run up against boundary, initialize the rest.
// int margin = 120;
// init_ul_ = make_pair(max(0, min(starti_, stopi) - margin),
// max(0, min(startj_, stopj) - margin));
// init_lr_ = make_pair(min(map_->size_x, max(starti_, stopi) + margin),
// min(map_->size_y, max(startj_, stopj) + margin));
// for (int j = init_ul.second; j < init_lr.second; ++j) {
// for (int i = init_ul.first; i < init_lr.first; ++i) {
// int ind = MAP_INDEX(map_, i, j);
// costs_[ind] = std::numeric_limits<float>::infinity();
// }
// }
// full_init_ = false;
for (int i = 0; i < ncells_; ++i) {
costs_[i] = std::numeric_limits<float>::infinity();
prev_i_[i] = -1;
prev_j_[i] = -1;
}
int start_ind = MAP_INDEX(map_, starti_, startj_);
costs_[start_ind] = 0.0;
prev_i_[starti_] = starti_;
prev_j_[startj_] = startj_;
Q_.reset(new set<Node, NodeCompare>());
Q_->insert(Node(make_pair(starti_, startj_), 0.0, 0.0));
stopi_ = -1;
stopj_ = -1;
}
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa)
{
int i, j;
map_cell_t *cell;
i = (int) MAP_GXWX(map, ox);
j = (int) MAP_GYWY(map, oy);
if (!MAP_VALID(map, i, j))
return NULL;
cell = map->cells + MAP_INDEX(map, i, j);
return cell;
}
Path OccupancyMap::astar(double startx, double starty,
double stopx, double stopy,
double max_occ_dist /* = 0.0 */,
bool allow_unknown /* = false */) {
Path path;
if (map_ == NULL) {
ROS_WARN("OccupancyMap::astar() Map not set");
return path;
}
int stopi = MAP_GXWX(map_, stopx), stopj = MAP_GYWY(map_, stopy);
if (!MAP_VALID(map_, stopi ,stopj)) {
ROS_ERROR("OccupancyMap::astar() Invalid stopping position");
ROS_BREAK();
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::astar() CSpace has been calculated up to %f, "
"but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
initializeSearch(startx, starty);
// Set stop to use heuristic
stopi_ = stopi;
stopj_ = stopj;
bool found = false;
Node curr_node;
while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
if (curr_node.coord.first == stopi && curr_node.coord.second == stopj) {
found = true;
break;
}
}
// Recreate path
if (found) {
buildPath(stopi, stopj, &path);
}
return Path(path.rbegin(), path.rend());
}
Path OccupancyMap::shortestPath(double stopx, double stopy) {
Path path;
if (map_ == NULL) {
ROS_WARN("OccupancyMap::shortestPath() Map not set");
return path;
}
int i = MAP_GXWX(map_, stopx);
int j = MAP_GYWY(map_, stopy);
int ind = MAP_INDEX(map_, i, j);
if (!MAP_VALID(map_, i, j)) {
ROS_ERROR("OccMap::shortestPath() Invalid destination: x=%f y=%f",
stopx, stopy);
ROS_BREAK();
return path; // return to prevent compiler warning
} else if ( prev_i_[ind] == -1 || prev_j_[ind] == -1) {
return path;
} else {
buildPath(i, j, &path);
return Path(path.rbegin(), path.rend());
}
}
////////////////////////////////////////////////////////////////////////////
// Load an occupancy grid
int map_load_occ(map_t *map, const char *filename, double scale, int negate)
{
FILE *file;
char magic[3];
int i, j;
int ch, occ;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
fclose(file);
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
{
fprintf(stderr, "Failed ot read image dimensions");
return -1;
}
// Allocate space in the map
if (map->cells == NULL)
{
map->scale = scale;
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
//PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
// Black-on-white images
if (!negate)
{
if (ch < depth / 4)
occ = +1;
else if (ch > 3 * depth / 4)
occ = -1;
else
occ = 0;
}
// White-on-black images
else
{
if (ch < depth / 4)
occ = -1;
else if (ch > 3 * depth / 4)
occ = +1;
else
occ = 0;
}
if (!MAP_VALID(map, i, j))
continue;
cell = map->cells + MAP_INDEX(map, i, j);
cell->occ_state = occ;
}
}
fclose(file);
return 0;
}
void
LaserMapFilterDataFilter::filter()
{
const unsigned int vecsize = in.size();
if (vecsize == 0) return;
for (unsigned int a = 0; a < vecsize; ++a) {
// get tf to map of laser input
fawkes::tf::StampedTransform transform;
try{
tf_listener_->lookup_transform(frame_map_.c_str(), in[a]->frame, *(in[a]->timestamp), transform);
} catch(fawkes::tf::TransformException &e) {
try{
tf_listener_->lookup_transform(frame_map_.c_str(), in[a]->frame, fawkes::Time(0, 0), transform);
logger_->log_debug("map_filter", "Can't transform laser-data using newest tf\n(%s\t%s\t\%lf)",
frame_map_.c_str(), in[a]->frame.c_str(), in[a]->timestamp->in_sec());
} catch(fawkes::tf::TransformException &e) {
logger_->log_debug("map_filter", "Can't transform laser-data at all (%s -> %s)",
frame_map_.c_str(), in[a]->frame.c_str());
return;
}
}
// set out meta info
out[a]->frame = in[a]->frame;
out[a]->timestamp = in[a]->timestamp;
// for each point
for (unsigned int i = 0; i < out_data_size; ++i) {
bool add = true;
// check nan
if ( std::isfinite(in[a]->values[i]) ) {
// transform to cartesian
double angle = M_PI * (360.f / out_data_size * i ) / 180;
float x, y;
fawkes::polar2cart2d(angle, in[a]->values[i], &x, &y);
// transform into map
fawkes::tf::Point p;
p.setValue(x, y, 0.);
p = transform * p;
// transform to map cells
int cell_x = (int)MAP_GXWX(map_, p.getX());
int cell_y = (int)MAP_GYWY(map_, p.getY());
// search in 8-neighborhood and itself for occupied pixels in map
for (int ox = -2; add && ox <= 2; ++ox) {
for (int oy = -2; oy <= 2; ++oy) {
int x = cell_x + ox;
int y = cell_y + oy;
if (MAP_VALID(map_, x, y)) {
if (map_->cells[MAP_INDEX(map_, x, y)].occ_state > 0) {
add = false;
break;
}
}
}
}
}
if (add) {
out[a]->values[i] = in[a]->values[i];
} else {
out[a]->values[i] = std::numeric_limits<float>::quiet_NaN();
}
}
}
}
////////////////////////////////////////////////////////////////////////////
// Load a wifi signal strength map
int map_load_wifi(map_t *map, const char *filename, int index)
{
FILE *file;
char magic[3];
int i, j;
int ch, level;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
fscanf(file, "%10s \n", magic);
if (strcmp(magic, "P5") != 0)
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
fscanf(file, " %d %d \n %d \n", &width, &height, &depth);
// Allocate space in the map
if (map->cells == NULL)
{
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
if (!MAP_VALID(map, i, j))
continue;
if (ch == 0)
level = 0;
else
level = ch * 100 / 255 - 100;
cell = map->cells + MAP_INDEX(map, i, j);
cell->wifi_levels[index] = level;
}
}
fclose(file);
return 0;
}
void OccupancyMap::addNeighbors(const Node &node, double max_occ_dist, bool allow_unknown) {
// TODO: Return to more efficient lazy-initialization
// // Check if we're neighboring nodes whose costs_ are uninitialized.
// // if (!full_init &&
// // ((ci + 1 >= init_lr.first) || (ci - 1 <= init_ul.first) ||
// // (cj + 1 >= init_lr.second) || (cj - 1 <= init_ul.second))) {
// // full_init = true;
// // for (int j = 0; j < map_->size_y; ++j) {
// // for (int i = 0; i < map_->size_x; ++i) {
// // // Only initialize costs_ that are outside original rectangle
// // if (!(init_ul.first <= i && i < init_lr.first &&
// // init_ul.second <= j && j < init_lr.second)) {
// // int ind = MAP_INDEX(map_, i, j);
// // costs_[ind] = std::numeric_limits<float>::infinity();
// // }
// // }
// // }
// // }
int ci = node.coord.first;
int cj = node.coord.second;
// Iterate over neighbors
for (int newj = cj - 1; newj <= cj + 1; ++newj) {
for (int newi = ci - 1; newi <= ci + 1; ++newi) {
// Skip self edges
if ((newi == ci && newj == cj) || !MAP_VALID(map_, newi, newj)) {
continue;
}
// fprintf(stderr, " Examining %i %i ", newi, newj);
int index = MAP_INDEX(map_, newi, newj);
map_cell_t *cell = map_->cells + index;
// If cell is occupied or too close to occupied cell, continue
if (isinff(cell->cost) ||
(!allow_unknown && cell->occ_state == map_cell_t::UNKNOWN)) {
// fprintf(stderr, "occupado\n");
continue;
}
// fprintf(stderr, "free\n");
double edge_cost = ci == newi || cj == newj ? 1 : sqrt(2);
double heur_cost = 0.0;
if (stopi_ != -1 && stopj_ != -1) {
heur_cost = hypot(newi - stopi_, newj - stopj_);
}
double total_cost = node.true_cost + edge_cost + cell->cost + heur_cost;
if (total_cost < costs_[index]) {
// fprintf(stderr, " Better path: new cost= % 6.2f\n", ttl_cost);
// If node has finite cost, it's in queue and needs to be removed
if (!isinf(costs_[index])) {
Q_->erase(Node(make_pair(newi, newj), costs_[index], 0.0));
}
costs_[index] = total_cost;
prev_i_[index] = ci;
prev_j_[index] = cj;
Q_->insert(Node(make_pair(newi, newj),
node.true_cost + edge_cost + cell->cost,
total_cost));
}
}
}
}
