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);
}
}
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::buildShortestPath(int ind) {
Path path;
if (map_ == NULL) {
ROS_WARN("OccupancyMap::buildShortestPath() Map not set");
return path;
}
// Recreate path
const Eigen::Vector2f &stop = endpoints_.at(ind);
int stopi = MAP_GXWX(map_, stop(0)), stopj = MAP_GYWY(map_, stop(1));
buildPath(stopi, stopj, &path);
return Path(path.rbegin(), path.rend());
}
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());
}
}
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();
}
}
}
}
