Ginger/trunk/code/readGridEye.py

import serial
import threading
import time

class Frame():
    def __init__(self, time, data):
        self.time = time
        self.data = data

class GridEye():
    def __init__(self, serialPort, baudrate):
        self.port = serial.Serial(serialPort, baudrate)
        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):
        self.reading = False
        self.thread.join()
        
    def reader(self):
        data = []
        data_time = 0;
        while (self.reading):
            line = b''
            while (self.reading):
                c = self.port.read()
                if c == b'\r':
                    c = self.port.read()
                    break
                if c == b'\n':
                    break
                line += c
            #line = self.port.readline()#.decode('utf-8')
            # if line:
                # print (line)
                # time.sleep(0.01)
            # if self.port.in_waiting > 0:
                # print (self.port.in_waiting)
            if b'#' in line:
                if len(data) == 8:
                    #print (data)
                    self.lock.acquire()
                    self.frame = Frame(data_time, data)
                    self.lock.release()
                else:
                    print ('something wrong', len(data))
                data_time = time.time()
                data = []
            else:
                try:
                    row = [float(x) for x in line.split()] 
                    if len(row) == 8:
                        data.append(row)
                except ValueError as e:
                    print ('error', e)
                    data_time = time.time()
                    data = []
                if len(data) > 8:
                    data.pop(0)

class Distance():
    def __init__(self, serialPort, baudrate):
        self.port = serial.Serial(serialPort, baudrate)
        self.distance = 200
        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):
        self.reading = False
        self.thread.join()
        
    def reader(self):
        while (self.reading):
            line = b''
            while (self.reading):
                c = self.port.read()
                if c == b'\r':
                    c = self.port.read()
                    break
                if c == b'\n':
                    break
                line += c
            if b'Distance' in line:
                try:
                    dist = float(line.decode('utf-8').split(':')[1])
                    print (dist)
                    if dist > 200.0:
                        dist = 200.0
                    self.lock.acquire()
                    self.distance = dist
                    self.lock.release()
                except ValueError as e:
                    print ('error', e)
                    
        
if __name__ == '__main__':
    import cv2
    import numpy as np
    import math
    def exponential(img, value):
        tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
        return tmp.astype(np.uint8)
        
    SIZE = 128
    overlap = 120
    AVERAGE_FRAME = 10
    distanceBetweenSensors = 7.7 #cm
    distance2Object = 60.0 #cm
    ADJUST_BACK = 5
    EXPONENTAL_VALUE = 0.4
    
    offset = (distanceBetweenSensors / (2*distance2Object*math.tan(30.0/180.0*math.pi))) * SIZE
    overlap = int(SIZE - offset)
    print (overlap)
    grideye = GridEye('COM15', 115200)
    grideye.start()
    grideye2 = GridEye('COM17', 115200)
    grideye2.start()
    
    distanceSensor = Distance('COM18', 9600)
    distanceSensor.start()
    
    fourcc = cv2.VideoWriter_fourcc(*'XVID')
    videoWriter = cv2.VideoWriter('output.avi', fourcc, 10.0, (SIZE*2,SIZE*3))
    cv2.imshow('sample', np.zeros((SIZE*3,SIZE*2), np.uint8))
    aver1 = np.zeros((SIZE,SIZE), np.uint16)
    aver2 = np.zeros((SIZE,SIZE), np.uint16)
    cnt = 0
    while True:
        if grideye.frame and grideye2.frame:
            grideye.lock.acquire()
            grideye2.lock.acquire()
            distanceSensor.lock.acquire()
            frame = grideye.frame
            grideye.frame = None
            frame2 = grideye2.frame
            grideye2.frame = None
            # frame2 = frame
            distance2Object = distanceSensor.distance
            distanceSensor.lock.release()
            grideye2.lock.release()
            grideye.lock.release()
            p = np.zeros((16,16), np.uint16)
            
            img = (np.array(frame.data)-15)*10
            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)
            
            if cnt < AVERAGE_FRAME:
                cnt += 1
                aver1 += img
                aver2 += img2
                if cnt == AVERAGE_FRAME:
                    aver1 = aver1/AVERAGE_FRAME
                    aver1 = aver1.astype(np.uint8)
                    aver1 += ADJUST_BACK
                    aver2 = aver2/AVERAGE_FRAME
                    aver2 = aver2.astype(np.uint8)
                    aver2 += ADJUST_BACK
                continue
            img = cv2.subtract(img, aver1)
            img2 = cv2.subtract(img2, aver2)
            
            out = np.full((SIZE*3, SIZE*2), 255, dtype=np.uint16)
            out[:SIZE, :SIZE] = img
            out[:SIZE, SIZE:] = img2
            
            try:
                overlap = int(SIZE - (distanceBetweenSensors / (2*distance2Object*math.tan(30.0/180.0*math.pi))) * SIZE)
            except:
                overlap = 0
            if overlap < 0:
                overlap = 0
            offset = int(overlap/2)
            # tmp = cv2.resize(img.astype(np.uint8), (SIZE*2-overlap, SIZE))
            # tmp.astype(np.uint16)
            tmp = np.zeros((SIZE, SIZE*2-overlap), dtype=np.uint16)
            tmp[:, :SIZE] = img
            tmp[:, -SIZE:] += img2
            tmp[:, (SIZE-overlap): SIZE] = tmp[:, (SIZE-overlap): SIZE]/2
            tmp = exponential(tmp, EXPONENTAL_VALUE)
            out[SIZE:SIZE*2, offset: SIZE*2-overlap+offset] = tmp
            # out[SIZE:SIZE*2, offset:SIZE+offset] = img
            # out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset] += img2[:,:overlap]
            # out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset] = out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset]/2
            # out[SIZE:SIZE*2, SIZE+offset:SIZE+(SIZE-overlap)+offset] = img2[:,overlap:SIZE]
            
            
            maxProduct = 0
            overlap2 = 0
            for i in range(80, 128):
                product = sum(img[:,SIZE-i:].astype(np.uint32)*img2[:,:i].astype(np.uint32))
                product = sum(product) / len(product)
                if product > maxProduct:
                    maxProduct = product
                    overlap2 = i
                    
            
            
            offset = int(overlap2/2)
            tmp = np.zeros((SIZE, SIZE*2-overlap2), dtype=np.uint16)
            tmp[:, :SIZE] = img
            tmp[:, -SIZE:] += img2
            tmp[:, (SIZE-overlap2): SIZE] = tmp[:, (SIZE-overlap2): SIZE]/2
            tmp = exponential(tmp, EXPONENTAL_VALUE)
            out[SIZE*2:, offset: SIZE*2-overlap2+offset] = tmp
            
            
            # out[SIZE*2:, offset:SIZE+offset] = img
            # out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset] += img2[:,:overlap2]
            # out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset] = out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset]/2
            # out[SIZE*2:, SIZE+offset:SIZE+(SIZE-overlap2)+offset] = img2[:,overlap2:SIZE]
            out = out.astype(np.uint8)
            
            img = (np.array(frame.data)-15)*10
            img = img.astype(np.uint8)
            img2 = (np.array(frame2.data)-15)*10
            img2 = img2.astype(np.uint8)
            p = np.zeros((8,16), np.uint8)
            for i in range(16):
                if i%2 == 0:
                    p[:,i] = img[:,int(i/2)]
                else:
                    p[:,i] = img2[:,int(i/2)]
            p = cv2.resize(p, (SIZE,SIZE), interpolation = cv2.INTER_CUBIC)
            
            cv2.imshow('sample', out)
            cv2.imshow('p', p)
            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()
    videoWriter.release()
    cv2.destroyAllWindows()