camera_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 CameraBagProcessor
00061 {
00062 
00063 public:
00064   CameraBagProcessor(const std::string& camera_base_topic) :
00065     camera_base_topic_(camera_base_topic),
00066     sync_(img_sub_, 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 cam_image = camera_base_topic_ + "/image_raw";
00086     std::string cam_info = camera_base_topic_ + "/camera_info";
00087 
00088     std::vector<std::string> topics;
00089     topics.push_back(cam_image);
00090     topics.push_back(cam_info);
00091 
00092     std::cout << "Starting processing " << filename << ", ";
00093     rosbag::Bag bag;
00094     bag.open(filename, rosbag::bagmode::Read);
00095     rosbag::View view(bag, rosbag::TopicQuery(topics));
00096 
00097     int num_messages = view.size();
00098     std::cout << num_messages << " messages to process." << std::endl;
00099 
00100     // Load all messages
00101     boost::progress_display show_progress(num_messages);
00102     BOOST_FOREACH(rosbag::MessageInstance const m, view)
00103     {
00104       if (m.getTopic() == cam_image || ("/" + m.getTopic() == cam_image))
00105       {
00106         sensor_msgs::Image::ConstPtr img = m.instantiate<sensor_msgs::Image>();
00107         if (img != NULL)
00108           img_sub_.newMessage(img);
00109       }
00110 
00111       if (m.getTopic() == cam_info || ("/" + m.getTopic() == cam_info))
00112       {
00113         sensor_msgs::CameraInfo::ConstPtr info = m.instantiate<sensor_msgs::CameraInfo>();
00114         if (info != NULL)
00115           info_sub_.newMessage(info);
00116       }
00117 
00118       ++show_progress;
00119     }
00120     bag.close();
00121     std::cout << "Finished processing " << filename << std::endl;
00122   }
00123 
00124 private:
00125 
00126   // Fake subscribers to capture images
00127   BagSubscriber<sensor_msgs::Image> img_sub_;
00128   BagSubscriber<sensor_msgs::CameraInfo> info_sub_;
00129 
00130   std::string camera_base_topic_;
00131 
00132   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
00133 
00134 };
00135 
00136 } // end of namespace
00137 


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