cartographer之OrderedMultiQueue::Dispatch()

Posted 2022-05-21 COCO_PEAK_NOODLE

先来看代码

void OrderedMultiQueue::Dispatch() 
  while (true) 
    const Data* next_data = nullptr;
    Queue* next_queue = nullptr;
    QueueKey next_queue_key;

    // check data is coming or not
    //LOG(INFO) << "okagv enter forloop";
    // must satisfy the condition that all type data coming
    for (auto it = queues_.begin(); it != queues_.end();) 

      //okagv
      if(odom_data_missing == true 
      && it->first.sensor_id == "odom")
        it++;
        continue;
      

      const auto* data = it->second.queue.Peek<Data>();
      if (data == nullptr) 
        if (it->second.finished) 
          queues_.erase(it++);
          continue;
        
      
        CannotMakeProgress(it->first);      
        return;
      

      if (next_data == nullptr || data->GetTime() < next_data->GetTime()) 
        next_data = data; // current_data
        next_queue = &it->second; //data and function
        next_queue_key = it->first;
      
      CHECK_LE(last_dispatched_time_, next_data->GetTime())
         << "Non-sorted data added to queue: '" << it->first << "'";
      ++it;
    


    if (next_data == nullptr) 
      CHECK(queues_.empty());
      return;
    

    // If we haven't dispatched any data for this trajectory yet, fast forward
    // all queues of this trajectory until a common start time has been reached.
    //LOG(INFO) << "coming here 0";
    data_initial = true;
    const common::Time common_start_time =
        GetCommonStartTime(next_queue_key.trajectory_id);

    //LOG(INFO) << "common_start_time " << common_start_time;
    //okagv
    /*
    if(next_queue_key.sensor_id == "odom")
    
       LOG(INFO) << "next_queue_key " << next_queue_key.sensor_id;
    */

    if (next_data->GetTime() >= common_start_time) 

      //LOG(INFO) << "coming here 1";
      // Happy case, we are beyond the 'common_start_time' already.
      last_dispatched_time_ = next_data->GetTime();
      next_queue->callback(next_queue->queue.Pop());
     else if (next_queue->queue.Size() < 2) 
      //LOG(INFO) << "coming here 2";
      if (!next_queue->finished) 
        // We cannot decide whether to drop or dispatch this yet.
        CannotMakeProgress(next_queue_key);
        
        //LOG(INFO) << "okagv CannotMakeProgress 1";
        //okagv
        //continue;
        //cartographer
        return;

      
      last_dispatched_time_ = next_data->GetTime();
      next_queue->callback(next_queue->queue.Pop());
     else 
      //LOG(INFO) << "coming here 3";
      // We take a peek at the time after next data. If it also is not beyond
      // 'common_start_time' we drop 'next_data', otherwise we just found the
      // first packet to dispatch from this queue.
      std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
      if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) 
        last_dispatched_time_ = next_data->GetTime();
        next_queue->callback(std::move(next_data_owner));
      
    
  

这个函数是做什么的呢？（这里有篇文章直接写了出来）就是在dispatch中while循环遍历所有queuekey,直至所有传感器队列都有值,然后分发最早的数据

是什么意思呢？
比如我们有imu、odom、scan三种数据，那三种数据传进来的时候肯定有先后顺序的，那要怎么安排这个先后顺序呢？就是按传进来的时间进行排序，既然要排序，肯定要比较，怎么比较呢？

我们有三个容器，分别是装imu、odom、scan的数据，这里作者的策略是当检测到三个容器里面都有值的时候，比较他们之间最早的数据，然后发布出去。

这一段是检测三个容器里面是否都有值，有一个没有就返回，如果三个容器里都有值，就查找最早的数据,放到next_data中

    // must satisfy the condition that all type data coming
    for (auto it = queues_.begin(); it != queues_.end();) 

      //okagv
      if(odom_data_missing == true 
      && it->first.sensor_id == "odom")
        it++;
        continue;
      

      const auto* data = it->second.queue.Peek<Data>();
      if (data == nullptr) 
        if (it->second.finished) 
          queues_.erase(it++);
          continue;
        
      
        CannotMakeProgress(it->first);      
        return;
      

      if (next_data == nullptr || data->GetTime() < next_data->GetTime()) 
        next_data = data; // current_data
        next_queue = &it->second; //data and function
        next_queue_key = it->first;
      
      CHECK_LE(last_dispatched_time_, next_data->GetTime())
         << "Non-sorted data added to queue: '" << it->first << "'";
      ++it;
    

后面就是发布出去就可以了