Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <ros/names.h>
00003
00004 #include <sensor_msgs/Image.h>
00005 #include <sensor_msgs/CameraInfo.h>
00006
00007 #include <image_transport/image_transport.h>
00008 #include <image_transport/subscriber_filter.h>
00009
00010 #include <message_filters/subscriber.h>
00011 #include <message_filters/time_synchronizer.h>
00012 #include <message_filters/sync_policies/approximate_time.h>
00013
00014 class StereoSynchronizer {
00015 private:
00016 ros::NodeHandle nh_;
00017 image_transport::ImageTransport it_, *it_l_, *it_r_;
00018 image_transport::CameraPublisher pub_left_, pub_right_;
00019
00020 image_transport::SubscriberFilter image_sub_l_, image_sub_r_;
00021 message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_l_, info_sub_r_;
00022
00023 message_filters::Synchronizer<
00024 message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo,
00025 sensor_msgs::Image, sensor_msgs::CameraInfo> > sync_;
00026 std::string left_ns_, right_ns_;
00027 public:
00028 StereoSynchronizer () : nh_(), it_(nh_), sync_(15) {
00029
00030 std::string left_raw = nh_.resolveName("left_raw");
00031 std::string right_raw = nh_.resolveName("right_raw");
00032 image_sub_l_.subscribe(it_, left_raw + "/image_raw", 4);
00033 info_sub_l_ .subscribe(nh_, left_raw + "/camera_info", 4);
00034 image_sub_r_.subscribe(it_, right_raw + "/image_raw", 4);
00035 info_sub_r_ .subscribe(nh_, right_raw + "/camera_info", 4);
00036
00037 left_ns_ = nh_.resolveName("left");
00038 right_ns_ = nh_.resolveName("right");
00039 ros::NodeHandle cam_l_nh(left_ns_);
00040 ros::NodeHandle cam_r_nh(right_ns_);
00041 it_l_ = new image_transport::ImageTransport(cam_l_nh);
00042 it_r_ = new image_transport::ImageTransport(cam_r_nh);
00043 pub_left_ = it_l_->advertiseCamera("image_raw", 1);
00044 pub_right_ = it_r_->advertiseCamera("image_raw", 1);
00045
00046 sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_);
00047 sync_.registerCallback(boost::bind(&StereoSynchronizer::imageCB, this, _1, _2, _3, _4));
00048 }
00049
00050 void imageCB(const sensor_msgs::ImageConstPtr& image_l,
00051 const sensor_msgs::CameraInfoConstPtr& info_l,
00052 const sensor_msgs::ImageConstPtr& image_r,
00053 const sensor_msgs::CameraInfoConstPtr& info_r) {
00054
00055 sensor_msgs::Image img = *image_r;
00056 sensor_msgs::CameraInfo info = *info_r;
00057
00058
00059
00060
00061 pub_left_.publish(image_l, info_l);
00062 pub_right_.publish(img, info, info_l->header.stamp);
00063
00064 }
00065 };
00066
00067 int main(int argc, char **argv) {
00068 ros::init(argc, argv, "stereo_synchronizer");
00069
00070 StereoSynchronizer node;
00071
00072 ros::spin();
00073 return 0;
00074 }