Пример #1
0
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);
}
Пример #3
0
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);

  
}