一直在用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);
#elseYAML::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 robustif (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 structureresp->map.info.width = img->w;                    //图像的宽度resp->map.info.height = img->h;                   //图像的高度resp->map.info.resolution = res;                  //图像的分辨率resp->map.info.origin.position.x = *(origin);     //地图原点xresp->map.info.origin.position.y = *(origin + 1); //地图坐标原点xresp->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 dataresp->map.data.resize(resp->map.info.width * resp->map.info.height);// Get values that we'll need to iterate through the pixelsrowstride = 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;elseavg_channels = n_channels - 1;// Copy pixel data into the map structurepixels = (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;elsealpha = *(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值为255occ = (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);
}

ROS-Navigation之map_server笔记及程序解析相关推荐

ROS Navigation之map_server完全详解
0. 写在最前面建议收藏: 本文持续更新地址:https://haoqchen.site/2018/11/27/map_server/ 本文将介绍自己在看ROS的Navigation stack中的 ...
基于STM32F103的树莓派ROS小车——PS2遥控程序解析
基于STM32F103ZET6的PS2遥控ROS小车程序解析序言 1. PS手柄介绍 2. 使用说明 2.1 引脚说明 2.2 时序图分析 3. 手柄测试 4. 程序解析 5. 仿真 6. 源码链接 ...
请利用SAX编写程序解析Yahoo的XML格式的天气预报，获取天气预报——python学习笔记
1. 题目: 请利用SAX编写程序解析Yahoo的XML格式的天气预报,获取天气预报: 题目是廖雪峰老师的python教程中XML的练习. 本篇博文只是针对这一题目,没有做详细的介绍,如果看不懂可以在 ...
Springboot框架学习笔记------项目搭建、程序解析、配置文件的作用
今天开始学习spring boot,这个框架整合spring和springmvc,简化了很多的配置,这是目前的理解. 搭建流程: 1.从idea搭建,利用maven创建项目,前提是联网,需要下载mav ...
理解ROS Navigation Stack，看完这篇你就知道啦！
前言 ROS Navigation Stack是ROS提供的一个二维的导航功能包集合,通过输入里程计.传感器信息和目标位姿,输出控制机器人到达目标状态的安全速度指令.ROS Navigation St ...
古月居ROS入门21讲笔记
ROS入门21讲笔记--古月居 1 C++&Python极简基础 1.1 安装编译/解析器 1.2 for循环 1.3 while循环 1.4 面向对象 2. ROS基础 2.1 ROS概念 ...
ROS Navigation Tuning Guide（导航调试指南）
ROS Navigation Tuning Guide 导航调试指南准备工作距离传感器里程计定位速度与加速度的设置获得最大速度获得最大加速度设置最小值 XY方向的速度 Global P ...
CUDA学习笔记之程序优化
CUDA学习笔记之程序优化标签: cuda优化conflict存储算法数学计算 2010-01-05 17:18 5035人阅读评论(4) 收藏举报分类: CUDA(6) 版权声明:本文为博主 ...
Couldn't find executable named map_saver below /opt/ros/indigo/share/map_server
在使用<Mastering ROS for Robotics Programming(PACKT,2015)>学习ros,学习到第四章的时候,先是安装Navigation一直找不到depe ...
逆波兰式 java_Java 实现《编译原理》中间代码生成 -逆波兰式生成与计算 - 程序解析...
Java 实现<编译原理>中间代码生成 -逆波兰式生成与计算 - 程序解析编译原理学习笔记 (一)逆波兰式是什么? 逆波兰式(Reverse Polish notation,RPN,或逆 ...

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