DPC

driver/code/config

@@ -1,2 +1,2 @@
-turning_offset = -25
+turning_offset = 0
 

driver/code/deep_pi_car.py

@@ -3,6 +3,7 @@
 import cv2
 import datetime
 from hand_coded_lane_follower import HandCodedLaneFollower
+from end_to_end_lane_follower_tpu import EndToEndLaneFollower
 from objects_on_road_processor import ObjectsOnRoadProcessor
 
 _SHOW_IMAGE = True
@@ -26,11 +27,11 @@ def __init__(self):
         self.camera.set(4, self.__SCREEN_HEIGHT)
 
         self.pan_servo = picar.Servo.Servo(1)
-        self.pan_servo.offset = -30  # calibrate servo to center
+        self.pan_servo.offset = 0  # calibrate servo to center
         self.pan_servo.write(90)
 
         self.tilt_servo = picar.Servo.Servo(2)
-        self.tilt_servo.offset = 20  # calibrate servo to center
+        self.tilt_servo.offset = 0  # calibrate servo to center
         self.tilt_servo.write(90)
 
         logging.debug('Set up back wheels')
@@ -39,18 +40,19 @@ def __init__(self):
 
         logging.debug('Set up front wheels')
         self.front_wheels = picar.front_wheels.Front_Wheels()
-        self.front_wheels.turning_offset = -25  # calibrate servo to center
+        self.front_wheels.turning_offset = 0  # calibrate servo to center
         self.front_wheels.turn(90)  # Steering Range is 45 (left) - 90 (center) - 135 (right)
 
-        self.lane_follower = HandCodedLaneFollower(self)
+#         self.lane_follower = HandCodedLaneFollower(self)
+        self.lane_follower = EndToEndLaneFollower(self)
         self.traffic_sign_processor = ObjectsOnRoadProcessor(self)
-        # lane_follower = DeepLearningLaneFollower()
+#         lane_follower = DeepLearningLaneFollower()
 
         self.fourcc = cv2.VideoWriter_fourcc(*'XVID')
         datestr = datetime.datetime.now().strftime("%y%m%d_%H%M%S")
-        self.video_orig = self.create_video_recorder('../data/tmp/car_video%s.avi' % datestr)
-        self.video_lane = self.create_video_recorder('../data/tmp/car_video_lane%s.avi' % datestr)
-        self.video_objs = self.create_video_recorder('../data/tmp/car_video_objs%s.avi' % datestr)
+        self.video_orig = self.create_video_recorder('../data/car_video%s.avi' % datestr)
+        self.video_lane = self.create_video_recorder('../data/car_video_lane%s.avi' % datestr)
+        self.video_objs = self.create_video_recorder('../data/car_video_objs%s.avi' % datestr)
 
         logging.info('Created a DeepPiCar')
 
@@ -67,9 +69,9 @@ def __exit__(self, _type, value, traceback):
             # Exception occurred:
             logging.error('Exiting with statement with exception %s' % traceback)
 
-        self.cleanup()
+        self.cleanup(self.i, self.first_time)
 
-    def cleanup(self):
+    def cleanup(self, steering_num, first_time):
         """ Reset the hardware"""
         logging.info('Stopping the car, resetting hardware.')
         self.back_wheels.speed = 0
@@ -78,6 +80,12 @@ def cleanup(self):
         self.video_orig.release()
         self.video_lane.release()
         self.video_objs.release()
+        last_time = datetime.datetime.now()
+        diff = last_time - first_time
+        diff = float(diff.total_seconds())
+        logging.info("steers: " + str(steering_num))
+        logging.info("time: " + str(diff))
+        logging.info("steers per min: " + str(steering_num/diff))
         cv2.destroyAllWindows()
 
     def drive(self, speed=__INITIAL_SPEED):
@@ -88,24 +96,26 @@ def drive(self, speed=__INITIAL_SPEED):
         """
 
         logging.info('Starting to drive at speed %s...' % speed)
+        self.back_wheels.forward()
         self.back_wheels.speed = speed
-        i = 0
+        self.i = 0
+        self.first_time = datetime.datetime.now()
         while self.camera.isOpened():
             _, image_lane = self.camera.read()
             image_objs = image_lane.copy()
-            i += 1
+            self.i += 1
             self.video_orig.write(image_lane)
 
-            image_objs = self.process_objects_on_road(image_objs)
-            self.video_objs.write(image_objs)
-            show_image('Detected Objects', image_objs)
+#             image_objs = self.process_objects_on_road(image_objs)
+#             self.video_objs.write(image_objs)
+#             show_image('Detected Objects', image_objs)
 
             image_lane = self.follow_lane(image_lane)
             self.video_lane.write(image_lane)
             show_image('Lane Lines', image_lane)
 
             if cv2.waitKey(1) & 0xFF == ord('q'):
-                self.cleanup()
+                self.cleanup(self.i, self.first_time)
                 break
 
     def process_objects_on_road(self, image):
@@ -129,7 +139,6 @@ def main():
     with DeepPiCar() as car:
         car.drive(40)
 
-
 if __name__ == '__main__':
     logging.basicConfig(level=logging.DEBUG, format='%(levelname)-5s:%(asctime)s: %(message)s')
 

driver/code/end_to_end_lane_follower.py

@@ -12,7 +12,7 @@ class EndToEndLaneFollower(object):
 
     def __init__(self,
                  car=None,
-                 model_path='/home/pi/DeepPiCar/models/lane_navigation/data/model_result/lane_navigation.h5'):
+                 model_path='/home/pi/DeepPiCar/models/lane_navigation/data/model_result/lane_navigation_final.h5'):
         logging.info('Creating a EndToEndLaneFollower...')
 
         self.car = car

driver/code/end_to_end_lane_follower_tpu.py

@@ -0,0 +1,167 @@
+import cv2
+import numpy as np
+import logging
+import math
+from keras.models import load_model
+from hand_coded_lane_follower import HandCodedLaneFollower
+import tflite_runtime.interpreter as tflite
+
+_SHOW_IMAGE = False
+
+
+class EndToEndLaneFollower(object):
+
+    def __init__(self,
+                 car=None,
+                 model_path='/home/pi/DeepPiCar/models/lane_navigation/data/model_result/lane_navigation_final.h5'):
+        logging.info('Creating a EndToEndLaneFollower...')
+
+        self.car = car
+        self.curr_steering_angle = 90
+        self.model = load_model(model_path)
+
+    def follow_lane(self, frame):
+        # Main entry point of the lane follower
+        show_image("orig", frame)
+
+        self.curr_steering_angle = self.compute_steering_angle(frame)
+        logging.debug("curr_steering_angle = %d" % self.curr_steering_angle)
+
+        if self.car is not None:
+            self.car.front_wheels.turn(self.curr_steering_angle)
+        final_frame = display_heading_line(frame, self.curr_steering_angle)
+
+        return final_frame
+
+#     def compute_steering_angle(self, frame):
+#         """ Find the steering angle directly based on video frame
+#             We assume that camera is calibrated to point to dead center
+#         """
+#         preprocessed = img_preprocess(frame)
+#         X = np.asarray([preprocessed])
+#         steering_angle = self.model.predict(X)[0]
+# 
+#         logging.debug('new steering angle: %s' % steering_angle)
+#         return int(steering_angle + 0.5) # round the nearest integer
+
+    def compute_steering_angle(self, frame):
+        """ Find the steering angle directly based on video frame
+            We assume that camera is calibrated to point to dead center
+        """
+        # Load the TFLite model and allocate tensors.
+        model_path = "/home/pi/DeepPiCar/tf_model_edgetpu.tflite"
+        proc_img = img_preprocess(frame)
+
+        interpreter = tflite.Interpreter(model_path, experimental_delegates=[tflite.load_delegate('libedgetpu.so.1')])
+        interpreter.allocate_tensors()
+
+        # Get input and output tensors.
+        input_details = interpreter.get_input_details()
+        output_details = interpreter.get_output_details()
+
+        input_shape = input_details[0]['shape']
+        input_data = np.asarray([proc_img], dtype=np.float32)
+        interpreter.set_tensor(input_details[0]['index'], input_data)
+
+        interpreter.invoke()
+        output_data = interpreter.get_tensor(output_details[0]['index'])
+        print(output_data)
+        return int(output_data[0][0] + 0.5) # round the nearest integer
+
+
+def img_preprocess(image):
+    height, _, _ = image.shape
+    image = image[int(height/2):,:,:]  # remove top half of the image, as it is not relevant for lane following
+    image = cv2.cvtColor(image, cv2.COLOR_BGR2YUV)  # Nvidia model said it is best to use YUV color space
+    image = cv2.GaussianBlur(image, (3,3), 0)
+    image = cv2.resize(image, (200,66)) # input image size (200,66) Nvidia model
+    image = image / 255 # normalizing, the processed image becomes black for some reason.  do we need this?
+    return image
+
+def display_heading_line(frame, steering_angle, line_color=(0, 0, 255), line_width=5, ):
+    heading_image = np.zeros_like(frame)
+    height, width, _ = frame.shape
+
+    # figure out the heading line from steering angle
+    # heading line (x1,y1) is always center bottom of the screen
+    # (x2, y2) requires a bit of trigonometry
+
+    # Note: the steering angle of:
+    # 0-89 degree: turn left
+    # 90 degree: going straight
+    # 91-180 degree: turn right 
+    steering_angle_radian = steering_angle / 180.0 * math.pi
+    x1 = int(width / 2)
+    y1 = height
+    x2 = int(x1 - height / 2 / math.tan(steering_angle_radian))
+    y2 = int(height / 2)
+
+    cv2.line(heading_image, (x1, y1), (x2, y2), line_color, line_width)
+    heading_image = cv2.addWeighted(frame, 0.8, heading_image, 1, 1)
+
+    return heading_image
+
+
+def show_image(title, frame, show=_SHOW_IMAGE):
+    if show:
+        cv2.imshow(title, frame)
+
+
+############################
+# Test Functions
+############################
+def test_photo(file):
+    lane_follower = EndToEndLaneFollower()
+    frame = cv2.imread(file)
+    combo_image = lane_follower.follow_lane(frame)
+    show_image('final', combo_image, True)
+    logging.info("filename=%s, model=%3d" % (file, lane_follower.curr_steering_angle))
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+
+def test_video(video_file):
+    end_to_end_lane_follower = EndToEndLaneFollower()
+    hand_coded_lane_follower = HandCodedLaneFollower()
+    cap = cv2.VideoCapture(video_file + '.avi')
+
+    # skip first second of video.
+    for i in range(3):
+        _, frame = cap.read()
+
+    video_type = cv2.VideoWriter_fourcc(*'XVID')
+    video_overlay = cv2.VideoWriter("%s_end_to_end.avi" % video_file, video_type, 20.0, (320, 240))
+    try:
+        i = 0
+        while cap.isOpened():
+            _, frame = cap.read()
+            frame_copy = frame.copy()
+            logging.info('Frame %s' % i)
+            combo_image1 = hand_coded_lane_follower.follow_lane(frame)
+            combo_image2 = end_to_end_lane_follower.follow_lane(frame_copy)
+
+            diff = end_to_end_lane_follower.curr_steering_angle - hand_coded_lane_follower.curr_steering_angle;
+            logging.info("desired=%3d, model=%3d, diff=%3d" %
+                          (hand_coded_lane_follower.curr_steering_angle,
+                          end_to_end_lane_follower.curr_steering_angle,
+                          diff))
+            video_overlay.write(combo_image2)
+            cv2.imshow("Hand Coded", combo_image1)
+            cv2.imshow("Deep Learning", combo_image2)
+
+            i += 1
+            if cv2.waitKey(1) & 0xFF == ord('q'):
+                break
+    finally:
+        cap.release()
+        video_overlay.release()
+        cv2.destroyAllWindows()
+
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.INFO)
+
+    test_video('/home/pi/DeepPiCar/models/lane_navigation/data/images/video01')
+    #test_photo('/home/pi/DeepPiCar/models/lane_navigation/data/images/video01_100_084.png')
+    # test_photo(sys.argv[1])
+    # test_video(sys.argv[1])

driver/code/hand_coded_lane_follower.py

@@ -4,9 +4,20 @@
 import math
 import datetime
 import sys
+import os.path
 
 _SHOW_IMAGE = False
 
+# create log file
+log_flag = True
+log_count = 1
+
+while log_flag:
+	if not os.path.isfile('logs/log_file' + str(log_count) + '.txt'):
+		log_history = open('logs/log_file' + str(log_count) + '.txt', "w")
+		log_flag = False
+	else:
+		log_count += 1
 
 class HandCodedLaneFollower(object):
 
@@ -68,7 +79,7 @@ def detect_edges(frame):
     # filter for blue lane lines
     hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
     show_image("hsv", hsv)
-    lower_blue = np.array([30, 40, 0])
+    lower_blue = np.array([30, 80, 20])
     upper_blue = np.array([150, 255, 255])
     mask = cv2.inRange(hsv, lower_blue, upper_blue)
     show_image("blue mask", mask)
@@ -78,6 +89,20 @@ def detect_edges(frame):
 
     return edges
 
+# def detect_edges(frame):
+#     # filter for blue lane lines
+#     hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
+#     show_image("hsv", hsv)
+#     lower_blue = np.array([30, 40, 0])
+#     upper_blue = np.array([150, 255, 255])
+#     mask = cv2.inRange(hsv, lower_blue, upper_blue)
+#     show_image("blue mask", mask)
+# 
+#     # detect edges
+#     edges = cv2.Canny(mask, 200, 400)
+# 
+#     return edges
+
 def detect_edges_old(frame):
     # filter for blue lane lines
     hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
@@ -125,7 +150,7 @@ def detect_line_segments(cropped_edges):
     rho = 1  # precision in pixel, i.e. 1 pixel
     angle = np.pi / 180  # degree in radian, i.e. 1 degree
     min_threshold = 10  # minimal of votes
-    line_segments = cv2.HoughLinesP(cropped_edges, rho, angle, min_threshold, np.array([]), minLineLength=8,
+    line_segments = cv2.HoughLinesP(cropped_edges, rho, angle, min_threshold, np.array([]), minLineLength=25,
                                     maxLineGap=4)
 
     if line_segments is not None:
@@ -163,10 +188,11 @@ def average_slope_intercept(frame, line_segments):
             fit = np.polyfit((x1, x2), (y1, y2), 1)
             slope = fit[0]
             intercept = fit[1]
-            if slope < 0:
+            log_history.write(datetime.datetime.now().strftime("%H:%M:%S") + " slope: " + str(slope) + "\n")
+            if slope < 0.85:
                 if x1 < left_region_boundary and x2 < left_region_boundary:
                     left_fit.append((slope, intercept))
-            else:
+            if slope > -0.85:
                 if x1 > right_region_boundary and x2 > right_region_boundary:
                     right_fit.append((slope, intercept))
 
@@ -281,7 +307,7 @@ def length_of_line_segment(line):
 
 def show_image(title, frame, show=_SHOW_IMAGE):
     if show:
-        cv2.imshow(title, frame)
+    	cv2.imshow(title, frame)
 
 
 def make_points(frame, line):
@@ -291,6 +317,8 @@ def make_points(frame, line):
     y2 = int(y1 * 1 / 2)  # make points from middle of the frame down
 
     # bound the coordinates within the frame
+    if slope == 0:
+    	slope = 0.0001 
     x1 = max(-width, min(2 * width, int((y1 - intercept) / slope)))
     x2 = max(-width, min(2 * width, int((y2 - intercept) / slope)))
     return [[x1, y1, x2, y2]]
@@ -346,4 +374,4 @@ def test_video(video_file):
     test_video('/home/pi/DeepPiCar/driver/data/tmp/video01')
     #test_photo('/home/pi/DeepPiCar/driver/data/video/car_video_190427_110320_073.png')
     #test_photo(sys.argv[1])
-    #test_video(sys.argv[1])
+    #test_video(sys.argv[1])