capture_server.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2010, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # Revision $Id: capture_server.py 47238 2010-12-17 22:23:01Z kwc $
00035 """
00036 Panorama capture action server.
00037 
00038 Authors: Ethan Rublee, Ken Conley
00039 
00040 Sample run command for using the kinect as a capture device:
00041 
00042 rosrun pano_ros capture_server.py camera:=/kinect/rgb
00043 """
00044 
00045 import roslib; roslib.load_manifest('pano_ros')
00046 import os
00047 import tempfile
00048 import threading
00049 import rospy
00050 
00051 from pano_ros.msg import PanoCaptureAction, PanoCaptureResult, PanoCaptureFeedback
00052 from std_msgs.msg import Empty
00053 from sensor_msgs.msg import Image, CameraInfo
00054 
00055 import actionlib
00056 import rosbag
00057 import message_filters
00058 
00059 import pano_py as pano
00060 import pano_cv as pcv
00061 import cv
00062 
00063 from cv_bridge import CvBridge, CvBridgeError
00064 
00065 class ImageGrabber:
00066     """
00067     Helper class for subscribing to synchronized camera and camera
00068     info data.
00069     """
00070 
00071     def __init__(self, image_topic, camera_info_topic, capture_fn):
00072         
00073         self._image_sub = message_filters.Subscriber(image_topic, Image)
00074         self._info_sub = message_filters.Subscriber(camera_info_topic, CameraInfo)
00075         self._ts = message_filters.TimeSynchronizer([self._image_sub, self._info_sub], 3)
00076         self._ts.registerCallback(capture_fn) 
00077 
00078     def stop(self):
00079         # stop subscribing
00080         self._image_sub.sub.unregister()
00081         self._info_sub.sub.unregister()
00082 
00083         del self._image_sub
00084         del self._info_sub        
00085         del self._ts
00086     
00087 class PanoCaptureJob(object):
00088 
00089     def __init__(self, pano_id, capture_fn):
00090         self.pano_id = pano_id
00091         self.capture_fn = capture_fn
00092         self.result = None
00093 
00094     def __call__(self, image, info):
00095         """
00096         Call job with latest captured imaged
00097         """
00098         # passthrough
00099         self.capture_fn(image, info)
00100         
00101     def _get_n_captures(self):
00102         return self.capture_fn.n_captures if self.capture_fn else self.result.n_captures
00103 
00104     n_captures = property(_get_n_captures)
00105     
00106     def cancel(self):
00107         if self.capture_fn is None:
00108             return
00109         capturer = self.capture_fn
00110         self.capture_fn = None
00111         capturer.cancel()
00112         
00113     def stop(self):
00114         if self.capture_fn is None:
00115             raise Exception("already stopped")
00116         capturer = self.capture_fn
00117         capturer.stop()
00118         #todo get rid of capture_fn.bag_filename
00119         self.result = PanoCaptureResult(pano_id=self.pano_id,
00120                                         n_captures=capturer.n_captures,
00121                                         bag_filename=capturer.bag_filename)
00122         self.capture_fn = None
00123 
00124 class CaptureInterface(object):
00125     """
00126     Common capture interface, like capture to bag, capture and republish, etc...
00127     """
00128     def start(self,pano_id, goal):
00129         """
00130         set up a new pano.
00131         """
00132         raise NotImplemented
00133     def __call__(self, image, camera_info):
00134         """
00135         capture your synced image and camera_info here
00136         """
00137         raise NotImplemented
00138 
00139     def stop(self):
00140         """
00141         done capturing, do clean up, close bags, etc.
00142         """
00143         raise NotImplemented
00144     
00145     def cancel(self):
00146         """
00147         no matter what stop capturing, you've been prempted
00148         """
00149         raise NotImplemented
00150 class PanoCaptureServer(object):
00151     def __init__(self, name, capture_interface = CaptureInterface):
00152         self._action_name = name
00153         self._server = actionlib.SimpleActionServer(self._action_name, 
00154                                                     PanoCaptureAction, execute_cb=self.pano_capture)
00155         
00156         # triggers during action execution
00157         self._snap_sub = rospy.Subscriber(rospy.names.ns_join(self._action_name, 'snap'), Empty, self.capture_snap)
00158         self._stop_sub = rospy.Subscriber(rospy.names.ns_join(self._action_name, 'stop'), Empty, self.capture_stop)
00159 
00160         # initialized during active job
00161         self._capture_job = None
00162         self._snap_requested = False
00163         self._capture_interface = capture_interface
00164 
00165         
00166     def pano_capture(self, goal):
00167         if self._capture_job is not None:
00168             raise Exception("cannot run multile capture jobs. TODO: pre-eempt existing job")
00169         
00170         rospy.loginfo('%s: Pano Capture Goal: \n\tCamera [%s]'%(self._action_name, goal.camera_topic))
00171                              
00172         pano_id = int(rospy.get_time())
00173         #TODO make this a parameter, bag, publisher, etc...
00174         capturer = self._capture_interface()#= BagCapture(pano_id,goal.bag_filename or None)
00175         capturer.start(pano_id,goal)
00176                 
00177         self._snap_requested = False #reset
00178         capture_job = self._capture_job = PanoCaptureJob(pano_id, capturer )
00179         camera_topic = goal.camera_topic or rospy.resolve_name('camera')
00180         #TODO: FIX ONCE OPENNI API IS FIXED
00181         image_topic = rospy.names.ns_join(camera_topic, 'image_color')
00182         camera_info_topic = rospy.names.ns_join(camera_topic, 'camera_info')
00183 
00184         rospy.loginfo('%s: Starting capture of pano_id %d.\n\tImage [%s]\n\tCamera Info[%s]'
00185                       %(self._action_name, pano_id, image_topic, camera_info_topic) )
00186         grabber = ImageGrabber(image_topic, camera_info_topic, self.capture_fn)
00187 
00188         # local vars
00189         server = self._server
00190         preempted = False        
00191 
00192         rospy.loginfo('%s: Executing capture of pano_id %d' % (self._action_name, pano_id))  
00193         # this will become true 
00194         while capture_job.result is None and not preempted and not rospy.is_shutdown():
00195             if server.is_preempt_requested():
00196                 rospy.loginfo('%s: Preempted' % self._action_name)
00197                 server.set_preempted()
00198                 capture_job.cancel()
00199                 preempted = True
00200             else:
00201                 rospy.sleep(0.001) #let the node rest a bit
00202                 
00203         result = capture_job.result
00204         grabber.stop()
00205         
00206         if result:
00207             rospy.loginfo('%s: Succeeded, [%s] images.\nResult: %s' % (self._action_name, result.n_captures, result))
00208             server.set_succeeded(result)
00209   
00210         self._capture_job = None
00211         
00212     def capture_fn(self, image, info):
00213         if self._capture_job is None:
00214             rospy.logerr("cannot capture photo while capture job is not active")
00215             return
00216         # not really concerned by the possible race condition here
00217         if self._snap_requested:
00218             rospy.loginfo("snap!")
00219             self._snap_requested = False
00220             self._capture_job.capture_fn(image, info)
00221             #only send feedback on snap
00222             feedback = PanoCaptureFeedback()
00223             feedback.n_captures = self._capture_job.n_captures
00224             rospy.loginfo("captured %d images so far" % feedback.n_captures)
00225             self._server.publish_feedback(feedback)
00226             
00227     def capture_snap(self, empty):
00228         if self._capture_job is None:
00229             rospy.logerr("cannot snap photo while capture job is not active")
00230             return
00231         self._snap_requested = True
00232         
00233     def capture_stop(self, empty):
00234         if self._capture_job is None:
00235             rospy.logerr("no active capture job to stop")
00236             return
00237 
00238         self._snap_requested = False
00239         self._capture_job.stop()
00240         
00241 
00242 
00243 class BagCapture(CaptureInterface):
00244     """
00245     Bag capture is not thread-safe.
00246     """     
00247     def start(self, pano_id, goal):
00248         """
00249         set up a new pano.
00250         """
00251         self.pano_id = pano_id
00252         if goal.bag_filename is None:
00253             self.bag_filename = os.path.abspath(os.path.join(tempfile.gettempdir(), "pano_%d.bag" % pano_id))
00254         else:
00255             self.bag_filename = goal.bag_filename
00256         self.bag = rosbag.Bag(self.bag_filename, 'w')
00257         self.n_captures = 0
00258         self.lock = threading.Lock()
00259 
00260     def __call__(self, image, camera_info):
00261         with self.lock:
00262             if self.bag is not None:
00263                 self.bag.write("image", image)
00264                 self.bag.write("camera_info", camera_info)
00265                 self.n_captures += 1
00266                           
00267     def stop(self):
00268         with self.lock:
00269             if self.bag is not None:
00270                 self.bag.close()
00271                 self.bag = None
00272             else:
00273                 raise IOError("already closed")
00274         
00275     def cancel(self):
00276         self.bag.close()
00277         self.bag = None
00278 
00279 class StitchCapture(CaptureInterface):
00280     """
00281     Bag capture is not thread-safe.
00282     """     
00283     def start(self, pano_id, goal):
00284         """
00285         set up a new pano.
00286         """
00287         self.bridge = CvBridge()
00288         self._bag_capture = BagCapture()
00289         self._bag_capture.start(pano_id, goal)
00290         self.img_pub = rospy.Publisher('~stitch',Image)
00291         
00292 
00293     def __call__(self, image, camera_info):
00294         if self._bag_capture.n_captures < 1:
00295             self.setupCamera(camera_info)
00296             self.setupStitch()
00297         try:
00298             self.img_pub.publish(image)
00299             cv_image = self.bridge.imgmsg_to_cv(image, "rgb8")
00300                            
00301             pano_image = pcv.convertCvMat2Mat(cv_image)
00302             self.stitcher.addNewImage(pano_image)
00303         except CvBridgeError, e:
00304             print e
00305         #capture to bag for back up and later stitching        
00306         self._bag_capture(image,camera_info)
00307         
00308     def setupStitch(self):
00309         options = pano.Options()
00310         options.camera = self.camera
00311         options.stitch_size = pcv.Size(4000,2000)
00312         #options.image_names.assign(image_names)
00313         params = options.fitter_params
00314         params.error_thresh = 6;
00315         params.inliers_thresh = 15;
00316         params.maxiters = 100;
00317         params.nNeeded = 2;
00318         self.stitcher = pano.StitchEngine(options)
00319           
00320     def _get_n_captures(self):
00321         return self._bag_capture.n_captures
00322     def _get_bag_filename(self):
00323         return self._bag_capture.bag_filename
00324     
00325     n_captures = property(_get_n_captures)
00326     bag_filename = property(_get_bag_filename)
00327     
00328     def setupCamera(self,camera_info):
00329         self.camera = pano.Camera()
00330         K = pcv.Mat(3,3,pcv.CV_32FC1)
00331         K.fromarray(camera_info.K)
00332         img_size = pcv.Size( camera_info.width, camera_info.height)                
00333         self.camera.setCameraIntrinsics(K,pcv.Mat(),img_size) 
00334         
00335     def stitch_cb(self, x):
00336         print "progress ",x
00337         return 0 
00338           
00339     def stop(self):
00340         #allocate a cv mat, this will be stitched to inplace
00341         #2000 rows = 2000 height
00342         #4000 cols = 4000 width (this is backwards from widthxheight
00343         if(self.stitcher):
00344             blended = cv.CreateMat(1500,3000,cv.CV_8UC3)
00345             self.stitcher.stitch(blended, self.stitch_cb)
00346             img_msg = self.bridge.cv_to_imgmsg(blended)
00347             self.img_pub.publish(img_msg)
00348         self._bag_capture.stop()
00349         
00350     def cancel(self):
00351         self._bag_capture.cancel()
00352     
00353 if __name__ == '__main__':
00354     rospy.init_node('pano_capture')
00355     PanoCaptureServer(rospy.get_name(), StitchCapture)
00356     rospy.spin()
00357 


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