ROS多Topic接收的时间同步

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) {}