Merge pull request #1 from abarobopec/indigo-devel

Indigo devel

Author: stwirth

bag_tools/include/bag_tools/image_bag_processor.h

@@ -34,6 +34,7 @@
 #include <boost/progress.hpp>
 
 #include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
 
 namespace bag_tools
 {
@@ -44,6 +45,7 @@ class ImageBagProcessor
 public:
 
   typedef boost::function<void (const sensor_msgs::ImageConstPtr&)> CallbackType;
+  typedef boost::function<void (const sensor_msgs::CompressedImageConstPtr&)> CallbackCompressedType;
 
   ImageBagProcessor(const std::string& image_topic) :
     image_topic_(image_topic)
@@ -56,6 +58,11 @@ class ImageBagProcessor
     callback_ = callback;
   }
 
+  void registerCompressedCallback(const CallbackCompressedType& callback)
+  {
+    callback_compressed_ = callback;
+  }
+
   /**
    * Processes given bagfile, calls registered callback function when
    * a message is found.
@@ -81,8 +88,17 @@ class ImageBagProcessor
       if (m.getTopic() == image_topic_)
       {
         sensor_msgs::Image::ConstPtr img_msg = m.instantiate<sensor_msgs::Image>();
-        if (img_msg != NULL)
+        if (img_msg != NULL && callback_.empty() == false)
+        {
           callback_(img_msg);
+        }else
+        {
+          sensor_msgs::CompressedImageConstPtr compressed = m.instantiate<sensor_msgs::CompressedImage>();
+          if(compressed != NULL && callback_compressed_.empty() == false)
+          {
+              callback_compressed_(compressed);
+          }
+        }
       }
       ++show_progress;
     }
@@ -94,6 +110,7 @@ class ImageBagProcessor
 
   std::string image_topic_;
   CallbackType callback_;
+  CallbackCompressedType callback_compressed_;
 
 };
 

bag_tools/scripts/bag_tools/cut.py

@@ -53,7 +53,7 @@ def cut(inbags, outbagfile, start, duration):
   outbag = rosbag.Bag(outbagfile, 'w')
   num_messages = 0
   for inbag in inbags:
-    rospy.loginfo('   Extracting messages from:', inbag
+    rospy.loginfo('   Extracting messages from:', inbag)
     for topic, msg, t in rosbag.Bag(inbag,'r').read_messages(start_time=start_time, end_time=end_time):
       outbag.write(topic, msg, t)
       num_messages = num_messages + 1

bag_tools/scripts/bag_tools/make_video.py

@@ -33,7 +33,7 @@
 PKG = 'bag_tools' # this package name
 
 import roslib; roslib.load_manifest(PKG)
-
+import rospy
 import tempfile
 import subprocess
 import glob
@@ -56,7 +56,7 @@ def create_video(tmp_dir, args):
     i = i + 1
 
   rospy.loginfo('Creating video...')
-  cmd = ["ffmpeg", "-f", "image2", "-r", str(args.fps), "-i", tmp_dir + "/img-%d.jpg", "-sameq", args.output]
+  cmd = ["ffmpeg", "-f", "image2", "-r", str(args.fps), "-i", tmp_dir + "/img-%d.jpg", args.output]
   rospy.loginfo('    {}'.format(' '.join(cmd)))
   subprocess.call(cmd)
 

bag_tools/src/extract_images.cpp

@@ -31,6 +31,7 @@
 #include <boost/format.hpp>
 
 #include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
 #include <sensor_msgs/CameraInfo.h>
 
 #include <image_proc/processor.h>
@@ -39,7 +40,7 @@
 #include <opencv2/highgui/highgui.hpp> // for cv::imwrite
 
 #include "bag_tools/image_bag_processor.h"
-
+#include <iostream>
 /**
  * Saves color images from raw input
  */
@@ -60,22 +61,33 @@ class ImageSaver
       std::cerr << "ERROR Processing image" << std::endl;
       return;
     }
-    std::string filename = 
-      boost::str(boost::format("%s/%s%lu.%s") 
-          % save_dir_ 
-          % prefix_ 
-          % img->header.stamp.toNSec() 
-          % filetype_);
-    if (!cv::imwrite(filename, output.color))
-      ROS_ERROR_STREAM("ERROR Saving " << filename);
-    else
-    {
-      ROS_DEBUG_STREAM("Saved " << filename);
-      num_saved_++;
-    }
+    save_image(img->header.stamp.toNSec(), output.color);
+  }
+  void save_compressed(const sensor_msgs::CompressedImageConstPtr& _compressed)
+  {
+    cv::Mat image_uncomrpessed = cv::imdecode(cv::Mat(_compressed->data),1);
+    save_image(_compressed->header.stamp.toNSec(), image_uncomrpessed);
   }
 
 private:
+  void save_image(uint64_t _time_stamp, const cv::Mat& _image)
+  {
+      std::string filename =
+        boost::str(boost::format("%s/%s%lu.%s")
+            % save_dir_
+            % prefix_
+            % _time_stamp
+            % filetype_);
+      if (!cv::imwrite(filename, _image))
+      {
+        ROS_ERROR_STREAM("ERROR Saving " << filename);
+      }
+      else
+      {
+        ROS_DEBUG_STREAM("Saved " << filename);
+        num_saved_++;
+      }
+  }
 
   image_proc::Processor processor_;
   std::string save_dir_;
@@ -104,6 +116,7 @@ int main(int argc, char** argv)
   ImageSaver saver(out_dir, filetype, prefix);
   bag_tools::ImageBagProcessor processor(image_topic);
   processor.registerCallback(boost::bind(&ImageSaver::save, saver, _1));
+  processor.registerCompressedCallback(boost::bind(&ImageSaver::save_compressed, saver, _1));
 
   for (int i = 4; i < argc; ++i)
     processor.processBag(argv[i]);