cartographer odom中途添加进来导致last_dispatched_time_时间检测失败

Posted 2022-09-03 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")

        //when odom data finished
        if(it->second.finished)
        
          //LOG(INFO) << "queues_ erase odom";
          queues_.erase(it);
        
        it++;
        continue;
      

      const auto* data = it->second.queue.Peek<Data>();
      if (data == nullptr) 
        if (it->second.finished) 
          //LOG(INFO) << "queues_ erase " << it->first.sensor_id;
          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;
      

      //okagv 
      //modify because odom coming in middle time;
      if(last_dispatched_time_ >= next_data->GetTime())
      
        LOG(INFO) << next_data->GetSensorId() 
                  << " time < last_dispatched_time_";
        ++it;
        continue;
      

      //CHECK_LE(last_dispatched_time_, next_data->GetTime())
      //   << "Non-sorted data added to queue: '" << it->first << "'";
      ++it;
    

    if (next_data == nullptr) 
      // okagv
      
      CHECK(queues_.empty());
      LOG(WARNING) << "next_data is nullptr, queues_ is 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));
      
    
  

修改处

      //modify because odom coming in middle time;
      if(last_dispatched_time_ >= next_data->GetTime())
      
        LOG(INFO) << next_data->GetSensorId() 
                  << " time < last_dispatched_time_";
        ++it;
        continue;
      

      //CHECK_LE(last_dispatched_time_, next_data->GetTime())
      //   << "Non-sorted data added to queue: '" << it->first << "'";