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;
}
Esempio n. 2
0
 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);
   }
 }
Esempio n. 3
0
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;
}
Esempio n. 4
0
// 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;
}
Esempio n. 5
0
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());
}
Esempio n. 6
0
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());
}
Esempio n. 7
0
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());
  }
}
Esempio n. 8
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();
      }
    }
  }
}