void OccupancyMap::buildPath(int i, int j, Path *path) {
while (!(i == starti_ && j == startj_)) {
int index = MAP_INDEX(map_, i, j);
float x = MAP_WXGX(map_, i);
float y = MAP_WYGY(map_, j);
path->push_back(Eigen::Vector2f(x, y));
i = prev_i_[index];
j = prev_j_[index];
}
float x = MAP_WXGX(map_, i);
float y = MAP_WYGY(map_, j);
path->push_back(Eigen::Vector2f(x, y));
}
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;
}
const Path&
OccupancyMap::prepareShortestPaths(double x, double y,
double min_distance, double max_distance,
double max_occ_dist,
bool allow_unknown) {
endpoints_.clear();
if (map_ == NULL) {
ROS_WARN("OccupancyMap::prepareShortestPaths() Map not set");
return endpoints_;
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::prepareShortestPaths() CSpace has been calculated "
"up to %f, but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
initializeSearch(x, y);
Node curr_node;
while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
double node_dist = curr_node.true_cost * map_->scale;
if (min_distance <= node_dist && node_dist < max_distance) {
float x = MAP_WXGX(map_, curr_node.coord.first);
float y = MAP_WYGY(map_, curr_node.coord.second);
endpoints_.push_back(Eigen::Vector2f(x, y));
} else if (node_dist > max_distance) {
break;
}
}
return endpoints_;
}
inline double OccupancyMap::maxY() {
return MAP_WYGY(map_, map_->size_y);
}
inline double OccupancyMap::minY() {
return MAP_WYGY(map_, 0);
}
