描述
这段时间的任务一直是阅读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()函数有很多个步骤,下面依次来梳理

  1. 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>,构建了一个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话题