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 }