publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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   //double constructed_;
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   // Resolve the name explicitly because otherwise the compressed topics don't remap
00108   // properly (#3652).
00109   impl_->base_topic_ = nh.resolveName(base_topic);
00110   impl_->loader_ = loader;
00111 
00112   std::vector<std::string> blacklist_vec;
00113   std::set<std::string> blacklist;
00114   nh.getParam(impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec);
00115   for (size_t i = 0; i < blacklist_vec.size(); ++i)
00116   {
00117     blacklist.insert(blacklist_vec[i]);
00118   }
00119 
00120   BOOST_FOREACH(const std::string& lookup_name, loader->getDeclaredClasses()) {
00121     const std::string transport_name = boost::erase_last_copy(lookup_name, "_pub");
00122     if (blacklist.count(transport_name))
00123     {
00124       continue;
00125     }
00126 
00127     try {
00128       boost::shared_ptr<PublisherPlugin> pub = loader->createInstance(lookup_name);
00129       impl_->publishers_.push_back(pub);
00130       pub->advertise(nh, impl_->base_topic_, queue_size, rebindCB(connect_cb),
00131                      rebindCB(disconnect_cb), tracked_object, latch);
00132     }
00133     catch (const std::runtime_error& e) {
00134       ROS_DEBUG("Failed to load plugin %s, error string: %s",
00135                 lookup_name.c_str(), e.what());
00136     }
00137   }
00138 
00139   if (impl_->publishers_.empty())
00140     throw Exception("No plugins found! Does `rospack plugins --attrib=plugin "
00141                     "image_transport` find any packages?");
00142 }
00143 
00144 uint32_t Publisher::getNumSubscribers() const
00145 {
00146   if (impl_ && impl_->isValid()) return impl_->getNumSubscribers();
00147   return 0;
00148 }
00149 
00150 std::string Publisher::getTopic() const
00151 {
00152   if (impl_) return impl_->getTopic();
00153   return std::string();
00154 }
00155 
00156 void Publisher::publish(const sensor_msgs::Image& message) const
00157 {
00158   if (!impl_ || !impl_->isValid()) {
00159     ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher");
00160     return;
00161   }
00162   
00163   BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, impl_->publishers_) {
00164     if (pub->getNumSubscribers() > 0)
00165       pub->publish(message);
00166   }
00167 }
00168 
00169 void Publisher::publish(const sensor_msgs::ImageConstPtr& message) const
00170 {
00171   if (!impl_ || !impl_->isValid()) {
00172     ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher");
00173     return;
00174   }
00175   
00176   BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, impl_->publishers_) {
00177     if (pub->getNumSubscribers() > 0)
00178       pub->publish(message);
00179   }
00180 }
00181 
00182 void Publisher::shutdown()
00183 {
00184   if (impl_) {
00185     impl_->shutdown();
00186     impl_.reset();
00187   }
00188 }
00189 
00190 Publisher::operator void*() const
00191 {
00192   return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00193 }
00194 
00195 void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr,
00196                                  const SingleSubscriberPublisher& plugin_pub,
00197                                  const SubscriberStatusCallback& user_cb)
00198 {
00199   if (ImplPtr impl = impl_wptr.lock())
00200     impl->subscriberCB(plugin_pub, user_cb);
00201 }
00202 
00203 SubscriberStatusCallback Publisher::rebindCB(const SubscriberStatusCallback& user_cb)
00204 {
00205   // Note: the subscriber callback must be bound to the internal Impl object, not
00206   // 'this'. Due to copying behavior the Impl object may outlive the original Publisher
00207   // instance. But it should not outlive the last Publisher, so we use a weak_ptr.
00208   if (user_cb)
00209   {
00210     ImplWPtr impl_wptr(impl_);
00211     return boost::bind(&Publisher::weakSubscriberCb, impl_wptr, _1, user_cb);
00212   }
00213   else
00214     return SubscriberStatusCallback();
00215 }
00216 
00217 } //namespace image_transport


image_transport
Author(s): Patrick Mihelich
autogenerated on Thu Jun 6 2019 21:19:55