simple_publisher_plugin.h

Go to the documentation of this file.

#ifndef MESSAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H
#define MESSAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H

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

namespace message_transport {

        template <class Base,class M>
                 class SimplePublisherPlugin : public PublisherPlugin<Base>
        {
                public:
            SimplePublisherPlugin(bool forceLatch = false) : forcelatch_(forceLatch) {}
                        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 Base& message) const
                        {
                                if (!simple_impl_ || !simple_impl_->pub_) {
                                        ROS_ASSERT_MSG(false, "Call to publish() on an invalid message_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 typename SingleSubscriberPublisher<Base>::StatusCB& user_connect_cb,
                                        const typename SingleSubscriberPublisher<Base>::StatusCB& user_disconnect_cb,
                                        const ros::VoidPtr& tracked_object, bool latch)
                        {
                                ros::NodeHandle param_nh(nh, getTopicToAdvertise(base_topic));
                                simple_impl_.reset(new SimplePublisherPluginImpl(nh,param_nh));
                                simple_impl_->pub_ = nh.advertise<M>(getTopicToAdvertise(base_topic), queue_size,
                                                bindCB(user_connect_cb, &SimplePublisherPlugin::connectCallbackHandle),
                                                bindCB(user_disconnect_cb, &SimplePublisherPlugin::disconnectCallbackHandle),
                                                tracked_object, latch | forcelatch_);
                                this->postAdvertiseInit();
                        }

                        //initialisation function once the node has been initialised
                        virtual void postAdvertiseInit() {
                // ROS_WARN("Calling default postAdvertiseInit");
            }

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

                        virtual void publish(const Base& message, const PublishFn& publish_fn) const = 0;

                        void publishInternal(const M& message) const {
                                if (simple_impl_) simple_impl_->pub_.publish(message);
            }

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

                        virtual void connectCallback(const ros::SingleSubscriberPublisher& pub) {}
                        void connectCallbackHandle(const ros::SingleSubscriberPublisher& pub) {
                this->connectCallback(pub);
            }

                        virtual void disconnectCallback(const ros::SingleSubscriberPublisher& pub) {}
                        void disconnectCallbackHandle(const ros::SingleSubscriberPublisher& pub) {
                this->disconnectCallback(pub);
            }

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

                        ros::NodeHandle& getNodeHandle() 
                        {
                                return simple_impl_->nh_;
                        }

                private:
                        struct SimplePublisherPluginImpl
                        {
                                SimplePublisherPluginImpl(ros::NodeHandle& nh, const ros::NodeHandle& param_nh)
                                        : nh_(nh), param_nh_(param_nh)
                                {
                                }

                                ros::NodeHandle nh_;
                                const ros::NodeHandle param_nh_;
                                ros::Publisher pub_;
                        };

                        boost::scoped_ptr<SimplePublisherPluginImpl> simple_impl_;
            bool forcelatch_;

                        typedef void (SimplePublisherPlugin::*SubscriberStatusMemFn)(const ros::SingleSubscriberPublisher& pub);

                        ros::SubscriberStatusCallback bindCB(const typename SingleSubscriberPublisher<Base>::StatusCB& 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 typename SingleSubscriberPublisher<Base>::StatusCB& 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 Base through the
                                // subclass-implemented publish() using the ros::SingleSubscriberPublisher to send
                                // messages of the transport-specific type.
                                typedef void (SimplePublisherPlugin::*PublishMemFn)(const Base&, const PublishFn&) const;
                                PublishMemFn pub_mem_fn = &SimplePublisherPlugin::publish;
                                MessagePublishFn message_publish_fn = boost::bind(pub_mem_fn, this, _1, bindInternalPublisher(ros_ssp));

                                SingleSubscriberPublisher<Base> ssp(ros_ssp.getSubscriberName(), getTopic(),
                                                boost::bind(&SimplePublisherPlugin::getNumSubscribers, this),
                                                message_publish_fn);
                                user_cb(ssp);
                        }

                        typedef boost::function<void(const Base&)> MessagePublishFn;

                        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 message_transport

#endif