cartographer错误终结者 ordered_multi_queue.cc:155 Queue waiting for data

错误描述

错误提示的感应器数据可能是:
Queue waiting for data:(0, points2)
Queue waiting for data:(0, odom)
说明在提供给cartographter的传感器数据出现了问题，没有被成功解析。

原因分析

小编是使用自己编写的odom话题发布时，cartographer爆出了该错误。
于是设想传感器数据相关的tf或lua配置或数据本身存在错误。
于是逐个排查，最终发现了错误原因。

排查可能原因：tf错误

小编之前已处理过tf与lua配置的问题
参考上一篇博文

再一次确认tf是否出错，运行

@ubuntu:~/catkin_ws$ rosrun tf tf_echo /odom /base_link

Failure at 1623140162.687587918
Exception thrown:"odom" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:
Frame imu_link exists with parent base_link.
Frame tof_front_link exists with parent base_link.
Frame tof_left_link exists with parent base_link.

于是尝试添加发布odom到base_link的tf消息

 geometry_msgs::TransformStamped odom_trans;
 q_odom =  tf::createQuaternionMsgFromYaw(encoder.m_phi);

 odom_trans.header.stamp = ros::Time::now();
 odom_trans.header.frame_id = "odom";
 odom_trans.child_frame_id="base_link";
 odom_trans.transform.translation.x = encoder.m_x;
 odom_trans.transform.translation.y = encoder.m_y;
 odom_trans.transform.translation.z = 0.0;
 odom_trans.transform.rotation = q_odom;
 // send  transform
 transformBroadcaster.sendTransform(odom_trans);

发布后在查看 rosrun tf tf_echo /odom /base_link

@ubuntu:~/catkin_ws$ rostopic echo /tf

---
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1623305659
        nsecs: 667076891
      frame_id: "odom"
    child_frame_id: "base_link"
    transform: 
      translation: 
        x: 0.115380195587
        y: -0.00152981112718
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: -0.0795900107579
        w: 0.996827683297
---

或者

@ubuntu:~/catkin_ws$ rosrun tf tf_echo /odom /base_link

At time 1623305628.527
- Translation: [0.169, -0.002, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.008, 1.000]
            in RPY (radian) [0.000, 0.000, -0.015]
            in RPY (degree) [0.000, 0.000, -0.860]

但是仍然无效，还是不断的爆Queue waiting for data错误，内心一顿挠。。
由此可能不是tf相关错误，看看别的方面原因

错误原因：数据本身存在错误

排查问题需要耐心
直接在源码搜索ordered_multi_queue.cc:155找到Queue waiting for data
分析出错的逻辑

void OrderedMultiQueue::CannotMakeProgress(const QueueKey& queue_key) {
  blocker_ = queue_key;
  for (auto& entry : queues_) {
    if (entry.second.queue.Size() > kMaxQueueSize) {
      LOG_EVERY_N(WARNING, 60) << "Queue waiting for data: " << queue_key;
      return;
    }
  }
}

搜索该方法
因为[ INFO] [1623377621.330938742, 1623305586.971584721]: I0610 19:13:41.000000 81479 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '637589023869974173'.已经在控制台打出，所以重点看GetCommonStartTime()函数后面的部分

void OrderedMultiQueue::Dispatch() {
  while (true) {

		... ...

    // 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.
    const common::Time common_start_time =
        GetCommonStartTime(next_queue_key.trajectory_id);

    if (next_data->GetTime() >= common_start_time) {// 本次错误发生的条件是时间戳问题
      // 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) {
      if (!next_queue->finished) {
        // We cannot decide whether to drop or dispatch this yet.
        CannotMakeProgress(next_queue_key);
        return;
      }
      last_dispatched_time_ = next_data->GetTime();
      next_queue->callback(next_queue->queue.Pop());
    } else {
      // 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));
      }
    }
  }
}

通过代码发现是本次错误发生的条件是时间戳问题，赶紧查看传感器数据本身时间戳，
不看不要紧，一看想剁了自己手，手残党没有把时间戳赋值，拷贝IMU话题代码时候没改时间戳赋值，等于IMU话题两次赋值时间戳。

正确赋值时间戳后，正常运行了。
很多bug其实都是自身代码的原因，良好的编程习惯和错误排查思维，有助于提高工作效率。