话题map应该由map_server发布，而不是SLAM算法发布。代价地图会订阅map话题，如果由SLAM发布，订阅的内容恐怕错误，甚至是空消息。
MapServer的代码，我简化了读yaml的部分1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82class MapServer
{
public:
/** Trivial constructor */
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);
if (!deprecated)
{
// 省略读 yaml 文件的代码，其实就是把yaml里的各个参数赋值给变量
// The document loading process changed in yaml-cpp 0.5.
YAML::Node doc = YAML::Load(fin);
YAML::Parser parser(fin);
YAML::Node doc;
parser.GetNextDocument(doc);
std::string modeS = "";
doc["mode"] >> modeS;
if(modeS=="trinary")
mode = TRINARY;
else if(modeS=="scale")
mode = SCALE;
else if(modeS=="raw")
mode = RAW;
else mode = TRINARY;
ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
// 这个核心函数是最底层的，使用SDL库解析图片
map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode);
// 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);
metadata_pub= n.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
metadata_pub.publish( meta_data_message_ );
map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
map_pub.publish( map_resp_.map );
}
private:
ros::NodeHandle n;
ros::Publisher map_pub;
ros::Publisher metadata_pub;
ros::ServiceServer service;
bool deprecated;
bool mapCallback(nav_msgs::GetMap::Request &req,
nav_msgs::GetMap::Response &res )
{
// request is empty; we ignore it
// = operator is overloaded to make deep copy (tricky!)
res = map_resp_;
ROS_INFO("Sending map");
return true;
}
// The map data is cached here, to be sent out to service callers
nav_msgs::MapMetaData meta_data_message_;
nav_msgs::GetMap::Response map_resp_;
};
}
这里最核心的函数是map_server::loadMapFromFile，再看它的源码，发现使用的是SDL库读图片，核心是函数SDL_Surface * pSurfaceTemp = IMG_Load(file_name.c_str());， IMG_Load()可以读取多种格式的图片文件，包括pgm。 到此为止，再深层的没必要了解了。
报错: Image Truncated
报错 [map_server-1] process has died [pid 635, exit code 255
首先修改 yaml 中的 origin: [0.000000, 0.000000, -nan] 为 origin: [0.000000, 0.000000, 0]，结果仍然报错
发现用Gimp也无法打开，报错 PNM Image plug-in could not open image，在Properties里看不出有什么特殊。
最终发现是 VINS 生成地图时，所用的点没有加时间戳
报错: failed to open image file “/home/user/map.pgm”: file truncated
用图片工具打开，发现pgm全黑，建图问题
无法针对pgm生成代价地图
pgm文件，为了保证边缘有黑边，只给一圈黑色像素不够，给4个黑色像素。