ROS-Navigation之map_server笔记及程序解析

一直在用map_server包，得时间整理了下自己的学习笔记。

１.map_server概述

map_server提供了map_server的ROS节点，它提供了作为ROS服务的地图数据。
它还提供了map_saver命令行实用程序，它允许动态生成的映射保存到文件。
其中包括两个节点

• map_server node：读取地图信息，并作为ROS service 为其余节点提供地图数据
• .map_saver node：保存地图数据到地图文件

2.接口

发布topic：

• /map ：代表地图中栅格内容。　
  数据格式：nav_msgs::OccupancyGrid
• /map_metadata: 表示地图描述信息如长、宽、分辨率。　
  数据格式：nav_msgs::MapMetaData
  提供的service
  /static_map
  将地图map_resp_深拷贝给客户端

nav_msgs::MapMetaData的信息格式如下

time map_load_time
float32 resolution
uint32 width
uint32 height
geometry_msgs/Pose origin

nav_msgs::OccupancyGrid的信息格式如下：

std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data

3.地图及其描述文件说明

map.yaml文件说明

image: map.pgm　　#文件名
resolution: 0.050000　　#地图分辨率 单位：米/像素
origin: [-49.0286, -107.401, 0.0]   #图像左下角在地图坐标下的坐标
negate: 0    #是否应该颠倒 白：自由/黑：的语义(阈值的解释不受影响)
occupied_thresh: 0.65   #占用概率大于此阈值的像素被认为已完全占用
free_thresh: 0.196   #用率小于此阈值的像素被认为是完全空闲的

占用概率，如当没有障碍物占据时，白色RGB值为255，
则占用概率occ = (255-255)/255=0, 没有被占据。

mode
这里有一个缺省的mode字段，

有三种配置，代表三种数据结构

enum MapMode {TRINARY, SCALE, RAW};

地图的结构表示形式，默认的TRINARY

• TRINARY:占据(像素100表示),free(像素0表示),未知(像素-1)
• Raw:所有像素用[0, 255]表示
• SCALE:未知用(0,100)表示,value >= occ_th，大于占有阈值表示占有，像素值用100表示 value <=
  free_th，小于占有阈值表示free，像素值用0表示。

4.map_saver功能及程序说明

map_saver可以保存地图数据到地图文件，生成.pgm和.yaml配置文件。

4.1用法说明

运行命令帮助信息，如下

• –occ //设置占有概率阈值
• –free //设置自由空间阈值
• -f //地图保存名称

4.2 map_saver程序说明

主要是定义了一个MapGenerator类。
该类初始化之后订阅map话题。在订阅的回调函数中，将订阅得到的map按照一定格式保存到当地的.pgm文件，同时将地图信息保存到对应的.yaml文件中。
详细如下：

class MapGenerator {
   

public:
  MapGenerator(const std::string &mapname, int threshold_occupied,
               int threshold_free)
      : mapname_(mapname), saved_map_(false),
        threshold_occupied_(threshold_occupied),
        threshold_free_(threshold_free) {
   
    ros::NodeHandle n;
    ROS_INFO("Waiting for the map");
    map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this);
  }

  void mapCallback(const nav_msgs::OccupancyGridConstPtr &map) {
   
    ROS_INFO("Received a %d X %d map @ %.3f m/pix", map->info.width,
             map->info.height, map->info.resolution);
    // Step:1保存地图pgm文件
    std::string mapdatafile = mapname_ + ".pgm";
    ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
    FILE *out = fopen(mapdatafile.c_str(), "w");
    if (!out) {
   
      ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
      return;
    }

    fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
            map->info.resolution, map->info.width, map->info.height);
    for (unsigned int y = 0; y < map->info.height; y++) {
   
      for (unsigned int x = 0; x < map->info.width; x++) {
   
        unsigned int i = x + (map->info.height - y - 1) * map->info.width;
        if (map->data[i] >= 0 && map->data[i] <= threshold_free_) {
    // [0,free)
          fputc(254, out);//小于自由阈值　设置为白色
        } else if (map->data[i] >= threshold_occupied_) {
    // (occ,255]
          fputc(000, out); //大于占有阈值　设置为黑色
        } else {
    // occ [0.25,0.65]
          fputc(205, out); //在占有与自由阈值之间，设置为未知－灰色
        }
      }
    }

    fclose(out);
    // Step:2保存yaml文件
    std::string mapmetadatafile = mapname_ + ".yaml";
    ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.