bag_stitcher.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 #sample run command for using the kinect as a capture device
00003 #rosrun pano_ros capture_pano.py image:=/kinect/rgb/image_raw camera_info:=/kinect/rgb/camera_info
00004 
00005 import roslib; roslib.load_manifest('pano_ros')
00006 import rospy
00007 
00008 import actionlib
00009 import numpy
00010 import pano_ros.msg
00011 import ros
00012 import cv
00013 import tempfile
00014 import rosbag
00015 from std_msgs.msg import String
00016 from cv_bridge import CvBridge, CvBridgeError
00017 import message_filters
00018 from sensor_msgs.msg import Image, CameraInfo
00019 import os
00020 
00021 
00022 class StitchJob(object):
00023     bag_name = "default.bag"
00024     stitch_file_name = "default.jpg"
00025     camera_info = CameraInfo()
00026     def __init__(self,bag_name,stitch_file_name):
00027         self.bag_name = bag_name
00028         self.stitch_file_name = stitch_file_name
00029         self.bridge = CvBridge()
00030     def unstuffBag(self):
00031         bag = rosbag.Bag(self.bag_name)
00032         saved_camera_info = False
00033         img_n = 0
00034         pano_dir = tempfile.mkdtemp("pano")
00035         image_names = []
00036         
00037         for topic, msg, t in bag.read_messages(topics=['image', 'camera_info']):
00038             if ('image' in topic):
00039                 try:
00040                     cv_image = self.bridge.imgmsg_to_cv(msg, "rgb8")                 
00041                     image_name = "%s/image_%05d.png" % (pano_dir , img_n)
00042                     image_names.append("image_%05d.png" % img_n)
00043                     cv.SaveImage(image_name,cv_image)
00044                     rospy.loginfo("saving image to %s",image_name)
00045                     img_n += 1                    
00046                 except CvBridgeError, e:
00047                     print e
00048             if ('camera_info' in topic and not saved_camera_info):
00049                 mK = numpy.array(msg.K)
00050       
00051                 mK = numpy.reshape(mK, (3,3), 'C' )
00052                 K = cv.fromarray(mK)
00053 
00054                 mD = numpy.array(msg.D)    
00055                 mD = numpy.reshape(mD, (5,1), 'C' )           
00056                 D = cv.fromarray(mD)
00057 
00058                 cv.Save(pano_dir +"/K.yml",K)
00059                 cv.Save(pano_dir +"/D.yml",D)
00060                
00061                 saved_camera_info = True
00062                 
00063                 
00064         bag.close()
00065         pano_name = self.stitch_file_name
00066         images = ""
00067         for x in image_names:
00068             images += x + " "
00069         cmd = 'rosrun pano_core stitcher -d %s -K %s -o %s %s' %(pano_dir,"%s/K.yml"% pano_dir,pano_name,images)
00070         os.system(cmd)
00071         
00072         
00073 class BagStitcher(object):
00074   # create messages that are used to publish feedback/result
00075   _panos = {}
00076  
00077   def __init__(self, name):
00078     self._action_name = name
00079     self._stitch_ac = actionlib.SimpleActionServer(self._action_name + "/stitch", pano_ros.msg.StitchAction, execute_cb=self.stitch)
00080 
00081 
00082   def stitch(self, goal):
00083     # helper variables
00084     stitch_job = StitchJob(goal.bag_file_name,goal.stitch_file_name)
00085     stitch_job.unstuffBag()
00086     self._stitch_ac.set_succeeded(pano_ros.msg.StitchResult(result_flags=pano_ros.msg.StitchResult.PANO_SUCCESS))
00087     
00088     
00089 if __name__ == '__main__':
00090   try:    
00091     
00092       rospy.init_node('bag_stitcher')
00093       BagStitcher(rospy.get_name())
00094       rospy.spin()
00095   except rospy.ROSInterruptException:
00096         print "program interrupted before completion"


pano_ros
Author(s): Ethan Rublee, Ken Conley
autogenerated on Mon Oct 6 2014 08:06:11