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 insertMapValue(string key, void* value, map m){ if(m->count >= m->size * m->load) resizeMap(m, 2); size_t index = hashKey(key) % m->size; var v = newVar(key, value); linkedNode node = newLinkedNode(v); linkedList list = m->values[index]; if(!list) list = m->values[index] = newLinkedList(NULL); appendLinkedNode(node, list); m->count++; }
void CostmapLayer::matchSize() { Costmap2D* master = layered_costmap_->getCostmap(); resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), master->getOriginX(), master->getOriginY()); }