git-svn-id: http://newslabx.csie.ntu.edu.tw/svn/Ginger@20 5747cdd2-2146-426f-b2b0-0570f90b98ed

trunk/code/readGridEye.py

@@ -11,12 +11,14 @@ class Frame():
 class GridEye():
     def __init__(self, serialPort, baudrate):
         self.port = serial.Serial(serialPort, baudrate)
-        self.frames = []
+        self.frame = None
         self.reading = True
         self.thread = threading.Thread(target = self.reader)
         self.thread.setDaemon(True)
+        self.lock = threading.Lock()
 
     def start(self):
+        self.port.reset_input_buffer()
         self.thread.start()
         
     def stop(self):
@@ -44,11 +46,10 @@ class GridEye():
                 # print (self.port.in_waiting)
             if b'#' in line:
                 if len(data) == 8:
-                    print (data_time)
                     #print (data)
-                    self.frames.append(Frame(data_time, data))
-                    if len(self.frames) > 100:
-                        self.frames.pop(0)
+                    self.lock.acquire()
+                    self.frame = Frame(data_time, data)
+                    self.lock.release()
                 else:
                     print ('something wrong', len(data))
                 data_time = time.time()
@@ -72,29 +73,56 @@ if __name__ == '__main__':
     import numpy as np
     SIZE = 128
     overlap = 120
-    grideye = GridEye('COM12', 115200)
+    grideye = GridEye('COM17', 115200)
     grideye.start()
-    cv2.imshow('sample', np.empty((SIZE*2,SIZE*2), np.uint8))
+    grideye2 = GridEye('COM15', 115200)
+    grideye2.start()
+    
+    fourcc = cv2.VideoWriter_fourcc(*'XVID')
+    videoWriter = cv2.VideoWriter('output.avi', fourcc, 10.0, (SIZE*2,SIZE*2))
+    cv2.imshow('sample', np.zeros((SIZE*2,SIZE*2), np.uint8))
     while True:
-        if len(grideye.frames) > 0:
-            frame = grideye.frames.pop(0)
+        if grideye.frame and grideye2.frame:
+            grideye.lock.acquire()
+            grideye2.lock.acquire()
+            frame = grideye.frame
+            grideye.frame = None
+            frame2 = grideye2.frame
+            grideye2.frame = None
+            # frame2 = frame
+            grideye2.lock.release()
+            grideye.lock.release()
+            
+            
             img = (np.array(frame.data)-15)*10
-            img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_LINEAR)
-            img2 = np.copy(img)
+            img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC) # INTER_LINEAR, INTER_CUBIC
+            img2 = (np.array(frame2.data)-15)*10
+            img2 = cv2.resize(img2.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC)
             
-            out = np.empty((SIZE*2, SIZE*2), dtype=np.uint16)
+            out = np.zeros((SIZE*2, SIZE*2), dtype=np.uint16)
             out[:SIZE, :SIZE] = img
             out[:SIZE, SIZE:] = img2
             
-            out[SIZE:, :SIZE] = img
-            out[SIZE:, (SIZE-overlap):SIZE] += img2[:,:overlap]
-            out[SIZE:, (SIZE-overlap):SIZE] = out[SIZE:, (SIZE-overlap):SIZE]/2
             
-            out[SIZE:, SIZE:SIZE+(SIZE-overlap)] = img2[:,overlap:SIZE]
+            offset = int(overlap/2)
+            out[SIZE:, offset:SIZE+offset] = img
+            out[SIZE:, (SIZE-overlap)+offset:SIZE+offset] += img2[:,:overlap]
+            out[SIZE:, (SIZE-overlap)+offset:SIZE+offset] = out[SIZE:, (SIZE-overlap)+offset:SIZE+offset]/2
+            
+            out[SIZE:, SIZE+offset:SIZE+(SIZE-overlap)+offset] = img2[:,overlap:SIZE]
             out = out.astype(np.uint8)
             cv2.imshow('sample', out)
-            if cv2.waitKey(1) == ord('q'):
+            videoWriter.write(cv2.cvtColor(out,cv2.COLOR_GRAY2BGR))
+            key = cv2.waitKey(1)
+            if key == ord('q'):
                 break
+            elif key == ord('a'):
+                overlap += 1
+            elif key == ord('d'):
+                overlap -= 1
+            elif key == ord('c'):
+                cv2.imwrite('out.jpg', out)
         time.sleep(0.001)
     grideye.stop()
+    grideye2.stop()