Ros用OccupancyGrid存储栅格地图，栅格图表示了障碍物在真实地图上的分布情况。
C:\Windows\System32>rosmsg show nav_msgs/OccupancyGrid std_msgs/Header header uint32 seq time stamp string frame_id nav_msgs/MapMetaData info time map_load_time float32 resolution uint32 width uint32 height geometry_msgs/Pose origin geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w int8[] data
nav_msgs::OccupancyGrid分为三个部分。以图1中比例尺(resolution)为0.05，栅格数为105x150，实际大小为5.249mx7.5m的地图举例说明。CreateOccupancyGridMsg代码则显示了是如何填充这些值。
第一部分：std_msgs/Header header
- uint32 seq。帧序列号。图1中这个值是0。
- time stamp。时间戳。
- string frame_id。地图参考的坐标系。图1中是“map”。
第二部分：nav_msgs/MapMetaData info
- time map_load_time。地图加载时间。图1中这值等于header.stamp。
- float32 resolution。比例尺，代表一个栅格在实际地图中的尺寸。单位：米/栅格。在后绪显示栅格图像时，往往用一个像素表示一个栅格，于是有时也称每个像素表示多少米。
- uint32 width。地图宽度，这里指的是栅格数量，不是指实际长度。图1是105。
- uint32 height。地图高度。和宽度一样，这里指的是栅格数量，不是指实际长度。图1是150。
- geometry_msgs/Pose origin。一般来说，geometry_msgs/Pose用于表示位姿，但这里不是该功能，只是恰好能借用它的两个字段。
- geometry_msgs/Point position。原点在真实地图下的坐标，单位米，对2D slam，z固定0。所谓原点，对应真实世界中机器人中心在开始时刻落在的那个位置。设想这么个场景，机器人放在位置p，这时激光雷达开始扫描，立刻会在p周围形成一圈障碍物，见图1中红色表示的障碍物，并产生一个能包含所有障碍物的最小矩形R。这时p点距R的左上角坐标就会产生一个偏移，postion就表示了这个偏移。当然，position值不是等于左上角偏移，而是左下角偏移，再取负，见图1中(-1.7; -2.95)指向的两个黑色刻度标记。图1中显示的R要比包含了所有障碍物的最小矩形上、下、左、右各外扩了kPaddingPixel(5)栅格，即0.25米，position要包括这0.25。何时外扩kPaddingPixel栅格参考cartographer的PaintSubmapSlices。随着机器人移动，position会不断变化。
- geometry_msgs/Quaternion orientation。位姿中的旋转部分。图1中header.frame_id坐标系就是世界坐标系map，表示旋转的四元数固定值(0; 0; 0; 1)，即无旋转。随着机器人移动，orientation不会变化
第三部分：int8[] data
存储着栅格图中各栅格数值。理论上，每个栅格值范围是[-128, 127]，实际使用时，往往会做限制，像图1中栅格，范围[-1, 100]。
对data，要额外注意放置次序。它不是通常认为的，首先放置栅格图左上角，放完0行后放1行，最后height-1行。正如图1右侧直接读取data生成的图像所示，是先存height-1行，然后height-2行，最后0行。
这种data放置次序，参考“world/map坐标系、updateOrigin、footprintCost”中“world坐标系”。
CreateOccupancyGridMsg
carographer提供了CreateOccupancyGridMsg方法，用于从PaintSubmapSlicesResult结构的地图painted_slices生成nav_msgs::OccupancyGrid结构的栅格图。
<cartographer_ros>\cartographer_ros\msg_conversion.cc
------
std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
const cartographer::io::PaintSubmapSlicesResult& painted_slices,
const double resolution, const std::string& frame_id,
const ros::Time& time) {
auto occupancy_grid = absl::make_unique<nav_msgs::OccupancyGrid>();
const int width = cairo_image_surface_get_width(painted_slices.surface.get());
const int height =
cairo_image_surface_get_height(painted_slices.surface.get());
occupancy_grid->header.stamp = time;
occupancy_grid->header.frame_id = frame_id;
occupancy_grid->info.map_load_time = time;
occupancy_grid->info.resolution = resolution;
occupancy_grid->info.width = width;
occupancy_grid->info.height = height;
occupancy_grid->info.origin.position.x =
-painted_slices.origin.x() * resolution;
occupancy_grid->info.origin.position.y =
(-height + painted_slices.origin.y()) * resolution;
// resolution: 0.05
// width x height: (105 x 150)(5.249, 7.500)
// painted_slices.origin: (34.000, 91.000)
// ==>
// info.origin.position: (-1.700, -2.950)
occupancy_grid->info.origin.position.z = 0.;
occupancy_grid->info.origin.orientation.w = 1.;
occupancy_grid->info.origin.orientation.x = 0.;
occupancy_grid->info.origin.orientation.y = 0.;
occupancy_grid->info.origin.orientation.z = 0.;
const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(
cairo_image_surface_get_data(painted_slices.surface.get()));
occupancy_grid->data.reserve(width * height);
for (int y = height - 1; y >= 0; --y) {
for (int x = 0; x < width; ++x) {
const uint32_t packed = pixel_data[y * width + x];
const unsigned char color = packed >> 16;
const unsigned char observed = packed >> 8;
const int value =
observed == 0
? -1
: ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
CHECK_LE(-1, value);
CHECK_GE(100, value);
occupancy_grid->data.push_back(value);
}
}
return occupancy_grid;
}对origin.orientation，固定值(0, 0, 0, 1)。栅格值范围[-1, 100]。-1表示该栅格上的障碍物情况未知，即图1中非灰色的透明部分。