$search
00001 #!/usr/bin/env python 00002 00003 import roslib; roslib.load_manifest('camera_pose_calibration') 00004 import cv 00005 from cv_bridge import CvBridge, CvBridgeError 00006 import rospy 00007 import threading 00008 import numpy 00009 from calibration_msgs.msg import Interval 00010 from calibration_msgs.msg import CalibrationPattern 00011 from sensor_msgs.msg import Image 00012 from sensor_msgs.msg import CameraInfo 00013 from camera_pose_calibration.msg import RobotMeasurement 00014 from camera_pose_calibration.msg import CameraCalibration 00015 00016 00017 def beep(conf): 00018 try: 00019 with file('/dev/audio', 'wb') as audio: 00020 for (frequency, amplitude, duration) in conf: 00021 sample = 8000 00022 half_period = int(sample/frequency/2) 00023 beep = chr(amplitude)*half_period+chr(0)*half_period 00024 beep *= int(duration*frequency) 00025 audio.write(beep) 00026 except: 00027 print "Beep beep" 00028 00029 00030 00031 class ImageRenderer: 00032 def __init__(self, ns): 00033 self.lock = threading.Lock() 00034 self.image_time = rospy.Time(0) 00035 self.info_time = rospy.Time(0) 00036 self.image = None 00037 self.interval = 0 00038 self.features = None 00039 self.bridge = CvBridge() 00040 self.ns = ns 00041 self.max_interval = 1.0 00042 00043 self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.30, 1.5, thickness = 2) 00044 self.font1 = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.10, 1, thickness = 1) 00045 self.info_sub = rospy.Subscriber(ns+'/camera_info', CameraInfo, self.info_cb) 00046 self.image_sub = rospy.Subscriber(ns+'/image_throttle', Image, self.image_cb) 00047 self.interval_sub = rospy.Subscriber(ns+'/settled_interval', Interval, self.interval_cb) 00048 self.features_sub = rospy.Subscriber(ns+'/features', CalibrationPattern, self.features_cb) 00049 00050 def info_cb(self, msg): 00051 with self.lock: 00052 self.info_time = rospy.Time.now() 00053 00054 def image_cb(self, msg): 00055 with self.lock: 00056 self.image_time = rospy.Time.now() 00057 self.image = msg 00058 00059 def interval_cb(self, msg): 00060 with self.lock: 00061 self.interval = (msg.end - msg.start).to_sec() 00062 00063 def features_cb(self, msg): 00064 with self.lock: 00065 self.features = msg 00066 00067 def render(self, window): 00068 with self.lock: 00069 if self.image and self.image_time + rospy.Duration(2.0) > rospy.Time.now() and self.info_time + rospy.Duration(2.0) > rospy.Time.now(): 00070 cv.Resize(self.bridge.imgmsg_to_cv(self.image, 'rgb8'), window) 00071 interval = min(1,(self.interval / self.max_interval)) 00072 cv.Rectangle(window, 00073 (int(0.05*window.width), int(window.height*0.9)), 00074 (int(interval*window.width*0.9+0.05*window.width), int(window.height*0.95)), 00075 (0, interval*255, (1-interval)*255), thickness=-1) 00076 cv.Rectangle(window, 00077 (int(0.05*window.width), int(window.height*0.9)), 00078 (int(window.width*0.9+0.05*window.width), int(window.height*0.95)), 00079 (0, interval*255, (1-interval)*255)) 00080 cv.PutText(window, self.ns, (int(window.width * .05), int(window.height * 0.1)), self.font1, (0,0,255)) 00081 if self.features and self.features.header.stamp + rospy.Duration(4.0) > self.image.header.stamp: 00082 w_scaling = float (window.width) / self.image.width 00083 h_scaling = float (window.height) / self.image.height 00084 if self.features.success: 00085 corner_color = (0,255,0) 00086 for cur_pt in self.features.image_points: 00087 cv.Circle(window, (int(cur_pt.x*w_scaling), int(cur_pt.y*h_scaling)), int(w_scaling*5), corner_color) 00088 else: 00089 window = add_text(window, ["Could not detect", "checkerboard"], False) 00090 else: 00091 window = add_text(window, ["Timed out waiting", "for checkerboard"], False) 00092 00093 else: 00094 # Generate random white noise (for fun) 00095 noise = numpy.random.rand(window.height, window.width)*256 00096 numpy.asarray(window)[:,:,0] = noise; 00097 numpy.asarray(window)[:,:,1] = noise; 00098 numpy.asarray(window)[:,:,2] = noise; 00099 cv.PutText(window, self.ns, (int(window.width * .05), int(window.height * .95)), self.font, (0,0,255)) 00100 00101 00102 def add_text(image, text, good = True): 00103 if good: 00104 color = (0, 255, 0) 00105 else: 00106 color = (0, 0, 255) 00107 w = image.cols 00108 h = image.rows 00109 for i in range(len(text)): 00110 font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.30, float(w)/350, thickness = 1) 00111 ((text_w, text_h), _) = cv.GetTextSize(text[i], font) 00112 cv.PutText(image, text[i], (w/2-text_w/2, h/2-text_h/2 + i*text_h*2), font, color) 00113 return image 00114 00115 00116 00117 def get_image(text, good=True, h=480, w=640): 00118 image = cv.CreateMat(h, w, cv.CV_8UC3) 00119 return add_text(image, text, good) 00120 00121 00122 class Aggregator: 00123 def __init__(self, ns_list): 00124 print "Creating aggregator for ", ns_list 00125 00126 self.lock = threading.Lock() 00127 00128 # image 00129 w = 640 00130 h = 480 00131 self.image_out = cv.CreateMat(h, w, cv.CV_8UC3) 00132 self.pub = rospy.Publisher('aggregated_image', Image) 00133 self.bridge = CvBridge() 00134 00135 self.image_captured = get_image(["Successfully captured checkerboard"]) 00136 self.image_optimized = get_image(["Successfully ran optimization"]) 00137 self.image_failed = get_image(["Failed to run optimization"], False) 00138 00139 # create render windows 00140 layouts = [ (1,1), (2,2), (2,2), (2,2), (3,3), (3,3), (3,3), (3,3), (3,3) ] 00141 layout = layouts[len(ns_list)-1] 00142 sub_w = w / layout[0] 00143 sub_h = h / layout[1] 00144 self.windows = [] 00145 for j in range(layout[1]): 00146 for i in range(layout[0]): 00147 self.windows.append( cv.GetSubRect(self.image_out, (i*sub_w, j*sub_h, sub_w, sub_h) ) ) 00148 00149 # create renderers 00150 self.renderer_list = [] 00151 for ns in ns_list: 00152 self.renderer_list.append(ImageRenderer(ns)) 00153 00154 # subscribers 00155 self.capture_time = rospy.Time(0) 00156 self.calibrate_time = rospy.Time(0) 00157 self.captured_sub = rospy.Subscriber('robot_measurement', RobotMeasurement, self.captured_cb) 00158 self.optimized_sub = rospy.Subscriber('camera_calibration', CameraCalibration, self.calibrated_cb) 00159 00160 00161 def captured_cb(self, msg): 00162 with self.lock: 00163 self.capture_time = rospy.Time.now() 00164 beep([(400, 63, 0.2)]) 00165 00166 def calibrated_cb(self, msg): 00167 with self.lock: 00168 self.calibrate_time = rospy.Time.now() 00169 00170 00171 def loop(self): 00172 r = rospy.Rate(20) 00173 beep_time = rospy.Time(0) 00174 00175 while not rospy.is_shutdown(): 00176 try: 00177 r.sleep() 00178 except: 00179 print "Shutting down" 00180 with self.lock: 00181 for window, render in zip(self.windows, self.renderer_list): 00182 render.render(window) 00183 00184 if self.capture_time+rospy.Duration(4.0) > rospy.Time.now(): 00185 if self.capture_time+rospy.Duration(2.0) > rospy.Time.now(): 00186 self.pub.publish(self.bridge.cv_to_imgmsg(self.image_captured, encoding="passthrough")) 00187 elif self.calibrate_time+rospy.Duration(5.0) > rospy.Time.now(): 00188 self.pub.publish(self.bridge.cv_to_imgmsg(self.image_optimized, encoding="passthrough")) 00189 if beep_time+rospy.Duration(4.0) < rospy.Time.now(): 00190 beep_time = rospy.Time.now() 00191 beep([(600, 63, 0.1), (800, 63, 0.1), (1000, 63, 0.3)]) 00192 else: 00193 self.pub.publish(self.bridge.cv_to_imgmsg(self.image_failed, encoding="passthrough")) 00194 if beep_time+rospy.Duration(4.0) < rospy.Time.now(): 00195 beep_time = rospy.Time.now() 00196 beep([(400, 63, 0.1), (200, 63, 0.1), (100, 63, 0.6)]) 00197 00198 else: 00199 self.pub.publish(self.bridge.cv_to_imgmsg(self.image_out, encoding="passthrough")) 00200 00201 00202 00203 def main(): 00204 rospy.init_node('capture_monitor') 00205 args = rospy.myargv() 00206 00207 a = Aggregator(args[1:]) 00208 a.loop() 00209 00210 00211 00212 if __name__ == '__main__': 00213 main()