00001 #ifndef IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H
00002 #define IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H
00003
00004 #include "image_transport/publisher_plugin.h"
00005 #include <boost/scoped_ptr.hpp>
00006
00007 namespace image_transport {
00008
00026 template <class M>
00027 class SimplePublisherPlugin : public PublisherPlugin
00028 {
00029 public:
00030 virtual ~SimplePublisherPlugin() {}
00031
00032 virtual uint32_t getNumSubscribers() const
00033 {
00034 if (simple_impl_) return simple_impl_->pub_.getNumSubscribers();
00035 return 0;
00036 }
00037
00038 virtual std::string getTopic() const
00039 {
00040 if (simple_impl_) return simple_impl_->pub_.getTopic();
00041 return std::string();
00042 }
00043
00044 virtual void publish(const sensor_msgs::Image& message) const
00045 {
00046 if (!simple_impl_ || !simple_impl_->pub_) {
00047 ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::SimplePublisherPlugin");
00048 return;
00049 }
00050
00051 publish(message, bindInternalPublisher(simple_impl_->pub_));
00052 }
00053
00054 virtual void shutdown()
00055 {
00056 if (simple_impl_) simple_impl_->pub_.shutdown();
00057 }
00058
00059 protected:
00060 virtual void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
00061 const SubscriberStatusCallback& user_connect_cb,
00062 const SubscriberStatusCallback& user_disconnect_cb,
00063 const ros::VoidPtr& tracked_object, bool latch)
00064 {
00065 std::string transport_topic = getTopicToAdvertise(base_topic);
00066 ros::NodeHandle param_nh(transport_topic);
00067 simple_impl_.reset(new SimplePublisherPluginImpl(param_nh));
00068 simple_impl_->pub_ = nh.advertise<M>(transport_topic, queue_size,
00069 bindCB(user_connect_cb, &SimplePublisherPlugin::connectCallback),
00070 bindCB(user_disconnect_cb, &SimplePublisherPlugin::disconnectCallback),
00071 tracked_object, latch);
00072 }
00073
00075 typedef boost::function<void(const M&)> PublishFn;
00076
00085 virtual void publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const = 0;
00086
00092 virtual std::string getTopicToAdvertise(const std::string& base_topic) const
00093 {
00094 return base_topic + "/" + getTransportName();
00095 }
00096
00102 virtual void connectCallback(const ros::SingleSubscriberPublisher& pub) {}
00103
00109 virtual void disconnectCallback(const ros::SingleSubscriberPublisher& pub) {}
00110
00114 const ros::NodeHandle& nh() const
00115 {
00116 return simple_impl_->param_nh_;
00117 }
00118
00125 const ros::Publisher& getPublisher() const
00126 {
00127 ROS_ASSERT(simple_impl_);
00128 return simple_impl_->pub_;
00129 }
00130
00131 private:
00132 struct SimplePublisherPluginImpl
00133 {
00134 SimplePublisherPluginImpl(const ros::NodeHandle& nh)
00135 : param_nh_(nh)
00136 {
00137 }
00138
00139 const ros::NodeHandle param_nh_;
00140 ros::Publisher pub_;
00141 };
00142
00143 boost::scoped_ptr<SimplePublisherPluginImpl> simple_impl_;
00144
00145 typedef void (SimplePublisherPlugin::*SubscriberStatusMemFn)(const ros::SingleSubscriberPublisher& pub);
00146
00151 ros::SubscriberStatusCallback bindCB(const SubscriberStatusCallback& user_cb,
00152 SubscriberStatusMemFn internal_cb_fn)
00153 {
00154 ros::SubscriberStatusCallback internal_cb = boost::bind(internal_cb_fn, this, _1);
00155 if (user_cb)
00156 return boost::bind(&SimplePublisherPlugin::subscriberCB, this, _1, user_cb, internal_cb);
00157 else
00158 return internal_cb;
00159 }
00160
00166 void subscriberCB(const ros::SingleSubscriberPublisher& ros_ssp,
00167 const SubscriberStatusCallback& user_cb,
00168 const ros::SubscriberStatusCallback& internal_cb)
00169 {
00170
00171 internal_cb(ros_ssp);
00172
00173
00174
00175
00176 typedef void (SimplePublisherPlugin::*PublishMemFn)(const sensor_msgs::Image&, const PublishFn&) const;
00177 PublishMemFn pub_mem_fn = &SimplePublisherPlugin::publish;
00178 ImagePublishFn image_publish_fn = boost::bind(pub_mem_fn, this, _1, bindInternalPublisher(ros_ssp));
00179
00180 SingleSubscriberPublisher ssp(ros_ssp.getSubscriberName(), getTopic(),
00181 boost::bind(&SimplePublisherPlugin::getNumSubscribers, this),
00182 image_publish_fn);
00183 user_cb(ssp);
00184 }
00185
00186 typedef boost::function<void(const sensor_msgs::Image&)> ImagePublishFn;
00187
00194 template <class PubT>
00195 PublishFn bindInternalPublisher(const PubT& pub) const
00196 {
00197
00198 typedef void (PubT::*InternalPublishMemFn)(const M&) const;
00199 InternalPublishMemFn internal_pub_mem_fn = &PubT::publish;
00200 return boost::bind(internal_pub_mem_fn, &pub, _1);
00201 }
00202 };
00203
00204 }
00205
00206 #endif