camera_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 <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

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


namespace bag_tools
{

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
    signalMessage(msg);
  }
};

class CameraBagProcessor
{

public:
  CameraBagProcessor(const std::string& camera_base_topic) :
    camera_base_topic_(camera_base_topic),
    sync_(img_sub_, info_sub_, 25)
  {
    ros::Time::init();
  }

  template<class C>
  void registerCallback(const C& callback)
  {
    sync_.registerCallback(callback);
  }

  void processBag(const std::string &filename)
  {

    // Image topics to load
    std::string cam_image = camera_base_topic_ + "/image_raw";
    std::string cam_info = camera_base_topic_ + "/camera_info";

    std::vector<std::string> topics;
    topics.push_back(cam_image);
    topics.push_back(cam_info);

    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() == cam_image || ("/" + m.getTopic() == cam_image))
      {
        sensor_msgs::Image::ConstPtr img = m.instantiate<sensor_msgs::Image>();
        if (img != NULL)
          img_sub_.newMessage(img);
      }

      if (m.getTopic() == cam_info || ("/" + m.getTopic() == cam_info))
      {
        sensor_msgs::CameraInfo::ConstPtr info = m.instantiate<sensor_msgs::CameraInfo>();
        if (info != NULL)
          info_sub_.newMessage(info);
      }

      ++show_progress;
    }
    bag.close();
    std::cout << "Finished processing " << filename << std::endl;
  }

private:

  // Fake subscribers to capture images
  BagSubscriber<sensor_msgs::Image> img_sub_;
  BagSubscriber<sensor_msgs::CameraInfo> info_sub_;

  std::string camera_base_topic_;

  message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;

};

} // end of namespace