ROS的多传感器时间同步机制Time Synchronizer

2024-07-08 00:15| 来源: 网络整理| 查看: 265

1、存在的问题

多传感器数据融合的时候，由于各个传感器采集数据的频率的不同，例如odom 50Hz、Imu 100Hz、camera 25Hz，需要将传感器数据进行时间同步后才能进行融合。

2、融合的原理：

An example is the time synchronizer, which takes in messages of different types from multiple sources, and outputs them only if it has received a message on each of those sources with the same timestamp. 分别订阅不同的需要融合的传感器的主题，通过TimeSynchronizer 统一接收多个主题，只有在所有的topic都有相同的时间戳时，才会产生一个同步结果的回调函数，在回调函数里处理同步时间后的数据。

注意只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了，则整个回调函数永远无法触发回调。当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率，一般会很低。实际测试订阅两个需要同步的主题，odom 50Hz、imu 50Hz，而回调函数的频率只有24Hz左右。 3、具体实现方式一: 全局变量形式 : TimeSynchronizer

步骤:

message_filter ::subscriber 分别订阅不同的输入topic

TimeSynchronizer 定义时间同步器;

sync.registerCallback 同步回调

void callback(const ImageConstPtr&image, const CameraInfoConstPtr& cam_info) 带多消息的消息同步自定义回调函数

相应的API message_filters::TimeSynchronizer

//wiki参考demo http://wiki.ros.org/message_filters #include #include #include #include using namespace sensor_msgs; using namespace message_filters; void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info) //回调中包含多个消息 { // Solve all of perception here... } int main(int argc, char** argv) { ros::init(argc, argv, "vision_node"); ros::NodeHandle nh; message_filters::Subscriber image_sub(nh, "image", 1); // topic1 输入 message_filters::Subscriber info_sub(nh, "camera_info", 1); // topic2 输入 TimeSynchronizer sync(image_sub, info_sub, 10); // 同步 sync.registerCallback(boost::bind(&callback, _1, _2)); // 回调 ros::spin(); return 0; } //

参考连接:http://wiki.ros.org/message_filters

方式二: 类成员的形式 message_filters::Synchronizer

说明: 我用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式.

头文件 #include #include #include 定义消息同步机制 typedef message_filters::sync_policies::ApproximateTime slamSyncPolicy; 定义类成员变量 message_filters::Subscriber* odom_sub_ ; // topic1 输入 message_filters::Subscriber* img_sub_; // topic2 输入 message_filters::Synchronizer* sync_;

4.类构造函数中开辟空间new

odom_sub_ = new message_filters::Subscriber(ar_handle, "/odom", 1); img_sub_ = new message_filters::Subscriber(ar_handle, "/usb_cam/image_raw", 1); sync_ = new message_filters::Synchronizer(slamSyncPolicy(10), *odom_sub_, *img_sub_); sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2)); 类成员函数回调处理 void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg) //回调中包含多个消息 { //TODO fStampAll

【本文地址】

公司简介

联系我们