void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;

  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);

  // resize costmap if size, resolution or origin do not match
  Costmap2D* master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
      master->getSizeInCellsY() != size_y ||
      master->getResolution() != new_map->info.resolution ||
      master->getOriginX() != new_map->info.origin.position.x ||
      master->getOriginY() != new_map->info.origin.position.y ||
      !layered_costmap_->isSizeLocked()))
  {
    // Update the size of the layered costmap (and all layers, including this one)
    ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
                                new_map->info.origin.position.y, true);
  }
  else if (size_x_ != size_x || size_y_ != size_y ||
           resolution_ != new_map->info.resolution ||
           origin_x_ != new_map->info.origin.position.x ||
           origin_y_ != new_map->info.origin.position.y)
  {
    // only update the size of the costmap stored locally in this layer
    ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    resizeMap(size_x, size_y, new_map->info.resolution,
              new_map->info.origin.position.x, new_map->info.origin.position.y);
  }

  unsigned int index = 0;

  // initialize the costmap with static data
  for (unsigned int i = 0; i < size_y; ++i)
  {
    for (unsigned int j = 0; j < size_x; ++j)
    {
      unsigned char value = new_map->data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
  map_frame_ = new_map->header.frame_id;

  // we have a new map, update full size of map
  x_ = y_ = 0;
  width_ = size_x_;
  height_ = size_y_;
  map_received_ = true;
  has_updated_data_ = true;

  // shutdown the map subscrber if firt_map_only_ flag is on
  if (first_map_only_)
  {
    ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
    map_sub_.shutdown();
  }
}
void StaticLayer::matchSize()
{
  // If we are using rolling costmap, the static map size is
  //   unrelated to the size of the layered costmap
  if (!layered_costmap_->isRolling())
  {
    Costmap2D* master = layered_costmap_->getCostmap();
    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
              master->getOriginX(), master->getOriginY());
  }
}
void CostmapLayer::matchSize()
{
    Costmap2D* master = layered_costmap_->getCostmap();
    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
            master->getOriginX(), master->getOriginY());
}
Ejemplo n.º 4
0
void ExploreFrontier::findFrontiers(Costmap2DROS& costmap_) {
  frontiers_.clear();

  Costmap2D costmap;
  costmap_.getCostmapCopy(costmap);

  int idx;
  int w = costmap.getSizeInCellsX();
  int h = costmap.getSizeInCellsY();
  int size = (w * h);
  int pts = 0;

  map_.info.width = w;
  map_.info.height = h;
  map_.set_data_size(size);
  map_.info.resolution = costmap.getResolution();
  map_.info.origin.position.x = costmap.getOriginX();
  map_.info.origin.position.y = costmap.getOriginY();


  // Find all frontiers (open cells next to unknown cells).
  const unsigned char* map = costmap.getCharMap();
  for (idx = 0; idx < size; idx++) {
    //get the world point for the index
    unsigned int mx, my;
    costmap.indexToCells(idx, mx, my);
    geometry_msgs::Point p;
    costmap.mapToWorld(mx, my, p.x, p.y);

    //check if the point has valid potential and is next to unknown space
    //bool valid_point = planner_->validPointPotential(p);
    //bool valid_point = planner_->computePotential(p) && planner_->validPointPotential(p);
    //bool valid_point = (map[idx] < LETHAL_OBSTACLE);
    //bool valid_point = (map[idx] < INSCRIBED_INFLATED_OBSTACLE );
    bool valid_point = (map[idx] == FREE_SPACE);

    if ((valid_point && map) &&
        (((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) ||
         ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) ||
         ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) ||
         ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION))))
    {
      map_.data[idx] = -128;
      //ROS_INFO("Candidate Point %d.", pts++ );
    } else {
      map_.data[idx] = -127;
    }
  }

  // Clean up frontiers detected on separate rows of the map
  //   I don't know what this means -- Travis.
  idx = map_.info.height - 1;
  for (unsigned int y=0; y < map_.info.width; y++) {
    map_.data[idx] = -127;
    idx += map_.info.height;
  }

  // Group adjoining map_ pixels
  int segment_id = 127;
  std::vector< std::vector<FrontierPoint> > segments;
  for (int i = 0; i < size; i++) {
    if (map_.data[i] == -128) {
      std::vector<int> neighbors, candidates;
      std::vector<FrontierPoint> segment;
      neighbors.push_back(i);

      int points_in_seg = 0;
      unsigned int mx, my;
      geometry_msgs::Point p_init, p;

      costmap.indexToCells(i, mx, my);
      costmap.mapToWorld(mx, my, p_init.x, p_init.y);

      // claim all neighbors
      while (neighbors.size() > 0) {
        int idx = neighbors.back();
        neighbors.pop_back();

        btVector3 tot(0,0,0);
        int c = 0;
        if ((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) {
          tot += btVector3(1,0,0);
          c++;
        }
        if ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) {
          tot += btVector3(-1,0,0);
          c++;
        }
        if ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) {
          tot += btVector3(0,1,0);
          c++;
        }
        if ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION)) {
          tot += btVector3(0,-1,0);
          c++;
        }
        assert(c > 0);

	// Only adjust the map and add idx to segment when its near enough to start point.
	//    This should prevent greedy approach where all cells belong to one frontier!

	costmap.indexToCells(idx, mx, my);
	costmap.mapToWorld(mx, my, p.x, p.y);
	float dist;
	dist = sqrt( pow( p.x - p_init.x, 2 ) + pow( p.y - p_init.y, 2 ));

	if ( dist <= 0.8 ){
	  map_.data[idx] = segment_id;  // This used to be up before "assert" block above.
	  points_in_seg += 1;
	  //ROS_INFO( "Dist to start cell: %3.2f", dist );

	  segment.push_back(FrontierPoint(idx, tot / c));

	  // consider 8 neighborhood
	  if (((idx-1)>0) && (map_.data[idx-1] == -128))
	    neighbors.push_back(idx-1);
	  if (((idx+1)<size) && (map_.data[idx+1] == -128))
	    neighbors.push_back(idx+1);
	  if (((idx-map_.info.width)>0) && (map_.data[idx-map_.info.width] == -128))
	    neighbors.push_back(idx-map_.info.width);
	  if (((idx-map_.info.width+1)>0) && (map_.data[idx-map_.info.width+1] == -128))
	    neighbors.push_back(idx-map_.info.width+1);
	  if (((idx-map_.info.width-1)>0) && (map_.data[idx-map_.info.width-1] == -128))
	    neighbors.push_back(idx-map_.info.width-1);
	  if (((idx+(int)map_.info.width)<size) && (map_.data[idx+map_.info.width] == -128))
	    neighbors.push_back(idx+map_.info.width);
	  if (((idx+(int)map_.info.width+1)<size) && (map_.data[idx+map_.info.width+1] == -128))
	    neighbors.push_back(idx+map_.info.width+1);
	  if (((idx+(int)map_.info.width-1)<size) && (map_.data[idx+map_.info.width-1] == -128))
	    neighbors.push_back(idx+map_.info.width-1);
	}
      }

      segments.push_back(segment);
      ROS_INFO( "Segment %d has %d costmap cells", segment_id, points_in_seg );
      segment_id--;
      if (segment_id < -127)
        break;
    }
  }

  int num_segments = 127 - segment_id;
  ROS_INFO("Segments: %d", num_segments );
  if (num_segments <= 0)
    return;

  for (unsigned int i=0; i < segments.size(); i++) {
    Frontier frontier;
    std::vector<FrontierPoint>& segment = segments[i];
    uint size = segment.size();

    //we want to make sure that the frontier is big enough for the robot to fit through
    //  This seems like a goofy heuristic rather than fact.  What happens if we don't do this...?
    if (size * costmap.getResolution() < costmap.getInscribedRadius()){
      ROS_INFO( "Discarding segment... too small?" );
      //continue;
    }

    float x = 0, y = 0;
    btVector3 d(0,0,0);

    for (uint j=0; j<size; j++) {
      d += segment[j].d;
      int idx = segment[j].idx;
      x += (idx % map_.info.width);
      y += (idx / map_.info.width);
      // x = (idx % map_.info.width);
      // y = (idx / map_.info.width);
    }
    d = d / size;
    frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / size);
    frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / size);
    // frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x);
    // frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y);
    frontier.pose.position.z = 0.0;

    frontier.pose.orientation = tf::createQuaternionMsgFromYaw(btAtan2(d.y(), d.x()));
    frontier.size = size;

    geometry_msgs::Point p;
    p.x = map_.info.origin.position.x + map_.info.resolution * (x);  // frontier.pose.position
    p.y = map_.info.origin.position.y + map_.info.resolution * (y);
    if (!planner_->validPointPotential(p)){
      ROS_INFO( "Discarding segment... can't reach?" );
      //continue;
    }

    frontiers_.push_back(frontier);
  }

}