ROS Nodelet receive and processing in parallel

edit

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();
}

For making sure the newest message is used, you should set your topic history queues to 1. That way, every time a new message is received it will overwrite the previous one, so only the most recent is available at all times.

For synchronising the depth and rgb images, you can use the Time Synchronizer message filter. I'm not sure how this filter handles history, so you might need to experiment to make sure it only passes the most recent of both the image and depth topics.