ROS多Topic接收的时间同步
ROS多Topic接收的时间同步。网上一搜教程挺多的,但是都是官网上的两个Topic的同步,说明文档说C++支持9个通道,如何同步两个以上的Topic我也摸索了很久,记录一下。
代码
这里采用的是近似同步的方式。
1)两个Topic的代码
void packetCallback(const joint::packet::ConstPtr &packet,const joint::packet::ConstPtr &packet2) {
t=packet->t;
cout << "time: " <<t << endl;
cout<<"time2:"<<packet2->t<<endl;
}
main(int argc, char **argv) {
ros::init(argc, argv, "joint");
ros::NodeHandle nh;
//要订阅的2个Topic
message_filters::Subscriber<joint::packet> uav1_sub(nh, "/uav1_packet", 1);
message_filters::Subscriber<joint::packet> uav2_sub(nh, "/uav2_packet", 1);
typedef sync_policies::ApproximateTime<joint::packet,joint::packet> MySyncPolicy;
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), uav1_sub,uav2_sub); //queue size=10
sync.registerCallback(boost::bind(&packetCallback ,_1, _2));
ros::spin();
return 0;
}
2)三个Topic的代码
void packetCallback(const joint::packet::ConstPtr &packet,const joint::packet::ConstPtr &packet2,const joint::packet::ConstPtr &packet3) {
t=packet->t;
cout << "time: " <<t << endl;
cout<<"time2:"<<packet2->t<<endl;
}
main(int argc, char **argv) {
ros::init(argc, argv, "joint");
ros::NodeHandle nh;
//要订阅的三个Topic
message_filters::Subscriber<joint::packet> uav1_sub(nh, "/uav1_packet", 1);
message_filters::Subscriber<joint::packet> uav2_sub(nh, "/uav2_packet", 1);
message_filters::Subscriber<joint::packet> car_sub(nh, "/car_packet", 1);
typedef sync_policies::ApproximateTime<joint::packet,joint::packet,joint::packet> MySyncPolicy;
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), uav1_sub,uav2_sub,car_sub); //queue size=10
sync.registerCallback(boost::bind(&packetCallback ,_1, _2,_3));
ros::spin();
return 0;
}
解决方案:
要改以下几个地方,更多的Topic以此类推:
1.添加订阅一个Topic,
message_filters::Subscriber<joint::packet> car_sub(nh, "/car_packet", 1);
2. 添加一个消息格式
typedef sync_policies::ApproximateTime<joint::packet,joint::packet,joint::packet> MySyncPolicy;
3. 添加新加的订阅者
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), uav1_sub,uav2_sub,car_sub);
4.加一个_3
sync.registerCallback(boost::bind(&packetCallback ,_1, _2,_3));
5.回调函数加一个消息指针
void packetCallback(const joint::packet::ConstPtr &packet,const joint::packet::ConstPtr &packet2,const joint::packet::ConstPtr &packet3) {}