Cartographer源码阅读(4)：Node和MapBuilder对象2

　　MapBuilder的成员变量sensor::Collator sensor_collator_;

　　再次阅读MapBuilder::AddTrajectoryBuilder方法。首先构造了mapping::GlobalTrajectoryBuilder实例，接着作为参数构造了CollatedTrajectoryBuilder实例。

trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder<mapping_2d::LocalTrajectoryBuilder,mapping_2d::proto::LocalTrajectoryBuilderOptions,mapping_2d::PoseGraph>>
(trajectory_options.trajectory_builder_2d_options(),trajectory_id, pose_graph_2d_.get(),local_slam_result_callback)
)
);

　　这里sensor_collator_作为参数传入，参与CollatedTrajectoryBuilder构造。查看构造函数：

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::Collator* const sensor_collator, const int trajectory_id, const std::unordered_set<std::string>& expected_sensor_ids,   std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
    : sensor_collator_(sensor_collator)
    , trajectory_id_(trajectory_id)
    , wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder))
    , last_logging_time_(std::chrono::steady_clock::now()) 
{
     sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_ids,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
      {
        HandleCollatedSensorData(sensor_id, std::move(data));
      }
      );
}

　　这里是回调函数，std::unique_ptr是表示参数为智能指针。

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

　　（1）查看sensor::Collator的AddTrajectory方法：

void Collator::AddTrajectory( const int trajectory_id, const std::unordered_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);
  }
}

　　for (const auto& sensor_id : expected_sensor_ids)用到了C++11的auto新特性。

　　（2）查看HandleCollatedSensorData方法。调用了data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());这里wrapped_trajectory_builder_是在CollatedTrajectoryBuilder构造函数中赋值的。为GlobalTrajectoryBuilder对象。因而查看sensor::Data的AddToTrajectoryBuilder() 方法。

　　virtual void AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;是sensor::Data类的一个虚方法。内部执行了trajectory_builder->AddSensorData(sensor_id_, data_);

最后调用的是GlobalTrajectoryBuilder对象的AddSensorData(xx)方法。

 1 void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
 2 {
 3   auto it = rate_timers_.find(sensor_id);
 4   if (it == rate_timers_.end())
 5   {
 6       it = rate_timers_ .emplace(
 7                  std::piecewise_construct, std::forward_as_tuple(sensor_id),
 8                  std::forward_as_tuple(common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first;
 9   }
10   it->second.Pulse(data->GetTime());
11 
12   if (std::chrono::steady_clock::now() - last_logging_time_ >
13       common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) 
14   {
15        for (const auto& pair : rate_timers_) 
16       {
17         LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
18       }
19       last_logging_time_ = std::chrono::steady_clock::now();
20   }
21 
22    data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
23   }
24 
25 } 

CollatedTrajectoryBuilder::HandleCollatedSensorData