SpinningNodeHandle: an extension to NodeHandle that guarantees that a spinner exists SpinnableNodeHandle: another extension that provides start() and stop(), a separate CallbackQueue and a separate asynchronous spinner

spinnable_nodehandle.h

#ifndef SPINNABLE_NODEHANDLE_H
#define SPINNABLE_NODEHANDLE_H

#include <boost/thread.hpp>
#include <ros/node_handle.h>
#include <ros/spinner.h>
#include <ros/callback_queue.h>

namespace ros
{

class AnonymousSpinner
{
public:
  AnonymousSpinner(int n_threads = 1,
      CallbackQueue* cbq = getGlobalCallbackQueue())
    : ok_(false)
    , cb_queue_(cbq)
    , n_threads_(n_threads)
  {}

  virtual ~AnonymousSpinner()
  {
    stop();
  }

  void start()
  {
    boost::mutex::scoped_lock(mutex_);

    ok_ = true;
    for (int i = 0; i < n_threads_; ++i)
      threads_.create_thread(boost::bind(&AnonymousSpinner::spin, this));
  }

  void stop()
  {
    boost::mutex::scoped_lock(mutex_);

    if (!ok_)
      return;

    ok_ = false;
    threads_.join_all();
  }

private:
  void spin()
  {
    WallDuration timeout(0.1);
    bool use_call_available = n_threads_ == 1;

    while(ok_ && nh_.ok())
    {
      if (use_call_available)
        cb_queue_->callAvailable(timeout);
      else
        cb_queue_->callOne(timeout);
    }
  }

  bool ok_;
  NodeHandle nh_;
  CallbackQueue* cb_queue_;
  int n_threads_;
  boost::thread_group threads_;
  boost::mutex mutex_;
};

class SpinnableNodeHandle : public NodeHandle
{
public:
    SpinnableNodeHandle(const std::string& ns = std::string(),
        const M_string& remappings = M_string())
      : NodeHandle(ns, remappings)
      , spinner_(1, &cb_queue_)
    {}

    SpinnableNodeHandle(int n_threads, const std::string& ns = std::string(),
        const M_string& remappings = M_string())
      : NodeHandle(ns, remappings)
      , spinner_(n_threads, &cb_queue_)
    {}

    SpinnableNodeHandle(const NodeHandle& rhs)
      : NodeHandle(rhs)
    {}

    SpinnableNodeHandle(const NodeHandle& parent, const std::string& ns)
      : NodeHandle(parent, ns)
    {}

    SpinnableNodeHandle(const NodeHandle& parent, const std::string& ns, const M_string& remappings)
      : NodeHandle(parent, ns, remappings)
    {}

    SpinnableNodeHandle& operator=(const NodeHandle& rhs)
    {
      NodeHandle::operator=(rhs);
      return *this;
    }

    void start()
    {
      spinner_.start();
    }

    void stop()
    {
      spinner_.stop();
    }

private:
    CallbackQueue cb_queue_;
    AnonymousSpinner spinner_;
};

}

#endif

spinning_nodehandle.h

#ifndef SPINNING_NODEHANDLE_H
#define SPINNING_NODEHANDLE_H

#include <ros/node_handle.h>
#include <ros/spinner.h>

namespace
{
boost::shared_ptr<ros::AsyncSpinner> g_aspin;
}

namespace ros
{

class SpinningNodeHandle : public NodeHandle
{
public:
    SpinningNodeHandle(const std::string& ns = std::string(), const M_string& remappings = M_string())
      : NodeHandle(ns, remappings) { init(); }

    SpinningNodeHandle(const NodeHandle& rhs)
      : NodeHandle(rhs) { init(); }

    SpinningNodeHandle(const NodeHandle& parent, const std::string& ns)
      : NodeHandle(parent, ns) { init(); }

    SpinningNodeHandle(const NodeHandle& parent, const std::string& ns, const M_string& remappings)
      : NodeHandle(parent, ns, remappings) { init(); }

    SpinningNodeHandle& operator=(const NodeHandle& rhs)
    {
      NodeHandle::operator=(rhs);
      return *this;
    }
private:
    void init()
    {
      ROS_INFO("creating a SpinningNodeHandle");
      if (!g_aspin)
      {
        ROS_INFO("launching an asynchronous spinner");
        g_aspin.reset(new ros::AsyncSpinner(1));
        g_aspin->start();
      }
    }
};

}

#endif