#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/CvBridge.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>

using namespace message_filters;
using namespace sensor_msgs;

sensor_msgs::CvBridge bridge;

void callback(const ImageConstPtr& left, const ImageConstPtr& right) {
	IplImage* im_left	= bridge.imgMsgToCv(left,  "rgb8");
	IplImage* im_right	= bridge.imgMsgToCv(right, "rgb8");
	
	cvShowImage("left", im_left);
	cvShowImage("right", im_right);
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "sync_node");
	ros::NodeHandle nh;
		
	cvNamedWindow("left");
	cvNamedWindow("right");
	cvStartWindowThread();
	
	message_filters::Subscriber<Image> left_sub(nh, "stereo/left/image", 1);
	message_filters::Subscriber<Image> right_sub(nh, "stereo/right/image", 1);
	
	TimeSynchronizer<Image, Image> sync(left_sub, right_sub, 10);
	sync.registerCallback(boost::bind(&callback, _1, _2));
	
	ros::spin();
	return 0;
}