ros 同步velodyne camera 两个topic

#include <Eigen/Dense>
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>

#include <image_transport/image_transport.h>   //image_transport
#include <cv_bridge/cv_bridge.h>              //cv_bridge
#include <sensor_msgs/image_encodings.h>    //图像编码格式
#include <opencv2/imgproc/imgproc.hpp>      //图像处理
#include <opencv2/highgui/highgui.hpp>       //opencv GUI

#include <sensor_msgs/PointField.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/PCLImage.h>
#include <pcl/point_cloud.h>
#include <pcl/exceptions.h>
#include <pcl/console/print.h>
#include <pcl/PCLPointField.h>
#include <pcl/PCLPointCloud2.h>

#include <iostream>
#include <ctime>
#include <cstdio>
#include <cstdlib>

#include <Eigen/Dense>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>


using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace geometry_msgs;
using namespace cv;
ros::Publisher pcl_pub;
// global variable
int cnt=0;
int j_num_wall=0;
char szName[200] = {'\0'};
char szName_pcd[200] = {'\0'};

typedef message_filters::sync_policies::ApproximateTime<Image,sensor_msgs::PointCloud2> sync_policy_classification;


void callback(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{

    cerr << "I should record the pose: "<<cnt++ << endl;

    cv_bridge::CvImagePtr cv_ptr;  //申明一个CvImagePtr
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*cloud_msg,pcl_pc2);

    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);


    pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);

    std::cout<<"the size of the point cloud is :"<<temp_cloud->points.size ()<<std::endl;

//    pcl::PCDWriter writer;
//    writer.write ("red2.pcd", *rgb_cloud);

//
//    sprintf(szName,
//          "/home/robot/work_space/chy_data/velodyne_camera/img/velo_image%d.jpg",
//            j_num_wall);
//
////    sprintf(szName_pcd,
////            "/home/robot/work_space/chy_data/velodyne_camera/pcd/velo_pcd%d.pcd",
////            j_num_wall);
////
////
////    pcl::PCDWriter writer;
////    writer.write (szName_pcd, *temp_cloud);
//
//    cv::imwrite(szName, cv_ptr->image);
//    j_num_wall++;


}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "ttomchy");

    ros::NodeHandle nh;
    pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/chy/pcl_output",1000, true);

    message_filters::Subscriber<Image> info_sub(nh, "/camera_17023549/pg_17023549/image_raw", 2000);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pose_sub(nh, "/left_velodyne/velodyne_points",2000);
    message_filters::Synchronizer<sync_policy_classification> sync(sync_policy_classification(2000), info_sub, pose_sub);
    //TimeSynchronizer<CameraInfo, PoseStamped> sync(info_sub, pose_sub, 10);
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();

    return 0;
}