Rviz中的地图绘制使用了cairo库。注意gflag的参数include_frozen_submaps 和 include_unfrozen_submaps不能都为false，否则会报错
cartographerrviz的绘制方式，就是`submap_node控制一个子图的绘制，每个子图有一个子图原点，这个是submap_query中的slice_pose。而每个子图之间是有微弱旋转的，而控制旋转的中心也是这个slice_pose。而每个子图之间的旋转和平移可以通过submap_list`的pose得到。
main函数值得看的就一句::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec);，构造函数如下1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22Node::Node(const double resolution, const double publish_period_sec)
: resolution_(resolution),
// submap_query 服务的客户端
client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
kSubmapQueryServiceName)),
// submap_list话题的订阅者，回调函数 HandleSubmapList
submap_list_subscriber_(node_handle_.subscribe(
kSubmapListTopic, kLatestOnlyPublisherQueueSize,
boost::function<void(
const cartographer_ros_msgs::SubmapList::ConstPtr&)>(
[this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
HandleSubmapList(msg);
}))),
// 发布话题 map
occupancy_grid_publisher_(
node_handle_.advertise<::nav_msgs::OccupancyGrid>(
kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
true /* latched */)),
// timer执行DrawAndPublish函数
occupancy_grid_publisher_timer_(
node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
&Node::DrawAndPublish, this)) {}Node::DrawAndPublish是画图和发布map，不是重点
HandleSubmapList函数大部分都很简单，终点是下面这句：1
2auto fetched_textures =
::cartographer_ros::FetchSubmapTextures(id, &client_);
内部其实就是submap_query的客户端发起请求，服务端的注册在Node的构造函数里，回调是Node::HandleSubmapQuery
代码调用顺序是： Node::HandleSubmapQuery —— MapBuilderBridge::HandleSubmapQuery —— MapBuilder::SubmapToProto —— PoseGraph2D::GetSubmapData 和 Submap2D::ToResponseProto —— ProbabilityGrid::DrawToSubmapTexture
重点：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// 这里比较简单，最终返回的是后端传来的 global坐标系下的位姿
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const SubmapId& submap_id) const
{
const auto it = data_.submap_data.find(submap_id);
if (it == data_.submap_data.end())
return {};
auto submap = it->data.submap;
if (data_.global_submap_poses_2d.Contains(submap_id) )
{
// We already have an optimized pose.
return {submap,
transform::Embed3D(
data_.global_submap_poses_2d.at(submap_id).global_pose) };
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
submap_id.trajectory_id) *
submap->local_pose() };
}
void Submap2D::ToResponseProto(
const transform::Rigid3d&,
proto::SubmapQuery::Response* const response) const
{
if (!grid_) return;
// 这个 version 是子图插入了多少雷达数据，完成的子图是 2* num_range_data
response->set_submap_version( num_range_data() );
proto::SubmapQuery::Response::SubmapTexture* const texture =
response->add_textures();
grid()->DrawToSubmapTexture(texture, local_pose());
}
再看DrawToSubmapTexture1
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
42bool ProbabilityGrid::DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const
{
Eigen::Array2i offset;
CellLimits cell_limits;
ComputeCroppedLimits(&offset, &cell_limits);
std::string cells;
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits))
{
if (!IsKnown(xy_index + offset)) {
cells.push_back(0 /* unknown log odds value */);
cells.push_back(0 /* alpha */);
continue;
}
// We would like to add 'delta' but this is not possible using a value and
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
// zero, and use 'alpha' to subtract. This is only correct when the pixel
// is currently white, so walls will look too gray. This should be hard to
// detect visually for the user, though.
// delta范围[-127, 127]
const int delta =
128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
// 存数据有2个值，栅格值value和alpha透明度
cells.push_back(value);
cells.push_back((value || alpha) ? alpha : 1);
}
// 保存地图栅格数据时进行压缩
common::FastGzipString(cells, texture->mutable_cells());
// 地图描述信息赋值，省略
......
*texture->mutable_slice_pose() = transform::ToProto(
local_pose.inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)) );
return true;
}
1 | // 解压后的栅格数据 |
