Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "image_transport/camera_subscriber.h"
00036 #include "image_transport/subscriber_filter.h"
00037 #include "image_transport/camera_common.h"
00038 #include <message_filters/subscriber.h>
00039 #include <message_filters/time_synchronizer.h>
00040
00041 inline void increment(int* value)
00042 {
00043 ++(*value);
00044 }
00045
00046 namespace image_transport {
00047
00048 struct CameraSubscriber::Impl
00049 {
00050 Impl(uint32_t queue_size)
00051 : sync_(queue_size),
00052 unsubscribed_(false),
00053 image_received_(0), info_received_(0), both_received_(0)
00054 {}
00055
00056 ~Impl()
00057 {
00058 shutdown();
00059 }
00060
00061 bool isValid() const
00062 {
00063 return !unsubscribed_;
00064 }
00065
00066 void shutdown()
00067 {
00068 if (!unsubscribed_) {
00069 unsubscribed_ = true;
00070 image_sub_.unsubscribe();
00071 info_sub_.unsubscribe();
00072 }
00073 }
00074
00075 void checkImagesSynchronized()
00076 {
00077 int threshold = 3 * both_received_;
00078 if (image_received_ > threshold || info_received_ > threshold) {
00079 ROS_WARN_NAMED("sync",
00080 "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. "
00081 "In the last 10s:\n"
00082 "\tImage messages received: %d\n"
00083 "\tCameraInfo messages received: %d\n"
00084 "\tSynchronized pairs: %d",
00085 image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(),
00086 image_received_, info_received_, both_received_);
00087 }
00088 image_received_ = info_received_ = both_received_ = 0;
00089 }
00090
00091 SubscriberFilter image_sub_;
00092 message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00093 message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
00094 bool unsubscribed_;
00095
00096 ros::WallTimer check_synced_timer_;
00097 int image_received_, info_received_, both_received_;
00098 };
00099
00100 CameraSubscriber::CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh,
00101 const std::string& base_topic, uint32_t queue_size,
00102 const Callback& callback, const ros::VoidPtr& tracked_object,
00103 const TransportHints& transport_hints)
00104 : impl_(new Impl(queue_size))
00105 {
00106
00107
00108 std::string image_topic = info_nh.resolveName(base_topic);
00109 std::string info_topic = getCameraInfoTopic(image_topic);
00110 impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints);
00111 impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints());
00112 impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
00113
00114 impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
00115
00116
00117 impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_));
00118 impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_));
00119 impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_));
00120 impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0),
00121 boost::bind(&Impl::checkImagesSynchronized, impl_.get()));
00122 }
00123
00124 std::string CameraSubscriber::getTopic() const
00125 {
00126 if (impl_) return impl_->image_sub_.getTopic();
00127 return std::string();
00128 }
00129
00130 std::string CameraSubscriber::getInfoTopic() const
00131 {
00132 if (impl_) return impl_->info_sub_.getTopic();
00133 return std::string();
00134 }
00135
00136 uint32_t CameraSubscriber::getNumPublishers() const
00137 {
00139
00140 if (impl_) return impl_->image_sub_.getNumPublishers();
00141 return 0;
00142 }
00143
00144 std::string CameraSubscriber::getTransport() const
00145 {
00146 if (impl_) return impl_->image_sub_.getTransport();
00147 return std::string();
00148 }
00149
00150 void CameraSubscriber::shutdown()
00151 {
00152 if (impl_) impl_->shutdown();
00153 }
00154
00155 CameraSubscriber::operator void*() const
00156 {
00157 return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00158 }
00159
00160 }