stereo_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 
00023 class StereoProcessor
00024 {
00025 
00026 private:
00027 
00028   // subscriber
00029   image_transport::SubscriberFilter left_sub_, right_sub_;
00030   message_filters::Subscriber<sensor_msgs::CameraInfo> left_info_sub_, right_info_sub_;
00031   typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactPolicy;
00032   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximatePolicy;
00033   typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
00034   typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
00035   boost::shared_ptr<ExactSync> exact_sync_;
00036   boost::shared_ptr<ApproximateSync> approximate_sync_;
00037   int queue_size_;
00038 
00039   // for sync checking
00040   ros::WallTimer check_synced_timer_;
00041   int left_received_, right_received_, left_info_received_, right_info_received_, all_received_;
00042 
00043   // for sync checking
00044   static void increment(int* value)
00045   {
00046     ++(*value);
00047   }
00048 
00049   void dataCb(const sensor_msgs::ImageConstPtr& l_image_msg,
00050               const sensor_msgs::ImageConstPtr& r_image_msg,
00051               const sensor_msgs::CameraInfoConstPtr& l_info_msg,
00052               const sensor_msgs::CameraInfoConstPtr& r_info_msg)
00053   {
00054  
00055     // For sync error checking
00056     ++all_received_; 
00057 
00058     // call implementation
00059     imageCallback(l_image_msg, r_image_msg, l_info_msg, r_info_msg);
00060   }
00061 
00062   void checkInputsSynchronized()
00063   {
00064     int threshold = 3 * all_received_;
00065     if (left_received_ >= threshold || right_received_ >= threshold || 
00066         left_info_received_ >= threshold || right_info_received_ >= threshold) {
00067       ROS_WARN("[stereo_processor] Low number of synchronized left/right/left_info/right_info tuples received.\n"
00068                "Left images received:       %d (topic '%s')\n"
00069                "Right images received:      %d (topic '%s')\n"
00070                "Left camera info received:  %d (topic '%s')\n"
00071                "Right camera info received: %d (topic '%s')\n"
00072                "Synchronized tuples: %d\n"
00073                "Possible issues:\n"
00074                "\t* stereo_image_proc is not running.\n"
00075                "\t  Does `rosnode info %s` show any connections?\n"
00076                "\t* The cameras are not synchronized.\n"
00077                "\t  Try restarting the node with parameter _approximate_sync:=True\n"
00078                "\t* The network is too slow. One or more images are dropped from each tuple.\n"
00079                "\t  Try restarting the node, increasing parameter 'queue_size' (currently %d)",
00080                left_received_, left_sub_.getTopic().c_str(),
00081                right_received_, right_sub_.getTopic().c_str(),
00082                left_info_received_, left_info_sub_.getTopic().c_str(),
00083                right_info_received_, right_info_sub_.getTopic().c_str(),
00084                all_received_, ros::this_node::getName().c_str(), queue_size_);
00085     }
00086   }
00087 
00088 
00089 protected:
00090 
00096   StereoProcessor(const std::string& transport) :
00097     left_received_(0), right_received_(0), left_info_received_(0), right_info_received_(0), all_received_(0)
00098   {
00099     // Read local parameters
00100     ros::NodeHandle local_nh("~");
00101 
00102     // Resolve topic names
00103     ros::NodeHandle nh;
00104     std::string stereo_ns = nh.resolveName("stereo");
00105     std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
00106     std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
00107 
00108     std::string left_info_topic = stereo_ns + "/left/camera_info";
00109     std::string right_info_topic = stereo_ns + "/right/camera_info";
00110 
00111     // Subscribe to four input topics.
00112     ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s", 
00113         left_topic.c_str(), right_topic.c_str(),
00114         left_info_topic.c_str(), right_info_topic.c_str());
00115 
00116     image_transport::ImageTransport it(nh);
00117     left_sub_.subscribe(it, left_topic, 1, transport);
00118     right_sub_.subscribe(it, right_topic, 1, transport);
00119     left_info_sub_.subscribe(nh, left_info_topic, 1);
00120     right_info_sub_.subscribe(nh, right_info_topic, 1);
00121 
00122     // Complain every 15s if the topics appear unsynchronized
00123     left_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_received_));
00124     right_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_received_));
00125     left_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_info_received_));
00126     right_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_info_received_));
00127     check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
00128                                              boost::bind(&StereoProcessor::checkInputsSynchronized, this));
00129 
00130     // Synchronize input topics. Optionally do approximate synchronization.
00131     local_nh.param("queue_size", queue_size_, 5);
00132     bool approx;
00133     local_nh.param("approximate_sync", approx, false);
00134     if (approx)
00135     {
00136       approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
00137                                                   left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
00138       approximate_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
00139     }
00140     else
00141     {
00142       exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
00143                                       left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
00144       exact_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
00145     }
00146   }
00147 
00151   virtual void imageCallback(const sensor_msgs::ImageConstPtr& l_image_msg,
00152                              const sensor_msgs::ImageConstPtr& r_image_msg,
00153                              const sensor_msgs::CameraInfoConstPtr& l_info_msg,
00154                              const sensor_msgs::CameraInfoConstPtr& r_info_msg) = 0;
00155 
00156 };
00157 
00158 } // end of namespace
00159 
00160 #endif
00161 


fovis_ros
Author(s): Stephan Wirth
autogenerated on Mon Sep 8 2014 10:17:46