cartographer_ros是一个Package, 包含多个node,每一个node也都有自己的入口main函数。但cartographer_node是其中最重要的一个node。所以,我们从这个node的入口函数开始。该文件路径为:/src/cartographer_ros/cartographer_ros/cartographer_ros下
2.node_main.cc中的run函数
这里注意:Node在/cartographer_ros/cartographer_ros/cartographer/node.h中定义;在该构造函数中订阅了很多传感器的topic。收集传感器数据。
该段代码前面部分都是在定义一些变量类型,然后调用LoadOptions函数读取目录下的配置文件,将一些参数赋给node_options, trajectory_options
接下来就是比较重要的一句
打开Node类,该类在/src/cartographer_ros/cartographer_ros/cartographer/node.h中定义,由.../node.cc实现:
对我们比较有用的信息就是这个node发布了如下Topic:
在发布Service时,我们为每个Service绑定了一个函数句柄。从名字我们也能才能每个函数是用来做什么的。其中比较重要的是:
而AddTrgkector是
最重要的 里面的LaunchSubscribers这个函数:
这里代码虽长,但结构差不多,都是在订阅传感器发布的消息。可以看到有Laser, MultiEchoLaser, PointCloud2, IMU, Odometry, NavSatFixMessage, Landmark等,其中后三项是用if来判断该传感器使用情况。所以不同的应用场景需要不同地修改配置文件
前三项是根据配置文件里点云输入的类型(scan 多回波 pointCloud
后面是是否有IMU ODO 等
回到Node的构造函数。可以看到接下来是为几个Topic设置了定时器,以及定时器函数。猜测这几个定时器函数里就是定时往Topic上发布消息。
以第一个Topic的定时器函数Node::PublishSubmapList为例:
对cartographer_node做一个总结
cartographer启动以后,做了如下的工作:
1. 注册并发布了5个Topic, 并为5个Topic分别设置了定时器函数,在定时器函数中定期向Topic上广播数据:
|===1) Topic 1: kSubmapListTopic: 广播构建出来的submap的list
|-----------发布数据的函数:Node::PublishSubmapList
|-----------调用函数:map_builder_bridge_.GetSubmapList();
|===2) Topic 2: kTrajectoryNodeListTopic:发布trajectory
|-----------发布数据的函数:Node::PublishTrajectoryNodeList Node::PublishTrajectoryStates:
|-----------调用函数:map_builder_bridge_.GetTrajectoryNodeList();
|===3) Topic3: kLandmarkPoseListTopic
|-----------发布数据的函数:Node::PublishLandmarkPosesList
|-----------调用的函数:map_builder_bridge_.GetLandmarkPoseList();
|===4) Topic4: kConstraintListTopic
|-----------发布数据的函数:Node::PublishConstraintList
|-----------调用的函数:map_builder_bridge_.GetConstraintList();
|===5)Topic5: kScanMatchedPointCloudTopic
|-----------发布数据的函数:Node::PublishTrajectoryStates
|-----------调用的函数:map_builder_bridge_.GetTrajectoryStates();
|-----------这个函数名与Topic名对应不上,但是查看Node::PublishTrajectoryStates不难发现,函数中有一句:
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_state.local_slam_data->time),
node_options_.map_frame,
carto::sensor::TransformTimedPointCloud(
point_cloud, trajectory_state.local_to_map.cast<float>())));
2. 发布了4个Service,并为4个Service分别设置了句柄函数,而句柄函数也是通过调用map_builder_bridge_的成员函数来处理的。
|===1)Service 1: kSubmapQueryServiceName
|-----------句柄函数:Node::HandleSubmapQuery
|-----------实际处理的函数:map_builder_bridge_.HandleSubmapQuery
|===2) Service 2: kStartTrajectoryServiceName
|-----------句柄函数:Node::HandleStartTrajectory
|-----------实际处理的函数:map_builder_bridge_.AddTrajectory等;
|-----------这里需要额外注意的是Node::LaunchSubscribers这个函数。这个函数负责处理各个传感器函数。仔细读其中的每个处理函数,比如处理IMU的Node::HandleImuMessage函数,发现其实际调用的是map_builder_bridge_中的一个成员类sensor_bridge_ptr的函数来处理:sensor_bridge_ptr->HandleImuMessage。
|===3) Service 3: kFinishTrajectoryServiceName
|------------句柄函数:Node::HandleFinishTrajectory
|------------实际处理的函数:map_builder_bridge_.FinishTrajectory
|===4) Service 4: kWriteStateServiceName
|------------句柄函数:Node::HandleWriteState
|------------实际处理的函数:map_builder_bridge_.SerializeState