camera_subscriber.cpp
Go to the documentation of this file.
00001 #include "image_transport/camera_subscriber.h"
00002 #include "image_transport/subscriber_filter.h"
00003 #include "image_transport/camera_common.h"
00004 #include <message_filters/subscriber.h>
00005 #include <message_filters/time_synchronizer.h>
00006 
00007 void increment(int* value)
00008 {
00009   ++(*value);
00010 }
00011 
00012 namespace image_transport {
00013 
00014 struct CameraSubscriber::Impl
00015 {
00016   Impl(uint32_t queue_size)
00017     : sync_(queue_size),
00018       unsubscribed_(false),
00019       image_received_(0), info_received_(0), both_received_(0)
00020   {}
00021 
00022   ~Impl()
00023   {
00024     shutdown();
00025   }
00026 
00027   bool isValid() const
00028   {
00029     return !unsubscribed_;
00030   }
00031   
00032   void shutdown()
00033   {
00034     if (!unsubscribed_) {
00035       unsubscribed_ = true;
00036       image_sub_.unsubscribe();
00037       info_sub_.unsubscribe();
00038     }
00039   }
00040 
00041   void checkImagesSynchronized()
00042   {
00043     int threshold = 3 * both_received_;
00044     if (image_received_ > threshold || info_received_ > threshold) {
00045       ROS_WARN_NAMED("sync", // Can suppress ros.image_transport.sync independent of anything else
00046                      "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. "
00047                      "In the last 10s:\n"
00048                      "\tImage messages received:      %d\n"
00049                      "\tCameraInfo messages received: %d\n"
00050                      "\tSynchronized pairs:           %d",
00051                      image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(),
00052                      image_received_, info_received_, both_received_);
00053     }
00054     image_received_ = info_received_ = both_received_ = 0;
00055   }
00056   
00057   SubscriberFilter image_sub_;
00058   message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00059   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
00060   bool unsubscribed_;
00061   // For detecting when the topics aren't synchronized
00062   ros::WallTimer check_synced_timer_;
00063   int image_received_, info_received_, both_received_;
00064 };
00065 
00066 CameraSubscriber::CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh,
00067                                    const std::string& base_topic, uint32_t queue_size,
00068                                    const Callback& callback, const ros::VoidPtr& tracked_object,
00069                                    const TransportHints& transport_hints)
00070   : impl_(new Impl(queue_size))
00071 {
00072   // Must explicitly remap the image topic since we then do some string manipulation on it
00073   // to figure out the sibling camera_info topic.
00074   std::string image_topic = info_nh.resolveName(base_topic);
00075   std::string info_topic = getCameraInfoTopic(image_topic);
00076   impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints);
00077   impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints());
00078   impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
00079   // need for Boost.Bind here is kind of broken
00080   impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
00081 
00082   // Complain every 10s if it appears that the image and info topics are not synchronized
00083   impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_));
00084   impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_));
00085   impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_));
00086   impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0),
00087                                                        boost::bind(&Impl::checkImagesSynchronized, impl_));
00088 }
00089 
00090 std::string CameraSubscriber::getTopic() const
00091 {
00092   if (impl_) return impl_->image_sub_.getTopic();
00093   return std::string();
00094 }
00095 
00096 std::string CameraSubscriber::getInfoTopic() const
00097 {
00098   if (impl_) return impl_->info_sub_.getTopic();
00099   return std::string();
00100 }
00101 
00102 uint32_t CameraSubscriber::getNumPublishers() const
00103 {
00105   //if (impl_) return std::max(impl_->image_sub_.getNumPublishers(), impl_->info_sub_.getNumPublishers());
00106   if (impl_) return impl_->image_sub_.getNumPublishers();
00107   return 0;
00108 }
00109 
00110 std::string CameraSubscriber::getTransport() const
00111 {
00112   if (impl_) return impl_->image_sub_.getTransport();
00113   return std::string();
00114 }
00115 
00116 void CameraSubscriber::shutdown()
00117 {
00118   if (impl_) impl_->shutdown();
00119 }
00120 
00121 CameraSubscriber::operator void*() const
00122 {
00123   return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00124 }
00125 
00126 } //namespace image_transport


image_transport
Author(s): Patrick Mihelich
autogenerated on Fri Jan 3 2014 11:24:08