Beagle Board - beagleboard.org

Young Boys

We are seeking to develop a lane-keeping and stop-sign recognition RC car using computer vision and the BeagleBone Black as a controller.

AdvancedFull instructions provided20 hours8
Young Boys

Things used in this project

Hardware components

BeagleBone Black
BeagleBoard.org BeagleBone Black
×1
USB WiFi Adapter
×1
Webcam
×1
USB Hub
×1

Software apps and online services

OpenCV
OpenCV

Story

Read more

Code

Young Boys Project

Python
its all of our code
import cv2
import numpy as np
import math
import sys
import time
import matplotlib.pyplot as plt
import Adafruit_BBIO.PWM as PWM
'''
YoungBoys
Made by Christian, Son, Mahmoud, and Robbie

Code inspired by:
User raja_961, "Autonomous Lane-Keeping Car Using Raspberry Pi and OpenCV"
https://www.instructables.com/Autonomous-Lane-Keeping-Car-Using-Raspberry-Pi-and/
'''

def detect_red(frame,threshold=1000):
    '''
    detect_red color with a given threshold. <:
    '''
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_red = np.array([150, 70, 50], dtype="uint8")
    upper_red = np.array([179, 255, 255], dtype="uint8")
    mask = cv2.inRange(hsv, lower_red, upper_red)
    #cv2.imshow("Red_color",mask)
    return mask.sum()>threshold

def detect_edges(frame):
    #print("Start ED")
    # filter for blue lane lines
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    #cv2.imshow("HSV",hsv)
    lower_blue = np.array([90, 120, 0], dtype = "uint8")
    upper_blue = np.array([150, 255, 255], dtype="uint8")
    mask = cv2.inRange(hsv,lower_blue,upper_blue)
    #cv2.imshow("mask",mask)

    # detect edges
    edges = cv2.Canny(mask, 50, 100)
    #cv2.imshow("edges",edges)
    #print("End ED")
    return edges

def region_of_interest(edges):
    height, width = edges.shape
    mask = np.zeros_like(edges)

    # only focus lower half of the screen
    polygon = np.array([[
        (0, height),
        (0,  height/2),
        (width , height/2),
        (width , height),
    ]], np.int32)

    cv2.fillPoly(mask, polygon, 255)

    cropped_edges = cv2.bitwise_and(edges, mask)
    #cv2.imshow("roi",cropped_edges)

    return cropped_edges

def detect_line_segments(cropped_edges):
    #detects line segment via HoughLines
    rho = 1
    theta = np.pi / 180
    min_threshold = 10

    line_segments = cv2.HoughLinesP(cropped_edges, rho, theta, min_threshold,
                                    np.array([]), minLineLength=5, maxLineGap=150)

    return line_segments

def average_slope_intercept(frame, line_segments):
    #finds average slope intercept
    lane_lines = []

    if line_segments is None:
        #print("no line segments detected")
        return lane_lines

    height, width,_ = frame.shape
    left_fit = []
    right_fit = []

    boundary = 1/3
    left_region_boundary = width * (1 - boundary)
    right_region_boundary = width * boundary

    for line_segment in line_segments:
        for x1, y1, x2, y2 in line_segment:
            if x1 == x2: #skip if slope is infinity
                continue

            #find line
            fit = np.polyfit((x1, x2), (y1, y2), 1)
            slope = (y2 - y1) / (x2 - x1)
            intercept = y1 - (slope * x1)

            #plot to where it converges at the center
            if slope < 0:
                if x1 < left_region_boundary and x2 < left_region_boundary:
                    left_fit.append((slope, intercept))
            else:
                if x1 > right_region_boundary and x2 > right_region_boundary:
                    right_fit.append((slope, intercept))

    left_fit_average = np.average(left_fit, axis=0)
    if len(left_fit) > 0:
        lane_lines.append(make_points(frame, left_fit_average))

    right_fit_average = np.average(right_fit, axis=0)
    if len(right_fit) > 0:
        lane_lines.append(make_points(frame, right_fit_average))

    return lane_lines

def make_points(frame, line):
    #helper function for drawing
    height, width, _ = frame.shape

    slope, intercept = line

    y1 = height  # bottom of the frame
    y2 = int(y1 / 2)  # make points from middle of the frame down

    if slope == 0:
        slope = 0.1

    x1 = int((y1 - intercept) / slope)
    x2 = int((y2 - intercept) / slope)

    return [[x1, y1, x2, y2]]

def display_lines(frame, lines, line_color=(0, 255, 0), line_width=6):
    #this just displays the boundary lines we previously found on the image
    line_image = np.zeros_like(frame)

    if lines is not None:
        for line in lines:
            for x1, y1, x2, y2 in line:
                #line displayed here
                cv2.line(line_image, (x1, y1), (x2, y2), line_color, line_width)

    line_image = cv2.addWeighted(frame, 0.8, line_image, 1, 1)
    return line_image

def display_heading_line(frame, steering_angle, line_color=(0, 0, 255), line_width=5 ):
    #this generates and displays the heading lines
    heading_image = np.zeros_like(frame)
    height, width, _ = frame.shape

    #find angle
    steering_angle_radian = steering_angle / 180.0 * math.pi

    #find the points for the line
    x1 = int(width / 2)
    y1 = height
    x2 = int(x1 - height / 2 / math.tan(steering_angle_radian))
    y2 = int(height / 2)

    #display heading line
    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 get_steering_angle(frame, lane_lines):
    #produces the steering angle for the heading line and 
    #used to find deviation
    height,width,_ = frame.shape

    if len(lane_lines) == 2:
        _, _, left_x2, _ = lane_lines[0][0]
        _, _, right_x2, _ = lane_lines[1][0]
        mid = int(width / 2)
        x_offset = (left_x2 + right_x2) / 2 - mid
        y_offset = int(height / 2)

    elif len(lane_lines) == 1:
        x1, _, x2, _ = lane_lines[0][0]
        x_offset = x2 - x1
        y_offset = int(height / 2)

    elif len(lane_lines) == 0:
        x_offset = 0
        y_offset = int(height / 2)

    #fun cosine math to find angle
    angle_to_mid_radian = math.atan(x_offset / y_offset)
    angle_to_mid_deg = int(angle_to_mid_radian * 180.0 / math.pi)
    steering_angle = angle_to_mid_deg + 90

    return steering_angle
    
def cam_test(res):
    '''
    Helper function for testing.
    Seeing the camera input and lines being detected.
    '''
    #Capturing
    video = cv2.VideoCapture(0)
    video.set(cv2.CAP_PROP_FRAME_WIDTH,320)#320
    video.set(cv2.CAP_PROP_FRAME_HEIGHT,60)#240
    while(True):      
        ret,frame = video.read()
        frame = cv2.resize(frame, (res, res))
        #frame = cv2.flip(frame,-1)
        #cv2.imshow("original",frame)
        edges = detect_edges(frame)
        roi = region_of_interest(edges)
        line_segments = detect_line_segments(roi)
        lane_lines = average_slope_intercept(frame,line_segments)
        lane_lines_image = display_lines(frame,lane_lines)
        steering_angle = get_steering_angle(frame, lane_lines)
        heading_image = display_heading_line(lane_lines_image,steering_angle)
        cv2.imshow("heading line",heading_image)
        print("Frame processed\n")
        k = cv2.waitKey(1) & 0xFF
        if k == ord('q'):
           break
    video.release()
    cv2.destroyAllWindows()
    
def main(res=100, kpr=12, kdr=8, driving_speed = 7.92 ):
    steering_port = "P9_14"
    driving_port = "P8_13"
    
    # Set up PWM
    PWM.start(steering_port, 7.5, 50)
    PWM.start(driving_port, 7.5, 50)

    #Capturing
    video = cv2.VideoCapture(0)
    video.set(cv2.CAP_PROP_FRAME_WIDTH,320)#320
    video.set(cv2.CAP_PROP_FRAME_HEIGHT,240)#240

    #Saving Vid
    savedvid = cv2.VideoWriter('carvid.avi',cv2.VideoWriter.fourcc(*'MJPG'), 10 , (res,res))

    # Calibration Setup
    rotbase = 7.5
    lastTime = 0
    lastError = 0
    j= 0

    #Adding data points
    der_r = np.array([])
    pro_r = np.array([])
    err = np.array([])
    steering = np.array([])
    speed = np.array([])
    
    time.sleep(3) # calibration for esc
    PWM.set_duty_cycle(driving_port, driving_speed)
    
    #Red detection
    red_detected = False
    a=50
    
    while(True):
        ret,frame = video.read()
        frame = cv2.resize(frame, (res, res))
        
        #Don't attempt to read the stop sign for the first a's iteration.
        if (j>a):
            if detect_red(frame):
                if red_detected: #Terminate if we already saw stop sign before.
                  break
                a = j + 100
                red_detected = True
                PWM.set_duty_cycle(driving_port,7.5)
                time.sleep(2)
        
        #Edge Detection
        edges = detect_edges(frame)
        roi = region_of_interest(edges)
        line_segments = detect_line_segments(roi)
        lane_lines = average_slope_intercept(frame,line_segments)
        steering_angle = get_steering_angle(frame, lane_lines)
        
        #deviation calculation for P and D
        now = time.time()
        dt = now - lastTime
        deviation = steering_angle - 90
        
        if deviation < 3 and deviation > -3: #Min margin before turning
             #Find new derivative and proportional gain
            deviation = 0
            derivative_r = kdr * (deviation - lastError) / dt
            proportional_r = kpr * deviation
            PD_r = derivative_r + proportional_r
            rot = rotbase - PD_r/90
            PWM.set_duty_cycle(steering_port, 7.5) #going straight
        else:
            #Find new derivative and proportional gain
            derivative_r = kdr * (deviation - lastError) / dt
            proportional_r = kpr * deviation
            PD_r = derivative_r + proportional_r
            rot = rotbase - PD_r/90
            
            #Making sure rot stay in the correct duty cycle range.
            if rot>9.5: 
                rot = 9.5
            elif rot<5.5:
                rot = 5.5
            PWM.set_duty_cycle(steering_port, rot)
        new_speed = driving_speed+.0008*(j//20)
        PWM.set_duty_cycle(driving_port, new_speed)
        
        #Collecting data
        speed=np.append(speed, new_speed)
        steering=np.append(steering, rot)
        der_r=np.append(der_r,derivative_r)
        pro_r=np.append(pro_r,proportional_r)
        err=np.append(err,deviation/10)
        
        #Showing and Saving Video - Comment Out for better performance
        lane_lines_image = display_lines(frame,lane_lines)
        screen_data = f"E:{str(deviation)[:4]}, Str:{str(rot)[:4]}, Spd:{str(driving_speed)[:4]}"
        heading_image = display_heading_line(lane_lines_image,steering_angle)
        heading_image = cv2.putText(heading_image, screen_data, (10,30), cv2.FONT_HERSHEY_PLAIN, 1.2, (0,255,255), cv2.LINE_4) 
        savedvid.write(heading_image)
        cv2.imshow("heading line",heading_image)

        k = cv2.waitKey(1) & 0xFF
        if k == ord('q'):
            break

        lastError = deviation
        lastTime = time.time()
        j += 1
        
    #setting values back to default
    PWM.set_duty_cycle(driving_port,7.5)
    PWM.set_duty_cycle(steering_port,7.5)
    savedvid.release()
    video.release()
    cv2.destroyAllWindows()
    j_array = np.array(range(j))

    
    #plotting error and pwm
    fig, ax = plt.subplots()
    ax.plot(j_array, steering, label="steering PWM")
    ax.plot(j_array, speed, label="speed PWM")
    ax.set_xlabel("Frame")
    ax.set_ylabel("PWM")
    ax.legend()

    ax2 = ax.twinx()
    ax2.plot(j_array, err, 'r-.', label="error")
    plt.grid(True)
    ax2.legend(loc=0)
    plt.title("PWM and Error")
    plt.show()

    plt.savefig("pwm.jpg")
    plt.clf()

    #Plotting proportional gain, der gain, error
    fig, ax = plt.subplots()
    ax.plot(j_array, der_r, 'g', label='derivative_r')
    ax.plot(j_array, pro_r, 'b', label='proportional_r')
    ax.set_xlabel("Frame")
    ax.legend()
    ax.set_ylabel("Proportional and Derivative Gain")

    ax2=ax.twinx()
    ax2.plot(j_array,err,'r-.', label='error')
    ax2.set_ylabel("Error")
    ax2.legend(loc=0)
    plt.title('KPR, KDR, and Error')
    plt.show()

    plt.savefig("prop.jpg")
    

#cam_test(res=80) #For testing the camera
main(res=250, kpr=10, kdr=0, driving_speed =7.88)

Credits

Robbie Kenworthy

Robbie Kenworthy

1 project • 0 followers
Christian Durante

Christian Durante

1 project • 2 followers
Son Nguyen

Son Nguyen

1 project • 1 follower
Mahmoud

Mahmoud

1 project • 0 followers
Thanks to raja_961.

Comments

Add projectSign up / Login