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 <sensor_msgs/Image.h> 00037 00038 namespace bag_tools 00039 { 00040 00041 class ImageBagProcessor 00042 { 00043 00044 public: 00045 00046 typedef boost::function<void (const sensor_msgs::ImageConstPtr&)> CallbackType; 00047 00048 ImageBagProcessor(const std::string& image_topic) : 00049 image_topic_(image_topic) 00050 { 00051 ros::Time::init(); 00052 } 00053 00054 void registerCallback(const CallbackType& callback) 00055 { 00056 callback_ = callback; 00057 } 00058 00063 void processBag(const std::string &filename) 00064 { 00065 // Image topics to load 00066 std::vector<std::string> topics; 00067 topics.push_back(image_topic_); 00068 00069 std::cout << "Starting processing " << filename << ", "; 00070 rosbag::Bag bag; 00071 bag.open(filename, rosbag::bagmode::Read); 00072 rosbag::View view(bag, rosbag::TopicQuery(topics)); 00073 00074 int num_messages = view.size(); 00075 std::cout << num_messages << " messages to process." << std::endl; 00076 00077 // Load all messages 00078 boost::progress_display show_progress(num_messages); 00079 BOOST_FOREACH(rosbag::MessageInstance const m, view) 00080 { 00081 if (m.getTopic() == image_topic_) 00082 { 00083 sensor_msgs::Image::ConstPtr img_msg = m.instantiate<sensor_msgs::Image>(); 00084 if (img_msg != NULL) 00085 callback_(img_msg); 00086 } 00087 ++show_progress; 00088 } 00089 bag.close(); 00090 std::cout << "Finished processing " << filename << std::endl; 00091 } 00092 00093 private: 00094 00095 std::string image_topic_; 00096 CallbackType callback_; 00097 00098 }; 00099 00100 } // end of namespace 00101