stereo_synchronizer.cpp
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     //img.header.stamp = image_l->header.stamp;
00059     //info.header.stamp = info_l->header.stamp;
00060 
00061     pub_left_.publish(image_l, info_l);
00062     pub_right_.publish(img, info, info_l->header.stamp);
00063     //pub_right_.publish(*image_r, *info_r, info_l->header.stamp);
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 }


stereo_synchronizer
Author(s): yohei kakiuchi, Yohei Kakiuchi
autogenerated on Thu Sep 25 2014 13:35:26