simple_publisher_plugin.h

Go to the documentation of this file.

#ifndef IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H
#define IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H

#include "image_transport/publisher_plugin.h"
#include <boost/scoped_ptr.hpp>

namespace image_transport {

template <class M>
class SimplePublisherPlugin : public PublisherPlugin
{
public:
  virtual ~SimplePublisherPlugin() {}

  virtual uint32_t getNumSubscribers() const
  {
    if (simple_impl_) return simple_impl_->pub_.getNumSubscribers();
    return 0;
  }

  virtual std::string getTopic() const
  {
    if (simple_impl_) return simple_impl_->pub_.getTopic();
    return std::string();
  }

  virtual void publish(const sensor_msgs::Image& message) const
  {
    if (!simple_impl_ || !simple_impl_->pub_) {
      ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::SimplePublisherPlugin");
      return;
    }
    
    publish(message, bindInternalPublisher(simple_impl_->pub_));
  }

  virtual void shutdown()
  {
    if (simple_impl_) simple_impl_->pub_.shutdown();
  }

protected:
  virtual void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
                             const SubscriberStatusCallback& user_connect_cb,
                             const SubscriberStatusCallback& user_disconnect_cb,
                             const ros::VoidPtr& tracked_object, bool latch)
  {
    std::string transport_topic = getTopicToAdvertise(base_topic);
    ros::NodeHandle param_nh(transport_topic);
    simple_impl_.reset(new SimplePublisherPluginImpl(param_nh));
    simple_impl_->pub_ = nh.advertise<M>(transport_topic, queue_size,
                                         bindCB(user_connect_cb, &SimplePublisherPlugin::connectCallback),
                                         bindCB(user_disconnect_cb, &SimplePublisherPlugin::disconnectCallback),
                                         tracked_object, latch);
  }

  typedef boost::function<void(const M&)> PublishFn;

  virtual void publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const = 0;

  virtual std::string getTopicToAdvertise(const std::string& base_topic) const
  {
    return base_topic + "/" + getTransportName();
  }

  virtual void connectCallback(const ros::SingleSubscriberPublisher& pub) {}

  virtual void disconnectCallback(const ros::SingleSubscriberPublisher& pub) {}

  const ros::NodeHandle& nh() const
  {
    return simple_impl_->param_nh_;
  }

  const ros::Publisher& getPublisher() const
  {
    ROS_ASSERT(simple_impl_);
    return simple_impl_->pub_;
  }

private:
  struct SimplePublisherPluginImpl
  {
    SimplePublisherPluginImpl(const ros::NodeHandle& nh)
      : param_nh_(nh)
    {
    }
    
    const ros::NodeHandle param_nh_;
    ros::Publisher pub_;
  };
  
  boost::scoped_ptr<SimplePublisherPluginImpl> simple_impl_;

  typedef void (SimplePublisherPlugin::*SubscriberStatusMemFn)(const ros::SingleSubscriberPublisher& pub);
  
  ros::SubscriberStatusCallback bindCB(const SubscriberStatusCallback& user_cb,
                                       SubscriberStatusMemFn internal_cb_fn)
  {
    ros::SubscriberStatusCallback internal_cb = boost::bind(internal_cb_fn, this, _1);
    if (user_cb)
      return boost::bind(&SimplePublisherPlugin::subscriberCB, this, _1, user_cb, internal_cb);
    else
      return internal_cb;
  }
  
  void subscriberCB(const ros::SingleSubscriberPublisher& ros_ssp,
                    const SubscriberStatusCallback& user_cb,
                    const ros::SubscriberStatusCallback& internal_cb)
  {
    // First call the internal callback (for sending setup headers, etc.)
    internal_cb(ros_ssp);
    
    // Construct a function object for publishing sensor_msgs::Image through the
    // subclass-implemented publish() using the ros::SingleSubscriberPublisher to send
    // messages of the transport-specific type.
    typedef void (SimplePublisherPlugin::*PublishMemFn)(const sensor_msgs::Image&, const PublishFn&) const;
    PublishMemFn pub_mem_fn = &SimplePublisherPlugin::publish;
    ImagePublishFn image_publish_fn = boost::bind(pub_mem_fn, this, _1, bindInternalPublisher(ros_ssp));
    
    SingleSubscriberPublisher ssp(ros_ssp.getSubscriberName(), getTopic(),
                                  boost::bind(&SimplePublisherPlugin::getNumSubscribers, this),
                                  image_publish_fn);
    user_cb(ssp);
  }

  typedef boost::function<void(const sensor_msgs::Image&)> ImagePublishFn;

  template <class PubT>
  PublishFn bindInternalPublisher(const PubT& pub) const
  {
    // Bind PubT::publish(const Message&) as PublishFn
    typedef void (PubT::*InternalPublishMemFn)(const M&) const;
    InternalPublishMemFn internal_pub_mem_fn = &PubT::publish;
    return boost::bind(internal_pub_mem_fn, &pub, _1);
  }
};

} //namespace image_transport

#endif