Revision history [back]

First of all, you can't do the processing in the callback, because it will (potentially) block the other nodelets.

Here is what you can do:

1. Create a Synchronizer of queue size 1.
2. Create your own thread to do the processing. It will contain a loop and will be waiting to be notified in order to start the processing.
3. In the callback, you should check if data are being currently processed. If yes, discrard the received data. Otherwise, store the data and notify the processing loop.

Some libraries of interest are thread, mutex, atomic, and condition variable. Here is how this could translate to code:

void processingLoop() {
  while (ros::ok()) {
    std::unique_lock<std::mutex> lock(img_mutex);
    if (has_new_data.load()) {
      // do heavy processing
    }
    has_new_data.store(false);
    processing_cv.wait(lock, [&] { return has_new_data.load(); });
  }
}

void callback(const RgbConstPtr& rgb, const DepthConstPtr& depth) {
  std::unique_lock<std::mutex> lock(img_mutex, std::try_to_lock);
  if (!lock.owns_lock()) return;

  img_rgb = rgb;
  img_depth = depth;
  has_new_data.store(true);

  lock.unlock();
  processing_cv.notify_one();
}