cartographer 源码解析（一）

淡淡的烟草味﹌ 2022-10-13 12:49 374阅读 0赞

## 相关链接： ##

#### [cartographer 源码解析(一)][cartographer] ####

#### [cartographer 源码解析(二)][cartographer 1] ####

#### [cartographer 源码解析(三)][cartographer 2] ####

#### [cartographer 源码解析(四)][cartographer 3] ####

#### [cartographer 源码解析(五)][cartographer 4] ####

#### [cartographer 源码解析(六)][cartographer 5] ####

cartographer算法是google大佬2016年写的开源SLAM算法，其主要是以2D定位为主。我们首先从cartographer\_ros开始解析。

参考链接：[https://www.bilibili.com/video/BV1mK4y1u7MK?from=search&seid=1825406683392869132][https_www.bilibili.com_video_BV1mK4y1u7MK_from_search_seid_1825406683392869132]

## 数据处理流水线搭建 ##

首先查看**cartographer**源码，首先查看launch文件。那么我们接下来看一下launch文件吧。  
随便找一个**backpack\_2d.launch**  
**backpack\_2d.launch**

<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>

我们找到最关键的就是**cartographer\_node**。 ros定义节点都是在main函数中定义节点。我们找到**node\_main.cc**这个文件，部分代码如下所示：  
**node\_main.cc**

int main(int argc, char** argv) {
        
      google::InitGoogleLogging(argv[0]);
      google::ParseCommandLineFlags(&argc, &argv, true);
    
      CHECK(!FLAGS_configuration_directory.empty())
          << "-configuration_directory is missing.";
      CHECK(!FLAGS_configuration_basename.empty())
          << "-configuration_basename is missing.";
    
      ::ros::init(argc, argv, "cartographer_node"); //在这里定义了节点的名字
      ::ros::start(); //开始启动
    
      cartographer_ros::ScopedRosLogSink ros_log_sink;
      cartographer_ros::Run();
      ::ros::shutdown();
    }

在上面代码中有一个Run函数，我们看一下Run函数干了什么。  
**node\_main.cc**

void Run() {
        
      constexpr double kTfBufferCacheTimeInSeconds = 10.;
      tf2_ros::Buffer tf_buffer{
        ::ros::Duration(kTfBufferCacheTimeInSeconds)};
      tf2_ros::TransformListener tf(tf_buffer);
      NodeOptions node_options; //从配置文件中读参数
      TrajectoryOptions trajectory_options;
      std::tie(node_options, trajectory_options) =
          LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
    
      // 地图构建类，根据解析的参数进行实例化
      auto map_builder =
          cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
      // 创建一个节点，把地图构建器给到这个节点，接下来就可以用地图构建器进行构建地图了
      Node node(node_options, std::move(map_builder), &tf_buffer,
                FLAGS_collect_metrics);
      if (!FLAGS_load_state_filename.empty()) {
        
        node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
      }
    // 不需要用户调用，直接启动一条轨迹 FLAGS_start_trajectory_with_default_topics是从配置参数中读出来的。机器人行走的时候会形成轨迹。程序启动起来，不需要订阅就直接启动一条轨迹
      if (FLAGS_start_trajectory_with_default_topics) {
        
        node.StartTrajectoryWithDefaultTopics(trajectory_options);
      }
    
      ::ros::spin();
      //停止所有的轨迹
      node.FinishAllTrajectories();
      //进行最后的优化
      node.RunFinalOptimization();
    
      if (!FLAGS_save_state_filename.empty()) {
        
        node.SerializeState(FLAGS_save_state_filename,
                            true /* include_unfinished_submaps */);
      }
    }

前面两行是跟TF相关的，TF是什么，是坐标系之间的关系。  
Node 都做了什么，需要我们看一下Node的构造函数。  
**node.cc**

Node::Node(
        const NodeOptions& node_options,
        std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
        tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
        : node_options_(node_options),
          map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
        
      absl::MutexLock lock(&mutex_);
      if (collect_metrics) {
        
        metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
        carto::metrics::RegisterAllMetrics(metrics_registry_.get());
      }
    
      submap_list_publisher_ =
          node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
              kSubmapListTopic, kLatestOnlyPublisherQueueSize);
      trajectory_node_list_publisher_ =
          node_handle_.advertise<::visualization_msgs::MarkerArray>(
              kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
      landmark_poses_list_publisher_ =
          node_handle_.advertise<::visualization_msgs::MarkerArray>(
              kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
      constraint_list_publisher_ =
          node_handle_.advertise<::visualization_msgs::MarkerArray>(
              kConstraintListTopic, kLatestOnlyPublisherQueueSize);
      if (node_options_.publish_tracked_pose) {
        
        tracked_pose_publisher_ =
            node_handle_.advertise<::geometry_msgs::PoseStamped>(
                kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
      }
      service_servers_.push_back(node_handle_.advertiseService(
          kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kWriteStateServiceName, &Node::HandleWriteState, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kReadMetricsServiceName, &Node::HandleReadMetrics, this));
    
      scan_matched_point_cloud_publisher_ =
          node_handle_.advertise<sensor_msgs::PointCloud2>(
              kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
    
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(node_options_.submap_publish_period_sec),
          &Node::PublishSubmapList, this));
      if (node_options_.pose_publish_period_sec > 0) {
        
        publish_local_trajectory_data_timer_ = node_handle_.createTimer(
            ::ros::Duration(node_options_.pose_publish_period_sec),
            &Node::PublishLocalTrajectoryData, this);
      }
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
          &Node::PublishTrajectoryNodeList, this));
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
          &Node::PublishLandmarkPosesList, this));
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(kConstraintPublishPeriodSec),
          &Node::PublishConstraintList, this));
    }

首先是从配置文件中读到的配置参数**node\_options**，然后是我们刚刚构建的地图构建器map\_builder， 还有我们说的查询坐标系的东西**tf\_buffer**。  
下面的一步是初始化列表，这一步比较关键

: node_options_(node_options),
          map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)

刚刚的那个地图构建器**map\_builder**用于创建**map\_builder\_bridge\_**，地图里面存放了一个**map\_builder\_bridge\_**:地图构建桥梁。我们来看看这个是什么  
**node.h**

MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

我们再具体看看MapBuilderBridge到底是一个什么样的东西。  
**map\_builder\_bridge.h**

/*
     * Copyright 2016 The Cartographer Authors
     *
     * Licensed under the Apache License, Version 2.0 (the "License");
     * you may not use this file except in compliance with the License.
     * You may obtain a copy of the License at
     *
     *      http://www.apache.org/licenses/LICENSE-2.0
     *
     * Unless required by applicable law or agreed to in writing, software
     * distributed under the License is distributed on an "AS IS" BASIS,
     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     * See the License for the specific language governing permissions and
     * limitations under the License.
     */
    
    #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
    #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
    
    #include <memory>
    #include <set>
    #include <string>
    #include <unordered_map>
    
    #include "absl/synchronization/mutex.h"
    #include "cartographer/mapping/map_builder_interface.h"
    #include "cartographer/mapping/pose_graph_interface.h"
    #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
    #include "cartographer/mapping/trajectory_builder_interface.h"
    #include "cartographer_ros/node_options.h"
    #include "cartographer_ros/sensor_bridge.h"
    #include "cartographer_ros/tf_bridge.h"
    #include "cartographer_ros/trajectory_options.h"
    #include "cartographer_ros_msgs/SubmapEntry.h"
    #include "cartographer_ros_msgs/SubmapList.h"
    #include "cartographer_ros_msgs/SubmapQuery.h"
    #include "cartographer_ros_msgs/TrajectoryQuery.h"
    #include "geometry_msgs/TransformStamped.h"
    #include "nav_msgs/OccupancyGrid.h"
    
    // Abseil unfortunately pulls in winnt.h, which #defines DELETE.
    // Clean up to unbreak visualization_msgs::Marker::DELETE.
    #ifdef DELETE
    #undef DELETE
    #endif
    #include "visualization_msgs/MarkerArray.h"
    
    namespace cartographer_ros {
        
    
    class MapBuilderBridge {
        
     public:
      struct LocalTrajectoryData {
        
        // Contains the trajectory data received from local SLAM, after
        // it had processed accumulated 'range_data_in_local' and estimated
        // current 'local_pose' at 'time'.
        struct LocalSlamData {
        
          ::cartographer::common::Time time;
          ::cartographer::transform::Rigid3d local_pose;
          ::cartographer::sensor::RangeData range_data_in_local;
        };
        std::shared_ptr<const LocalSlamData> local_slam_data;
        cartographer::transform::Rigid3d local_to_map;
        std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
        TrajectoryOptions trajectory_options;
      };
    
      MapBuilderBridge(
          const NodeOptions& node_options,
          std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
          tf2_ros::Buffer* tf_buffer);
    
      MapBuilderBridge(const MapBuilderBridge&) = delete;
      MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
    
      void LoadState(const std::string& state_filename, bool load_frozen_state);
      int AddTrajectory(
          const std::set<
              ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
              expected_sensor_ids,
          const TrajectoryOptions& trajectory_options);
      void FinishTrajectory(int trajectory_id);
      void RunFinalOptimization();
      bool SerializeState(const std::string& filename,
                          const bool include_unfinished_submaps);
    
      void HandleSubmapQuery(
          cartographer_ros_msgs::SubmapQuery::Request& request,
          cartographer_ros_msgs::SubmapQuery::Response& response);
      void HandleTrajectoryQuery(
          cartographer_ros_msgs::TrajectoryQuery::Request& request,
          cartographer_ros_msgs::TrajectoryQuery::Response& response);
    
      std::map<int /* trajectory_id */,
               ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
      GetTrajectoryStates();
      cartographer_ros_msgs::SubmapList GetSubmapList();
      std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
          LOCKS_EXCLUDED(mutex_);
      visualization_msgs::MarkerArray GetTrajectoryNodeList();
      visualization_msgs::MarkerArray GetLandmarkPosesList();
      visualization_msgs::MarkerArray GetConstraintList();
    
      SensorBridge* sensor_bridge(int trajectory_id);
    
     private:
      void OnLocalSlamResult(const int trajectory_id,
                             const ::cartographer::common::Time time,
                             const ::cartographer::transform::Rigid3d local_pose,
                             ::cartographer::sensor::RangeData range_data_in_local)
          LOCKS_EXCLUDED(mutex_);
    
      absl::Mutex mutex_;
      const NodeOptions node_options_;
      std::unordered_map<int,
                         std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
          local_slam_data_ GUARDED_BY(mutex_);
      std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
      tf2_ros::Buffer* const tf_buffer_;
    
      std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
    
      // These are keyed with 'trajectory_id'.
      std::unordered_map<int, TrajectoryOptions> trajectory_options_;
      std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
      std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
    };
    
    }  // namespace cartographer_ros
    
    #endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H

首先看看这个类里面都有哪些成员？再private里面  
**map\_builder\_bridge.h**

private:
      void OnLocalSlamResult(const int trajectory_id,
                             const ::cartographer::common::Time time,
                             const ::cartographer::transform::Rigid3d local_pose,
                             ::cartographer::sensor::RangeData range_data_in_local)
          LOCKS_EXCLUDED(mutex_);
    
      absl::Mutex mutex_;
      const NodeOptions node_options_;
      std::unordered_map<int,
                         std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
          local_slam_data_ GUARDED_BY(mutex_);
      std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
      tf2_ros::Buffer* const tf_buffer_;
    
      std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
    
      // These are keyed with 'trajectory_id'.
      std::unordered_map<int, TrajectoryOptions> trajectory_options_;
      std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
      std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;

我们直接找到最重要的地方  
**map\_builder\_bridge.h**

std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;

我们看到**map\_builder\_bridge** 里面有一个**map\_builder\_**，我们可以看到map\_builder\_bridge在Node里面，在**Node**里面调用他，那么谁调用**Node**呢，当然是用户了。也就是说当我们在调用的Node的时候，其实也是在用Node调用**map\_builder\_bridge**，最终调用的是**map\_builder\_bridge**里面的**map\_builder**  
**map\_builder\_bridge.h**

MapBuilderBridge(
          const NodeOptions& node_options,
          std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
          tf2_ros::Buffer* tf_buffer);

**node.cc**

了解完之后，我们再去看**node.cc** 接下来还干了什么，首先一堆publisher发布器

submap_list_publisher_ =
          node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
              kSubmapListTopic, kLatestOnlyPublisherQueueSize); //submap 是什么样的
      trajectory_node_list_publisher_ =
          node_handle_.advertise<::visualization_msgs::MarkerArray>(
              kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
      landmark_poses_list_publisher_ =
          node_handle_.advertise<::visualization_msgs::MarkerArray>(
              kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
      constraint_list_publisher_ =
          node_handle_.advertise<::visualization_msgs::MarkerArray>(
              kConstraintListTopic, kLatestOnlyPublisherQueueSize);

接下来是定时器，我们等待发布的东西不能一直发布，放在while里面太耗时了。放在定时器里面就好了

wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(node_options_.submap_publish_period_sec),
          &Node::PublishSubmapList, this));
      if (node_options_.pose_publish_period_sec > 0) {
        
        publish_local_trajectory_data_timer_ = node_handle_.createTimer(
            ::ros::Duration(node_options_.pose_publish_period_sec),
            &Node::PublishLocalTrajectoryData, this);
      }
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
          &Node::PublishTrajectoryNodeList, this));
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
          &Node::PublishLandmarkPosesList, this));
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(kConstraintPublishPeriodSec),
          &Node::PublishConstraintList, this));

service是给用户一个借口，发布一些命令，去操作

service_servers_.push_back(node_handle_.advertiseService(
          kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kWriteStateServiceName, &Node::HandleWriteState, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
      service_servers_.push_back(node_handle_.advertiseService(
          kReadMetricsServiceName, &Node::HandleReadMetrics, this));

那么Node的构造函数就讲完了。

其中，关于**MapBuilderBridge**的构造函数中有一个**MapBuildInterface**类型  
**MapBuilderBridge.class**

MapBuilderBridge(
          const NodeOptions& node_options,
          std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
          tf2_ros::Buffer* tf_buffer);

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]

在流程图中，不同颜色代表不同的类。  
为什么实参和形参是不一样的呢，是因为实参是形参的子类。所有实参是**map\_builder**,形参是**MapBuilderInterface**，为实现多态。

然后我们再从cartographer中而非cartographer\_ros看map\_builder 这个类的定义。  
**map\_builder.h**

/*
     * Copyright 2016 The Cartographer Authors
     *
     * Licensed under the Apache License, Version 2.0 (the "License");
     * you may not use this file except in compliance with the License.
     * You may obtain a copy of the License at
     *
     *      http://www.apache.org/licenses/LICENSE-2.0
     *
     * Unless required by applicable law or agreed to in writing, software
     * distributed under the License is distributed on an "AS IS" BASIS,
     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     * See the License for the specific language governing permissions and
     * limitations under the License.
     */
    
    #ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_
    #define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_
    
    #include <memory>
    
    #include "cartographer/common/thread_pool.h"
    #include "cartographer/mapping/map_builder_interface.h"
    #include "cartographer/mapping/pose_graph.h"
    #include "cartographer/mapping/proto/map_builder_options.pb.h"
    #include "cartographer/sensor/collator_interface.h"
    
    namespace cartographer {
        
    namespace mapping {
        
    
    // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
    // and a PoseGraph for loop closure.
    class MapBuilder : public MapBuilderInterface {
        
     public:
      explicit MapBuilder(const proto::MapBuilderOptions &options);
      ~MapBuilder() override {
        }
    
      MapBuilder(const MapBuilder &) = delete;
      MapBuilder &operator=(const MapBuilder &) = delete;
    
      int AddTrajectoryBuilder(
          const std::set<SensorId> &expected_sensor_ids,
          const proto::TrajectoryBuilderOptions &trajectory_options,
          LocalSlamResultCallback local_slam_result_callback) override;
    
      int AddTrajectoryForDeserialization(
          const proto::TrajectoryBuilderOptionsWithSensorIds
              &options_with_sensor_ids_proto) override;
    
      void FinishTrajectory(int trajectory_id) override;
    
      std::string SubmapToProto(const SubmapId &submap_id,
                                proto::SubmapQuery::Response *response) override;
    
      void SerializeState(bool include_unfinished_submaps,
                          io::ProtoStreamWriterInterface *writer) override;
    
      bool SerializeStateToFile(bool include_unfinished_submaps,
                                const std::string &filename) override;
    
      std::map<int, int> LoadState(io::ProtoStreamReaderInterface *reader,
                                   bool load_frozen_state) override;
    
      std::map<int, int> LoadStateFromFile(const std::string &filename,
                                           const bool load_frozen_state) override;
    
      mapping::PoseGraphInterface *pose_graph() override {
        
        return pose_graph_.get();
      }
    
      int num_trajectory_builders() const override {
        
        return trajectory_builders_.size();
      }
    
      mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
          int trajectory_id) const override {
        
        return trajectory_builders_.at(trajectory_id).get();
      }
    
      const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
          &GetAllTrajectoryBuilderOptions() const override {
        
        return all_trajectory_builder_options_;
      }
    
     private:
      const proto::MapBuilderOptions options_;
      common::ThreadPool thread_pool_;
    
      std::unique_ptr<PoseGraph> pose_graph_;
    
      std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
      std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
          trajectory_builders_;
      std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
          all_trajectory_builder_options_;
    };
    
    std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
        const proto::MapBuilderOptions& options);
    
    }  // namespace mapping
    }  // namespace cartographer
    
    #endif  // CARTOGRAPHER_MAPPING_MAP_BUILDER_H_

他是从MapBuilderInterface中继承的。

MapBuilderBridge 增加一些小逻辑，小判断。

首先我们创建了一个Node，这Node的主要成员是MapBuilderBridge，其里面最主要的就是MapBuild，MapBuilderBridge还有其他的一些发布器Publisher，还有一些Service。  
然后用户通过某个service 进行调用。

Node其中一条service 可以增加一条轨迹。  
**node.cc**

service_servers_.push_back(node_handle_.advertiseService(
          kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));

#### 总结一下： ####

首先是用户通过**launch**文件调用**node**节点，那么**node** 都做了什么呢。  
node里面的主要成员是**MapBuilderBridge**。  
![在这里插入图片描述][20210716220128351.png]

然后是**map\_builder\_bridge**里面最重要的是**map\_builder**  
![在这里插入图片描述][20210716220258819.png]

**node**里面有一些**publisher**和**services**  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]我们具体来分析一下，之前我们提到了在**node**里面提供了很多的**service**, 其中一个**service** 可以提供用户调用开始一条轨迹，也就是可以开始建图了，对应的响应函数是**HandleStartTrajectory**

**node.cc**

service_servers_.push_back(node_handle_.advertiseService(
          kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
     ......
      service_servers_.push_back(node_handle_.advertiseService(
          kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
     ......
      service_servers_.push_back(node_handle_.advertiseService(
          kReadMetricsServiceName, &Node::HandleReadMetrics, this));

那我们进去看一下，到底做了什么呢？

bool Node::HandleStartTrajectory(
        ::cartographer_ros_msgs::StartTrajectory::Request& request,
        ::cartographer_ros_msgs::StartTrajectory::Response& response) {
        
      TrajectoryOptions trajectory_options;
      std::tie(std::ignore, trajectory_options) = LoadOptions(
          request.configuration_directory, request.configuration_basename);
    
      if (request.use_initial_pose) {
        
        const auto pose = ToRigid3d(request.initial_pose);
        if (!pose.IsValid()) {
        
          response.status.message =
              "Invalid pose argument. Orientation quaternion must be normalized.";
          LOG(ERROR) << response.status.message;
          response.status.code =
              cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
          return true;
        }
    
        // Check if the requested trajectory for the relative initial pose exists.
        response.status = TrajectoryStateToStatus(
            request.relative_to_trajectory_id,
            {
        TrajectoryState::ACTIVE, TrajectoryState::FROZEN,
             TrajectoryState::FINISHED} /* valid states */);
        if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
        
          LOG(ERROR) << "Can't start a trajectory with initial pose: "
                     << response.status.message;
          return true;
        }
    
        ::cartographer::mapping::proto::InitialTrajectoryPose
            initial_trajectory_pose;
        initial_trajectory_pose.set_to_trajectory_id(
            request.relative_to_trajectory_id);
        *initial_trajectory_pose.mutable_relative_pose() =
            cartographer::transform::ToProto(pose);
        initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal(
            ::cartographer_ros::FromRos(ros::Time(0))));
        *trajectory_options.trajectory_builder_options
             .mutable_initial_trajectory_pose() = initial_trajectory_pose;
      }

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 3]  
**HandleStartTrajectory** 第一步，首先让他的map\_builder\_bridge去添加它的轨迹。  
![在这里插入图片描述][20210716222348875.png]然后呢，添加一外推器**AddExtrapolator**，再添加一个数据采样器**AddSensorSamplers**，再添加一个订阅器**LaunchSubscribers**

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 4]  
**Extrapolator**首先解释一下外推器，结合激光和其他传感器数据经过处理之后传入到cartographer进行处理之后，匹配，都是要经过时间。  
第二个就是所有的数据不可能每一帧都处理，不可能是实时的，也不是连续的，是离散的。  
但是我们再发布的pose需要发布处理结果或者建图的pose的时候，我们希望发布的频率高一点，发布的轨迹更顺滑一点，那么就需要怎么做呢，那就是当有反馈数据的时候，数据相对于现在来讲已经比较迟了。要根据之前的那么pose的那些状态去计算出来一个速度，然后再根据上一时刻的位置，使用这个速度去递推出来当前时刻的机器人的pose，这样更精准仪器。

**SensorSamplers** 就是频率很高，我们不希望那么高。那么就采样一下

**Subscribers** 订阅器，随时准备接收处理这个数据。

了解了这个大概，我们再看一下**HandleStartTrajectory**代码  
**node.cc**

首先是加载配置参数，代码如下：

std::tie(std::ignore, trajectory_options) = LoadOptions(
          request.configuration_directory, request.configuration_basename);

同时还给了一个初始的pose，比如说机器人同时支持多个机器人。他们这些机器人开始的位置又是不一样的。比如说第一个机器人在地图原点开始建图，第二个是在地图上某一个坐标开始建图。相当于这一条轨迹的初始**pose** \------**request.use\_initial\_pose**

request.use_initial_pose

这一步是检测选项options是够是合理的，如果不合理，就及时反馈给用户

if (!ValidateTrajectoryOptions(trajectory_options)) {
        
        response.status.message = "Invalid trajectory options.";
        LOG(ERROR) << response.status.message;
        response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
      } else if (!ValidateTopicNames(trajectory_options)) {
        
        response.status.message = "Topics are already used by another trajectory.";
        LOG(ERROR) << response.status.message;
        response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
      } else {
        
        response.status.message = "Success.";
        response.trajectory_id = AddTrajectory(trajectory_options);
        response.status.code = cartographer_ros_msgs::StatusCode::OK;
      }

其中倒数第二行代码中有一个AddTrajectory，这是最重要的，然后看他到底做了什么  
**node.cc**

int Node::AddTrajectory(const TrajectoryOptions& options) {
        
      const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
          expected_sensor_ids = ComputeExpectedSensorIds(options);
      const int trajectory_id =
          map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
      AddExtrapolator(trajectory_id, options);
      AddSensorSamplers(trajectory_id, options);
      LaunchSubscribers(options, trajectory_id);
      wall_timers_.push_back(node_handle_.createWallTimer(
          ::ros::WallDuration(kTopicMismatchCheckDelaySec),
          &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
      for (const auto& sensor_id : expected_sensor_ids) {
        
        subscribed_topics_.insert(sensor_id.id);
      }
      return trajectory_id;
    }

其中，最重要的是这个

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

是用node 里面的map\_builder\_bridge调用AddTrajectory()方法，添加一个外推器AddExtrapolator，添加一个数据采样器，AddSensorSamplers，然后再把订阅器给订阅起来。在下面的wall\_timers\_是一个定时器，这不重要。

然后我们再看**MapBuilderBridge**里面的**AddTrajectory**()

**MapBuilderBridge.cc**

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,
          [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);
          });
      LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
    
      // Make sure there is no trajectory with 'trajectory_id' yet.
      CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
      sensor_bridges_[trajectory_id] = absl::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));
      auto emplace_result =
          trajectory_options_.emplace(trajectory_id, trajectory_options);
      CHECK(emplace_result.second == true);
      return trajectory_id;
    }

那么我们为什么不在node里面直接放map\_builder 而是放了一个map\_builder\_bridge去调用map\_builder呢。因为在map\_builder\_bridge添加了一些小的逻辑，小的判断以及连接输出等。

在代码中，我们看到了sensor\_bridge,那么它是干什么的呢？

sensor_bridges_[trajectory_id] = absl::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));

就是把**ros**的消息转化成**cartographer**的消息，因为**cartographer**不是专门为**ros**设计的。所以它接收的消息不是直接就是ros类型。ros采集到的数据需要经过转换，然后再给到cartographer核心的模块去处理它。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 5]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 6]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 7]

map\_builder\_bridge里面添加了一个轨迹构建器，还创建了一个sensor\_bridge，一个数据类型传感器。  
然后我们再看一下在MapBuilderBridge的AddTrajectory()中比较关键的一步

map_builder_->GetTrajectoryBuilder(trajectory_id));

map\_builder 里面的轨迹构建器的地址获得到了。那么为什么要获取地址呢？因为sensor\_bridge是一个数据格式转化器，那么把转化后的数据送给谁呢？当然送给构建轨迹的模块。在map\_builder 里面的轨迹构建器。

那么具体的，MapBuilderBridge在添加轨迹构建器的时候，到底做了什么呢？  
**map\_builder\_bridge.cc**

首先看他传入的参数，首先是期望的传感器的id，expected\_sensor\_ids，还有其他的一些参数，还有一个比较重要的Lambda表达式。  
**map\_builder\_bridge.cc**

[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);
          });

这个**this**代表他能够访问**map\_builder\_bridge**里面所有的函数。  
这个匿名函数的形参都有trajectory\_id，time，local\_pose，range\_data\_in\_local，那么谁会给它实参呢，因为这个是在轨迹构建器里面，所以调用轨迹构建器的时候，会传给它。最终呢，这些实参会给到**OnLocalSlamResult**这个函数。这个函数是**map\_builder\_bridge**本身的。

每当**AddTrajectoryBuilder** 有反馈结果之后，就通过**OnLocalSlamResult** 反馈给**map\_builder\_bridge**。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 8]

**local**在**cartographer**中的意思就是前端的意思。**global**是整体，包括前端和位姿图。轨迹构建器是**CollatedTrakectoryBuilder**。  
在MapBuilder 被创建的时候，它的构造函数里就是实现了sensor\_collator\_。我们可以找到MapBuilder的构造函数看一下。  
**mapbuilder.cc**

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
        : options_(options), thread_pool_(options.num_background_threads()) {
        
      CHECK(options.use_trajectory_builder_2d() ^
            options.use_trajectory_builder_3d());
      if (options.use_trajectory_builder_2d()) {
        
        pose_graph_ = absl::make_unique<PoseGraph2D>(
            options_.pose_graph_options(),
            absl::make_unique<optimization::OptimizationProblem2D>(
                options_.pose_graph_options().optimization_problem_options()),
            &thread_pool_);
      }
      if (options.use_trajectory_builder_3d()) {
        
        pose_graph_ = absl::make_unique<PoseGraph3D>(
            options_.pose_graph_options(),
            absl::make_unique<optimization::OptimizationProblem3D>(
                options_.pose_graph_options().optimization_problem_options()),
            &thread_pool_);
      }
      if (options.collate_by_trajectory()) {
        
        sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
      } else {
        
        sensor_collator_ = absl::make_unique<sensor::Collator>();
      }
    }

我们在构架map\_builder的时候，就已经构建了sensor\_collator。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 9]

我们的pose\_graph也是在map\_builder构建的时候构建的，代码如下

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
        : options_(options), thread_pool_(options.num_background_threads()) {
        
    ......
      if (options.use_trajectory_builder_2d()) {
        
        pose_graph_ = absl::make_unique<PoseGraph2D>(
            options_.pose_graph_options(),
            absl::make_unique<optimization::OptimizationProblem2D>(
                options_.pose_graph_options().optimization_problem_options()),
            &thread_pool_);
      }
      if (options.use_trajectory_builder_3d()) {
        
        pose_graph_ = absl::make_unique<PoseGraph3D>(
            options_.pose_graph_options(),
            absl::make_unique<optimization::OptimizationProblem3D>(
                options_.pose_graph_options().optimization_problem_options()),
            &thread_pool_);
      }
      ......
    }

上面说的this的lamdba表达式传进了AddTrajectoryBuilder里面，然后给到了全局的轨迹构建器。

总结：首先是node.cc中调用Node,Node调用Map\_builder\_bridge增加一条轨迹，首先从MapBuilderBridge::AddTrajectory这个函数，通过调用map\_builder的AddTrajectory。其中map\_builder的AddTrajectory方法中再调用全局的轨迹构建器，在地图中构建出一条轨迹。CreateGlobalTrajectoryBuilder2D中捆绑了local\_slam\_result\_callback函数，可以把全局的轨迹反馈数据往外发布出去。

int MapBuilder::AddTrajectoryBuilder(
        const std::set<SensorId>& expected_sensor_ids,
        const proto::TrajectoryBuilderOptions& trajectory_options,
        LocalSlamResultCallback local_slam_result_callback) {
        
     ......
        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)));
      }
    ......
    }

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 10]  
sensor\_collator 是数据收集器，把所需要的数据都收集齐了往外分发，否则就不往外分发。

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 11]

分发完之后就开始轨迹构建。

激光SLAM核心的核心，一个前端，一个位姿图。

在构架轨迹构建器的时候，因为多态的问题，轨迹在定义的时候是一种类型，赋值的时候是另一种类型。  
**map\_builder.cc**

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)));
      }

我们可以先看一下**CollatedTrajectoryBuilder**的构造函数。  
**collated\_trajectory\_builder.cc**

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));
          });
    }

可以看到这里接收全局的轨迹构建器的时候名称变了，叫**wrapped\_trajectory\_builder**。实际上给它赋值，是一个全局的轨迹构建器。还有多态问题，这里形参定义的是**TrajectoryBuilderInterface**。一般定义的都父类，用父类定义一个成员函数。  
我们看一下调用它的上一层是在**map\_builder.cc**  
**map\_builder.cc**

CreateGlobalTrajectoryBuilder2D(
                std::move(local_trajectory_builder), trajectory_id,
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback, pose_graph_odometry_motion_filter)));

我们再看看**CreateGlobalTrajectoryBuilder2D**的返回值。  
**global\_trajectory\_builder.cc**

std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
        std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
        const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
        const TrajectoryBuilderInterface::LocalSlamResultCallback&
            local_slam_result_callback,
        const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
        
      return absl::make_unique<
          GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
          std::move(local_trajectory_builder), trajectory_id, pose_graph,
          local_slam_result_callback, pose_graph_odometry_motion_filter);
    }

定义的是**TrajectoryBuilderInterface**，返回的是**GlobalTrajectoryBuilder**，我们推测**GlobalTrajectoryBuilder**是**TrajectoryBuilderInterface**的子类。  
**global\_trajectory\_builder.cc**

> class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface

找到代码之后，发现果然是这个样子。

好了，那我们进行整理一下，首先是**node**里面的**service**接收用户的命令，开始调用**HandleStartTrajectory** ，让谁添加轨迹呢，是让**node**里面的**map\_build\_bridge**来添加轨迹。**map\_builder\_bridge** 实际上干活的是**map\_builder**.然后就是**map\_builder** 添加一条轨迹。然后还创建了一个数据转换器，这个数据转换器还获得了**map\_builder**的构建轨迹的地址，转化后的数据给到数据转换器**sensor\_bridge**。然后再看**map\_builder**添加地图构建器的时候都做了什么，首先是局部的轨迹构建器。**CollatedTrajectoryBuilder** 是具有数据收集功能。全局构建器等于前端+pose\_graph  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 12]  
**collator.cc**

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);
      }
    }

**trajectory\_id** 轨迹的id  
**expected\_sensor\_ids** 管控的传感器名字  
**Callback** : **lamdba**表达式

这个**AddTrajectory**是在哪里调用的呢，可以回溯到到**collated\_trajectory\_builder.cc** 的构造方法中，代码如下：  
**collated\_trajectory\_builder.cc**

可以看到

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

也是一个拉姆达表达式。

在Collator::AddTrajectory中，有一个queue\_.AddQueue，我们具体看看它做了什么  
**collator.cc**

**ordered\_multi\_queue.cc**

void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
        
      CHECK_EQ(queues_.count(queue_key), 0);
      queues_[queue_key].callback = std::move(callback);
    }

我们在传感器中找到对应的队列，队列对应的callback 赋值给它。我们再把callback分发出去，至于怎么送给轨迹构建器，就是通过这个函数送过去。  
然后我们再看callback 被送到了哪里。  
从**OrderedMultiQueue::AddQueue** 回溯到**Collator::AddTrajectory**，然后再回溯到**collated\_trajectory\_builder.cc**里面的**sensor\_collator**的**AddTrajectory()**。

到这里，我们局部的进行捋一捋。  
首先是用户启动一个**node** 节点，然后**node** 节点启动**map\_builder\_bridge**，并启动**services**。  
然后用户发送指令，说我想创建一个地图轨迹。然后在**Service**中调用**HandleStartTrajectory**()方法，在各方面传感器数据都比较合适的情况下，他就开始调用自己所在**node.cc**里面的**AddTrajectory**()方法。然后在它自己的**AddTrajectory()方法中，调用了map\_builder\_bridge\_的AddTrajectory()方法，map\_builder\_bridge\_自己的方法又将自己获得的相关参数传到map\_builder调用AddTrajectory()方法。同时将结果反馈给前端，也就是局部SLAM。然后map\_builder** 有他自己的\*\*AddTrajectory()\*\*方法，这次是一戳到底了。在map\_builder.h的头文件里面定义了轨迹构建器  
**map\_builder.h**

class MapBuilder : public MapBuilderInterface {
        
     public:
     ......
     private:
     ......
      std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
          trajectory_builders_;
    ......
    };

在**map\_builder**自己的\*\*AddTrajectory()\*\*方法中，trajectory\_builders中添加的数据类型如下所示，  
**map\_builder.cc**

要添加的数据类型是**CollatedTrajectoryBuilder**类型，而这个**CollatedTrajectoryBuilder**的构造方法又干了什么事情呢，我们可以回顾一下。  
**collated\_trajectory\_builder.cc**

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));
          });

可以看到调用了\*\*sensor\_collator\_->AddTrajectory()\*\*方法。后面的内容就是捋一捋上面的详细解释了。就不再展开讲了。

上面的代码，其中的拉姆达表达式，也就是**this**可以访问的是什么，**collated\_trajectory\_builder.cc**里面的都可以访问。其中有一个**HandleCollatedSensorData**函数，我们可以想到**OnlocalSlamResult()** 函数是一样的。**CollatedTrajectoryBuilder**这个类本身能获得分发之后的数据。经过数据的分发之后，我们这个类又获取到了分发之后的数据了。获得分发之后的数据之后，就可以分发给**trajectory\_builder**，也就是轨迹构建器。可以去做**scan-match**了。  
然后我们再看一下**HandleCollatedSensorData**这个是干什么的吧。  
**collated\_trajectory\_builder.cc**

void CollatedTrajectoryBuilder::HandleCollatedSensorData(
        const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        
      auto it = rate_timers_.find(sensor_id);
      if (it == rate_timers_.end()) {
        
        it = rate_timers_
                 .emplace(
                     std::piecewise_construct, std::forward_as_tuple(sensor_id),
                     std::forward_as_tuple(
                         common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
                 .first;
      }
      it->second.Pulse(data->GetTime());
    
      if (std::chrono::steady_clock::now() - last_logging_time_ >
          common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
        
        for (const auto& pair : rate_timers_) {
        
          LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
        }
        last_logging_time_ = std::chrono::steady_clock::now();
      }
    
      data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
    }

我们在**sensor\_collator**得到**data**之后，再将data传给**HandleCollatedSensorData**。  
其中该函数中最重要的是这一句。

data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());

那么我们有疑问了，怎么这个data还有成员函数呢？这里给大家解释一下，我们的数据收集器，在同步数据的时候，会把**data**封装到结构体里，结构体里有这个函数。让他把这个**data**数据给到这个轨迹构建器，这个轨迹构建器是谁呢，就是**wrapped\_trajectory\_builder\_.get()**,其实际上是**global\_trajectory\_builder**。之后的数据传送给这个轨迹构建器。我们可以看一下这个打开之后是怎么实现的。  
dispatchable.h

class Dispatchable : public Data {
        
     public:
      Dispatchable(const std::string &sensor_id, const DataType &data)
          : Data(sensor_id), data_(data) {
        }
    
      common::Time GetTime() const override {
         return data_.time; }
      void AddToTrajectoryBuilder(
          mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
        
        trajectory_builder->AddSensorData(sensor_id_, data_);
      }
      const DataType &data() const {
         return data_; }
    
     private:
      const DataType data_;
    };

可以看到`trajectory_builder->AddSensorData(sensor_id_, data_)` 添加数据去构建轨迹。

这样数据的流水线就搭建起来了。

[cartographer]: https://dqwang.blog.csdn.net/article/details/118539690
[cartographer 1]: https://dqwang.blog.csdn.net/article/details/118881867
[cartographer 2]: https://dqwang.blog.csdn.net/article/details/119023628
[cartographer 3]: https://dqwang.blog.csdn.net/article/details/119154163
[cartographer 4]: https://dqwang.blog.csdn.net/article/details/119845028
[cartographer 5]: https://dqwang.blog.csdn.net/article/details/120352115
[https_www.bilibili.com_video_BV1mK4y1u7MK_from_search_seid_1825406683392869132]: https://www.bilibili.com/video/BV1mK4y1u7MK?from=search&seid=1825406683392869132
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]: /images/20221005/7ba9ab2da41d4002bc310d42e5484fc6.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]: /images/20221005/3a4b2a968b7a475799f4ac7078161b39.png
[20210716220128351.png]: /images/20221005/587e243e31a54f23948b4c543fa159ab.png
[20210716220258819.png]: /images/20221005/fbbe1f4747224c82ab155f89e25e4013.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]: /images/20221005/c76fab06e9774cfe857dc95fd817e0eb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 3]: /images/20221005/4c899e6e0f784c23a413c00ec20e3415.png
[20210716222348875.png]: /images/20221005/d1fa9bf2226345e5a41063ee597133ee.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 4]: /images/20221005/0a7c2dff5f3e4e62921c78c81fd75e5f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 5]: /images/20221005/d53940a03be14e38bf885ffa8cd1de9d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 6]: /images/20221005/25c68d87bf0b4e5c8cbef25f2766a8df.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 7]: /images/20221005/45c31f7481344f86b091e36e84e88a04.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 8]: /images/20221005/c2e1a96d2ca5426bb496bdce4b4e554e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 9]: /images/20221005/211a5d8206174e0cbaee10843c827b00.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 10]: /images/20221005/6acc718144e34fed9d4f85883ecf0e65.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 11]: /images/20221005/e2cfbf68931f4193b0d72c33207d1de1.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 12]: /images/20221005/ba35e940c92640f098c6e70f8ebaab33.png