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/publisher.h"
00036 #include "image_transport/publisher_plugin.h"
00037 #include <pluginlib/class_loader.h>
00038 #include <boost/foreach.hpp>
00039 #include <boost/algorithm/string/erase.hpp>
00040
00041 namespace image_transport {
00042
00043 struct Publisher::Impl
00044 {
00045 Impl()
00046 : unadvertised_(false)
00047 {
00048 }
00049
00050 ~Impl()
00051 {
00052 shutdown();
00053 }
00054
00055 uint32_t getNumSubscribers() const
00056 {
00057 uint32_t count = 0;
00058 BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, publishers_)
00059 count += pub->getNumSubscribers();
00060 return count;
00061 }
00062
00063 std::string getTopic() const
00064 {
00065 return base_topic_;
00066 }
00067
00068 bool isValid() const
00069 {
00070 return !unadvertised_;
00071 }
00072
00073 void shutdown()
00074 {
00075 if (!unadvertised_) {
00076 unadvertised_ = true;
00077 BOOST_FOREACH(boost::shared_ptr<PublisherPlugin>& pub, publishers_)
00078 pub->shutdown();
00079 publishers_.clear();
00080 }
00081 }
00082
00083 void subscriberCB(const SingleSubscriberPublisher& plugin_pub,
00084 const SubscriberStatusCallback& user_cb)
00085 {
00086 SingleSubscriberPublisher ssp(plugin_pub.getSubscriberName(), getTopic(),
00087 boost::bind(&Publisher::Impl::getNumSubscribers, this),
00088 plugin_pub.publish_fn_);
00089 user_cb(ssp);
00090 }
00091
00092 std::string base_topic_;
00093 PubLoaderPtr loader_;
00094 std::vector<boost::shared_ptr<PublisherPlugin> > publishers_;
00095 bool unadvertised_;
00096
00097 };
00098
00099
00100 Publisher::Publisher(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
00101 const SubscriberStatusCallback& connect_cb,
00102 const SubscriberStatusCallback& disconnect_cb,
00103 const ros::VoidPtr& tracked_object, bool latch,
00104 const PubLoaderPtr& loader)
00105 : impl_(new Impl)
00106 {
00107
00108
00109 impl_->base_topic_ = nh.resolveName(base_topic);
00110 impl_->loader_ = loader;
00111
00112 BOOST_FOREACH(const std::string& lookup_name, loader->getDeclaredClasses()) {
00113 try {
00114 boost::shared_ptr<PublisherPlugin> pub = loader->createInstance(lookup_name);
00115 impl_->publishers_.push_back(pub);
00116 pub->advertise(nh, impl_->base_topic_, queue_size, rebindCB(connect_cb),
00117 rebindCB(disconnect_cb), tracked_object, latch);
00118 }
00119 catch (const std::runtime_error& e) {
00120 ROS_DEBUG("Failed to load plugin %s, error string: %s",
00121 lookup_name.c_str(), e.what());
00122 }
00123 }
00124
00125 if (impl_->publishers_.empty())
00126 throw Exception("No plugins found! Does `rospack plugins --attrib=plugin "
00127 "image_transport` find any packages?");
00128 }
00129
00130 uint32_t Publisher::getNumSubscribers() const
00131 {
00132 if (impl_ && impl_->isValid()) return impl_->getNumSubscribers();
00133 return 0;
00134 }
00135
00136 std::string Publisher::getTopic() const
00137 {
00138 if (impl_) return impl_->getTopic();
00139 return std::string();
00140 }
00141
00142 void Publisher::publish(const sensor_msgs::Image& message) const
00143 {
00144 if (!impl_ || !impl_->isValid()) {
00145 ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher");
00146 return;
00147 }
00148
00149 BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, impl_->publishers_) {
00150 if (pub->getNumSubscribers() > 0)
00151 pub->publish(message);
00152 }
00153 }
00154
00155 void Publisher::publish(const sensor_msgs::ImageConstPtr& message) const
00156 {
00157 if (!impl_ || !impl_->isValid()) {
00158 ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher");
00159 return;
00160 }
00161
00162 BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, impl_->publishers_) {
00163 if (pub->getNumSubscribers() > 0)
00164 pub->publish(message);
00165 }
00166 }
00167
00168 void Publisher::shutdown()
00169 {
00170 if (impl_) {
00171 impl_->shutdown();
00172 impl_.reset();
00173 }
00174 }
00175
00176 Publisher::operator void*() const
00177 {
00178 return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00179 }
00180
00181 void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr,
00182 const SingleSubscriberPublisher& plugin_pub,
00183 const SubscriberStatusCallback& user_cb)
00184 {
00185 if (ImplPtr impl = impl_wptr.lock())
00186 impl->subscriberCB(plugin_pub, user_cb);
00187 }
00188
00189 SubscriberStatusCallback Publisher::rebindCB(const SubscriberStatusCallback& user_cb)
00190 {
00191
00192
00193
00194 if (user_cb)
00195 {
00196 ImplWPtr impl_wptr(impl_);
00197 return boost::bind(&Publisher::weakSubscriberCb, impl_wptr, _1, user_cb);
00198 }
00199 else
00200 return SubscriberStatusCallback();
00201 }
00202
00203 }