cartographer为Google提供的激光SLAM开源库,通常通过其提供的ROS平台封装进行使用,该库结构清晰,模块完整,值得深入研究。
项目官网:https://google-cartographer.readthedocs.io/en/latest/
项目Github:https://github.com/cartographer-project/cartographer
环境安装与配置具体参考:https://www.cnblogs.com/lvchaoshun/p/9824528.html
安装过程中可能会碰到一些版本冲突之类的问题,耐心上网寻求解决方法。
温馨提示:本系列主要介绍源码内容,不介绍环境搭建,仅作为自己学习过程记录,可能会比较乱,有疑问欢迎留言讨论。
下面进入正题,下图是cartographer官网提供的软件架构图,本系列将通过cartographer_ros封装库中调用顺序,对程序主线进行介绍,暂未使用到的或次要的信息以后有空再分析。
首先我们找到cartographer_ros中程序入口,即cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc,其中的main函数为cartographer_ros节点启动的程序入口。
其他都是ros平台初始化相关的函数,只用看98行Run方法。
可以看到该函数内主要是些初始化工作,创建map_builder传给node进行初始化、读取pbstream地图文件,并调用StartTrajectoryWithDefaultTopics函数,该函数定义在node.cc中。
我们会发现AddTrajectory是核心功能入口。××该函数需重点关注,之后我们还会回来一个个分析其中调用的关键函数。
该函数功能比较多,我们一步步看。首先363行创建一个set保存传感器话题的编号,用于之后订阅器订阅。366行调用map_builder_bridge_实例的AddTrajectory函数,在cartographer_ros中类名包含“bridge”的类基本上都是作为“桥梁”调用cartographer源码的类。我们可以看到map_builder_bridge_为map_builder_bridge.cc中定义的MapBuilderBridge的实例,此时关注map_builder_bridge.cc中定义的AddTrajectory函数内容。
根据cartographer全局配置参数和最初Run方法中定义的map_builder实例类型,我们了解到map_builder_为cartographer/mapping/map_builder.cc中定义的MapBuilder类的实例。上图函数内容分为两块,一块调用map_builder_的AddTrajectoryBuilder函数,创建trajectory_builder并设置结果回调,返回一个路径id作为当前轨迹的编号,详情下篇再介绍;另一块主要初始化一个SensorBridge实例,并放入sensor_birdges_这个map中,以及保存该路径id对应的配置参数。
接下来我们回到node.cc中核心功能入口函数。368与369行分别创建与当前路径编号对应的位姿预测器(PoseExtrapolator)与传感器采样器(TrajectorySensorSamplers),根据配置参数创建实例没啥好说的。371行启动ros话题订阅器,并设置回调函数。这之后的创建定时器和记录话题编号也很简单。我们主要关注话题订阅的回调函数。
上图截取了LaunchSubsribers函数的部分代码,通过其回调函数,我们发现最终会调用sensor_bridge.cc中定义的HandleLaserScan函数。此处仅仅举个例子,该函数仅处理激光雷达数据(普通LaserScan类型消息、多回波、点云等),其他传感器数据(如IMU、里程计等)会调用不同的回调函数。
我们观察HandleLaserScan函数实现。
首先对点云细分,默认num_subdivisions_per_laser_scan_=10。points对象的类型为cartographer_ros自定义类型,其中保存了每个点的光强、位置与时间增量。time_to_subdivision_end保存了最后一个点的时间增量,即一块细分点云间时间间隔(第一个点之前的时刻到最后一个点之后的时刻之间的时间间隔)。之后做一些时间戳的运算,或许是诸如(..., -0.06, -0.04, -0.02, 0)的一组时间戳。最后调用HandleRangefinder将数据添加到全局路径构造器,如下图所示。
此处trajectory_builder_会是GlobalTrajectoryBuilder类的实例,是由于map_builder_的AddTrajectoryBuilder函数中调用了对应的构造函数,此处已涉及cartographer源码。
本节主要介绍了cartographer_ros封装是如何跳转到cartographer源码中的,主要包括ros节点、map_builder与trajectory_builder的创建,pose_extrapolator与trajectory_sensor_samplers的初始化以及传感器话题订阅与处理。从下一节开始,将根据调用顺序介绍cartographer源码。