managed_subscriber.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, University of Luxembourg
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of University of Luxembourg nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Maciej Zurad
 *********************************************************************/
#ifndef ROBOT_ACTIVITY_RESOURCE_MANAGED_SUBSCRIBER_H
#define ROBOT_ACTIVITY_RESOURCE_MANAGED_SUBSCRIBER_H

#include <ros/ros.h>
#include <ros/console.h>

#include <robot_activity/resource/managed_resource.h>

#include <string>

namespace robot_activity
{
namespace resource
{

class ManagedSubscriber : public Managed<ManagedSubscriber, ros::Subscriber>
{
public:
  using Managed<ManagedSubscriber, ros::Subscriber>::Managed;

  void subscribe(const ros::NodeHandlePtr& node_handle)
  {
    return acquire(node_handle);
  }

  void shutdown()
  {
    return release();
  }

  template <class Message>
  using MessageCallback = boost::function<void(Message)>;

  template<class Message>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    const MessageCallback<Message>& callback,
    const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr(),
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG("makeLazyAcquirer MessageCallback<Message>& callback form exec");
    return [ = ](const ros::NodeHandlePtr & nh) -> ros::Subscriber
    {
      ROS_DEBUG("Subscribing...");
      return nh->subscribe<Message>(
        topic,
        queue_size,
        static_cast<MessageCallback<Message>>(wrapMessageCallback(callback)),
        tracked_object,
        transport_hints);
    };
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG("makeLazyAcquirer void(T::*fp)(M), T* obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj, _1);
    return makeLazyAcquirer(topic, queue_size, callback, ros::VoidConstPtr(), transport_hints);
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size, void(T::*fp)(M) const, T* obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG("makeLazyAcquirer void(T::*fp)(M) const, T* obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj, _1);
    return makeLazyAcquirer(topic, queue_size, callback, ros::VoidConstPtr(), transport_hints);
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    void(T::*fp)(const boost::shared_ptr<M const>&), T* obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(T::*fp)(const boost::shared_ptr<M const>&), "
                     << "T* obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj, _1);
    return makeLazyAcquirer(topic, queue_size, callback, ros::VoidConstPtr(), transport_hints);
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    void(T::*fp)(const boost::shared_ptr<M const>&) const, T* obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(T::*fp)(const boost::shared_ptr<M const>&) const, "
                     << "T* obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj, _1);
    return makeLazyAcquirer(topic, queue_size, callback, ros::VoidConstPtr(), transport_hints);
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    void(T::*fp)(M), const boost::shared_ptr<T>& obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(T::*fp)(M), "
                     << "const boost::shared_ptr<T>& obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj.get(), _1);
    return makeLazyAcquirer(topic, queue_size, callback, obj, transport_hints);
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    void(T::*fp)(M) const, const boost::shared_ptr<T>& obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(T::*fp)(M) const, "
                     << "const boost::shared_ptr<T>& obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj.get(), _1);
    return makeLazyAcquirer(topic, queue_size, callback, obj, transport_hints);
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    void(T::*fp)(const boost::shared_ptr<M const>&),
    const boost::shared_ptr<T>& obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(T::*fp)(const boost::shared_ptr<M const>&), "
                     << "const boost::shared_ptr<T>& obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj.get(), _1);
    return makeLazyAcquirer(topic, queue_size, callback, obj, transport_hints);
  }

  template<class M, class T>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    void(T::*fp)(const boost::shared_ptr<M const>&) const,
    const boost::shared_ptr<T>& obj,
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(T::*fp)(const boost::shared_ptr<M const>&) const, "
                     << "const boost::shared_ptr<T>& obj, form exec");
    MessageCallback<M> callback = boost::bind(fp, obj.get(), _1);
    return makeLazyAcquirer(topic, queue_size, callback, obj, transport_hints);
  }

  template<class M>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size, void(*fp)(M),
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(*fp)(M) form exec");
    MessageCallback<M> callback = boost::bind(fp);
    return makeLazyAcquirer(topic, queue_size, callback, ros::VoidConstPtr(), transport_hints);
  }

  template<class M>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size, void(*fp)(const boost::shared_ptr<M const>&),
    const ros::TransportHints& transport_hints = ros::TransportHints()) const
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
                     << "void(*fp)(const boost::shared_ptr<M const>&) form exec");
    MessageCallback<M> callback = boost::bind(fp);
    return makeLazyAcquirer(topic, queue_size, callback, ros::VoidConstPtr(), transport_hints);
  }

  /* TODO - remove if unneccessary
  template<class M>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    const boost::function<void (const boost::shared_ptr<M const>&)>& callback,
    const ros::VoidConstPtr& tracked_object,
    const ros::TransportHints& transport_hints)
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
      << "const boost::function<void (const boost::shared_ptr<M const>&)>& callback, "
      << "form exec");
    //MessageCallback<const boost::shared_ptr<M const>&> callback = callback;
    return makeLazyAcquirer(topic, queue_size, callback, tracked_object, transport_hints);
  }

  template<class M, class C>
  LazyAcquirer makeLazyAcquirer(
    const std::string& topic, uint32_t queue_size,
    const boost::function<void (C)>& callback,
    const ros::VoidConstPtr& tracked_object,
    const ros::TransportHints& transport_hints)
  {
    ROS_DEBUG_STREAM("makeLazyAcquirer "
      << "const boost::function<void (C)>& callback form exec");
    return makeLazyAcquirer(topic, queue_size, callback, tracked_object, transport_hints);
  }
  */
private:
  template<class Message>
  MessageCallback<Message> wrapMessageCallback(const MessageCallback<Message>& callback) const
  {
    return [this, &callback](Message message)
    {
      if (!paused_)
        callback(message);
      else
        ROS_DEBUG("callback is paused!");
    };
  }
};

}  // namespace resource
}  // namespace robot_activity

#endif  // ROBOT_ACTIVITY_RESOURCE_MANAGED_SUBSCRIBER_H