void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex)
{
nav_msgs::GetMap::Response& map_ (mapPublisher.map_);
//only update map if it changed
// if (lastGetMapUpdateIndex != gridMap.getUpdateIndex())
// {
int sizeX = gridMap.getSizeX();
int sizeY = gridMap.getSizeY();
int size = sizeX * sizeY;
std::vector<int8_t>& data = map_.map.data;
//Std::vector contents are guaranteed to be contiguous, use memset to set all to unknown to save time in loop
memset(&data[0], -1, sizeof(int8_t) * size);
if (mapMutex)
{
mapMutex->lockMap();
}
for(int i=0; i < size; ++i)
{
if(p_show_gradient_){
data[i] = convertToOccupancyGrid(gridMap.getValue(i));
}
else{
if(gridMap.isFree(i))
{
data[i] = 0;
}
else if (gridMap.isOccupied(i))
{
data[i] = 100;
}
}
}
lastGetMapUpdateIndex = gridMap.getUpdateIndex();
if (mapMutex)
{
mapMutex->unlockMap();
}
map_.map.header.stamp = timestamp;
mapPublisher.mapPublisher_.publish(map_.map);
}
bool HectorMappingRos::saveMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, MapLockerInterface* mapMutex){
nav_msgs::GetMap::Response& map_ (mapPublisher.map_);
int sizeX = gridMap.getSizeX();
int sizeY = gridMap.getSizeY();
int totalsize = sizeX * sizeY;
std::vector<int8_t>& data = map_.map.data;
if (mapMutex)
{
mapMutex->lockMap();
}
//First open the file you wish to save
mapSaver.open(p_map_data_output_path_.c_str());
ROS_INFO("Map Data file opened.");
//Float buffer
char tempstr[300];
//first write the grid size and resolution
int mapDetails = sprintf(tempstr,"%f %f %f %f %f ", p_map_resolution_, 0.0, 0.0, float (p_map_size_x_), float (p_map_size_y_));
//int mapDetails = sprintf(tempstr,"%f %i %i %i %i ", p_map_resolution_, 0 , 0, p_map_size_x_, p_map_size_y_);
mapSaver.write(tempstr, mapDetails);
for(int i=0; i < totalsize; i++){
float temp = gridMap.getValue(i);
//write temp into str format names tempstr
int sizeofStr = sprintf(tempstr,"%f ",temp);
mapSaver.write(tempstr, sizeofStr);
}
;
//Now close the file you have opened
mapSaver.close();
ROS_INFO("Map data file closed. A map of size %i was saved.",totalsize);
if (mapMutex)
{
mapMutex->unlockMap();
}
return true;
}
void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap)
{
Eigen::Vector2f mapOrigin (gridMap.getWorldCoords(Eigen::Vector2f::Zero()));
mapOrigin.array() -= gridMap.getCellLength()*0.5f;
map_.map.info.origin.position.x = mapOrigin.x();
map_.map.info.origin.position.y = mapOrigin.y();
map_.map.info.origin.orientation.w = 1.0;
map_.map.info.resolution = gridMap.getCellLength();
map_.map.info.width = gridMap.getSizeX();
map_.map.info.height = gridMap.getSizeY();
map_.map.header.frame_id = p_map_frame_;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
}