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",
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
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
00073
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
00080 impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
00081
00082
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
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 }