Cartographer源码篇:源码分析(1)
在安装编译cartographer-1.0.0的时候，我们可以看到代码主要包括cartorgarpher_ros、cartographer、ceres-sover三个部分。其中，ceres-solver用于非线性优化，求解最小二乘问题;cartographer_ros为ROS平台的封装，获取传感器数据，显示和保存处理结果;cartographer为算法的底层实现，处理传感器数据，构建地图。作为ROS平台的封装，cartographer_ros没做实质性的工作，但是其充当了cartographer程序的守门人的角色，包含通往cartographer的钥匙，故本次先针对cartographer_ros作一个简单快速的分析。
cartographer_ros文件结构
首先看下cartographer_ros的文件结构，如下图所示：
然后我们具体分析下各目录的内容：
cartographer_ros_msgs：部分msg、srv文件;
cartographer_rviz：与rviz显示相关的文件;
cartographer_ros/cartographer_ros：基于ROS实现的主要程序，包含各节点的实现文件，入口节点cartographer_node的实现文件node_mian.cc就在这里;
cartographer_ros/configuration_files：参数配置文件，包括机器人参数、建图参数、定位参数、文件保存参数等;
cartographer_ros/launch：节点启动文件，包括机器人、建图、定位、文件保存等节点;
cartographer_ros节点关系
通过实际运行，分析一下ros节点的情况：
cartographer_node
cartographer_ros的主节点，主要负责订阅传感器数据、发布显示数据。
-
订阅话题
订阅各种传感器的数据;
scan (sensor_msgs/LaserScan)
echoes (sensor_msgs/MultiEchoLaserScan)
points2 (sensor_msgs/PointCloud2)
imu (sensor_msgs/Imu)
odom (nav_msgs/Odometry)
-
发布话题
发布cartographer处理数据的结果;
scan_matched_points2 (sensor_msgs/PointCloud2)
submap_list (cartographer_ros_msgs/SubmapList)
trajectory_node_list
landmark_poses_list
constraint_list
-
服务
提供子图查询、轨迹新建与结束、地图保存的服务接口;
submap_query (cartographer_ros_msgs/SubmapQuery)
查询submap;
start_trajectory (cartographer_ros_msgs/StartTrajectory)
开始一条轨迹;
trajectory_query (cartographer_ros_msgs/TrajectoryQuery)
finish_trajectory (cartographer_ros_msgs/FinishTrajectory)
结束一条轨迹;
write_state (cartographer_ros_msgs/WriteState)
保存状态;
-
tf变换
坐标系之间的位姿变换关系;
tf图
occupancy_grid_node
主要负责订阅子图与发布栅格地图。
-
订阅话题
submap_list (cartographer_ros_msgs/SubmapList)
-
发布话题
map (nav_msgs/OccupancyGrid)
pbstream_map_publisher
主要负责发布地图文件。
-
发布话题
map (nav_msgs/OccupancyGrid)
offline_node
主要用于离线运行的节点，功能类似cartographer_node节点。
cartographer_ros代码结构
正如前面说的那样，cartographer_ros主要有两部分功能：
1.订阅传感器数据与结果展示;
2.cartographer api的ros封装;
故针对cartographer_ros，我们主要作简单的梳理，将大部分的时间放到以后对cartographer算法底层的研究，代码流程如图所示：
原图： https://github.com/yinqijun/img/blob/master/20200721205145.png
简单说明一下，蓝线是cartographer_ros的主流程，紫线是新建轨迹的流程，绿线是结束轨迹、优化的流程，黑线是发布显示所需数据的流程。黑色填充的文件在cartographer_ros目录下的，绿色与紫色填充的文件在cartographer目录下。
沿着每个流程去理代码，一层层剥开封装，最后发现都是通过map_builder_bridge、sensor_bridge进入cartographer目录下，去做实质的数据处理、建图的底层工作。给人一种感觉，就是看完cartographer_ros发现还没看到真正的程序，全是花里胡哨的层层封装以及C++的高级用法，也给阅读源码的新手很大的压力。但是，作为一个产品级的源码，还是可以从背后多琢磨一下谷歌工程师的意图，这就是仁者见仁智者见智的事情了。总之，从全局结构去看，cartographer_ros的代码结构就是这样啦。
接下来，具体看一下主程序的流程：
-
通过launch文件启动节点
backpack_3d.launch & backpack_3d.lua 以
backpack_3d.launch为例，主要做了以下工作：启动cartographer_node、cartographer_occupancy_grid_node节点;
导入.urdf文件、.lua配置文件路径;
其中，.urdf文件定义了机器人的坐标变换，.lua文件定义了建图算法的配置，包括传感器的种类、采样率、发布频率，以及位姿优化的参数等。
-
订阅tf，使用.lua配置cartographer算法参数、ros订阅发布参数
-
定时发布submap/trajectory/landmark_pose/constraint/scan_matched_point_cloud
-
新建轨迹
代码流程：AddTrajectory()AddExtrapolator() + AddSensorSampler() + LaunchSubscriber()Handle…Message()
文件流程：node_main.ccnode.ccmap_builder_bridge{map_builder_interface.h(cartographer) + sensor_bridgetrajectory_builder_interface.h(cartographer)}
-
结束轨迹
-
优化
-
保存
参考资料：