mono_depth_processor.hpp
Go to the documentation of this file.
00001 #ifndef STEREO_PROCESSOR_H_
00002 #define STEREO_PROCESSOR_H_
00003 
00004 #include <ros/ros.h>
00005 #include <sensor_msgs/Image.h>
00006 #include <sensor_msgs/CameraInfo.h>
00007 
00008 #include <message_filters/subscriber.h>
00009 #include <message_filters/synchronizer.h>
00010 #include <message_filters/sync_policies/exact_time.h>
00011 #include <message_filters/sync_policies/approximate_time.h>
00012 #include <image_transport/subscriber_filter.h>
00013 
00014 namespace fovis_ros
00015 {
00016 
00024 class MonoDepthProcessor
00025 {
00026 
00027 private:
00028 
00029   // subscriber
00030   image_transport::SubscriberFilter image_sub_, depth_sub_;
00031   message_filters::Subscriber<sensor_msgs::CameraInfo> image_info_sub_, depth_info_sub_;
00032   typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactPolicy;
00033   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximatePolicy;
00034   typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
00035   typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
00036   boost::shared_ptr<ExactSync> exact_sync_;
00037   boost::shared_ptr<ApproximateSync> approximate_sync_;
00038   int queue_size_;
00039 
00040   // for sync checking
00041   ros::WallTimer check_synced_timer_;
00042   int image_received_, depth_received_, image_info_received_, depth_info_received_, all_received_;
00043 
00044   // for sync checking
00045   static void increment(int* value)
00046   {
00047     ++(*value);
00048   }
00049 
00050   void dataCb(const sensor_msgs::ImageConstPtr& image_msg,
00051               const sensor_msgs::ImageConstPtr& depth_image_msg,
00052               const sensor_msgs::CameraInfoConstPtr& image_info_msg,
00053               const sensor_msgs::CameraInfoConstPtr& depth_info_msg)
00054   {
00055  
00056     // For sync error checking
00057     ++all_received_; 
00058 
00059     // call implementation
00060     imageCallback(image_msg, depth_image_msg, image_info_msg, depth_info_msg);
00061   }
00062 
00063   void checkInputsSynchronized()
00064   {
00065     int threshold = 3 * all_received_;
00066     if (image_received_ >= threshold || depth_received_ >= threshold || 
00067         image_info_received_ >= threshold || depth_info_received_ >= threshold) {
00068       ROS_WARN("[stereo_processor] Low number of synchronized image/depth/image_info/depth_info tuples received.\n"
00069                "Images received:            %d (topic '%s')\n"
00070                "Depth images received:      %d (topic '%s')\n"
00071                "Image camera info received: %d (topic '%s')\n"
00072                "Depth camera info received: %d (topic '%s')\n"
00073                "Synchronized tuples: %d\n",
00074                image_received_, image_sub_.getTopic().c_str(),
00075                depth_received_, depth_sub_.getTopic().c_str(),
00076                image_info_received_, image_info_sub_.getTopic().c_str(),
00077                depth_info_received_, depth_info_sub_.getTopic().c_str(),
00078                all_received_);
00079     }
00080   }
00081 
00082 
00083 protected:
00084 
00090   MonoDepthProcessor(const std::string& transport) :
00091     image_received_(0), depth_received_(0), image_info_received_(0), depth_info_received_(0), all_received_(0)
00092   {
00093     // Read local parameters
00094     ros::NodeHandle local_nh("~");
00095 
00096     // Resolve topic names
00097     ros::NodeHandle nh;
00098     std::string camera_ns = nh.resolveName("camera");
00099     std::string image_topic = ros::names::clean(camera_ns + "/rgb/image_rect");
00100     std::string depth_topic = ros::names::clean(camera_ns + "/depth_registered/image_rect");
00101 
00102     std::string image_info_topic = camera_ns + "/rgb/camera_info";
00103     std::string depth_info_topic = camera_ns + "/depth_registered/camera_info";
00104 
00105     // Subscribe to four input topics.
00106     ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s", 
00107         image_topic.c_str(), depth_topic.c_str(),
00108         image_info_topic.c_str(), depth_info_topic.c_str());
00109 
00110     image_transport::ImageTransport it(nh);
00111     image_sub_.subscribe(it, image_topic, 1, transport);
00112     depth_sub_.subscribe(it, depth_topic, 1, transport);
00113     image_info_sub_.subscribe(nh, image_info_topic, 1);
00114     depth_info_sub_.subscribe(nh, depth_info_topic, 1);
00115 
00116     // Complain every 15s if the topics appear unsynchronized
00117     image_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_received_));
00118     depth_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_received_));
00119     image_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_info_received_));
00120     depth_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_info_received_));
00121     check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
00122                                              boost::bind(&MonoDepthProcessor::checkInputsSynchronized, this));
00123 
00124     // Synchronize input topics. Optionally do approximate synchronization.
00125     local_nh.param("queue_size", queue_size_, 5);
00126     bool approx;
00127     local_nh.param("approximate_sync", approx, true);
00128     if (approx)
00129     {
00130       approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
00131                                                   image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
00132       approximate_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
00133     }
00134     else
00135     {
00136       exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
00137                                       image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
00138       exact_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
00139     }
00140   }
00141 
00145   virtual void imageCallback(const sensor_msgs::ImageConstPtr& image_msg,
00146                              const sensor_msgs::ImageConstPtr& depth_msg,
00147                              const sensor_msgs::CameraInfoConstPtr& image_info_msg,
00148                              const sensor_msgs::CameraInfoConstPtr& depth_info_msg) = 0;
00149 
00150 };
00151 
00152 } // end of namespace
00153 
00154 #endif
00155 


fovis_ros
Author(s): Stephan Wirth
autogenerated on Fri Sep 12 2014 12:12:54