cartographer源码解析（九）代码运行顺序梳理

描述

这段时间的任务一直是阅读cartographer，这篇主要分析cartographer的代码运行顺序，再好好理清一下cartographer的运行逻辑

顺序

我在代码中cout一些信息，帮助我们梳理一下代码的运行时的进进出出

这里会梳理出全部的关键程序语句

1. node_main.cc

   开始运行launch文件后，就进入Run()函数

   功能：开始运行建图节点

2. node_options.cc

   在node_main.cc的Run()中，执行了LoadOption()函数

   std::tie(node_options, trajectory_options) =
     LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
   

   功能：加载所有的lua文件参数

3. map_builder.cc

   auto map_builder =
     cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
   

   CreateMapBuilder()在map_builder.cc

   功能：创建了一个建图实例

4. node_main.cc

   if (FLAGS_start_trajectory_with_default_topics) {
   	node.StartTrajectoryWithDefaultTopics(trajectory_options);
   }
   

   功能：开始建图

5. node.cc

   从node_main.cc进入node.cc的StartTrajectoryWithDefaultTopics

   void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
   	absl::MutexLock lock(&mutex_);
   	CHECK(ValidateTrajectoryOptions(options));
   	AddTrajectory(options);
   }
   

   功能：StartTrajectoryWithDefaultTopics函数中去执行AddTrajectory的新建轨迹功能

AddTrajectory()函数有很多个步骤，下面依次来梳理

6. node.cc

   const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
     expected_sensor_ids = ComputeExpectedSensorIds(options)
   

   函数的定义在这儿

   std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
   Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
   using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
   using SensorType = SensorId::SensorType;
   std::set<SensorId> expected_topics;
   // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
   for (const std::string& topic :
      ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
   	expected_topics.insert(SensorId{SensorType::RANGE, topic});
   }
   for (const std::string& topic : ComputeRepeatedTopicNames(
          kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
   	expected_topics.insert(SensorId{SensorType::RANGE, topic});
   }
   for (const std::string& topic :
      ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
   	expected_topics.insert(SensorId{SensorType::RANGE, topic});
   }
   // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
   // required.
   if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
     (node_options_.map_builder_options.use_trajectory_builder_2d() &&
      options.trajectory_builder_options.trajectory_builder_2d_options()
          .use_imu_data())) {
   	expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
   }
   // Odometry is optional.
   if (options.use_odometry) {
   	expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
   }
   // NavSatFix is optional.
   if (options.use_nav_sat) {
   expected_topics.insert(
       SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
   }
   // Landmark is optional.
   if (options.use_landmarks) {
   	expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
   }
   	return expected_topics;
   }
   

   三个for循环，第二个for语句运行了，topic为echoes
   四个if语句，第一个if语句运行了，topic为imu

   该函数的结果是向expected_topic中，添加了两个话题，它们的id一个叫做echoes，另一个叫做imu，它们的type一个是RANGE，另一个是IMU
   
   功能：确定建图过程中要订阅的topic名字

7. node.cc

   const int trajectory_id =
     map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
   

   功能：进一步调用MapBuilderBridge的函数AddTrajectory

8. map_builder_bridge.cc

   const int trajectory_id = map_builder_->AddTrajectoryBuilder(
     expected_sensor_ids, trajectory_options.trajectory_builder_options,
     [this](const int trajectory_id, const ::cartographer::common::Time time,
            const Rigid3d local_pose,
            ::cartographer::sensor::RangeData range_data_in_local,
            const std::unique_ptr<
                const ::cartographer::mapping::TrajectoryBuilderInterface::
                    InsertionResult>) {
       OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
     });
   

   功能：进一步调用MapBuilder的函数AddTrajectoryBuilder

9. map_builder.cc

   int MapBuilder::AddTrajectoryBuilder(
   const std::set<SensorId>& expected_sensor_ids,
   const proto::TrajectoryBuilderOptions& trajectory_options,
   LocalSlamResultCallback local_slam_result_callback)
   

   功能：在这个函数中，会判断是2D建图还是3D建图

10. map_builder.cc

    直接进入else语句

    if (options_.use_trajectory_builder_3d()) {
    	.....
    } else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
    }
    

    这段有两个个关键语句，依次来拆开看

11. map_builder.cc

    最开始是一个if语句

    std::vector<std::string> SelectRangeSensorIds(
    const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
    std::vector<std::string> range_sensor_ids;
    for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
    	if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
      	range_sensor_ids.push_back(sensor_id.id);
    	}
    }
    return range_sensor_ids;
    }
    

    传进来的参数expected_sensor_ids，实际上就是上面第6小节分析过的。阅读这段代码，可以看出最后return的range_sensor_ids就是RANGE类型的echoes

12. map_builder.cc

    紧接if语句之后

    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
    

    核心语句是absl::make_unique，构建了一个CollatedTrajectoryBuilder的指针，代码在collated_trajectory_builder.cc中

13. collated_trajectory_builder.cc

    这是CollatedTrajectoryBuilder的构造函数

    CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    const proto::TrajectoryBuilderOptions& trajectory_options,
    sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
    const std::set<SensorId>& expected_sensor_ids,
    std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
    : sensor_collator_(sensor_collator),
      collate_landmarks_(trajectory_options.collate_landmarks()),
      collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
      trajectory_id_(trajectory_id),
      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
      last_logging_time_(std::chrono::steady_clock::now()) {
    absl::flat_hash_set<std::string> expected_sensor_id_strings;
    for (const auto& sensor_id : expected_sensor_ids) {
    	if (sensor_id.type == SensorId::SensorType::LANDMARK &&
        	!collate_landmarks_) {
      	continue;
    	}
    	if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
        	!collate_fixed_frame_) {
      	continue;
    	}
    	expected_sensor_id_strings.insert(sensor_id.id);
    }
    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));
      });
    }
    

    第一个for循环中含有两个if，都没有进到if中。因此只执行了for循环中的唯一一句insert语句，insert中的参数sensor_id.id的值依靠for循环，产生了两个：echoes和imu
    因此核心语句是指针sensor_collator_ 的AddTrajectory()

14. Collator.cc

    sensor_collator_指针的声明在collated_trajectory_builder.h中

    sensor::CollatorInterface* const sensor_collator_;
    

    CollatorInterface类中的AddTrajectory是一个虚函数。Collator是CollatorInterface的子类，在Collator.cc中对AddTrajectory进行了重写

    Collator.cc中AddTrajectory的定义

    void Collator::AddTrajectory(
    const int trajectory_id,
    const absl::flat_hash_set<std::string>& expected_sensor_ids,
    const Callback& callback) {
    for (const auto& sensor_id : expected_sensor_ids) {
    	const auto queue_key = QueueKey{trajectory_id, sensor_id};
    	queue_.AddQueue(queue_key,
                    [callback, sensor_id](std::unique_ptr<Data> data) {
                      callback(sensor_id, std::move(data));
                    });
    	queue_keys_[trajectory_id].push_back(queue_key);
    	}
    }
    

    cout可以发现，for循环了两次，两次的sensor_id分别是echoes、imu，trajectory_id一直是0

    功能：向队列queue_keys里，压入了两个QueueKey

15. collated_trajectory_builder.cc

    结束了Collator.cc的AddTrajectory()
    返回上一层，也同时结束了collated_trajectory_builder.cc中的CollatedTrajectoryBuilder的构造函数

16. map_builder.cc

    结束了CollatedTrajectoryBuilder的构造函数
    返回上一层map_builder.cc继续执行AddTrajectoryBuilder()

17. map_builder.cc

    MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                  pose_graph_.get());
    

    进入MaybeAddPureLocalizationTrimmer函数后，没有进行任何操作，相当于被跳过了

18. map_builder.cc

    if (trajectory_options.has_initial_trajectory_pose()) {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
    }
    

    这段if也没有进去，跳过了

19. map_builder.cc

    proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
    for (const auto& sensor_id : expected_sensor_ids) {
    	*options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
    }
    *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
      trajectory_options;
    all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
    

    这篇先不展开去找函数定义了。结合这段函数的上下文可以得到这段函数的意义

    功能：这段语句的前面一大段向expected_sensor_ids添加了两个话题，一个echoes，另外一个imu。这段语句将两个话题，配合函数输入的参数trajectory_options（这个变量中包含了从lua文件读出的参数），一起添加到了all_trajectory_builder_options_中

20. node.cc

    运行完18小节解释的代码，会返回node.cc，继续执行node.cc中的AddTrajectory()

    AddExtrapolator(trajectory_id, options);
    AddSensorSamplers(trajectory_id, options);
    

    这两句话这篇不解释了，以前的一篇解释过

21. node.cc

    LaunchSubscribers(options, trajectory_id);
    

    这句话订阅echoes和imu话题