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