之前做了深度相机kinect v2.0通过鼠标点击图像即可确定物体的距离信息和坐标信息。分析其中的源码,发现,它是将点云图和RGB图进行了时间的同步,点云提供了坐标信息xyz,你点击图像他会匹配到点云图然后通过解算输出坐标和距离参见我上次博客https://blog.csdn.net/qq_40464599/article/details/107557651。
所以,在这里我所感兴趣的是时间同步问题,之前做传感器融合的自动驾驶算法时,基于卡尔曼滤波融合激光雷达和毫米波雷达两种传感器信息,实际上使用的是异步融合,所谓异步就是这一次使用激光雷达作为卡尔曼的测量更新,下一次就会使用毫米波雷达进行测量更新,如此交替反复。同步融合显然要比异步融合好得多,但是难点如何将两传感器时间对其即时间同步是一个很大的问题。
所以ROS中的message_filters就是来解决这个问题。
本文主要参考了http://wiki.ros.org/message_filters。这里上面写的比较清楚。我这里就是利用IMU和GPS验证了时间不同步和同步两种情况。不同步时候,不会有输出,同步才会有输出。
时间同步的输出如下图所示:源码还是参见我的github地址:https://github.com/JackJu-HIT/learning_messages_filters
首先,清楚一个概念,ros中同步是利用时间戳同步的,这个时间戳就是你的系统时间,传感器发布的信息都会带有时间戳,message_files就是利用时间戳去确定是否同步,如果同步就会有回调函数,不同步就不调用回调函数,你的信息融合算法也是在回调函数里面去写。
int main(int argc, char** argv)
{
ros::init(argc, argv, "vision_node");
ros::NodeHandle nh;
// message_filters::Subscriber<Image> image_sub(nh, "image", 1);
message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh, "/imu", 1);
//message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub(nh, "/fix", 1);
typedef sync_policies::ExactTime<Imu, NavSatFix> MySyncPolicy;
// ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), imu_sub, gps_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
return 0;
}
主函数包括节点的初始化,这里会订阅两个话题,/imu和 /fix。
void callback(const sensor_msgs::Imu::ConstPtr& imu, const sensor_msgs::NavSatFix::ConstPtr& gps)
{
// Solve all of perception here...?>
cout<<"传感器数据同步..."<<endl;
cout<<"imu:"<<imu->linear_acceleration.x<<endl;
cout<<"gps:"<<gps->latitude<<endl;
}
这个就是回调函数,时间同步才能调用它。
这个就是时间精确同步的用法,还有一个是时间近似,如果感兴趣可以去官网,个人认为没有比官网更好的教程了。
20200725
Jack Ju于研究室