ros 回调接口和回调队列

/**
 * \brief Abstract interface for items which can be added to a CallbackQueueInterface
 */
class CallbackInterface
{
public:
  /**
   * \brief Possible results for the call() method
   */
  enum CallResult
  {
    Success,   ///< Call succeeded
    TryAgain,  ///< Call not ready, try again later
    Invalid,   ///< Call no longer valid
  };

  virtual ~CallbackInterface() {}

  /**
   * \brief Call this callback
   * \return The result of the call
   */
  virtual CallResult call() = 0;
  /**
   * \brief Provides the opportunity for specifying that a callback is not ready to be called
   * before call() actually takes place.
   */
  virtual bool ready() { return true; }
};
typedef boost::shared_ptr<CallbackInterface> CallbackInterfacePtr;

/**
 * \brief Abstract interface for a queue used to handle all callbacks within roscpp.
 *
 * Allows you to inherit and provide your own implementation that can be used instead of our
 * default CallbackQueue
 */
class CallbackQueueInterface
{
public:
  virtual ~CallbackQueueInterface() {}

  /**
   * \brief Add a callback, with an optional owner id.  The owner id can be used to
   * remove a set of callbacks from this queue.
   */
  virtual void addCallback(const CallbackInterfacePtr& callback, uint64_t owner_id = 0) = 0;

  /**
   * \brief Remove all callbacks associated with an owner id
   */
  virtual void removeByID(uint64_t owner_id) = 0;
};

ros_common/callback_queue.h

/**
 * \brief This is the default implementation of the ros::CallbackQueueInterface
 */
class ROSCPP_DECL CallbackQueue : public CallbackQueueInterface
{
public:
  CallbackQueue(bool enabled = true);
  virtual ~CallbackQueue();

  virtual void addCallback(const CallbackInterfacePtr& callback, uint64_t removal_id = 0);
  virtual void removeByID(uint64_t removal_id);

  enum CallOneResult
  {
    Called,
    TryAgain,
    Disabled,
    Empty,
  };

  /**
   * \brief Pop a single callback off the front of the queue and invoke it.  If the callback was not ready to be called,
   * pushes it back onto the queue.
   */
  CallOneResult callOne()
  {
    return callOne(ros::WallDuration());
  }

  /**
   * \brief Pop a single callback off the front of the queue and invoke it.  If the callback was not ready to be called,
   * pushes it back onto the queue.  This version includes a timeout which lets you specify the amount of time to wait for
   * a callback to be available before returning.
   *
   * \param timeout The amount of time to wait for a callback to be available.  If there is already a callback available,
   * this parameter does nothing.
   */
  CallOneResult callOne(ros::WallDuration timeout);

  /**
   * \brief Invoke all callbacks currently in the queue.  If a callback was not ready to be called, pushes it back onto the queue.
   */
  void callAvailable()
  {
    callAvailable(ros::WallDuration());
  }
  /**
   * \brief Invoke all callbacks currently in the queue.  If a callback was not ready to be called, pushes it back onto the queue.  This version
   * includes a timeout which lets you specify the amount of time to wait for a callback to be available before returning.
   *
   * \param timeout The amount of time to wait for at least one callback to be available.  If there is already at least one callback available,
   * this parameter does nothing.
   */
  void callAvailable(ros::WallDuration timeout);

  /**
   * \brief returns whether or not the queue is empty
   */
  bool empty() { return isEmpty(); }
  /**
   * \brief returns whether or not the queue is empty
   */
  bool isEmpty();
  /**
   * \brief Removes all callbacks from the queue.  Does \b not wait for calls currently in progress to finish.
   */
  void clear();

  /**
   * \brief Enable the queue (queue is enabled by default)
   */
  void enable();
  /**
   * \brief Disable the queue, meaning any calls to addCallback() will have no effect
   */
  void disable();
  /**
   * \brief Returns whether or not this queue is enabled
   */
  bool isEnabled();

protected:
  void setupTLS();

  struct TLS;
  CallOneResult callOneCB(TLS* tls);

  struct IDInfo
  {
    uint64_t id;
    boost::shared_mutex calling_rw_mutex;
  };
  typedef boost::shared_ptr<IDInfo> IDInfoPtr;
  typedef std::map<uint64_t, IDInfoPtr> M_IDInfo;

  IDInfoPtr getIDInfo(uint64_t id);

  struct CallbackInfo
  {
    CallbackInfo()
    : removal_id(0)
    , marked_for_removal(false)
    {}
    CallbackInterfacePtr callback;
    uint64_t removal_id;
    bool marked_for_removal;
  };
  typedef std::list<CallbackInfo> L_CallbackInfo;
  typedef std::deque<CallbackInfo> D_CallbackInfo;
  D_CallbackInfo callbacks_;
  size_t calling_;
  boost::mutex mutex_;
  boost::condition_variable condition_;

  boost::mutex id_info_mutex_;
  M_IDInfo id_info_;

  struct TLS
  {
    TLS()
    : calling_in_this_thread(0xffffffffffffffffULL)
    , cb_it(callbacks.end())
    {}
    uint64_t calling_in_this_thread;
    D_CallbackInfo callbacks;
    D_CallbackInfo::iterator cb_it;
  };
  boost::thread_specific_ptr<TLS> tls_;

  bool enabled_;
};
typedef boost::shared_ptr<CallbackQueue> CallbackQueuePtr;

ros_common/callback_queue.cpp

CallbackQueue::CallbackQueue(bool enabled)
: calling_(0)
, enabled_(enabled)
{
}

CallbackQueue::~CallbackQueue()
{
  disable();
}

void CallbackQueue::enable()
{
  boost::mutex::scoped_lock lock(mutex_);
  enabled_ = true;

  condition_.notify_all();
}

void CallbackQueue::disable()
{
  boost::mutex::scoped_lock lock(mutex_);
  enabled_ = false;

  condition_.notify_all();
}

void CallbackQueue::clear()
{
  boost::mutex::scoped_lock lock(mutex_);

  callbacks_.clear();
}

bool CallbackQueue::isEmpty()
{
  boost::mutex::scoped_lock lock(mutex_);

  return callbacks_.empty() && calling_ == 0;
}

bool CallbackQueue::isEnabled()
{
  boost::mutex::scoped_lock lock(mutex_);

  return enabled_;
}

void CallbackQueue::setupTLS()
{
  if (!tls_.get())
  {
    tls_.reset(new TLS);
  }
}

void CallbackQueue::addCallback(const CallbackInterfacePtr& callback, uint64_t removal_id)
{
  CallbackInfo info;
  info.callback = callback;
  info.removal_id = removal_id;

  {
    boost::mutex::scoped_lock lock(id_info_mutex_);

    M_IDInfo::iterator it = id_info_.find(removal_id);
    if (it == id_info_.end())
    {
      IDInfoPtr id_info(boost::make_shared<IDInfo>());
      id_info->id = removal_id;
      id_info_.insert(std::make_pair(removal_id, id_info));
    }
  }

  {
    boost::mutex::scoped_lock lock(mutex_);

    if (!enabled_)
    {
      return;
    }

    callbacks_.push_back(info);
  }

  condition_.notify_one();
}

CallbackQueue::IDInfoPtr CallbackQueue::getIDInfo(uint64_t id)
{
  boost::mutex::scoped_lock lock(id_info_mutex_);
  M_IDInfo::iterator it = id_info_.find(id);
  if (it != id_info_.end())
  {
    return it->second;
  }

  return IDInfoPtr();
}

void CallbackQueue::removeByID(uint64_t removal_id)
{
  setupTLS();

  {
    IDInfoPtr id_info;
    {
      boost::mutex::scoped_lock lock(id_info_mutex_);
      M_IDInfo::iterator it = id_info_.find(removal_id);
      if (it != id_info_.end())
      {
        id_info = it->second;
      }
      else
      {
        return;
      }
    }

    // If we're being called from within a callback from our queue, we must unlock the shared lock we already own
    // here so that we can take a unique lock.  We'll re-lock it later.
    if (tls_->calling_in_this_thread == id_info->id)
    {
      id_info->calling_rw_mutex.unlock_shared();
    }

    {
      boost::unique_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex, boost::defer_lock);
      if (rw_lock.try_lock())
      {
        boost::mutex::scoped_lock lock(mutex_);
        D_CallbackInfo::iterator it = callbacks_.begin();
        for (; it != callbacks_.end();)
        {
          CallbackInfo& info = *it;
          if (info.removal_id == removal_id)
          {
            it = callbacks_.erase(it);
          }
          else
          {
            ++it;
          }
        }
      }
      else
      {
        // We failed to acquire the lock, it can be that we are trying to remove something from the callback queue
        // while it is being executed. Mark it for removal and let it be cleaned up later.
        boost::mutex::scoped_lock lock(mutex_);
        for (D_CallbackInfo::iterator it = callbacks_.begin(); it != callbacks_.end(); it++)
        {
          CallbackInfo& info = *it;
          if (info.removal_id == removal_id)
          {
            info.marked_for_removal = true;
          }
        }
      }
    }

    if (tls_->calling_in_this_thread == id_info->id)
    {
      id_info->calling_rw_mutex.lock_shared();
    }
  }

  // If we're being called from within a callback, we need to remove the callbacks that match the id that have already been
  // popped off the queue
  {
    D_CallbackInfo::iterator it = tls_->callbacks.begin();
    D_CallbackInfo::iterator end = tls_->callbacks.end();
    for (; it != end; ++it)
    {
      CallbackInfo& info = *it;
      if (info.removal_id == removal_id)
      {
        info.marked_for_removal = true;
      }
    }
  }

  {
    boost::mutex::scoped_lock lock(id_info_mutex_);
    id_info_.erase(removal_id);
  }
}

CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
{
  setupTLS();
  TLS* tls = tls_.get();

  CallbackInfo cb_info;

  {
    boost::mutex::scoped_lock lock(mutex_);

    if (!enabled_)
    {
      return Disabled;
    }

    if (callbacks_.empty())
    {
      if (!timeout.isZero())
      {
        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
      }

      if (callbacks_.empty())
      {
        return Empty;
      }

      if (!enabled_)
      {
        return Disabled;
      }
    }

    D_CallbackInfo::iterator it = callbacks_.begin();
    for (; it != callbacks_.end();)
    {
      CallbackInfo& info = *it;

      if (info.marked_for_removal)
      {
        it = callbacks_.erase(it);
        continue;
      }

      if (info.callback->ready())
      {
        cb_info = info;
        it = callbacks_.erase(it);
        break;
      }

      ++it;
    }

    if (!cb_info.callback)
    {
      return TryAgain;
    }

    ++calling_;
  }

  bool was_empty = tls->callbacks.empty();
  tls->callbacks.push_back(cb_info);
  if (was_empty)
  {
    tls->cb_it = tls->callbacks.begin();
  }

  CallOneResult res = callOneCB(tls);
  if (res != Empty)
  {
    boost::mutex::scoped_lock lock(mutex_);
    --calling_;
  }
  return res;
}

void CallbackQueue::callAvailable(ros::WallDuration timeout)
{
  setupTLS();
  TLS* tls = tls_.get();

  {
    boost::mutex::scoped_lock lock(mutex_);

    if (!enabled_)
    {
      return;
    }

    if (callbacks_.empty())
    {
      if (!timeout.isZero())
      {
        condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
      }

      if (callbacks_.empty() || !enabled_)
      {
        return;
      }
    }

    bool was_empty = tls->callbacks.empty();

    tls->callbacks.insert(tls->callbacks.end(), callbacks_.begin(), callbacks_.end());
    callbacks_.clear();

    calling_ += tls->callbacks.size();

    if (was_empty)
    {
      tls->cb_it = tls->callbacks.begin();
    }
  }

  size_t called = 0;

  while (!tls->callbacks.empty())
  {
    if (callOneCB(tls) != Empty)
    {
      ++called;
    }
  }

  {
    boost::mutex::scoped_lock lock(mutex_);
    calling_ -= called;
  }
}

CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
{
  // Check for a recursive call.  If recursive, increment the current iterator.  Otherwise
  // set the iterator it the beginning of the thread-local callbacks
  if (tls->calling_in_this_thread == 0xffffffffffffffffULL)
  {
    tls->cb_it = tls->callbacks.begin();
  }

  if (tls->cb_it == tls->callbacks.end())
  {
    return Empty;
  }

  ROS_ASSERT(!tls->callbacks.empty());
  ROS_ASSERT(tls->cb_it != tls->callbacks.end());

  CallbackInfo info = *tls->cb_it;
  CallbackInterfacePtr& cb = info.callback;

  IDInfoPtr id_info = getIDInfo(info.removal_id);
  if (id_info)
  {
    boost::shared_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);

    uint64_t last_calling = tls->calling_in_this_thread;
    tls->calling_in_this_thread = id_info->id;

    CallbackInterface::CallResult result = CallbackInterface::Invalid;

    {
      // Ensure that thread id gets restored, even if callback throws.
      // This is done with RAII rather than try-catch so that the source
      // of the original exception is not masked in a crash report.
      BOOST_SCOPE_EXIT(&tls, &last_calling)
      {
        tls->calling_in_this_thread = last_calling;
      }
      BOOST_SCOPE_EXIT_END

      if (info.marked_for_removal)
      {
        tls->cb_it = tls->callbacks.erase(tls->cb_it);
      }
      else
      {
        tls->cb_it = tls->callbacks.erase(tls->cb_it);
        result = cb->call();
      }
    }

    // Push TryAgain callbacks to the back of the shared queue
    if (result == CallbackInterface::TryAgain && !info.marked_for_removal)
    {
      boost::mutex::scoped_lock lock(mutex_);
      callbacks_.push_back(info);

      return TryAgain;
    }

    return Called;
  }
  else
  {
    tls->cb_it = tls->callbacks.erase(tls->cb_it);
  }

  return Called;
}