CoppeliaSim 仿真的翻译教程已经完成，接下来计划一边更新 CoppeliaSim 实战教程，一边进行轮式机器人基础软件包的源码学习整理。按照个人习惯，先对简单的软件包 map_server 下手。源码与流程框图相互结合更容易理解，也能帮助我后期快速回顾。于是调研了一番应用在代码结构整理层面上的流程框图软件，很多人推荐了Understand，号称是高效代码静态分析的神器，一方面可以阅读代码，另一方面Butterfly还能导出关系图。map_server 包比较小，后边Gmapping 等稍微复杂的包再用 Understand 画着试试，先用 xmind 绘制了自己理解的整个过程，加入了一些个人的思想，不喜请轻喷，不足之处还望不吝赐教！如果你们有什么好的、方便的、实用的画图工具，可以推荐下，非常感谢。接下来上神图😄 😄：
上图为 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 type | Gmapping value | Cartographer value |
|---|---|---|
| free | 0 | 0-N |
| occupied | 100 | M-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);
}
}
}
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>
参考：
- ROS Navigation之map_server完全详解 - 白夜行的狼的博客
- http://wiki.ros.org/map_server
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
