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 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;
}
/**
* Convert an OccupancyGrid map message into the internal representation.
*/
void AMCLocalizer::initMap( const nav_msgs::OccupancyGrid& map_msg ) {
/*
* Convert an OccupancyGrid map message into the internal representation.
*/
std::cout << "1" << std::endl;
freeMapDependentMemory();
// Clear queued laser objects because they hold pointers to the existing map, #5202.
// lasers_.clear();
// lasers_update_.clear();
std::cout << "2" << std::endl;
map_t* map = map_alloc();
// ROS_ASSERT(map);
std::cout << "3" << std::endl;
map->size_x = map_msg.info.width;
map->size_y = map_msg.info.height;
map->scale = map_msg.info.resolution;
map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
// Convert to player format
std::cout << "4" << std::endl;
map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
//ROS_ASSERT(map->cells);
std::cout << "5" << std::endl;
for(int i=0; i<map->size_x * map->size_y; i++) {
if(map_msg.data[i] == 0)
map->cells[i].occ_state = -1;
else if(map_msg.data[i] == 100)
map->cells[i].occ_state = +1;
else
map->cells[i].occ_state = 0;
}
std::cout << "6" << std::endl;
first_map_received_ = true;
/*
* Initialize the particle filter
*/
#if NEW_UNIFORM_SAMPLING
// Index of free space
std::cout << "7" << std::endl;
free_space_indices.resize(0);
std::cout << "8" << std::endl;
for(int i = 0; i < map_->size_x; i++)
for(int j = 0; j < map_->size_y; j++)
if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
free_space_indices.push_back(std::make_pair(i,j));
#endif
std::cout << "9" << std::endl;
// boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
(pf_init_model_fn_t)AMCLocalizer::uniformPoseGenerator,
(void *)map_);
std::cout << "10" << std::endl;
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;
/*
* Instantiate the sensor objects: Odometry
*/
delete odom_;
odom_ = new AMCLOdom();
// ROS_ASSERT(odom_);
if(odom_model_type_ == ODOM_MODEL_OMNI)
odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
else
odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
/*
* Instantiate the sensor objects: Laser
*/
delete laser_;
laser_ = new AMCLLaser(max_beams_, map_);
// ROS_ASSERT(laser_);
if(laser_model_type_ == LASER_MODEL_BEAM)
laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
sigma_hit_, lambda_short_, 0.0);
else {
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
laser_likelihood_max_dist_);
}
// In case the initial pose message arrived before the first map,
// try to apply the initial pose now that the map has arrived.
applyInitialPose();
}
bool operator<(const CellData& a, const CellData& b)
{
return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
}
int map_get_home() {
DARNIT_TILEMAP *toplayer_tilemap=map->layer[map->layers-1].tilemap;
return map_isset_home()?MAP_INDEX(home_x, home_y):-1;
}
void map_update_cspace_changed_cells1(const int min_x, const int max_x, const int min_y, const int max_y, const double max_occ_dist, map_t *map)
{
// init distance;
for(int x=min_x+1; x<max_x-1; x++){
for(int y=min_y+1; y<max_y-1; y++){
if(map->cells[MAP_INDEX(map, x, y)].occ_state == +1)
{
map->cells[MAP_INDEX(map, x, y)].occ_dist = 0.0;
}
else {
map->cells[MAP_INDEX(map, x, y)].occ_dist = max_occ_dist;
}
}
}
// BUG FixMe;
double ds=sqrt(2)*0.05; //resolution;
double ls=0.05; //resolution;
// distance propagation
for(int x=min_x+1; x<=max_x-1; x++) {
for(int y=min_y+1; y<=max_y-1; y++){
double mval=max_occ_dist;
int idx0 = MAP_INDEX(map, x, y);
int idx1 = MAP_INDEX(map, x-1, y-1);
int idx2 = MAP_INDEX(map, x-1, y);
int idx3 = MAP_INDEX(map, x-1, y+1);
int idx4 = MAP_INDEX(map, x, y-1);
int idx5 = MAP_INDEX(map, x, y+1);
int idx6 = MAP_INDEX(map, x+1, y-1);
int idx7 = MAP_INDEX(map, x+1, y);
int idx8 = MAP_INDEX(map, x+1, y+1);
mval = mval < map->cells[idx1].occ_dist + ds? mval: map->cells[idx1].occ_dist + ds; // x-1, y-1
mval = mval < map->cells[idx2].occ_dist + ls? mval: map->cells[idx2].occ_dist + ls; // x-1, y
mval = mval < map->cells[idx3].occ_dist + ds? mval: map->cells[idx3].occ_dist + ds; // x-1, y+1
mval = mval < map->cells[idx4].occ_dist + ls? mval: map->cells[idx4].occ_dist + ls; // x, y-1
mval = mval < map->cells[idx5].occ_dist + ls? mval: map->cells[idx5].occ_dist + ls; // x, y+1
mval = mval < map->cells[idx6].occ_dist + ds? mval: map->cells[idx6].occ_dist + ds; // x+1, y-1
mval = mval < map->cells[idx7].occ_dist + ls? mval: map->cells[idx7].occ_dist + ls; // x+1, y
mval = mval < map->cells[idx8].occ_dist + ds? mval: map->cells[idx8].occ_dist + ds; // x+1, y+1
mval = mval < max_occ_dist ? mval:max_occ_dist;
map->cells[idx0].occ_dist = map->cells[idx0].occ_dist < mval ? map->cells[idx0].occ_dist : mval;
}
}
for(int x=max_x-1; x>=min_x+1; x--) {
for(int y=max_y-1; y>=min_y+1; y--) {
double mval=max_occ_dist;
int idx0 = MAP_INDEX(map, x, y);
int idx1 = MAP_INDEX(map, x-1, y-1);
int idx2 = MAP_INDEX(map, x-1, y);
int idx3 = MAP_INDEX(map, x-1, y+1);
int idx4 = MAP_INDEX(map, x, y-1);
int idx5 = MAP_INDEX(map, x, y+1);
int idx6 = MAP_INDEX(map, x+1, y-1);
int idx7 = MAP_INDEX(map, x+1, y);
int idx8 = MAP_INDEX(map, x+1, y+1);
mval = mval < map->cells[idx1].occ_dist + ds? mval: map->cells[idx1].occ_dist + ds; // x-1, y-1
mval = mval < map->cells[idx2].occ_dist + ls? mval: map->cells[idx2].occ_dist + ls; // x-1, y
mval = mval < map->cells[idx3].occ_dist + ds? mval: map->cells[idx3].occ_dist + ds; // x-1, y+1
mval = mval < map->cells[idx4].occ_dist + ls? mval: map->cells[idx4].occ_dist + ls; // x, y-1
mval = mval < map->cells[idx5].occ_dist + ls? mval: map->cells[idx5].occ_dist + ls; // x, y+1
mval = mval < map->cells[idx6].occ_dist + ds? mval: map->cells[idx6].occ_dist + ds; // x+1, y-1
mval = mval < map->cells[idx7].occ_dist + ls? mval: map->cells[idx7].occ_dist + ls; // x+1, y
mval = mval < map->cells[idx8].occ_dist + ds? mval: map->cells[idx8].occ_dist + ds; // x+1, y+1
mval = mval < max_occ_dist ? mval:max_occ_dist;
map->cells[idx0].occ_dist = map->cells[idx0].occ_dist < mval ? map->cells[idx0].occ_dist : mval;
}
}
}
////////////////////////////////////////////////////////////////////////////
// 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;
}
const map_cell_t* at(int xi, int yi) const {
return map_->cells + MAP_INDEX(map_, xi, yi);
}
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));
}
}
}
}
