image_bag_processor.h

Go to the documentation of this file.

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <boost/foreach.hpp>
#include <boost/progress.hpp>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>

namespace bag_tools
{

class ImageBagProcessor
{

public:

  typedef boost::function<void (const sensor_msgs::ImageConstPtr&)> CallbackType;
  typedef boost::function<void (const sensor_msgs::CompressedImageConstPtr&)> CallbackCompressedType;

  ImageBagProcessor(const std::string& image_topic) :
    image_topic_(image_topic)
  {
    ros::Time::init();
  }

  void registerCallback(const CallbackType& callback)
  {
    callback_ = callback;
  }

  void registerCompressedCallback(const CallbackCompressedType& callback)
  {
    callback_compressed_ = callback;
  }

  void processBag(const std::string &filename)
  {
    // Image topics to load
    std::vector<std::string> topics;
    topics.push_back(image_topic_);

    std::cout << "Starting processing " << filename << ", ";
    rosbag::Bag bag;
    bag.open(filename, rosbag::bagmode::Read);
    rosbag::View view(bag, rosbag::TopicQuery(topics));

    int num_messages = view.size();
    std::cout << num_messages << " messages to process." << std::endl;

    // Load all messages
    boost::progress_display show_progress(num_messages);
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
      if (m.getTopic() == image_topic_)
      {
        sensor_msgs::Image::ConstPtr img_msg = m.instantiate<sensor_msgs::Image>();
        if (img_msg != NULL && callback_.empty() == false)
        {
          callback_(img_msg);
        }else
        {
          sensor_msgs::CompressedImageConstPtr compressed = m.instantiate<sensor_msgs::CompressedImage>();
          if(compressed != NULL && callback_compressed_.empty() == false)
          {
              callback_compressed_(compressed);
          }
        }
      }
      ++show_progress;
    }
    bag.close();
    std::cout << "Finished processing " << filename << std::endl;
  }

private:

  std::string image_topic_;
  CallbackType callback_;
  CallbackCompressedType callback_compressed_;

};

} // end of namespace