cartographer开始轨迹时的处理

首先是MapBuilder的声明与构造

node_main.cc

auto map_builder = 
cartographer::mapping::CreateMapBuilder(node_options.map_builder_opt
ions);
Node node(node_options, std::move(map_builder), &tf_buffer, 
FLAGS_collect_metrics);

 CreateMapBuilder函数

std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

函数使用 absl::make_unique 来创建一个 MapBuilder 对象，并将 options 作为参数传递给 MapBuilder 的构造函数，然后将其封装在一个 std::unique_ptr<MapBuilderInterface> 中，并返回该指针。

在Node的构造函数中

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

 将map_builder的所有权给转移

map_builder_bridge_是MapBuilderBridge类

再看MapBuilderBridge类的构造

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的所有权在给转移

MapBuilder的构造在上文的工厂函数就创建了

轨迹开始的处理

node_main.cc的StartTrajectoryWithDefaultTopics函数

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

 Node的AddTrajectory函数

int Node::AddTrajectory(const TrajectoryOptions& options) {

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

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);

  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);

  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);

  // 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
  // 检查设置的topic名字是否在ros中存在, 不存在则报错
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }

  return trajectory_id;
}

map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

 MapBuilderBridge类的AddTrajectory函数、

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      // lambda表达式 local_slam_result_callback_
      [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>) {
        // 保存local slam 的结果数据 5个参数实际只用了4个
        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);
  // Step: 2 为这个新轨迹 添加一个SensorBridge
  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)); // CollatedTrajectoryBuilder
  
  // Step: 3 保存轨迹的参数配置
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

 调用了MapBuilder::AddTrajectoryBuilder函数

int MapBuilder::AddTrajectoryBuilder(
 const std::set<SensorId>& expected_sensor_ids,
 const proto::TrajectoryBuilderOptions& trajectory_options,
 LocalSlamResultCallback local_slam_result_callback) {
 // CollatedTrajectoryBuilder 初始化
 
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryB
uilder>(
 trajectory_options, 
 sensor_collator_.get(), // sensor::Collator
 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)));
 return trajectory_id;
}
// 返回指向 CollatedTrajectoryBuilder 的指针
mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
 int trajectory_id) const override {
 return trajectory_builders_.at(trajectory_id).get(); 
}