add files

arduino/rc_keyboard_control.ino

@@ -0,0 +1,102 @@
+// assign pin num
+int right_pin = 6;
+int left_pin = 7;
+int forward_pin = 10;
+int reverse_pin = 9;
+
+// duration for output
+int time = 50;
+// initial command
+int command = 0;
+
+void setup() {
+  pinMode(right_pin, OUTPUT);
+  pinMode(left_pin, OUTPUT);
+  pinMode(forward_pin, OUTPUT);
+  pinMode(reverse_pin, OUTPUT);
+  Serial.begin(115200);
+}
+
+void loop() {
+  //receive command
+  if (Serial.available() > 0){
+    command = Serial.read();
+  }
+  else{
+    reset();
+  }
+   send_command(command,time);
+}
+
+void right(int time){
+  digitalWrite(right_pin, LOW);
+  delay(time);
+}
+
+void left(int time){
+  digitalWrite(left_pin, LOW);
+  delay(time);
+}
+
+void forward(int time){
+  digitalWrite(forward_pin, LOW);
+  delay(time);
+}
+
+void reverse(int time){
+  digitalWrite(reverse_pin, LOW);
+  delay(time);
+}
+
+void forward_right(int time){
+  digitalWrite(forward_pin, LOW);
+  digitalWrite(right_pin, LOW);
+  delay(time);
+}
+
+void reverse_right(int time){
+  digitalWrite(reverse_pin, LOW);
+  digitalWrite(right_pin, LOW);
+  delay(time);
+}
+
+void forward_left(int time){
+  digitalWrite(forward_pin, LOW);
+  digitalWrite(left_pin, LOW);
+  delay(time);
+}
+
+void reverse_left(int time){
+  digitalWrite(reverse_pin, LOW);
+  digitalWrite(left_pin, LOW);
+  delay(time);
+}
+
+void reset(){
+  digitalWrite(right_pin, HIGH);
+  digitalWrite(left_pin, HIGH);
+  digitalWrite(forward_pin, HIGH);
+  digitalWrite(reverse_pin, HIGH);
+}
+
+void send_command(int command, int time){
+  switch (command){
+
+     //reset command
+     case 0: reset(); break;
+
+     // single command
+     case 1: forward(time); break;
+     case 2: reverse(time); break;
+     case 3: right(time); break;
+     case 4: left(time); break;
+
+     //combination command
+     case 6: forward_right(time); break;
+     case 7: forward_left(time); break;
+     case 8: reverse_right(time); break;
+     case 9: reverse_left(time); break;
+
+     default: Serial.print("Inalid Command\n");
+    }
+}

computer/collect_training_data.py

@@ -0,0 +1,163 @@
+__author__ = 'zhengwang'
+
+import numpy as np
+import cv2
+import serial
+import pygame
+from pygame.locals import *
+import socket
+
+
+class CollectTrainingData(object):
+    
+    def __init__(self):
+
+        self.server_socket = socket.socket()
+        self.server_socket.bind(('192.168.1.100', 8000))
+        self.server_socket.listen(0)
+
+        # accept a single connection
+        self.connection = self.server_socket.accept()[0].makefile('rb')
+
+        # connect to a seral port
+        self.ser = serial.Serial('/dev/tty.usbmodem1421', 115200, timeout=1)
+        self.send_inst = True
+
+        # create labels
+        self.k = np.zeros((4, 4), 'float')
+        for i in range(4):
+            self.k[i, i] = 1
+        self.temp_label = np.zeros((1, 4), 'float')
+
+        pygame.init()
+        self.collect_image()
+
+    def collect_image(self):
+
+        saved_frame = 0
+        total_frame = 0
+
+        # collect images for training
+        print 'Start collecting images...'
+        e1 = cv2.getTickCount()
+        image_array = np.zeros((1, 38400))
+        label_array = np.zeros((1, 4), 'float')
+
+        # stream video frames one by one
+        try:
+            stream_bytes = ' '
+            frame = 1
+            while self.send_inst:
+                stream_bytes += self.connection.read(1024)
+                first = stream_bytes.find('\xff\xd8')
+                last = stream_bytes.find('\xff\xd9')
+                if first != -1 and last != -1:
+                    jpg = stream_bytes[first:last + 2]
+                    stream_bytes = stream_bytes[last + 2:]
+                    image = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8), cv2.CV_LOAD_IMAGE_GRAYSCALE)
+                    
+                    # select lower half of the image
+                    roi = image[120:240, :]
+                    
+                    # save streamed images
+                    cv2.imwrite('training_images/frame{:>05}.jpg'.format(frame), image)
+                    
+                    #cv2.imshow('roi_image', roi)
+                    cv2.imshow('image', image)
+                    
+                    # reshape the roi image into one row array
+                    temp_array = roi.reshape(1, 38400).astype(np.float32)
+                    
+                    frame += 1
+                    total_frame += 1
+
+                    # get input from human driver
+                    for event in pygame.event.get():
+                        if event.type == KEYDOWN:
+                            key_input = pygame.key.get_pressed()
+
+                            # complex orders
+                            if key_input[pygame.K_UP] and key_input[pygame.K_RIGHT]:
+                                print("Forward Right")
+                                image_array = np.vstack((image_array, temp_array))
+                                label_array = np.vstack((label_array, self.k[1]))
+                                saved_frame += 1
+                                self.ser.write(chr(6))
+
+                            elif key_input[pygame.K_UP] and key_input[pygame.K_LEFT]:
+                                print("Forward Left")
+                                image_array = np.vstack((image_array, temp_array))
+                                label_array = np.vstack((label_array, self.k[0]))
+                                saved_frame += 1
+                                self.ser.write(chr(7))
+
+                            elif key_input[pygame.K_DOWN] and key_input[pygame.K_RIGHT]:
+                                print("Reverse Right")
+                                self.ser.write(chr(8))
+                            
+                            elif key_input[pygame.K_DOWN] and key_input[pygame.K_LEFT]:
+                                print("Reverse Left")
+                                self.ser.write(chr(9))
+
+                            # simple orders
+                            elif key_input[pygame.K_UP]:
+                                print("Forward")
+                                saved_frame += 1
+                                image_array = np.vstack((image_array, temp_array))
+                                label_array = np.vstack((label_array, self.k[2]))
+                                self.ser.write(chr(1))
+
+                            elif key_input[pygame.K_DOWN]:
+                                print("Reverse")
+                                saved_frame += 1
+                                image_array = np.vstack((image_array, temp_array))
+                                label_array = np.vstack((label_array, self.k[3]))
+                                self.ser.write(chr(2))
+                            
+                            elif key_input[pygame.K_RIGHT]:
+                                print("Right")
+                                image_array = np.vstack((image_array, temp_array))
+                                label_array = np.vstack((label_array, self.k[1]))
+                                saved_frame += 1
+                                self.ser.write(chr(3))
+
+                            elif key_input[pygame.K_LEFT]:
+                                print("Left")
+                                image_array = np.vstack((image_array, temp_array))
+                                label_array = np.vstack((label_array, self.k[0]))
+                                saved_frame += 1
+                                self.ser.write(chr(4))
+
+                            elif key_input[pygame.K_x] or key_input[pygame.K_q]:
+                                print 'exit'
+                                self.send_inst = False
+                                self.ser.write(chr(0))
+                                break
+                                    
+                        elif event.type == pygame.KEYUP:
+                            self.ser.write(chr(0))
+
+            # save training images and labels
+            train = image_array[1:, :]
+            train_labels = label_array[1:, :]
+
+            # save training data as a numpy file
+            np.savez('training_data_temp/test08.npz', train=train, train_labels=train_labels)
+
+            e2 = cv2.getTickCount()
+            # calculate streaming duration
+            time0 = (e2 - e1) / cv2.getTickFrequency()
+            print 'Streaming duration:', time0
+
+            print(train.shape)
+            print(train_labels.shape)
+            print 'Total frame:', total_frame
+            print 'Saved frame:', saved_frame
+            print 'Dropped frame', total_frame - saved_frame
+
+        finally:
+            self.connection.close()
+            self.server_socket.close()
+
+if __name__ == '__main__':
+    CollectTrainingData()

computer/mlp_predict_test.py

@@ -0,0 +1,51 @@
+__author__ = 'zhengwang'
+
+import cv2
+import numpy as np
+import glob
+
+
+# load training data
+image_array = np.zeros((1, 38400))
+label_array = np.zeros((1, 4), 'float')
+training_data = glob.glob('testing_data/*.npz')
+
+for single_npz in training_data:
+    with np.load(single_npz) as data:
+        print data.files
+        test_temp = data['train']
+        test_labels_temp = data['train_labels']
+        print test_temp.shape
+        print test_labels_temp.shape
+    image_array = np.vstack((image_array, test_temp))
+    label_array = np.vstack((label_array, test_labels_temp))
+
+test = image_array[1:, :]
+test_labels = label_array[1:, :]
+print test.shape
+print test_labels.shape
+
+# create MLP
+layer_sizes = np.int32([38400, 32, 4])
+model = cv2.ANN_MLP()
+model.create(layer_sizes)
+model.load('mlp_xml/mlp.xml')
+
+# generate predictions
+e0 = cv2.getTickCount()
+ret, resp = model.predict(test)
+prediction = resp.argmax(-1)
+e00 = cv2.getTickCount()
+time0 = (e00 - e0)/cv2.getTickFrequency()
+print 'Prediction time per frame:', time0/(test.shape[0])
+
+print 'Prediction:', prediction
+
+true_labels = test_labels.argmax(-1)
+print 'True labels:', true_labels
+
+print 'Testing...'
+num_correct = np.sum( true_labels == prediction )
+print(num_correct)
+test_rate = np.mean(prediction == true_labels)
+print 'Test rate: %f' % (test_rate*100)

computer/mlp_training.py

@@ -0,0 +1,69 @@
+__author__ = 'zhengwang'
+
+import cv2
+import numpy as np
+import glob
+
+print 'Loading training data...'
+e0 = cv2.getTickCount()
+
+# load training data
+image_array = np.zeros((1, 38400))
+label_array = np.zeros((1, 4), 'float')
+training_data = glob.glob('training_data/*.npz')
+
+for single_npz in training_data:
+    with np.load(single_npz) as data:
+        print data.files
+        train_temp = data['train']
+        train_labels_temp = data['train_labels']
+        print train_temp.shape
+        print train_labels_temp.shape
+    image_array = np.vstack((image_array, train_temp))
+    label_array = np.vstack((label_array, train_labels_temp))
+
+train = image_array[1:, :]
+train_labels = label_array[1:, :]
+print train.shape
+print train_labels.shape
+
+e00 = cv2.getTickCount()
+time0 = (e00 - e0)/ cv2.getTickFrequency()
+print 'Loading image duration:', time0
+
+# set start time
+e1 = cv2.getTickCount()
+
+# create MLP
+layer_sizes = np.int32([38400, 32, 4])
+model = cv2.ANN_MLP()
+model.create(layer_sizes)
+criteria = (cv2.TERM_CRITERIA_COUNT | cv2.TERM_CRITERIA_EPS, 500, 0.0001)
+criteria2 = (cv2.TERM_CRITERIA_COUNT, 100, 0.001)
+params = dict(term_crit = criteria,
+               train_method = cv2.ANN_MLP_TRAIN_PARAMS_BACKPROP,
+               bp_dw_scale = 0.001,
+               bp_moment_scale = 0.0 )
+
+print 'Training MLP ...'
+num_iter = model.train(train, train_labels, None, params = params)
+
+# set end time
+e2 = cv2.getTickCount()
+time = (e2 - e1)/cv2.getTickFrequency()
+print 'Training duration:', time
+
+# save param
+model.save('mlp_xml/mlp.xml')
+
+print 'Ran for %d iterations' % num_iter
+
+ret, resp = model.predict(train)
+prediction = resp.argmax(-1)
+print 'Prediction:', prediction
+true_labels = train_labels.argmax(-1)
+print 'True labels:', true_labels
+
+print 'Testing...'
+train_rate = np.mean(prediction == true_labels)
+print 'Train rate: %f:' % (train_rate*100)

computer/picam_calibration.py

@@ -0,0 +1,59 @@
+
+"""
+Reference:
+OpenCV-Python Tutorials - Camera Calibration and 3D Reconstruction
+http://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html
+"""
+
+import cv2
+import numpy as np
+import glob
+
+# termination criteria
+criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+# 6x9 chess board, prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
+object_point = np.zeros((6*9, 3), np.float32)
+object_point[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)
+
+# 3d point in real world space
+object_points = []
+# 2d points in image plane
+image_points = []
+h, w = 0, 0
+
+images = glob.glob('chess_board/*.jpg')
+
+for file_name in images:
+    image = cv2.imread(file_name)
+    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+    h, w = gray.shape[:2]
+
+    # find chess board corners
+    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
+
+    # add object points, image points
+    if ret:
+        object_points.append(object_point)
+        cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
+        image_points.append(corners)
+
+        # draw and display the corners
+        cv2.drawChessboardCorners(image, (9, 6), corners, ret)
+        cv2.imshow('image', image)
+        cv2.waitKey(500)
+
+# calibration
+retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(object_points, image_points, (w, h), None, None)
+
+print "camera matrix:\n", cameraMatrix
+
+# pi camera intrinsic parameters
+ay = cameraMatrix[1, 1]
+u0 = cameraMatrix[0, 2]
+v0 = cameraMatrix[1, 2]
+print "Ay:", ay
+print "u0:", u0
+print "v0:", v0
+
+cv2.destroyAllWindows()

computer/rc_control_test.py

@@ -0,0 +1,69 @@
+__author__ = 'zhengwang'
+
+import serial
+import pygame
+from pygame.locals import *
+
+
+class RCTest(object):
+
+    def __init__(self):
+        pygame.init()
+        self.ser = serial.Serial('/dev/tty.usbmodem1421', 115200, timeout=1)
+        self.send_inst = True
+        self.steer()
+
+    def steer(self):
+
+        while self.send_inst:
+            for event in pygame.event.get():
+                if event.type == KEYDOWN:
+                    key_input = pygame.key.get_pressed()
+
+                    # complex orders
+                    if key_input[pygame.K_UP] and key_input[pygame.K_RIGHT]:
+                        print("Forward Right")
+                        self.ser.write(chr(6))
+
+                    elif key_input[pygame.K_UP] and key_input[pygame.K_LEFT]:
+                        print("Forward Left")
+                        self.ser.write(chr(7))
+
+                    elif key_input[pygame.K_DOWN] and key_input[pygame.K_RIGHT]:
+                        print("Reverse Right")
+                        self.ser.write(chr(8))
+
+                    elif key_input[pygame.K_DOWN] and key_input[pygame.K_LEFT]:
+                        print("Reverse Left")
+                        self.ser.write(chr(9))
+
+                    # simple orders
+                    elif key_input[pygame.K_UP]:
+                        print("Forward")
+                        self.ser.write(chr(1))
+
+                    elif key_input[pygame.K_DOWN]:
+                        print("Reverse")
+                        self.ser.write(chr(2))
+
+                    elif key_input[pygame.K_RIGHT]:
+                        print("Right")
+                        self.ser.write(chr(3))
+
+                    elif key_input[pygame.K_LEFT]:
+                        print("Left")
+                        self.ser.write(chr(4))
+
+                    # exit
+                    elif key_input[pygame.K_x] or key_input[pygame.K_q]:
+                        print 'Exit'
+                        self.send_inst = False
+                        self.ser.write(chr(0))
+                        self.ser.close()
+                        break
+
+                elif event.type == pygame.KEYUP:
+                    self.ser.write(chr(0))
+
+if __name__ == '__main__':
+    RCTest()

computer/rc_driver.py

@@ -0,0 +1,285 @@
+__author__ = 'zhengwang'
+
+import threading
+import SocketServer
+import serial
+import cv2
+import numpy as np
+import math
+
+# distance data measured by ultrasonic sensor
+sensor_data = " "
+
+
+class NeuralNetwork(object):
+
+    def __init__(self):
+        self.model = cv2.ANN_MLP()
+
+    def create(self):
+        layer_size = np.int32([38400, 32, 4])
+        self.model.create(layer_size)
+        self.model.load('mlp_xml/mlp.xml')
+
+    def predict(self, samples):
+        ret, resp = self.model.predict(samples)
+        return resp.argmax(-1)
+
+
+class RCControl(object):
+
+    def __init__(self):
+        self.serial_port = serial.Serial('/dev/tty.usbmodem1421', 115200, timeout=1)
+
+    def steer(self, prediction):
+        if prediction == 2:
+            self.serial_port.write(chr(1))
+            print("Forward")
+        elif prediction == 0:
+            self.serial_port.write(chr(7))
+            print("Left")
+        elif prediction == 1:
+            self.serial_port.write(chr(6))
+            print("Right")
+        else:
+            self.stop()
+
+    def stop(self):
+        self.serial_port.write(chr(0))
+
+
+class DistanceToCamera(object):
+
+    def __init__(self):
+        # camera params
+        self.alpha = 8.0 * math.pi / 180
+        self.v0 = 119.865631204
+        self.ay = 332.262498472
+
+    def calculate(self, v, h, x_shift, image):
+        # compute and return the distance from the target point to the camera
+        d = h / math.tan(self.alpha + math.atan((v - self.v0) / self.ay))
+        if d > 0:
+            cv2.putText(image, "%.1fcm" % d,
+                (image.shape[1] - x_shift, image.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+        return d
+
+
+class ObjectDetection(object):
+
+    def __init__(self):
+        self.red_light = False
+        self.green_light = False
+        self.yellow_light = False
+
+    def detect(self, cascade_classifier, gray_image, image):
+
+        # y camera coordinate of the target point 'P'
+        v = 0
+
+        # detection
+        cascade_obj = cascade_classifier.detectMultiScale(
+            gray_image,
+            scaleFactor=1.1,
+            minNeighbors=5,
+            minSize=(30, 30),
+            flags=cv2.cv.CV_HAAR_SCALE_IMAGE
+        )
+
+        # draw a rectangle around the objects
+        for (x_pos, y_pos, width, height) in cascade_obj:
+            cv2.rectangle(image, (x_pos+5, y_pos+5), (x_pos+width-5, y_pos+height-5), (255, 255, 255), 2)
+            v = y_pos + height - 5
+            #print(x_pos+5, y_pos+5, x_pos+width-5, y_pos+height-5, width, height)
+
+            # stop sign
+            if width/height == 1:
+                cv2.putText(image, 'STOP', (x_pos, y_pos-10), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
+            
+            # traffic lights
+            else:
+                roi = gray_image[y_pos+10:y_pos + height-10, x_pos+10:x_pos + width-10]
+                mask = cv2.GaussianBlur(roi, (25, 25), 0)
+                (minVal, maxVal, minLoc, maxLoc) = cv2.minMaxLoc(mask)
+                cv2.circle(roi, maxLoc, 5, (255, 0, 0), 2)
+                
+                # Red light
+                if 1.0/8*(height-30) < maxLoc[1] < 4.0/8*(height-30):
+                    cv2.putText(image, 'Red', (x_pos+5, y_pos-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
+                    self.red_light = True
+                
+                # Green light
+                elif 5.5/8*(height-30) < maxLoc[1] < height-30:
+                    cv2.putText(image, 'Green', (x_pos+5, y_pos - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+                    self.green_light = True
+
+                # yellow light
+                #elif 4.0/8*(height-30) < maxLoc[1] < 5.5/8*(height-30):
+                #    cv2.putText(image, 'Yellow', (x_pos+5, y_pos - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
+                #    self.yellow_light = True
+        return v
+
+
+class SensorDataHandler(SocketServer.BaseRequestHandler):
+
+    data = " "
+
+    def handle(self):
+        global sensor_data
+        try:
+            while self.data:
+                self.data = self.request.recv(1024)
+                sensor_data = round(float(self.data), 1)
+                #print "{} sent:".format(self.client_address[0])
+                print sensor_data
+        finally:
+            print "Connection closed on thread 2"
+
+
+class VideoStreamHandler(SocketServer.StreamRequestHandler):
+
+    # h1: stop sign
+    h1 = 15.5 - 10  # cm
+    # h2: traffic light
+    h2 = 15.5 - 10
+
+    # create neural network
+    model = NeuralNetwork()
+    model.create()
+
+    obj_detection = ObjectDetection()
+    rc_car = RCControl()
+
+    # cascade classifiers
+    stop_cascade = cv2.CascadeClassifier('cascade_xml/stop_sign.xml')
+    light_cascade = cv2.CascadeClassifier('cascade_xml/traffic_light.xml')
+
+    d_to_camera = DistanceToCamera()
+    d_stop_sign = 25
+    d_light = 25
+
+    stop_start = 0              # start time when stop at the stop sign
+    stop_finish = 0
+    stop_time = 0
+    drive_time_after_stop = 0
+
+    def handle(self):
+
+        global sensor_data
+        stream_bytes = ' '
+        stop_flag = False
+        stop_sign_active = True
+
+        # stream video frames one by one
+        try:
+            while True:
+                stream_bytes += self.rfile.read(1024)
+                first = stream_bytes.find('\xff\xd8')
+                last = stream_bytes.find('\xff\xd9')
+                if first != -1 and last != -1:
+                    jpg = stream_bytes[first:last+2]
+                    stream_bytes = stream_bytes[last+2:]
+                    gray = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8), cv2.CV_LOAD_IMAGE_GRAYSCALE)
+                    image = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8), cv2.CV_LOAD_IMAGE_UNCHANGED)
+
+                    # lower half of the image
+                    half_gray = gray[120:240, :]
+
+                    # object detection
+                    v_param1 = self.obj_detection.detect(self.stop_cascade, gray, image)
+                    v_param2 = self.obj_detection.detect(self.light_cascade, gray, image)
+
+                    # distance measurement
+                    if v_param1 > 0 or v_param2 > 0:
+                        d1 = self.d_to_camera.calculate(v_param1, self.h1, 300, image)
+                        d2 = self.d_to_camera.calculate(v_param2, self.h2, 100, image)
+                        self.d_stop_sign = d1
+                        self.d_light = d2
+
+                    cv2.imshow('image', image)
+                    #cv2.imshow('mlp_image', half_gray)
+
+                    # reshape image
+                    image_array = half_gray.reshape(1, 38400).astype(np.float32)
+                    
+                    # neural network makes prediction
+                    prediction = self.model.predict(image_array)
+
+                    # stop conditions
+                    if sensor_data is not None and sensor_data < 30:
+                        print("Stop, obstacle in front")
+                        self.rc_car.stop()
+                    
+                    elif 0 < self.d_stop_sign < 25 and stop_sign_active:
+                        print("Stop sign ahead")
+                        self.rc_car.stop()
+
+                        # stop for 5 seconds
+                        if stop_flag is False:
+                            self.stop_start = cv2.getTickCount()
+                            stop_flag = True
+                        self.stop_finish = cv2.getTickCount()
+
+                        self.stop_time = (self.stop_finish - self.stop_start)/cv2.getTickFrequency()
+                        print "Stop time: %.2fs" % self.stop_time
+
+                        # 5 seconds later, continue driving
+                        if self.stop_time > 5:
+                            print("Waited for 5 seconds")
+                            stop_flag = False
+                            stop_sign_active = False
+
+                    elif 0 < self.d_light < 30:
+                        #print("Traffic light ahead")
+                        if self.obj_detection.red_light:
+                            print("Red light")
+                            self.rc_car.stop()
+                        elif self.obj_detection.green_light:
+                            print("Green light")
+                            pass
+                        elif self.obj_detection.yellow_light:
+                            print("Yellow light flashing")
+                            pass
+                        
+                        self.d_light = 30
+                        self.obj_detection.red_light = False
+                        self.obj_detection.green_light = False
+                        self.obj_detection.yellow_light = False
+
+                    else:
+                        self.rc_car.steer(prediction)
+                        self.stop_start = cv2.getTickCount()
+                        self.d_stop_sign = 25
+
+                        if stop_sign_active is False:
+                            self.drive_time_after_stop = (self.stop_start - self.stop_finish)/cv2.getTickFrequency()
+                            if self.drive_time_after_stop > 5:
+                                stop_sign_active = True
+
+                    if cv2.waitKey(1) & 0xFF == ord('q'):
+                        self.rc_car.stop()
+                        break
+
+            cv2.destroyAllWindows()
+
+        finally:
+            print "Connection closed on thread 1"
+
+
+class ThreadServer(object):
+
+    def server_thread(host, port):
+        server = SocketServer.TCPServer((host, port), VideoStreamHandler)
+        server.serve_forever()
+
+    def server_thread2(host, port):
+        server = SocketServer.TCPServer((host, port), SensorDataHandler)
+        server.serve_forever()
+
+    distance_thread = threading.Thread(target=server_thread2, args=('192.168.1.100', 8002))
+    distance_thread.start()
+    video_thread = threading.Thread(target=server_thread('192.168.1.100', 8000))
+    video_thread.start()
+
+if __name__ == '__main__':
+    ThreadServer()