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