stereo_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 StereoBagProcessor
{

public:
  StereoBagProcessor(const std::string& stereo_base_topic) :
    stereo_base_topic_(stereo_base_topic),
    sync_(l_img_sub_, r_img_sub_, l_info_sub_, r_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 l_cam = stereo_base_topic_ + "/left";
    std::string r_cam = stereo_base_topic_ + "/right";
    std::string l_cam_image = l_cam + "/image_raw";
    std::string r_cam_image = r_cam + "/image_raw";
    std::string l_cam_info = l_cam + "/camera_info";
    std::string r_cam_info = r_cam + "/camera_info";

    std::vector<std::string> topics;
    topics.push_back(l_cam_image);
    topics.push_back(r_cam_image);
    topics.push_back(l_cam_info);
    topics.push_back(r_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() == l_cam_image || ("/" + m.getTopic() == l_cam_image))
      {
        sensor_msgs::Image::ConstPtr l_img = m.instantiate<sensor_msgs::Image>();
        if (l_img != NULL)
          l_img_sub_.newMessage(l_img);
      }

      if (m.getTopic() == r_cam_image || ("/" + m.getTopic() == r_cam_image))
      {
        sensor_msgs::Image::ConstPtr r_img = m.instantiate<sensor_msgs::Image>();
        if (r_img != NULL)
          r_img_sub_.newMessage(r_img);
      }

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

      if (m.getTopic() == r_cam_info || ("/" + m.getTopic() == r_cam_info))
      {
        sensor_msgs::CameraInfo::ConstPtr r_info = m.instantiate<sensor_msgs::CameraInfo>();
        if (r_info != NULL)
          r_info_sub_.newMessage(r_info);
      }
      ++show_progress;
    }
    bag.close();
    std::cout << "Finished processing " << filename << std::endl;
  }

private:

  // Fake subscribers to capture images
  BagSubscriber<sensor_msgs::Image> l_img_sub_, r_img_sub_;
  BagSubscriber<sensor_msgs::CameraInfo> l_info_sub_, r_info_sub_;

  std::string stereo_base_topic_;

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

};

} // end of namespace