stereo_bag_processor.h
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 #include <ros/ros.h>
00030 #include <rosbag/bag.h>
00031 #include <rosbag/view.h>
00032 
00033 #include <boost/foreach.hpp>
00034 #include <boost/progress.hpp>
00035 
00036 #include <message_filters/subscriber.h>
00037 #include <message_filters/time_synchronizer.h>
00038 
00039 #include <sensor_msgs/Image.h>
00040 #include <sensor_msgs/CameraInfo.h>
00041 
00042 
00043 namespace bag_tools
00044 {
00045 
00050 template <class M>
00051 class BagSubscriber : public message_filters::SimpleFilter<M>
00052 {
00053 public:
00054   void newMessage(const boost::shared_ptr<M const> &msg)
00055   {
00056     signalMessage(msg);
00057   }
00058 };
00059 
00060 class StereoBagProcessor
00061 {
00062 
00063 public:
00064   StereoBagProcessor(const std::string& stereo_base_topic) :
00065     stereo_base_topic_(stereo_base_topic),
00066     sync_(l_img_sub_, r_img_sub_, l_info_sub_, r_info_sub_, 25)
00067   {
00068     ros::Time::init();
00069   }
00070 
00071   template<class C>
00072   void registerCallback(const C& callback)
00073   {
00074     sync_.registerCallback(callback);
00075   }
00076 
00081   void processBag(const std::string &filename)
00082   {
00083 
00084     // Image topics to load
00085     std::string l_cam = stereo_base_topic_ + "/left";
00086     std::string r_cam = stereo_base_topic_ + "/right";
00087     std::string l_cam_image = l_cam + "/image_raw";
00088     std::string r_cam_image = r_cam + "/image_raw";
00089     std::string l_cam_info = l_cam + "/camera_info";
00090     std::string r_cam_info = r_cam + "/camera_info";
00091 
00092     std::vector<std::string> topics;
00093     topics.push_back(l_cam_image);
00094     topics.push_back(r_cam_image);
00095     topics.push_back(l_cam_info);
00096     topics.push_back(r_cam_info);
00097 
00098     std::cout << "Starting processing " << filename << ", ";
00099     rosbag::Bag bag;
00100     bag.open(filename, rosbag::bagmode::Read);
00101     rosbag::View view(bag, rosbag::TopicQuery(topics));
00102 
00103     int num_messages = view.size();
00104     std::cout << num_messages << " messages to process." << std::endl;
00105 
00106     // Load all messages
00107     boost::progress_display show_progress(num_messages);
00108     BOOST_FOREACH(rosbag::MessageInstance const m, view)
00109     {
00110       if (m.getTopic() == l_cam_image || ("/" + m.getTopic() == l_cam_image))
00111       {
00112         sensor_msgs::Image::ConstPtr l_img = m.instantiate<sensor_msgs::Image>();
00113         if (l_img != NULL)
00114           l_img_sub_.newMessage(l_img);
00115       }
00116 
00117       if (m.getTopic() == r_cam_image || ("/" + m.getTopic() == r_cam_image))
00118       {
00119         sensor_msgs::Image::ConstPtr r_img = m.instantiate<sensor_msgs::Image>();
00120         if (r_img != NULL)
00121           r_img_sub_.newMessage(r_img);
00122       }
00123 
00124       if (m.getTopic() == l_cam_info || ("/" + m.getTopic() == l_cam_info))
00125       {
00126         sensor_msgs::CameraInfo::ConstPtr l_info = m.instantiate<sensor_msgs::CameraInfo>();
00127         if (l_info != NULL)
00128           l_info_sub_.newMessage(l_info);
00129       }
00130 
00131       if (m.getTopic() == r_cam_info || ("/" + m.getTopic() == r_cam_info))
00132       {
00133         sensor_msgs::CameraInfo::ConstPtr r_info = m.instantiate<sensor_msgs::CameraInfo>();
00134         if (r_info != NULL)
00135           r_info_sub_.newMessage(r_info);
00136       }
00137       ++show_progress;
00138     }
00139     bag.close();
00140     std::cout << "Finished processing " << filename << std::endl;
00141   }
00142 
00143 private:
00144 
00145   // Fake subscribers to capture images
00146   BagSubscriber<sensor_msgs::Image> l_img_sub_, r_img_sub_;
00147   BagSubscriber<sensor_msgs::CameraInfo> l_info_sub_, r_info_sub_;
00148 
00149   std::string stereo_base_topic_;
00150 
00151   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> sync_;
00152 
00153 };
00154 
00155 } // end of namespace
00156 


bag_tools
Author(s): Stephan Wirth , Miquel Massot
autogenerated on Fri Aug 28 2015 13:15:13