错误描述
错误提示的感应器数据可能是:
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其实都是自身代码的原因,良好的编程习惯和错误排查思维,有助于提高工作效率。