$search
00001 #!/usr/bin/env python 00002 ################################################################# 00003 ##\file 00004 # 00005 # \note 00006 # Copyright (c) 2011-2012 \n 00007 # Fraunhofer Institute for Manufacturing Engineering 00008 # and Automation (IPA) \n\n 00009 # 00010 ################################################################# 00011 # 00012 # \note 00013 # Project name: care-o-bot 00014 # \note 00015 # ROS stack name: cob_calibration 00016 # \note 00017 # ROS package name: cob_image_capture 00018 # 00019 # \author 00020 # Author: Sebastian Haug, email:sebhaug@gmail.com 00021 # \author 00022 # Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de 00023 # 00024 # \date Date of creation: December 2011 00025 # 00026 ################################################################# 00027 # 00028 # Redistribution and use in source and binary forms, with or without 00029 # modification, are permitted provided that the following conditions are met: 00030 # 00031 # - Redistributions of source code must retain the above copyright 00032 # notice, this list of conditions and the following disclaimer. \n 00033 # - Redistributions in binary form must reproduce the above copyright 00034 # notice, this list of conditions and the following disclaimer in the 00035 # documentation and/or other materials provided with the distribution. \n 00036 # - Neither the name of the Fraunhofer Institute for Manufacturing 00037 # Engineering and Automation (IPA) nor the names of its 00038 # contributors may be used to endorse or promote products derived from 00039 # this software without specific prior written permission. \n 00040 # 00041 # This program is free software: you can redistribute it and/or modify 00042 # it under the terms of the GNU Lesser General Public License LGPL as 00043 # published by the Free Software Foundation, either version 3 of the 00044 # License, or (at your option) any later version. 00045 # 00046 # This program is distributed in the hope that it will be useful, 00047 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00048 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00049 # GNU Lesser General Public License LGPL for more details. 00050 # 00051 # You should have received a copy of the GNU Lesser General Public 00052 # License LGPL along with this program. 00053 # If not, see <http://www.gnu.org/licenses/>. 00054 # 00055 ################################################################# 00056 PKG = 'cob_image_capture' 00057 NODE = 'image_capture' 00058 import roslib; roslib.load_manifest(PKG) 00059 import rospy 00060 00061 import cv 00062 from sensor_msgs.msg import Image 00063 from cob_calibration_srvs.srv import Capture, CaptureResponse 00064 from cv_bridge import CvBridge, CvBridgeError 00065 00066 class ImageCaptureNode(): 00067 ''' 00068 @summary: Captures Images from one or more cameras (Image message topics) to files. 00069 00070 Number of cameras, output folder and file names are configurable via ROS parameters. 00071 After startup call "~capture_images" ROS service to save images of all 00072 cameras to output folder. 00073 ''' 00074 00075 def __init__(self): 00076 ''' 00077 Initializes storage, gets parameters from parameter server and logs to rosinfo 00078 ''' 00079 rospy.init_node(NODE) 00080 self.bridge = CvBridge() 00081 self.counter = 0 00082 00083 # Get params from ros parameter server or use default 00084 self.numCams = int(rospy.get_param("~number_of_cameras", "1")) 00085 self.output_folder = rospy.get_param("~output_folder", "/tmp") 00086 self.save_header_stamp = rospy.get_param("~save_header_stamp", "False") 00087 self.camera = [] 00088 self.file_prefix = [] 00089 for id in range(self.numCams): 00090 self.camera.append( rospy.get_param("~camera%d" % id, "/stereo/left/image_raw")) 00091 self.file_prefix.append(rospy.get_param("~file_prefix%d" % id, "cam%d_" % id)) 00092 00093 # Init images 00094 self.image = [] 00095 for id in range(self.numCams): 00096 self.image.append(Image()) 00097 00098 # Subscribe to images 00099 self.imageSub = [] 00100 for id in range(self.numCams): 00101 self.imageSub.append(rospy.Subscriber(self.camera[id], Image, self._imageCallback, id)) 00102 00103 # Wait for image messages 00104 for id in range(self.numCams): 00105 rospy.wait_for_message(self.camera[id], Image, 5) 00106 00107 # Report 00108 rospy.loginfo("started capture process...") 00109 rospy.loginfo("capturing images from") 00110 for id in range(self.numCams): 00111 rospy.loginfo(" %s -> files %s*" % (self.camera[id], self.file_prefix[id])) 00112 rospy.loginfo("to output folder %s" % self.output_folder) 00113 00114 def _imageCallback(self, data, id): 00115 ''' 00116 Copy image message to local storage 00117 00118 @param data: Currently received image message 00119 @type data: ROS Image() message 00120 @param id: Id of camera from which the image was received 00121 @type id: integer 00122 ''' 00123 #print "cb executed" 00124 self.image[id] = data 00125 00126 def _convertAndSaveImage(self, rosImage, filenamePrefix, counter): 00127 ''' 00128 Convert image to cvImage and store to file as jpg image. 00129 00130 @param rosImage: Image 00131 @type rosImage: ROS Image() message 00132 @param filenamePrefix: Prefix to be prepended to image filename 00133 @type filenamePrefix: string 00134 @param counter: Number to be appended to image filename 00135 @type counter: integer 00136 ''' 00137 # save image 00138 cvImage = cv.CreateImage((1,1), 1 , 3) 00139 try: 00140 cvImage = self.bridge.imgmsg_to_cv(rosImage, "bgr8") 00141 except CvBridgeError, e: 00142 print e 00143 cv.SaveImage(self.output_folder+'/'+filenamePrefix+'%05d.jpg' % counter, cvImage) 00144 00145 # save header 00146 if self.save_header_stamp: 00147 f = open(self.output_folder+'/'+filenamePrefix+'%05d_header.txt' % counter, "w") 00148 f.writelines(str(rosImage.header)) 00149 f.close() 00150 00151 def _captureHandle(self, req): 00152 ''' 00153 Service handle for "Capture" Service 00154 Grabs images, converts them and saves them to files 00155 00156 @param req: service request 00157 @type req: CaptureRequest() message 00158 00159 @return: CaptureResponse() message 00160 ''' 00161 # grab image messages 00162 localImages = [] 00163 for id in range(self.numCams): 00164 if self.image[id].header.stamp > rospy.Time(0): 00165 localImages.append(self.image[id]) 00166 rospy.loginfo(" header.stamp of cam %d: %s" % (id, str(self.image[id].header.stamp))) 00167 00168 if len(localImages) == 0: 00169 rospy.loginfo("No sample captured, no image in queue.") 00170 return CaptureResponse(False) 00171 00172 # convert and save 00173 for id in range(self.numCams): 00174 self._convertAndSaveImage(localImages[id], self.file_prefix[id], self.counter) 00175 self.counter = self.counter + 1 00176 00177 # log infos 00178 rospy.loginfo("-> %s image(s) captured | captured %d set(s) of images in total" % (self.numCams, self.counter)) 00179 00180 # return service response 00181 return CaptureResponse(True) 00182 00183 def run(self): 00184 # Start service 00185 srv = rospy.Service('/image_capture/capture', Capture, self._captureHandle) 00186 rospy.loginfo("service '/image_capture/capture' started, waiting for requests...") 00187 rospy.spin() 00188 00189 00190 if __name__ == '__main__': 00191 node = ImageCaptureNode() 00192 node.run()