通往Cartographer的桥梁——map_builder_bridge_

在上一章中我们分析了node对象，它负责完成ROS主题的订阅与发布提供服务。 在代码的分析过程中，我们发现ROS系统与Cartographer内核之间的信息交换都是通过对象map_builder_bridge_来完成的。本文来分析这个对象。

1. 对象的构造与初始化

按照我个人查看代码的习惯，我们还是先来看看它的构造函数吧，如下面的代码片段所示，可以看到除了为必要的变量提供初值之外没有任何其他的操作。 这个函数的三个输入变量都是cartographer_node在系统运行之初构建的。 其中，node_options是从配置文件中加载的配置项，map_builder则是Cartographer的地图构建器，tf_buffer是ROS系统中坐标变换库tf2的监听缓存。

        MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
                std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, tf2_ros::Buffer* const tf_buffer)
            : node_options_(node_options),
              map_builder_(std::move(map_builder)),
              tf_buffer_(tf_buffer) {}

map_builder_bridge_对象是node对象的一个成员，在node对象的构造函数中构建。 而它的初始化需要在node对象调用它的成员函数AddTrajectory之后才能触发。在这个函数中，会调用map_builder_bridge_对象的AddTrajectory成员函数，下面是其代码片段。

第一句话就很长，它通知map_builder_对象添加一个轨迹跟踪器，同时将构建成功的索引返回保存在局部变量trajectory_id中。虽然这句话很长，但这个被调用的函数只有三个参数。 前两个参数无非是关于传感器和轨迹跟踪器的一些配置信息。第三个参数比较重要，看字面意思，它相当于注册了一个回调函数OnLocalSlamResult， 用于响应map_builder_完成一个局部SLAM或者说是成功构建了一个子图的事件。

        int MapBuilderBridge::AddTrajectory(const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, const TrajectoryOptions& trajectory_options) {
            const int trajectory_id = map_builder_->AddTrajectoryBuilder(expected_sensor_ids, trajectory_options.trajectory_builder_options,
                                                                         ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, ::std::placeholders::_1, ::std::placeholders::_2, ::std::placeholders::_3, ::std::placeholders::_4, ::std::placeholders::_5));
            LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

然后在检查trajectory_id确保之前没有使用过之后，为之构建一个SensorBridge对象。 SensorBridge(sensor_bridge.h, sensor_bridge.cc)是cartographer_ros中对于传感器的一个封装， 用于将ROS的消息转换成Cartographer中的传感器数据类型。

            CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
            sensor_bridges_[trajectory_id] = cartographer::common::make_unique<SensorBridge>(trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_->GetTrajectoryBuilder(trajectory_id));

最后将轨迹相关的配置保存到容器对象trajectory_options_中并检查后，返回刚刚生成的索引trajectory_id。

            auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options);
            CHECK(emplace_result.second == true);
            return trajectory_id;
        }

2. OnLocalSlamResult

下面是函数OnLocalSlamResult的代码片段，该函数的目的就是记录下轨迹状态，他有5个参数。前四个参数的意义如注释所示，第五个参数是一个指针似乎没有用到。 其中第四个参数的数据类型RangeData是Cartographer定义的激光雷达传感器测量数据的存储结构， 它有三个字段，origin描述了传感器的坐标，returns和misses都是点云数据，分别表示有物体反射和空闲区域。

        void MapBuilderBridge::OnLocalSlamResult(
            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> insertion_result) {

然后根据输入的参数构建对象local_slam_data。它的数据类型LocalSlamData是定义在MapBuilderBridge内部的结构体，用于记录局部SLAM反馈的状态。

            std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data = std::make_shared<TrajectoryState::LocalSlamData>(TrajectoryState::LocalSlamData{time, local_pose, std::move(range_data_in_local)});

最后加锁，写入容器trajectory_state_data_中。

            cartographer::common::MutexLocker lock(&mutex_);
            trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
        }

3. map_builder_bridge_中重要的字段

下表是类MapBuilderBridge在头文件中定义的成员变量。 互斥量mutex_没有什么好说的。node_options_, map_builder_以及tf_buffer_都是在构造的时候完成初始化的，它们所对应的对象都是在系统运行之初就已经加载或者构建的。

剩下的那些用unordered_map实现的对象都是一些映射表，除了landmark_to_index_之外都是以"trajectory_id"为索引的。也就是说对于每条轨迹都有一组配置，一个用于转换传感器数据的SensorBridge对象。

4. SensorBridge

在介绍node对象的订阅回调函数的时候，我们看到在处理激光传感器的数据， 是通过如下的语句完成的。

        map_builder_bridge_.sensor_bridge(trajectory_id)->HandleMultiEchoLaserScanMessage(sensor_id, msg);

类似的处理IMU的数据也是先通过成员函数sensor_bridge获取SensorBridge对象，再通过该对象的相应接口处理ROS消息。map_builder_bridge_的成员函数sensor_bridge十分简单， 就是返回map容器中trajectory_id所对应的对象而已。

        SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
            return sensor_bridges_.at(trajectory_id).get();
        }

先一起来看一看SensorBridge的构造函数，和MapbuilderBridge一样它的函数主体也是空的没有任何操作。这里我们主要关注一下它的输入参数，前面四个参数是从配置文件中加载的配置， 第五个参数则是Cartographer的一个核心对象，通过sensor_bridge对象转换后的数据都是通过它喂给Cartographer的， 在map_builder_bridge_的成员函数AddTrajectory中通过map_builder_->GetTrajectoryBuilder(trajectory_id)获得。

        SensorBridge::SensorBridge(const int num_subdivisions_per_laser_scan,   // 激光雷达数据分段数量
                                   const std::string& tracking_frame,           // 参考坐标系
                                   const double lookup_transform_timeout_sec,   // tf坐标变换查询超时设置
                                   tf2_ros::Buffer* const tf_buffer,            // tf坐标变换缓存
                                   carto::mapping::TrajectoryBuilderInterface* const trajectory_builder) // 轨迹构建器
        : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan), tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), trajectory_builder_(trajectory_builder)
        {}

下表是SensorBridge中一些重要的字段。其中num_subdivisions_per_laser_scan_和sensor_to_previous_subdivision_time_主要用于激光传数据的分段。tf_bridge_是对tf2的封装， 用于将ROS系统的坐标变换转换成Cartographer的。trajectory_builder_实质上就是刚刚在构造函数中讨论的第五个参数。

5. 完

本文中，我们分析了map_builder_bridge_对象的构造函数和一些重要的成员变量。该对象通过SensorBridge将ROS系统中的传感器数据转换到Cartographer中。