00001
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
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
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
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
00150 self.renderer_list = []
00151 for ns in ns_list:
00152 self.renderer_list.append(ImageRenderer(ns))
00153
00154
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()
