一、概述

下圖為 map_server 整個(gè)包的內(nèi)容逃默，主要分為三塊內(nèi)容：map_server（加載地圖）、image_loader 和 map_saver（保存地圖）。

其功能就是地圖服務(wù)器咧欣，原始代碼圖像數(shù)據(jù)經(jīng)由 SDL_Image 讀取，map_server 節(jié)點(diǎn)主要就是為了加載 pgm 格式的地圖轨帜，用 yaml 文件描述的魄咕，同時(shí)發(fā)布 map_metadata 和 map 話題，以及 static_map 服務(wù) 蚌父，其目的都是為了方便其他節(jié)點(diǎn)獲取到地圖數(shù)據(jù)哮兰。

二、文件結(jié)構(gòu)分析

查看包下的 CMakeLists.txt 文件苟弛，查看當(dāng)前包添加了幾個(gè)可執(zhí)行文件 add_executable喝滞。當(dāng)前包有兩個(gè)可執(zhí)行文件，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}
)

三右遭、map_server

首先從程序運(yùn)行、函數(shù)調(diào)用順序的角度出發(fā)對(duì) map_server節(jié)點(diǎn)進(jìn)行學(xué)習(xí)。節(jié)點(diǎn)的起源在 main 函數(shù)窘哈，如下言蛇。

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

3.1 命令行運(yùn)行參數(shù)賦值解析

rosrun map_server map_server map.yaml

主要的變量有

• fname即 argv[1]，命令行執(zhí)行程序名后的第一個(gè)參數(shù) map.yaml宵距，后邊會(huì)通過(guò)其加載地圖腊尚；

• res 通過(guò) argc 命令行參數(shù)個(gè)數(shù)判斷，上述命令行指令為2個(gè)參數(shù)满哪，及 res 為零婿斥。如果參數(shù)個(gè)數(shù)不等于2，后邊程序會(huì)選擇使用棄用的 map_server 接口 map_server （不關(guān)注）哨鸭。

3.2 構(gòu)造函數(shù)

創(chuàng)建 MapServer 類對(duì)象 ms民宿，MapServer ms(fname, res);，程序運(yùn)行 MapServer 構(gòu)造函數(shù) 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

通過(guò)命令行參數(shù)個(gè)數(shù)賦值變量 res活鹰，判斷需要加載的地圖格式是現(xiàn)用的還是現(xiàn)在棄用的。如果使用現(xiàn)用格式只估，則變量 deprecated為 false志群，通過(guò)第三方庫(kù)YAML 加載 map.yaml 文件，并解析賦值給相應(yīng)變量 res蛔钙、negate锌云、occ_th 等等，針對(duì)這部分變量含義在文末會(huì)詳細(xì)解析吁脱。

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

通過(guò)loadMapFromFile()函數(shù)根據(jù)參數(shù)讀入地圖文件

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

函數(shù)loadMapFromFile()在 image_loader.cpp 內(nèi)定義的 map_server 命名空間內(nèi)桑涎。\

函數(shù)首先使用 SDL 庫(kù) IMG_Load 函數(shù)加載 fname (map.png) 圖片信息 img，然后使用 setEulerZYX 將圖片的航向信息轉(zhuǎn)換為姿態(tài)四元數(shù)兼贡。

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

加載地圖分三種模式：TRINARY攻冷，SCALE，RAW遍希。

• TRINARY模式：占據(jù)（Occupied）像素用100表示等曼，自由（Free）像素用0表示，不明（Unknow）像素用-1表示孵班；
• SCALE模式：占據(jù)（Occupied）像素用100表示涉兽，自由（Free）像素用0表示，不明（Unknow）像素用(0, 100)篙程；
• RAW模式：所有像素用 [0, 255] 表示枷畏，直接輸出像素值為柵格值。

接著通過(guò)圖片信息計(jì)算 rgb 平均值 color_avg虱饿，根據(jù)計(jì)算公式 occ = (255 - color_avg) / 255.0拥诡；計(jì)算每個(gè)像素的占用概率 occ触趴。

在TRINARY 模式下，如果 occ 大于占用概率閾值 occ_th渴肉，則當(dāng)前像素被占用（用100表示）冗懦，小于 free_th（用0表示）、不確定（用-1表示）仇祭。

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

最后將地圖信息發(fā)布出去披蕉，共三個(gè)topic。

• /map_metadata: 發(fā)布地圖的描述信息
• /map: 發(fā)布鎖存的地圖消息
• static_map: 用于請(qǐng)求和響應(yīng)當(dāng)前的靜態(tài)地圖乌奇。

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

四没讲、map_saver

rosrun map_server map_saver -f map

只考慮常用的-f參數(shù)，保存地圖礁苗。 此節(jié)點(diǎn)主要函數(shù)是類 MapGenerator 的構(gòu)造函數(shù)爬凑，訂閱來(lái)自建圖發(fā)布的 /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);
        }
    }
}

map.yaml寫入函數(shù)

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

map.yaml中參數(shù)含義：

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

• image: map.png 包含占用數(shù)據(jù)的圖像文件的路徑嘁信； 可以是絕對(duì)的，或相對(duì)于 YAML 文件的位置
• resolution: 0.04 地圖的分辨率疏叨，米/像素
• origin: [-20.499653, -9.56729, 0.0] (x, y, yaw)（地圖左下角在地圖上的位置）初始點(diǎn)位置 在地圖上的坐標(biāo)以及航偏潘靖。yaw逆時(shí)針旋轉(zhuǎn)。航偏是指角度 rad考廉。
• occupied_thresh: 0.65 當(dāng)像素占據(jù)的概率大于 0.65 時(shí)候認(rèn)為是完全占據(jù)的秘豹。
• free_thresh: 0.196 當(dāng)像素占據(jù)概率小于0.196的時(shí)候，認(rèn)為完全是空的
• negate: 0 0代表 白色為空閑 黑色為占據(jù)昌粤，白/黑自由/占用語(yǔ)義是否應(yīng)該被反轉(zhuǎn)（閾值的解釋不受影響）

圖像像素被占據(jù)的概率是這么計(jì)算的：occ = (255 - color_avg) / 255.0，其中color_avg 是用8位數(shù)表示的來(lái)自于所有通道的平均值啄刹；

例如像素顏色是 0xeeeeee涮坐，則占用概率是0.07，這意味著幾乎沒有被占用誓军。

將 0xeeeeee 十六進(jìn)制表示的顏色轉(zhuǎn)為 rgb 為 238/238/238袱讹，occ = (255-238)/255 = 0.07

轉(zhuǎn)載：https://blog.csdn.net/Csdn_Darry/article/details/111773701