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其实都是自身代码的原因,良好的编程习惯和错误排查思维,有助于提高工作效率。

  • 2
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 9
    评论
FAILED: CMakeFiles/cartographer.transform.timestamped_transform_test.dir/cartographer/transform/timestamped_transform_test.cc.o /usr/bin/c++ -DBOOST_ALL_NO_LIB -DBOOST_IOSTREAMS_DYN_LINK -DGFLAGS_IS_A_DLL=0 -I../cartographer -I. -I../ -isystem /usr/include/eigen3 -isystem /usr/include/lua5.2 -O3 -DNDEBUG -pthread -fPIC -Wall -Wpedantic -Werror=format-security -Werror=missing-braces -Werror=reorder -Werror=return-type -Werror=switch -Werror=uninitialized -O3 -DNDEBUG -pthread -fPIC -Wall -Wpedantic -Werror=format-security -Werror=missing-braces -Werror=reorder -Werror=return-type -Werror=switch -Werror=uninitialized -O3 -DNDEBUG -std=gnu++11 -MD -MT CMakeFiles/cartographer.transform.timestamped_transform_test.dir/cartographer/transform/timestamped_transform_test.cc.o -MF CMakeFiles/cartographer.transform.timestamped_transform_test.dir/cartographer/transform/timestamped_transform_test.cc.o.d -o CMakeFiles/cartographer.transform.timestamped_transform_test.dir/cartographer/transform/timestamped_transform_test.cc.o -c ../cartographer/transform/timestamped_transform_test.cc In file included from ../cartographer/transform/timestamped_transform_test.cc:17: ../cartographer/transform/timestamped_transform.h:21:10: fatal error: cartographer/transform/proto/timestamped_transform.pb.h: No such file or directory 21 | #include "cartographer/transform/proto/timestamped_transform.pb.h" | ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ compilation terminated. [44/380] Building CXX object CMakeFiles/cartographer.sensor.internal.voxel_filter_test.dir/cartographer/sensor/internal/voxel_filter_test.cc.o
07-23
评论 9
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值