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()); }
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); } }