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

向阳花开_

已于 2022-05-03 12:39:17 修改

阅读量7k

点赞数 8

于 2022-05-03 12:37:34 首次发布

本文链接：https://blog.csdn.net/Draonly/article/details/124553198

版权

ROS 同时被 3 个专栏收录

24 篇文章 4 订阅

订阅专栏

ROS_Navigation

11 篇文章 12 订阅

订阅专栏

Navigation

6 篇文章 4 订阅

订阅专栏

一直在用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.c_str());
    FILE *yaml = fopen(mapmetadatafile.c_str(), "w");

    /*yaml文件样例：
resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

     */

    geometry_msgs::Quaternion orientation = map->info.origin.orientation;
    tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y,
                                       orientation.z, orientation.w));
    double yaw, pitch, roll;
    mat.getEulerYPR(yaw, pitch, roll);
    //使用fprintf输出配置信息到yaml文件
    fprintf(yaml,
            "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: "
            "0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
            mapdatafile.c_str(), map->info.resolution,
            map->info.origin.position.x, map->info.origin.position.y, yaw);

    fclose(yaml);

    ROS_INFO("Done\n");
    saved_map_ = true;
  }

  std::string mapname_;
  ros::Subscriber map_sub_;
  bool saved_map_;
  int threshold_occupied_;
  int threshold_free_;
};

5.map_server功能及程序说明

5.1 功能说明

读取地图信息，并作为ROS service 为其余节点提供地图数据。
USAGE:

#define USAGE                                                                  \
  "\nUSAGE: map_server <map.yaml>\n"                                           \
  "  map.yaml: map description file\n"                                         \
  "DEPRECATED USAGE: map_server <map> <resolution>\n"                          \
  "  map: image file to load\n"                                                \
  "  resolution: map resolution [meters/pixel]"

一种是直接指定加载map.yaml文件

rosrun map_server map_server mymap.yaml

另一种是指定加载map.pgm和指定分辨率(官方说明：不建议使用)

5.2 主要程序说明

主要依赖的库包括：sdl-image（用来加载地图图片）、yaml-cpp（配置中用到挺多yaml文件的）和tf。
主要流程

在main函数中定义了一个MapServer对象。在该对象的构造函数中，通过描述文件和ROS参数服务器获得地图相应参数后，调用[image_loader动态库][image_loader动态库]中的loadMapFromFile函数将地图加载到私有成员nav_msgs::GetMap::Response map_resp_中。
然后提供一个名为static_map的服务，回调函数是MapServer::mapCallback
接着发布一个内容为nav_msgs::MapMetaData，名为map_metadata的话题

main函数

int main(int argc, char **argv) {
  ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
  // step 1 : 检查rosrun map_server的命令是否正确
  ros::NodeHandle nh("~");
  if (argc != 3 && argc != 2) {
    ROS_ERROR("%s", USAGE);
    exit(-1);
  }
  if (argc != 2) {
    ROS_WARN("Using deprecated map server interface. Please switch to new "
             "interface.");
  }
  std::string fname(argv[1]);
  //把参数str所指向的字符串转换为一个浮点数
  double res = (argc == 2) ? 0.0 : atof(argv[2]);

  // step 2: 创建MapServer类对象ms，运行MapServer构造函数
  try {
    MapServer ms(fname, res);
    ros::spin();
  } catch (std::runtime_error &e) {
    ROS_ERROR("map_server exception: %s", e.what());
    return -1;
  }

  return 0;
}

MapServer类

  MapServer(const std::string &fname, double res) {
    std::string mapfname = "";
    double origin[3];
    int negate;
    double occ_th, free_th;
    MapMode mode = TRINARY;
    ros::NodeHandle private_nh("~");
    private_nh.param("frame_id", frame_id_, std::string("map"));

    // 请求后返回当前地图的复制
    get_map_service_ =
        nh_.advertiseService("static_map", &MapServer::mapCallback, this);

    // 改变当前发布的地图
    change_map_srv_ =
        nh_.advertiseService("change_map", &MapServer::changeMapCallback, this);

    // 发布地图的描述信息
    metadata_pub_ =
        nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);

    // 发布锁存的地图消息
    map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);

loadMapFromYaml()函数

主要是从yaml文件获取读取参数，在读取yiyoushuom时，检查参数是否存在

//从给定路径下的yaml文件获取读取参数
  bool loadMapFromYaml(std::string path_to_yaml) {
    std::string mapfname;
    MapMode mode;
    double res;
    int negate;
    double occ_th;
    double free_th;
    double origin[3];
    std::ifstream fin(path_to_yaml.c_str());
    if (fin.fail()) {
      ROS_ERROR("Map_server could not open %s.", path_to_yaml.c_str());
      return false;
    }
#ifdef HAVE_YAMLCPP_GT_0_5_0
    // The document loading process changed in yaml-cpp 0.5.
    YAML::Node doc = YAML::Load(fin);
#else
    YAML::Parser parser(fin);
    YAML::Node doc;
    parser.GetNextDocument(doc);
#endif
    // 检查各个参数是否包含
    try {
      doc["resolution"] >> res;
    } catch (YAML::InvalidScalar &) {
      ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
      return false;
    }
    try {
      doc["negate"] >> negate;
    } catch (YAML::InvalidScalar &) {
      ROS_ERROR("The map does not contain a negate tag or it is invalid.");
      return false;
    }
    try {
      doc["occupied_thresh"] >> occ_th;
    } catch (YAML::InvalidScalar &) {
      ROS_ERROR(
          "The map does not contain an occupied_thresh tag or it is invalid.");
      return false;
    }
    try {
      doc["free_thresh"] >> free_th;
    } catch (YAML::InvalidScalar &) {
      ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
      return false;
    }
    try {
      std::string modeS = "";
      doc["mode"] >> modeS;

      if (modeS == "trinary")
        mode = TRINARY;
      else if (modeS == "scale")
        mode = SCALE;
      else if (modeS == "raw")
        mode = RAW;
      else {
        ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
        return false;
      }
    } catch (YAML::Exception &) {
      ROS_DEBUG("The map does not contain a mode tag or it is invalid... "
                "assuming Trinary");
      mode = TRINARY;
    }
    try {
      doc["origin"][0] >> origin[0];
      doc["origin"][1] >> origin[1];
      doc["origin"][2] >> origin[2];
    } catch (YAML::InvalidScalar &) {
      ROS_ERROR("The map does not contain an origin tag or it is invalid.");
      return false;
    }
    try {
      doc["image"] >> mapfname;
      // TODO: make this path-handling more robust
      if (mapfname.size() == 0) {
        ROS_ERROR("The image tag cannot be an empty string.");
        return false;
      }
      // is_absolute获取文件的绝对路径，如果yaml文件中，设置的不是绝对路径,
      //则进行路径拼接
      boost::filesystem::path mapfpath(mapfname);
      if (!mapfpath.is_absolute()) {
        // yaml文件的路径
        boost::filesystem::path dir(path_to_yaml);
        //获取父目录
        dir = dir.parent_path();
        mapfpath = dir / mapfpath;
        mapfname = mapfpath.string();
      }
    } catch (YAML::InvalidScalar &) {
      ROS_ERROR("The map does not contain an image tag or it is invalid.");
      return false;
    }
    //将地图信息发布出去
    return loadMapFromValues(mapfname, res, negate, occ_th, free_th, origin,
                             mode);
  }

loadMapFromFile()函数说明

该函数实际是封装了一下SDL_image的功能写了一个loadMapFromFile函数
处理后，将图像封装成nav_msgs::GetMap::Response*结构
该结构的表示形式,上面已有说明，{TRINARY, SCALE, RAW｝
默认的TRINARY：占据(像素100表示),free(像素0表示),未知(像素-1)
Raw:所有像素用[0, 255]表示
SCALE:未知用[0,100]表示。

void loadMapFromFile(nav_msgs::GetMap::Response *resp, const char *fname,
                     double res, bool negate, double occ_th, double free_th,
                     double *origin, MapMode mode) {
  SDL_Surface *img;

  unsigned char *pixels;
  unsigned char *p;
  unsigned char value;
  int rowstride, n_channels, avg_channels;
  unsigned int i, j;
  int k;
  double occ;
  int alpha;
  int color_sum;
  double color_avg;

  // Load the image using SDL.  If we get NULL back, the image load failed.
  if (!(img = IMG_Load(fname))) {
    std::string errmsg = std::string("failed to open image file \"") +
                         std::string(fname) + std::string("\": ") +
                         IMG_GetError();
    throw std::runtime_error(errmsg);
  }

  // Copy the image data into the map structure
  resp->map.info.width = img->w;                    //图像的宽度
  resp->map.info.height = img->h;                   //图像的高度
  resp->map.info.resolution = res;                  //图像的分辨率
  resp->map.info.origin.position.x = *(origin);     //地图原点x
  resp->map.info.origin.position.y = *(origin + 1); //地图坐标原点x
  resp->map.info.origin.position.z = 0.0; // z值默认填充0(二维地图)
  btQuaternion q;
  // setEulerZYX(yaw, pitch, roll)
  q.setEulerZYX(*(origin + 2), 0, 0); //地图原点角度r,转换成四元素
  resp->map.info.origin.orientation.x = q.x();
  resp->map.info.origin.orientation.y = q.y();
  resp->map.info.origin.orientation.z = q.z();
  resp->map.info.origin.orientation.w = q.w();

  // Allocate space to hold the data
  resp->map.data.resize(resp->map.info.width * resp->map.info.height);

  // Get values that we'll need to iterate through the pixels
  rowstride = img->pitch;
  n_channels = img->format->BytesPerPixel;

  // NOTE: Trinary mode still overrides here to preserve existing behavior.
  // Alpha will be averaged in with color channels when using trinary mode.
  //?: 使用trinary模式时，Alpha为color的通道值
  if (mode == TRINARY || !img->format->Amask)
    avg_channels = n_channels;
  else
    avg_channels = n_channels - 1;

  // Copy pixel data into the map structure
  pixels = (unsigned char *)(img->pixels);
  for (j = 0; j < resp->map.info.height; j++) {
    for (i = 0; i < resp->map.info.width; i++) {
      // Compute mean of RGB for this pixel
      //计算该像素的RGB均值
      p = pixels + j * rowstride + i * n_channels;
      color_sum = 0;
      for (k = 0; k < avg_channels; k++)
        color_sum += *(p + (k));
      color_avg = color_sum / (double)avg_channels;

      if (n_channels == 1)
        alpha = 1;
      else
        alpha = *(p + n_channels - 1);
      //这里判断一下是否需要黑白颠倒，即：白色->障碍物
      if (negate)
        color_avg = 255 - color_avg;
      //如果是按原始模式，就直接设置[0,255]像素原始值
      if (mode == RAW) {
        value = color_avg;
        resp->map.data[MAP_IDX(resp->map.info.width, i,
                               resp->map.info.height - j - 1)] = value;
        continue;
      }

      // If negate is true, we consider blacker pixels free, and whiter
      // pixels occupied.  Otherwise, it's vice versa.
      //计算该像素的占有概率，如当没有障碍物占据时，白色RGB值为255
      occ = (255 - color_avg) / 255.0;

      // Apply thresholds to RGB means to determine occupancy values for
      // map.  Note that we invert the graphics-ordering of the pixels to
      // produce a map with cell (0,0) in the lower-left corner.
      if (occ > occ_th)
        value = +100; //占据
      else if (occ < free_th)
        value = 0; //自由
      else if (mode == TRINARY || alpha < 1.0)
        value = -1; //未知,TRINARY模式
      else {
        double ratio = (occ - free_th) / (occ_th - free_th);
        value = 1 + 98 * ratio; //未知(0,100)　占据和自由阈值之间
      }
      //更新到map中对应像素点数据
      resp->map.data[MAP_IDX(resp->map.info.width, i,
                             resp->map.info.height - j - 1)] = value;
    }
  }

  SDL_FreeSurface(img);
}