ros 同步 laserscan with camera

#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/CameraInfo.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <beginner_tutorials/myNum.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

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace geometry_msgs;

// global variable
int cnt=0;
int j_num_wall=0;
char szName[200] = {'\0'};

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

void callback(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::LaserScan::ConstPtr& msg_laser)
{

    //cerr << "I should record the pose: "<<cnt++ << endl;
    std::cerr<<" the max angle is :"<<msg_laser->angle_max <<std::endl;
    std::cerr<<" the min angle is :"<<msg_laser->angle_min <<std::endl;


    //std::cerr<<"the time stamp is :"<<msg->header<<std::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;
    }

    sprintf(szName,
            "/home/robot/work_space/lidar_image_bag/imagel%d.jpg",
            j_num_wall);


    cv::imwrite(szName, cv_ptr->image);
    j_num_wall++;



    int number_points=(msg_laser->angle_max-msg_laser->angle_min)/msg_laser->angle_increment;

    std::cout<<msg_laser->header.stamp<<" ";
    std::cout<<msg_laser->angle_min<<" "<<msg_laser->angle_max<<" "<<msg_laser->angle_increment<<" "<<1<<" "<<number_points<<" ";



    //std::cout<<"save an image!!!"<<std::endl;

    // int number_points=(msg_laser->angle_max-msg_laser->angle_min)/msg_laser->angle_increment;
    //std::cout<<"the number points  is :"<<number_points<<std::endl;
    for(int i=0;i<number_points;i++){

        if (msg_laser->ranges[i]>=10){
            std::cout<<0<<" ";
        }

        else if (msg_laser->ranges[i]<=0){
            std::cout<<0<<" ";
        }
        else{
            std::cout<<msg_laser->ranges[i]*1000<<" ";

        }

    }

    std::cout<<std::endl;


}



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

    ros::NodeHandle nh;

    message_filters::Subscriber<Image> info_sub(nh, "/camera_17082034/pg_17082034/image_raw", 2000);
    message_filters::Subscriber<sensor_msgs::LaserScan> pose_sub(nh, "/scan_filtered",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;
}