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   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   // Note: the subscriber callback must be bound to the internal Impl object, not
00192   // 'this'. Due to copying behavior the Impl object may outlive the original Publisher
00193   // instance. But it should not outlive the last Publisher, so we use a weak_ptr.
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 } //namespace image_transport


image_transport
Author(s): Patrick Mihelich
autogenerated on Tue Jun 6 2017 02:33:36