cr_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 CRSynchronizer {
00015 private:
00016   ros::NodeHandle nh_;
00017   image_transport::ImageTransport it_, *it_left_, *it_right_, *it_range_;
00018   image_transport::CameraPublisher pub_left_, pub_right_, pub_range_;
00019   image_transport::Publisher pub_intent_, pub_confi_;
00020   image_transport::SubscriberFilter image_sub_left_, image_sub_right_;
00021   image_transport::SubscriberFilter image_sub_depth_, image_sub_intent_, image_sub_confi_;
00022   message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_left_, info_sub_right_, info_sub_range_;
00023 
00024   message_filters::Synchronizer<
00025     message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo,
00026                                                     sensor_msgs::Image, sensor_msgs::CameraInfo,
00027                                                     sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image,
00028                                                     sensor_msgs::CameraInfo> > sync_;
00029   std::string left_ns_, right_ns_, range_ns_;
00030 public:
00031   CRSynchronizer () : nh_(), it_(nh_), sync_(15) {
00032 
00033     std::string left_raw = nh_.resolveName("left_raw");
00034     std::string right_raw = nh_.resolveName("right_raw");
00035     std::string range_raw = nh_.resolveName("range_raw");
00036 
00037     image_sub_left_.subscribe(it_, left_raw  + "/image_raw", 8);
00038     info_sub_left_ .subscribe(nh_, left_raw  + "/camera_info", 8);
00039 
00040     image_sub_right_.subscribe(it_, right_raw + "/image_raw", 8);
00041     info_sub_right_ .subscribe(nh_, right_raw + "/camera_info", 8);
00042 
00043     image_sub_depth_.subscribe(it_, range_raw + "/distance/image_raw16", 8);
00044     image_sub_intent_.subscribe(it_, range_raw + "/intensity/image_raw", 8);
00045     image_sub_confi_.subscribe(it_, range_raw + "/confidence/image_raw", 8);
00046     info_sub_range_ .subscribe(nh_, range_raw + "/camera_info", 8);
00047 
00048     left_ns_ = nh_.resolveName("left");
00049     right_ns_ = nh_.resolveName("right");
00050     range_ns_ = nh_.resolveName("range");
00051     ros::NodeHandle cam_left_nh(left_ns_);
00052     ros::NodeHandle cam_right_nh(right_ns_);
00053     ros::NodeHandle cam_range_nh(range_ns_);
00054 
00055     it_left_ = new image_transport::ImageTransport(cam_left_nh);
00056     it_right_ = new image_transport::ImageTransport(cam_right_nh);
00057     it_range_ = new image_transport::ImageTransport(cam_range_nh);
00058 
00059     pub_left_ = it_left_->advertiseCamera("image_raw", 1);
00060     pub_right_ = it_right_->advertiseCamera("image_raw", 1);
00061     pub_range_ = it_range_->advertiseCamera("distance/image_raw16", 1);
00062     pub_intent_ = it_range_->advertise("intensity/image_raw", 1);
00063     pub_confi_ = it_range_->advertise("confidence/image_raw", 1);
00064 
00065     sync_.connectInput(image_sub_left_, info_sub_left_, image_sub_right_, info_sub_right_,
00066                        image_sub_depth_, image_sub_intent_, image_sub_confi_, info_sub_range_);
00067     sync_.registerCallback(boost::bind(&CRSynchronizer::imageCB, this, _1, _2, _3, _4, _5, _6, _7, _8));
00068   }
00069 
00070   void imageCB(const sensor_msgs::ImageConstPtr& image_left,
00071                const sensor_msgs::CameraInfoConstPtr& info_left,
00072                const sensor_msgs::ImageConstPtr& image_right,
00073                const sensor_msgs::CameraInfoConstPtr& info_right,
00074                const sensor_msgs::ImageConstPtr& image_depth,
00075                const sensor_msgs::ImageConstPtr& image_intent,
00076                const sensor_msgs::ImageConstPtr& image_confi,
00077                const sensor_msgs::CameraInfoConstPtr& info_range )  {
00078 
00079     sensor_msgs::Image img_l = *image_left;
00080     sensor_msgs::CameraInfo info_l = *info_left;
00081     sensor_msgs::Image img_r = *image_right;
00082     sensor_msgs::CameraInfo info_r = *info_right;
00083 
00084     pub_left_.publish(img_l, info_l, info_range->header.stamp);
00085     pub_right_.publish(img_r, info_r, info_range->header.stamp);
00086     pub_range_.publish(image_depth, info_range);
00087     pub_intent_.publish(image_intent);
00088     pub_confi_.publish(image_confi);
00089   }
00090 };
00091 
00092 int main(int argc, char **argv) {
00093   ros::init(argc, argv, "cr_synchronizer");
00094 
00095   CRSynchronizer node;
00096 
00097   ros::spin();
00098   return 0;
00099 }


stereo_synchronizer
Author(s): yohei kakiuchi, Yohei Kakiuchi
autogenerated on Wed Sep 24 2014 11:40:36