cartographer代码阅读(1)流程梳理

准备

• 1、终端起roslaunch cartographer_ros bslidar.launch
• 2、clion里面：Run ==> Attach to Process，在下拉框中选择cartographer_node来执行，可以debug

debug栏中可以看到线程信息，从下到上是执行调用到的函数，从这里就可以很清晰的看到数据流。可以根据这些信息来一步步看代码。

切换node debug的状态：左侧绿色竖线箭头Resume Program和停止按钮Pause Program来切换debug的状态。

这里说明下主要函数调用关系:

• 1、定义Node对象，定义了订阅的各传感器数据的回调函数及发布话题节点Node::Node()
• 2、根据FLAGS_start_trajectory_with_default_topics，用默认话题开始构建轨迹Node::StartTrajectoryWithDefaultTopic()
• 3、调用 Node::AddTrajectory()
• 4、用map_builder_bridge来创建轨迹id，调用 MapBuilderBridge::AddTrajectory(),，接着启动订阅函数Node::LaunchSubscribers(),这样数据就开始进入回调函数，且数据都在当前轨迹下

把所有的对象初始化好后，数据进入回调函数后

• 5、通过SensorBridge::HandleLaserScanHandleLaserScan()函数，为每一个激光点打上时间戳

• 6、随后在SensorBridge::HandleRangefinder() ，将激光数据点坐标系转换到tracking frame下，并将激光点云carto::sensor::TimedPointCloudData 加入到轨迹构建器接口(trajectory_builder_->AddSensorData)中，进而调用子类接口CollatedTrajectoryBuilder::AddData()，然后进一步调用到Sensor下的Collator::AddSensorData()。

这里的数据关系层层递进，实质上是将ROS数据经过Bridge接口，将数据加入到轨迹构建器trajectory_builder_中，因为轨迹构建器中包含了sensor构建器，所以最终由sensor构建器处理数据的收集及分发。

• 7、数据到sensor收集器中后，调用OrderedMultiQueue::Add()，存入数据并OrderedMultiQueue::Dispatch()分发数据，分发数据用的是Queue类中定义的Callback callback;回调函数。想了解这个回调函数是如何定义的，查看OrderedMultiQueue类。(C++知识加强下:std::function)

using Callback = std::function<void(std::unique_ptr<Data>)>;

• 8、分发数据是怎么回到轨迹构建器中的呢？这里就用到了lambda表达式。在初始化CollatedTrajectoryBuilder的类对象时调用了sensor_collator_的AddTrajectory函数

 sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        HandleCollatedSensorData(sensor_id, std::move(data));
      });

在AddTrajectory中用了lambda表达式(C++知识加强下)，来执行回调，意思就是，在程序最开始时就初始化了回调函数，当传感器数据收到后，就执行加入到轨迹的操作。

• 9、lambda函数体中，调用了CollatedTrajectoryBuilder::HandleCollatedSensorData()函数，进而调用数据接口类data的data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get())函数，这样就把获取到的数据加入到轨迹中。

• 10、Dispatchable类继承自data，实现了AddToTrajectoryBuilder的函数，trajectory_builder接口对象调用AddSensorData()

 void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

• 11、mapping::TrajectoryBuilderInterface类是模板类，GlobalTrajectoryBuilder类和接口类是什么关系？调用该子类的AddSensorData函数

• 12、随后调用local_trajectory_builder_类对象的AddRangeData，数据就送到了局部轨迹构建器中，随后就是算法的核心部分。依次执行LocalTrajectoryBuilder2D::AddRangeData、LocalTrajectoryBuilder2D::AddAccumulatedRangeData、
  LocalTrajectoryBuilder2D::ScanMatch等函数

至此，整个数据流通了，接下来开始看激光数据的预处理细节，IMU和ODOM数据的处理细节，随后是算法核心部分，前端匹配和后端位姿优化。

这里本文没有对每一个步骤梳理的很细节，也不连贯，完全是按照debug数据流来梳理，关于各种类对象的创建，继承关系是怎么样的暂时不做详细说明。有可能存在错误，后面细看代码后再整理更新。