ros下 同步保存双目数据 raw image

使用message_filters 以时间戳过滤同步保存双目数据 

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>

图像编码类型参考：http://docs.ros.org/jade/api/sensor_msgs/html/namespacesensor__msgs_1_1image__encodings.html

完整代码：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include "sensor_msgs/image_encodings.h"     
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <iostream>

using namespace cv;
using namespace std;


int rate  = 30;
//Mat globalImage(Size(640,480),CV_8UC3);
int chess_num =0;

void callback_raw_image(
    const sensor_msgs::ImageConstPtr &img_l_msg,
    const sensor_msgs::ImageConstPtr &img_r_msg)
{
    ROS_INFO("save images with header: %f", img_l_msg->header.stamp.toSec());
    
    
    try
    {
      cv_bridge::CvImagePtr img_l_ptr = cv_bridge::toCvCopy(img_l_msg, sensor_msgs::image_encodings::BGR8);
      cv_bridge::CvImagePtr img_r_ptr = cv_bridge::toCvCopy(img_r_msg, sensor_msgs::image_encodings::BGR8);
      
      cv::Mat img_ll=img_l_ptr->image;
      cv::Mat img_rr=img_r_ptr->image;
      
      cv::Mat img_show(img_ll.rows,img_ll.cols+img_rr.cols,img_ll.type());
      img_ll.copyTo(img_show.colRange(0,img_ll.cols));
      img_rr.copyTo(img_show.colRange(img_ll.cols,img_ll.cols+img_rr.cols));
      cv::imshow("show",img_show);
      
      if(cv::waitKey(33)=='s')
      {
	char path[30];
	sprintf(path, "%s%d%s", "/home/oatrc/Desktop/pic/left", chess_num, ".jpg");
	imwrite(path,img_ll);
	sprintf(path, "%s%d%s", "/home/oatrc/Desktop/pic/right", chess_num, ".jpg");
	imwrite(path,img_rr);
	chess_num++;
      }
         
    }
    catch(cv_bridge::Exception &e)
    {
      ROS_ERROR("cv_bridge Exception %s",e.what());
    }
  
}

void imageCallback1(const sensor_msgs::ImageConstPtr& tem_msg)
{
    ROS_INFO("save images with header: %f", tem_msg->header.stamp.toSec());
  
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(tem_msg, "bgr8");

        cv::Mat chess = cv_ptr->image;
        cv::imshow("color", chess);
	
        if(waitKey(33) == 's')
        {
	    char path[30];
            sprintf(path, "%s%d%s", "/home/oatrc/Desktop/pic/pic", chess_num++, ".jpg");
            imwrite(path,chess);
        }

    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", tem_msg->encoding.c_str());
	ROS_ERROR("cv_bridge Exception %s",e.what());
    }
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "SavePicture");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);

    //image_transport::Subscriber sub1 = it.subscribe("/camera/color/image_raw",1,imageCallback1);
    
    message_filters::Subscriber<sensor_msgs::Image> sub_img_l(nh, "/mynteye/left/image_raw", 100);
    message_filters::Subscriber<sensor_msgs::Image> sub_img_r(nh, "/mynteye/right/image_raw", 100);
    
    typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy_img;
    message_filters::Synchronizer<MySyncPolicy_img> sync_img(MySyncPolicy_img(1000), sub_img_l, sub_img_r);
    sync_img.registerCallback(boost::bind(&callback_raw_image, _1, _2));

    ros::Rate loop_rate(30);
    while (nh.ok()) {
        //pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }


}