时间同步及在回调函数中发布ROS消息

1、五种时间同步方法（粗略同步）

本文介绍的是时间粗略同步，时间精准同步的函数是：ExactTime，将ApproximateTime换成ExactTime即可；

注意:
只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了，则整个回调函数永远无法触发回调。
当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率，一般会很低。实际测试订阅两个需要同步的主题，odom 50Hz、imu 50Hz，而回调函数的频率只有24Hz左右(博主本人未实测，这是其他博主实测的结果)

//头文件
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>              //法1的头文件
#include <message_filters/synchronizer.h>                   //法2、法3、法4、法5的头文件
#include <message_filters/sync_policies/approximate_time.h> //法2、法3、法4、法5的头文件
#include "chuangxin_technology_driver/Objects.h"
#include "perception_msgs/objects.h"
using namespace message_filters;
using namespace std;

//法1
void callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg,const perception_msgs::objectsConstPtr& lidarmsg){
    cout << "时间同步后毫米波雷达时间戳是:  " << radarmsg->header.stamp << endl;
    cout << "时间同步后激光雷达时间戳是:    " << lidarmsg->header.stamp << endl;
}
int main( int argc, char** argv ){
    ros::init(argc, argv, "Node");
    ros::NodeHandle nh;
    message_filters::Subscriber<chuangxin_technology_driver::Objects> p1_sub(nh,"/radar_data", 1);
    message_filters::Subscriber<perception_msgs::objects> p2_sub(nh,"/lidar_perception/objects", 1);
    TimeSynchronizer<chuangxin_technology_driver::Objects,perception_msgs::objects> sync(p1_sub,p2_sub,10);
    sync.registerCallback(boost::bind(&callback,_1,_2));
    ros::spin();
    return 0;
}
//好像法1无法输出结果，不知道是什么问题，但是官网也有这种方法，也许这是一个时间精确同步，因为输出信号的时间戳不够一致，所以无信号输出；

//法2
void callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg,const perception_msgs::objectsConstPtr& lidarmsg){
    cout << "时间同步后毫米波雷达时间戳是:  " << radarmsg->header.stamp << endl;
    cout << "时间同步后激光雷达时间戳是:    " << lidarmsg->header.stamp << endl;
}
int main( int argc, char** argv ){
    ros::init(argc, argv, "Node");
    ros::NodeHandle nh;
    message_filters::Subscriber<chuangxin_technology_driver::Objects> p1_sub(nh,"/radar_data", 1);
    message_filters::Subscriber<perception_msgs::objects> p2_sub(nh,"/lidar_perception/objects", 1);
    typedef message_filters::sync_policies::ApproximateTime<chuangxin_technology_driver::Objects,perception_msgs::objects> mysync;
    Synchronizer<mysync> sync(mysync(10),p1_sub,p2_sub);
    sync.registerCallback(boost::bind(&callback,_1,_2));
    ros::spin();
    return 0;
}

//法3 
class SubscribeAndPublish{
  public:  
    SubscribeAndPublish(ros::NodeHandle& _nh);
    void callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg, const perception_msgs::objectsConstPtr& lidarmsg);
  private:
    ros::NodeHandle nh;
};

SubscribeAndPublish::SubscribeAndPublish(ros::NodeHandle& _nh){
    nh=_nh;
    message_filters::Subscriber<chuangxin_technology_driver::Objects> p1_sub(nh,"/radar_data", 1);
    message_filters::Subscriber<perception_msgs::objects> p2_sub(nh,"/lidar_perception/objects", 1);
    typedef message_filters::sync_policies::ApproximateTime<chuangxin_technology_driver::Objects,perception_msgs::objects> mysync;
    Synchronizer<mysync> sync(mysync(10),p1_sub,p2_sub);
    sync.registerCallback(boost::bind(&SubscribeAndPublish::callback,this,_1,_2));
    ros::spin();
}

void SubscribeAndPublish::callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg,const perception_msgs::objectsConstPtr& lidarmsg){
    cout << "时间同步后毫米波雷达时间戳是:  " << radarmsg->header.stamp << endl;
    cout << "时间同步后激光雷达时间戳是:    " << lidarmsg->header.stamp << endl;
}

int main( int argc, char** argv ){
    ros::init(argc, argv, "Node");
    ros::NodeHandle n;
    SubscribeAndPublish s(n);
    return 0;
}

//法4 
class SubscribeAndPublish{
  public:  
    SubscribeAndPublish(ros::NodeHandle& _nh);
    void callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg, const perception_msgs::objectsConstPtr& lidarmsg);
  private:
    ros::NodeHandle nh;
    message_filters::Subscriber<chuangxin_technology_driver::Objects> *p1_sub;
    message_filters::Subscriber<perception_msgs::objects> *p2_sub;
};

SubscribeAndPublish::SubscribeAndPublish(ros::NodeHandle& _nh){
    nh=_nh;
    p1_sub=new message_filters::Subscriber<chuangxin_technology_driver::Objects>(nh,"/radar_data", 1);
    p2_sub=new message_filters::Subscriber<perception_msgs::objects>(nh,"/lidar_perception/objects", 1);

    typedef message_filters::sync_policies::ApproximateTime<chuangxin_technology_driver::Objects,perception_msgs::objects> mysync;
    Synchronizer<mysync> *sync=new Synchronizer<mysync>(mysync(10),*p1_sub,*p2_sub);
    sync->registerCallback(boost::bind(&SubscribeAndPublish::callback,this,_1,_2));
    ros::spin();
}

void SubscribeAndPublish::callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg,const perception_msgs::objectsConstPtr& lidarmsg){
    cout << "时间同步后毫米波雷达时间戳是:  " << radarmsg->header.stamp << endl;
    cout << "时间同步后激光雷达时间戳是:    " << lidarmsg->header.stamp << endl;
}

int main( int argc, char** argv ){
    ros::init(argc, argv, "Node");
    ros::NodeHandle n;
    SubscribeAndPublish s(n);
    return 0;
}

//法5 
//将mysync写作全局变量
typedef message_filters::sync_policies::ApproximateTime<chuangxin_technology_driver::Objects,perception_msgs::objects> mysync;

class SubscribeAndPublish{
  public:  
    SubscribeAndPublish(ros::NodeHandle& _nh);
    void callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg, const perception_msgs::objectsConstPtr& lidarmsg);
  private:
    ros::NodeHandle nh;
    message_filters::Subscriber<chuangxin_technology_driver::Objects> *p1_sub;
    message_filters::Subscriber<perception_msgs::objects> *p2_sub;
    Synchronizer<mysync> *sync;
};

SubscribeAndPublish::SubscribeAndPublish(ros::NodeHandle& _nh){
    nh=_nh;
    p1_sub=new message_filters::Subscriber<chuangxin_technology_driver::Objects>(nh,"/radar_data", 1);
    p2_sub=new message_filters::Subscriber<perception_msgs::objects>(nh,"/lidar_perception/objects", 1);
    sync=new Synchronizer<mysync>(mysync(10),*p1_sub,*p2_sub);

    sync->registerCallback(boost::bind(&SubscribeAndPublish::callback,this,_1,_2));
    ros::spin();
}

void SubscribeAndPublish::callback(const chuangxin_technology_driver::ObjectsConstPtr& radarmsg,const perception_msgs::objectsConstPtr& lidarmsg){
    cout << "时间同步后毫米波雷达时间戳是:  " << radarmsg->header.stamp << endl;
    cout << "时间同步后激光雷达时间戳是:    " << lidarmsg->header.stamp << endl;
}

int main( int argc, char** argv ){
    ros::init(argc, argv, "Node");
    ros::NodeHandle n;
    SubscribeAndPublish s(n);
    return 0;
}

非类内实现时，registerCallback(boost::bind(&callback,_1,_2));不需要添加this；
类内实现时，registerCallback(boost::bind(&SubscribeAndPublish::callback,this,_1,_2));需要添加this；在类内使用this，是因为我们把sub定义在了构造函数里，在实例化对象时，this是指向对象的指针；

2. 发布ros消息

情况1：在main函数中发布

int main(int argc, char **argv){
    ros::init(argc, argv, "talker");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<话题类型>("topix名", 1);
    ros::Rate loop_rate(2.0);//一秒发几帧
    
    while (ros::ok()) {
    	/*
    	***
    	*/
        pub.publish(***);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

情况2：在callback函数中发布
这种情况在类内实现最好，借用1-方法3的类SubscribeAndPublish；

void SubscribeAndPublish::callback(const A::B::ConstPtr &radarmsg ,const C::D::ConstPtr &lidarmsg ){
	/*
	***
	*/
    pub.publish(new_msg); //nh在private中定义，在构造函数中实现的优势就体现出来了；
    //不需要其他的语句，如：while(ros::ok());ros::spinOnce();loop_rate.sleep();等
}