capture_monitor.py
Go to the documentation of this file.
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()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08