[Cartographer]初始化

1. [Cartographer]初始化
2. [Cartographer]AddTrajectory
3. [Cartographer]传感器数据流

1.Cartographer文件树

cartographer算法包括cartographer和cartographer_ros两部分，其中cartographer为算法的核心实现，cartographer_ros为算法和ROS的对接的桥梁，方便用户使用。

.
├── cartographer
│   ├── bazel
│   │   └── third_party
│   ├── cartographer
│   │   ├── cloud
│   │   ├── common
│   │   ├── ground_truth
│   │   ├── io
│   │   ├── mapping
│   │   ├── metrics
│   │   ├── sensor
│   │   └── transform
│   ├── cmake
│   │   └── modules
│   ├── configuration_files
│   ├── docs
│   │   ├── assets
│   │   └── source
│   └── scripts
└── cartographer_ros
    ├── cartographer_ros
    │   ├── cartographer_ros
    │   ├── configuration_files
    │   ├── launch
    │   ├── scripts
    │   └── urdf
    ├── cartographer_ros_msgs
    │   ├── msg
    │   └── srv
    ├── cartographer_rviz
    │   ├── cartographer_rviz
    │   └── ogre_media
    ├── docs
    │   └── source
    └── scripts

2.Cartographer初始化

1. 从src/cartographer_ros/cartographer_ros/launch/backpack_2d.launch进入，可以看到cartographer_node节点的启动

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d.lua"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
launch>

2. 可以发现cartographer_node是在src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc中创建的，这里主要关注函数cartographer_ros::Run()。

int main(int argc, char** argv) {

  // note: 初始化glog库
  google::InitGoogleLogging(argv[0]);
  
  // 使用gflags进行参数的初始化. 其中第三个参数为remove_flag
  // 如果为true, gflags会移除parse过的参数, 否则gflags就会保留这些参数, 但可能会对参数顺序进行调整.
  google::ParseCommandLineFlags(&argc, &argv, true);

  /**
   * @brief glog里提供的CHECK系列的宏, 检测某个表达式是否为真
   * 检测expression如果不为真, 则打印后面的description和栈上的信息
   * 然后退出程序, 出错后的处理过程和FATAL比较像.
   */
  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";

  // ros节点的初始化
  ::ros::init(argc, argv, "cartographer_node");

  // 一般不需要在自己的代码中显式调用
  // 但是若想在创建任何NodeHandle实例之前启动ROS相关的线程, 网络等, 可以显式调用该函数.
  ::ros::start();

  // 使用ROS_INFO进行glog消息的输出
  cartographer_ros::ScopedRosLogSink ros_log_sink;

  // 开始运行cartographer_ros
  cartographer_ros::Run();

  // 结束ROS相关的线程, 网络等
  ::ros::shutdown();
}

3. 在cartographer_ros::Run()函数中创建了map_builder和node，node控制着整个系统，而map_builder如名字一样，用于地图的构建，另外map_builder这个变量移动到MapBuilderBridge中的std::unique_ptr map_builder_

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  // 开启监听tf的独立线程
  tf2_ros::TransformListener tf(tf_buffer);
  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  // c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple
  // 根据Lua配置文件中的内容, 为node_options, trajectory_options 赋值
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);
  ......
}

4. 从node的构造开始，初始的流程如图所示