ROS之map_server 源码解析


  CoppeliaSim 仿真的翻译教程已经完成,接下来计划一边更新 CoppeliaSim 实战教程,一边进行轮式机器人基础软件包的源码学习整理。按照个人习惯,先对简单的软件包 map_server 下手。源码与流程框图相互结合更容易理解,也能帮助我后期快速回顾。于是调研了一番应用在代码结构整理层面上的流程框图软件,很多人推荐了Understand,号称是高效代码静态分析的神器,一方面可以阅读代码,另一方面Butterfly还能导出关系图。map_server 包比较小,后边Gmapping 等稍微复杂的包再用 Understand 画着试试,先用 xmind 绘制了自己理解的整个过程,加入了一些个人的思想,不喜请轻喷,不足之处还望不吝赐教!如果你们有什么好的、方便的、实用的画图工具,可以推荐下,非常感谢。接下来上神图😄 😄:

在这里插入图片描述

图1 map_server流程框图

  上图为 map_server 整个包的内容,主要分为三块内容:map_server(加载地图)、image_loader 和 map_saver(保存地图)。其功能就是地图服务器,原始代码图像数据经由 SDL_Image 读取,map_server 节点主要就是为了加载 pgm 格式的地图,用 yaml 文件描述的,同时发布 map_metadata 和 map 话题,以及 static_map 服务 ,其目的都是为了方便其他节点获取到地图数据。

  接下来开始克隆软件包代码(Kinetic版 2020.12.3 提交的 9c5fb8bc042bebfabe466158a18e593ac8052ad1)进行解析,第一步先查看包下的 CMakeLists.txt 文件,查看当前包添加了几个可执行文件 add_executable。当前包有两个可执行文件,map_server(涉及到源文件 main.cpp)和 map_saver(涉及到源文件map_saver.cpp)。

add_executable(map_server src/main.cpp)
add_dependencies(map_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(map_server
    map_server_image_loader
    ${YAMLCPP_LIBRARIES}
    ${catkin_LIBRARIES}
)

add_executable(map_server-map_saver src/map_saver.cpp)
add_dependencies(map_server-map_saver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
set_target_properties(map_server-map_saver PROPERTIES OUTPUT_NAME map_saver)
target_link_libraries(map_server-map_saver
    ${catkin_LIBRARIES}
)

1.1 map_server

  首先从程序运行、函数调用顺序的角度出发对 map_server节点进行学习。节点的起源在 main 函数,如下。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
    // Part 1 -------------------------------------
    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]);
    double res = (argc == 2) ? 0.0 : atof(argv[2]);
    // Part 2 -------------------------------------
    try
    {
        MapServer ms(fname, res);
        ros::spin();
    }
    catch(std::runtime_error& e)
    {
        ROS_ERROR("map_server exception: %s", e.what());
        return -1;
    }
    return 0;
}

Part_1

rosrun map_server map_server map.yaml

  这部分是程序对命令行参数的解析赋值。主要的变量有

  • fname即 argv[1],命令行执行程序名后的第一个参数 map.yaml,后边会通过其加载地图;
  • res 通过 argc 命令行参数个数判断,上述命令行指令为2个参数,及 res 为零。如果参数个数不等于2,后边程序会选择使用弃用的 map_server 接口 map_server (不关注)。

Part_2

  创建 MapServer 类对象 ms,MapServer ms(fname, res);,程序运行 MapServer 构造函数 MapServer(const std::string& fname, double res)

MapServer(const std::string& fname, double res)
{
    std::string mapfname = "";
    double origin[3];
    int negate;
    double occ_th, free_th;
    MapMode mode = TRINARY;
    std::string frame_id;
    ros::NodeHandle private_nh("~");
    private_nh.param("frame_id", frame_id, std::string("map"));
    deprecated = (res != 0);  // false
    // Part 3 --------------------------------------------
    if (!deprecated) {
        //mapfname = fname + ".pgm";
        //std::ifstream fin((fname + ".yaml").c_str());
        std::ifstream fin(fname.c_str());
        if (fin.fail()) {
            ROS_ERROR("Map_server could not open %s.", fname.c_str());
            exit(-1);
        }
#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.");
            exit(-1);
        }
        try {
            doc["negate"] >> negate;
        } catch (YAML::InvalidScalar) {
            ROS_ERROR("The map does not contain a negate tag or it is invalid.");
            exit(-1);
        }
        try {
            doc["occupied_thresh"] >> occ_th;
        } catch (YAML::InvalidScalar) {
            ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
            exit(-1);
        }
        try {
            doc["free_thresh"] >> free_th;
        } catch (YAML::InvalidScalar) {
            ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
            exit(-1);
        }
        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());
                exit(-1);
            }
        } 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.");
            exit(-1);
        }
        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.");
                exit(-1);
            }
            if(mapfname[0] != '/')
            {
                // dirname can modify what you pass it
                char* fname_copy = strdup(fname.c_str());
                mapfname = std::string(dirname(fname_copy)) + '/' + mapfname;
                free(fname_copy);
            }
        } catch (YAML::InvalidScalar) {
            ROS_ERROR("The map does not contain an image tag or it is invalid.");
            exit(-1);
        }
    } else {
        private_nh.param("negate", negate, 0);
        private_nh.param("occupied_thresh", occ_th, 0.65);
        private_nh.param("free_thresh", free_th, 0.196);
        mapfname = fname;
        origin[0] = origin[1] = origin[2] = 0.0;
    }
	
    // Part 4 --------------------------------------------
    ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
    try
    {
        map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode);
    }
    catch (std::runtime_error e)
    {
        ROS_ERROR("%s", e.what());
        exit(-1);
    }
    // Part 5 --------------------------------------------
    // To make sure get a consistent time in simulation
    ros::Time::waitForValid();
    map_resp_.map.info.map_load_time = ros::Time::now();
    map_resp_.map.header.frame_id = frame_id;
    map_resp_.map.header.stamp = ros::Time::now();
    ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
             map_resp_.map.info.width,
             map_resp_.map.info.height,
             map_resp_.map.info.resolution);
    meta_data_message_ = map_resp_.map.info;

    service = n.advertiseService("static_map", &MapServer::mapCallback, this);
    //pub = n.advertise<nav_msgs::MapMetaData>("map_metadata", 1,

    // Latched publisher for metadata
    metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
    metadata_pub.publish( meta_data_message_ );

    // Latched publisher for data
    map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
    map_pub.publish( map_resp_.map );
}

Part_3

  通过命令行参数个数赋值变量 res,判断需要加载的地图格式是现用的还是现在弃用的。如果使用现用格式,则变量 deprecated为 false,通过第三方库YAML 加载 map.yaml 文件,并解析赋值给相应变量 res、negate、occ_th 等等,针对这部分变量含义在文末会详细解析。

Part_4 loadMapFromFile

  函数在 image_loader.cpp 内定义的 map_server 命名空间内。

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);
    resp->map.info.origin.position.y = *(origin+1);
    resp->map.info.origin.position.z = 0.0;
    btQuaternion q;
    // setEulerZYX(yaw, pitch, roll)
    q.setEulerZYX(*(origin+2), 0, 0);
    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.
    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
            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;

            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 free.  Otherwise, it's vice versa.
            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;
            else {
                double ratio = (occ - free_th) / (occ_th - free_th);
                value = 99 * ratio;
            }
            resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
        }
    }
    SDL_FreeSurface(img);
}

  函数首先使用 SDL 库 IMG_Load 函数加载 fname (map.png) 图片信息 img,然后使用 setEulerZYX 将图片的航向信息转换为姿态四元数。

q.setEulerZYX(*(origin+2), 0, 0);
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();

  最后通过图片信息计算 rgb 平均值 color_avg,根据计算公式 occ = (255 - color_avg) / 255.0;计算每个像素的占用概率 occ,TRINARY 模式如果 occ 大于占用概率阈值 occ_th,则当前像素被占用(用100表示),小于 free_th(用0表示)、不确定(用-1表示)。

Part_5 Publish topic and call service

  • /map_metadata: 发布地图的描述信息
  • /map: 发布锁存的地图消息
  • static_map: 用于请求和响应当前的静态地图。

1.2 map_saver

rosrun map_server map_saver -f map

  只考虑常用的-f参数,保存地图。 此节点主要函数是类 MapGenerator 的构造函数,订阅来自建图发布的 /map topic,从而写地图 map.png/map.pgm 和 map.yaml 文件。

MapGenerator(const std::string& mapname, int threshold_occupied = 100, int threshold_free = 0)
	: 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);
}

---
    
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_) { //occ [0,0.1)
            fputc(254, out);
        } else if (map->data[i] <= 100 && map->data[i] >= threshold_occupied_) { //occ (0.65,1]
            fputc(000, out);
        } else { //occ [0.1,0.65]
            fputc(205, out);
        }
    }
}
  • write map.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);

// map.yaml
image: map.pgm
resolution: 0.050000
origin: [-5.781524, -6.753651, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
  • orientation_q --> euler
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);
  • euler–> orientation_q
// ros convert yaw angle to quaternion
btQuaternion q;

// setEulerZYX(yaw, pitch, roll)
q.setEulerZYX(*(origin+2), 0, 0);
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();

// tf Quaternion.h
void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
{
    setRPY(roll, pitch, yaw); 
}

void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)
{
    tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);  
    tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);  
    tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);  
    tfScalar cosYaw = tfCos(halfYaw);
    tfScalar sinYaw = tfSin(halfYaw);
    tfScalar cosPitch = tfCos(halfPitch);
    tfScalar sinPitch = tfSin(halfPitch);
    tfScalar cosRoll = tfCos(halfRoll);
    tfScalar sinRoll = tfSin(halfRoll);

    setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
             cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
             cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
             cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
}

1.3 附

  • parsing map.yaml
free_thresh: 0.196
image: map.png
negate: 0
occupied_thresh: 0.65
origin: [-20.499653, -9.56729, 0.0]
resolution: 0.04

image: map.png 包含占用数据的图像文件的路径; 可以是绝对的,或相对于 YAML 文件的位置
resolution: 0.04 地图的分辨率,米/像素
origin: [-20.499653, -9.56729, 0.0] (x, y, yaw)(地图左下角在地图上的位置)初始点位置 在地图上的坐标以及航偏。yaw逆时针旋转。航偏是指角度 rad
occupied_thresh: 0.65 当像素占据的概率大于 0.65 时候认为是完全占据的。
free_thresh: 0.196 当像素占据概率小于0.196的时候,认为完全是空的
negate: 0 0代表 白色为空闲 黑色为占据,白/黑自由/占用语义是否应该被反转(阈值的解释不受影响)

图像像素被占据的概率是这么计算的:occ = (255 - color_avg) / 255.0
color_avg 是用8位数表示的来自于所有通道的平均值;
如果像素颜色是 0xeeeeee,则占用概率是0.07,这意味着几乎没有被占用。(将 0xeeeeee 十六进制表示的颜色转为 rgb 为 238/238/238,occ = (255-238)/255 = 0.07, 转换链接
加载地图分三种模式:TRINARY,SCALE,RAW。Trinary,即占据(像素用100表示),free(像素用0表示)、不明(像素用-1表示);Scale,占据和free与三值一样,但是不明变成了(0, 100);Raw,所有像素用 [0, 255] 表示,直接输出像素值为栅格值。

  需要注意的是,如果你是想保存 Cartographer 生成的地图,是需要对源代码做一些修改的。因为 Cartographer 的 map 格式跟 GMapping 中的格式不太一样。具体不一样如下:(此部分摘自 https://blog.csdn.net/u013834525/article/details/84622496)此部分有网友说是一致的,暂时还未测试

point typeGmapping valueCartographer value
free00-N
occupied100M-100
unknown-1-1 & N-M

Gmapping

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);
        }
    }
}

Cartographer

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] < 40) { //occ [0,0.1)
			fputc(254, out);
		} else if (map->data[i] > +50) { //occ (0.65,1]
			fputc(000, out);
		} else { //occ [0.1,0.65]
			fputc(205, out);
		}
	}
}
  • map_server.launch
<launch>
    <arg name="node_name"         default="map_server"/>
    <arg name="map_file"          default="$(find map_server)/map.yaml"/>

    <node name="$(arg node_name)" pkg="map_server" type="map_server" args="$(arg map_file)" output="screen"/>
</launch>

参考:

  • 5
    点赞
  • 37
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值