├── trackbars_mask_script.py ├── xb.py ├── pi_drive.py └── lane_methods.py /trackbars_mask_script.py: -------------------------------------------------------------------------------- 1 | # script for finding out HSV values for masking 2 | 3 | import cv2 4 | import numpy as np 5 | 6 | 7 | # Track Bars to update values whenever called 8 | def tbVals(x): 9 | global H_low, H_high, S_low, S_high, V_low, V_high 10 | # assign trackbar position value to H,S,V High and low variable 11 | H_low = cv2.getTrackbarPos('low H', 'controls') 12 | H_high = cv2.getTrackbarPos('high H', 'controls') 13 | S_low = cv2.getTrackbarPos('low S', 'controls') 14 | S_high = cv2.getTrackbarPos('high S', 'controls') 15 | V_low = cv2.getTrackbarPos('low V', 'controls') 16 | V_high = cv2.getTrackbarPos('high V', 'controls') 17 | 18 | 19 | #create a seperate window named 'controls' for trackbar 20 | cv2.namedWindow('controls',2) 21 | cv2.resizeWindow("controls", 550,10); 22 | 23 | #global variable 24 | H_low = 0 25 | H_high = 179 26 | S_low= 0 27 | S_high = 255 28 | V_low= 0 29 | V_high = 255 30 | 31 | 32 | #create trackbars for high,low H,S,V 33 | cv2.createTrackbar('low H','controls',0,179,tbVals) 34 | cv2.createTrackbar('high H','controls',179,179,tbVals) 35 | 36 | cv2.createTrackbar('low S','controls',0,255,tbVals) 37 | cv2.createTrackbar('high S','controls',255,255,tbVals) 38 | 39 | cv2.createTrackbar('low V','controls',0,255,tbVals) 40 | cv2.createTrackbar('high V','controls',255,255,tbVals) 41 | 42 | while (1): 43 | # read source image 44 | imgBig = cv2.imread("lane.jpg") 45 | img = cv2.resize(imgBig, (320, 240)) 46 | # convert sourece image to HSC color mode 47 | hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) 48 | 49 | # 50 | hsv_low = np.array([H_low, S_low, V_low], np.uint8) 51 | hsv_high = np.array([H_high, S_high, V_high], np.uint8) 52 | 53 | # making mask for hsv range 54 | mask = cv2.inRange(hsv, hsv_low, hsv_high) 55 | print(mask) 56 | # masking HSV value selected color becomes black 57 | res = cv2.bitwise_and(img, img, mask=mask) 58 | 59 | # show image 60 | cv2.imshow('mask', mask) 61 | cv2.imshow('res', res) 62 | 63 | # waitfor the user to press escape and break the while loop 64 | k = cv2.waitKey(1) & 0xFF 65 | if k == 27: 66 | break 67 | 68 | # destroys all window 69 | cv2.destroyAllWindows() 70 | 71 | 72 | -------------------------------------------------------------------------------- /xb.py: -------------------------------------------------------------------------------- 1 | 2 | # Script for debugging and testing car manually 3 | 4 | import RPi.GPIO as GPIO 5 | import pigpio 6 | import time 7 | from time import sleep 8 | import pygame 9 | 10 | pygame.init() 11 | controller = pygame.joystick.Joystick(0) 12 | controller.init() 13 | 14 | GPIO.setmode(GPIO.BCM) 15 | GPIO.setwarnings(False) 16 | 17 | servo = 12 18 | Ena= 26 19 | In1= 6 20 | In2= 5 21 | 22 | 23 | GPIO.setup(Ena,GPIO.OUT) 24 | GPIO.setup(In1,GPIO.OUT) 25 | GPIO.setup(In2,GPIO.OUT) 26 | pwmA =GPIO.PWM(Ena,100); 27 | pwmA.start(0); 28 | 29 | pwm = pigpio.pi() 30 | pwm.set_mode(servo, pigpio.OUTPUT) 31 | pwm.set_PWM_frequency( servo, 50 ) 32 | 33 | axis_start = -0.99 34 | axis_end = 0.99 35 | axis_range = axis_end - axis_start 36 | 37 | throttle_start = 0 38 | throttle_end = 100 39 | throttle_range = throttle_end - throttle_start 40 | # output_start + ((output_end - output_start) / (input_end - input_start)) * (input - input_start) 41 | 42 | steering_start = 1000 43 | steering_end = 2000 44 | steering_range = steering_end - steering_start 45 | 46 | 47 | def getJS(name=''): 48 | 49 | global buttons 50 | # retrieve any events ... 51 | for event in pygame.event.get(): # Analog Sticks 52 | if event.type == pygame.JOYAXISMOTION: 53 | #print("AXIS", event.axis , "Value = " , event.value) 54 | 55 | # throttle 56 | # axis range = -1 to 1 57 | # car throttle range = 0 to 100 58 | if event.axis == 5: 59 | current_axis = event.value 60 | throttle_input = throttle_start + ((throttle_range) / (axis_range)) * (current_axis - axis_start) 61 | print("Throttle Input : ",int(throttle_input)) 62 | 63 | pwmA.ChangeDutyCycle(int(throttle_input)); 64 | GPIO.output(In1,GPIO.LOW); 65 | GPIO.output(In2,GPIO.HIGH); 66 | 67 | if event.axis == 2: 68 | current_axis = event.value 69 | throttle_input = throttle_start + ((throttle_range) / (axis_range)) * (current_axis - axis_start) 70 | print("Reverse Throttle Input : ",int(throttle_input)) 71 | pwmA.ChangeDutyCycle(int(throttle_input)); 72 | GPIO.output(In1,GPIO.HIGH); 73 | GPIO.output(In2,GPIO.LOW); 74 | 75 | if event.axis == 0: 76 | current_axis = event.value 77 | steering_input = steering_start + ((steering_range) / (axis_range)) * (current_axis - axis_start) 78 | print("Steering Input : ",int(steering_input)) 79 | pwm.set_servo_pulsewidth( servo, steering_input ) ; 80 | 81 | 82 | 83 | 84 | elif event.type == pygame.JOYBUTTONDOWN: # When button pressed 85 | print(event.dict, event.joy, event.button, 'PRESSED') 86 | pwmA.ChangeDutyCycle(27); 87 | GPIO.output(In1,GPIO.LOW); 88 | GPIO.output(In2,GPIO.HIGH); 89 | 90 | 91 | 92 | elif event.type == pygame.JOYBUTTONUP: # When button released 93 | print(event.dict, event.joy, event.button, 'released') 94 | 95 | 96 | 97 | 98 | 99 | def main(): 100 | #print(getJS()) # To get all values 101 | #sleep(0.05) 102 | getJS('share') # To get a single value 103 | sleep(0.05) 104 | 105 | 106 | if __name__ == '__main__': 107 | while True: 108 | main() 109 | 110 | 111 | -------------------------------------------------------------------------------- /pi_drive.py: -------------------------------------------------------------------------------- 1 | from lane_methods import mask_img, detect_edges, crop_roi, \ 2 | detect_lines, draw_lines, lane_search_area, \ 3 | group_lines, line_to_point, steering_angle 4 | import cv2 5 | import time 6 | import RPi.GPIO as GPIO 7 | import pigpio 8 | import time 9 | from time import sleep 10 | 11 | 12 | 13 | # motor driver pins 14 | Ena= 26 15 | In1= 6 16 | In2= 5 17 | 18 | # servo ppin 19 | servo = 12 20 | 21 | 22 | # GPIO SETUP 23 | GPIO.setmode(GPIO.BCM) 24 | GPIO.setwarnings(False) 25 | GPIO.setup(Ena,GPIO.OUT) 26 | GPIO.setup(In1,GPIO.OUT) 27 | GPIO.setup(In2,GPIO.OUT) 28 | 29 | # speed control with pwmA 30 | pwmA =GPIO.PWM(Ena,100) 31 | pwmA.start(0) 32 | 33 | # servo setup 34 | pwm = pigpio.pi() 35 | pwm.set_mode(servo, pigpio.OUTPUT) 36 | pwm.set_PWM_frequency( servo, 50 ) 37 | 38 | 39 | 40 | # range for steering input 41 | axis_start = 50 42 | axis_end = -50 43 | axis_range = axis_end - axis_start 44 | 45 | # range for throttle 46 | throttle_start = 0 47 | throttle_end = 100 48 | throttle_range = throttle_end - throttle_start 49 | 50 | # range for car servo 51 | steering_start = 900 52 | steering_end = 2100 53 | steering_range = steering_end - steering_start 54 | 55 | 56 | # list to keep last 'n' outputs. Idea is to average all of them and then produce a final output, 57 | # this helps in smoothing out any jerks and sudden changes in servo and speed 58 | steering_last_five = [] 59 | throttle_last_three = [] 60 | 61 | # for caliberating steering of car 62 | steering_factor = 1 63 | 64 | # starting throttle value, will be 0 65 | th = 0 66 | 67 | def main(): 68 | cam = cv2.VideoCapture(0) 69 | 70 | while True: 71 | 72 | ret, frame = cam.read() 73 | frame = cv2.resize(frame, (640, 480)) 74 | frame_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) 75 | 76 | # for debug, sh = True 77 | sh = False 78 | 79 | # Masking 80 | frame_masked = mask_img(frame_hsv, show=sh) 81 | 82 | 83 | # Extract Edges 84 | frame_edges = detect_edges(frame_masked, show=sh) 85 | 86 | 87 | # Crop top half, Only lower half is region of interest 88 | frame_cropped = crop_roi(frame_edges, show=sh) 89 | 90 | 91 | # Detect line segments 92 | frame_lines = detect_lines(frame_cropped) 93 | #draw_lines(frame, frame_lines) 94 | 95 | 96 | # Group lines into left and right catgry 97 | frame_lines_grouped = group_lines(frame, frame_lines) 98 | 99 | #this is to view where is search area for lanes, helps in debugging 100 | #lane_search_area(frame, 0.7) 101 | 102 | # Draw Lines 103 | draw_lines(frame, frame_lines_grouped) 104 | 105 | 106 | steerin_input_raw = steering_angle(frame, frame_lines_grouped, show=True) 107 | print("Steering Input Raw: ",int(steerin_input_raw)) 108 | 109 | 110 | # caliberating with some factor 111 | current_axis = steerin_input_raw * steering_factor 112 | print("Steering Input Raw Corrected : ",int(current_axis)) 113 | 114 | # mapping steering output to servo angle 115 | steering_input = steering_start + ((steering_range) / (axis_range)) * (current_axis - axis_start) 116 | print("Steering Input Final: ",int(steering_input), '\n\n') 117 | 118 | st = int(steering_input) 119 | 120 | # appending it to the list for averaging with past 5 values 121 | steering_last_five.append(st) 122 | if len(steering_last_five) > 5: 123 | steering_last_five.pop(0) 124 | st = int(sum(steering_last_five) / len(steering_last_five)) 125 | 126 | 127 | 128 | 129 | # if only one lane line detected, means car need to take hard right or left 130 | # also steering_angle() returns slope of the one lane if there is only one lane. 131 | # this slope range is different from when two lanes are deected. 132 | # So we define new range that is here 70 to 20. and map servo angle accordingly 133 | if(len(frame_lines_grouped) == 1): 134 | print("ONE LANE SLOPE : ", steerin_input_raw) 135 | axs = 70 136 | axe = 20 137 | axr = axe - axs 138 | st = steering_start + ((steering_range) / (axr)) * (steerin_input_raw - axs) 139 | print("ONE LANE STEER :", st) 140 | 141 | # also we have to slow down the car as there is steep turn when there is only one lane marking 142 | th = th - 2 143 | throttle_input = th 144 | print("Throttle Down : ", throttle_input) 145 | 146 | else: 147 | # if two name marking visible, means road is mostly straingt, 148 | # so we can increase throttle input 149 | th = th + 3 150 | throttle_input = th 151 | print("Throttle Up : ", throttle_input) 152 | 153 | # making sure angle remain in range of what wervo can handel 154 | if st < 900: 155 | st = 900 156 | if st > 2100: 157 | st = 2100 158 | 159 | # assigning the final values 160 | pwm.set_servo_pulsewidth( servo, st ) ; 161 | pwmA.ChangeDutyCycle(int(throttle_input)); 162 | 163 | 164 | 165 | 166 | if cv2.waitKey(1) & 0xFF == ord('q'): 167 | break 168 | 169 | pwmA.ChangeDutyCycle(int(0)); 170 | cam.release() 171 | cv2.destroyAllWindows() 172 | 173 | 174 | 175 | if __name__ == '__main__': 176 | main() 177 | 178 | 179 | 180 | -------------------------------------------------------------------------------- /lane_methods.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import math 4 | from lane_detection import average_slope_intercept 5 | 6 | # Masking 7 | def mask_img(img, show=False): # H S V 8 | lower_thr = np.array([0, 0, 0]) 9 | upper_thr = np.array([179, 255, 87]) 10 | img_masked = cv2.inRange(img, lower_thr, upper_thr) 11 | if show: 12 | cv2.imshow("Masked Frame", img_masked) 13 | return img_masked 14 | 15 | 16 | # Canny Edge Detection 17 | def detect_edges(img, show=False): 18 | # img_bgr = cv2.cvtColor(img, cv2.COLOR_HSV2BGR) 19 | img_gre = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 20 | blur = cv2.GaussianBlur(img_gre, (5, 5), 0) 21 | img_canny = cv2.Canny(blur, 200, 400) 22 | if show: 23 | cv2.imshow("Canny Filter", img_canny) 24 | return img_canny 25 | 26 | 27 | # Cropping , Region of Interest 28 | def crop_roi(img, show=False): 29 | height = img.shape[0] 30 | width = img.shape[1] 31 | # print(height, width) 32 | mask = np.zeros_like(img) 33 | cv2.rectangle(mask, (0, height // 2), (width, height), 255, -1) # -1 -> fill 34 | roi = cv2.bitwise_and(img, mask) 35 | if show: 36 | cv2.imshow("mask", mask) 37 | cv2.imshow("roi", roi) 38 | return roi 39 | 40 | 41 | # Hough Transform 42 | def detect_lines(img, show=False): 43 | # function that we use is HoughLinesP 44 | ''' 45 | Arguments for HoughLinesP 46 | 47 | rho : Distance Precision 48 | The hough Line Transform algorithm represents line in polar coordinates -> origin (rho) and angle (theta) 49 | rho specifies distance resolution in pixels. 50 | rho of 1 means two lines that are very close to each other but differ by a singe pixel will be considered different lines 51 | 52 | theta : Angular Precision 53 | It defines angular precision of hough transform. Means precision with which algo detects lines at different angle 54 | It is defined in radians 55 | If theta is np.Pi / 180 , that gives precision of 1 degree 56 | 57 | min_threshold : Minimum number of votes required for line to be considered 58 | 59 | lines : np.array([]) Empty array to store detected line segments 60 | 61 | min_line_length : Minimum length of a line to be considered 62 | 63 | max_line_gap : Max gap in segment to be considered as same line 64 | 65 | ''' 66 | 67 | rho = 1 68 | theta = np.pi / 180 69 | min_threshold = 10 70 | min_line_length = 20 71 | max_line_gap = 4 72 | 73 | lines = cv2.HoughLinesP(img, rho, theta, min_threshold, np.array([]), min_line_length, max_line_gap) 74 | 75 | return lines 76 | 77 | 78 | # Draw Lines 79 | def draw_lines(img, lines): 80 | line_color = (0, 255, 82) 81 | line_width = 2 82 | 83 | for line in lines: 84 | for x1, y1, x2, y2 in line: 85 | cv2.line(img, (x1, y1), (x2, y2), line_color, line_width) 86 | cv2.imshow("Lines", img) 87 | 88 | 89 | # Visually see grouping ares , [for debugging] 90 | def lane_search_area(img, boundary = 1/2): 91 | height = img.shape[0] 92 | width = img.shape[1] 93 | left_lane_area_width = int(width * (1 - boundary)) 94 | right_lane_area_width = int(width * boundary) 95 | # left_region = np.zeros_like(img) 96 | # right_region = np.zeros_like(img) 97 | 98 | cv2.rectangle(img, (0, 0), (left_lane_area_width, height), (0, 244, 233), 5) # -1 -> fill 99 | cv2.rectangle(img, (right_lane_area_width, 0), (width, height), (0, 0, 255), 5) # -1 -> fill 100 | 101 | cv2.imshow("left and right region", img) 102 | 103 | 104 | # Idea is to group positive slop and negative slop lines , which will define left and right lane markings 105 | def group_lines(img, lines): 106 | height = img.shape[0] 107 | width = img.shape[1] 108 | 109 | lane_lines = [] 110 | 111 | # No lines found 112 | if lines is None: 113 | return lane_lines 114 | 115 | left_lane = [] 116 | right_lane = [] 117 | 118 | boundary = 1 / 3 119 | left_lane_area_width = width * (1 - boundary) 120 | right_lane_area_width = width * boundary 121 | 122 | 123 | 124 | for line in lines: 125 | for x1, y1, x2, y2 in line: 126 | # skip vertical lines as they have infinite slope 127 | if x1 == x2: 128 | continue 129 | 130 | # np.polyfit can be used to get slop and intercept from two points on the line 131 | coff = np.polyfit((x1, x2), (y1, y2), 1) 132 | slope = coff[0] 133 | intercept = coff[1] 134 | 135 | # note that y axis is inverted in matrix of images. 136 | # so as x (width) increases, y(height) values decreases 137 | # this is reason why slope of right nane is positive and left name is negative 138 | 139 | # positive slop -> right lane marking \ 140 | # \ 141 | # \ 142 | # \ 143 | if slope > 0: 144 | # search area check 145 | if x1 > right_lane_area_width and x2 > right_lane_area_width: 146 | right_lane.append((slope, intercept)) 147 | 148 | 149 | # negative slop -> left lane marking / 150 | # / 151 | # / 152 | # / 153 | else: 154 | if x1 < left_lane_area_width and x2 < left_lane_area_width: 155 | left_lane.append((slope, intercept)) 156 | 157 | # averaging all the lines in each group to get a single line out of them 158 | left_avg = np.average(left_lane, axis=0) 159 | right_avg = np.average(right_lane, axis=0) 160 | 161 | # if got left lane, convert to point form from intercept form 162 | if len(left_lane) > 0: 163 | lane_lines.append(line_to_point(img, left_avg)) 164 | if len(right_lane) > 0: 165 | lane_lines.append((line_to_point(img, right_avg))) 166 | 167 | return lane_lines 168 | 169 | 170 | # Create points from the lane lines with slop and intercept 171 | def line_to_point(img, line): 172 | slop = line[0] 173 | intercept = line[1] 174 | height = img.shape[0] 175 | width = img.shape[1] 176 | 177 | # 178 | # 179 | # left right 180 | # x1,y1 x1,y1 181 | # 182 | # 183 | # 184 | # x2,y2 x2,y2 185 | 186 | # y = mx + c 187 | # x = (y - c) / m 188 | 189 | y1 = int(height / 2) # middle 190 | x1 = int((y1 - intercept) / slop) 191 | if x1 < 0: 192 | x1 = 0 193 | if x1 > width: 194 | x1 = width 195 | 196 | y2 = int(height) # bottom 197 | x2 = int((y2 - intercept) / slop) 198 | if x2 < 0: 199 | x2 = 0 200 | if x2 > width: 201 | x2 = width 202 | print(x1, y1, x2, y2) 203 | return [[x1, y1, x2, y2]] 204 | 205 | 206 | # steering angle 207 | 208 | # IMAGE 209 | # 210 | # 211 | # 212 | # 213 | # 214 | # 215 | # 216 | # 217 | # |-------- X_div ^ 218 | # | / | 219 | # | / | 220 | # | / | 221 | # | / | 222 | # | th / y/2 223 | # | / | 224 | # | / | 225 | # | / | 226 | # |/ _ 227 | 228 | 229 | # We can calculate th (theta) deviation angle by tan = (opposite side) / (adjacent side) = x_div / y/2 230 | def steering_angle(img, lane, show=False): 231 | 232 | height = img.shape[0] 233 | width = img.shape[1] 234 | 235 | # if there is only one lane , we will set deviation to slope of the lane 236 | if len(lane) == 1: 237 | x1, y1, x2, y2 = lane[0][0] 238 | slope = x2 - x1 239 | x_deviation = slope 240 | 241 | # if two lines, get average of far end points 242 | else: 243 | l1x1, l1y1, l1x2, l1y2 = lane[0][0] 244 | l2x1, l2y1, l2x2, l2y2 = lane[1][0] 245 | average_point = int(l1x2 + l2x2 / 2) 246 | x_deviation = int(average_point) - int(width / 2) 247 | 248 | if show: 249 | steering_img = cv2.circle(img, (average_point, (int(height/2))), radius=3, color=(0, 0, 255), thickness=-1) 250 | steering_img = cv2.line(steering_img, (int(width / 2) , 0), (int(width / 2), height), (0, 255, 0), 1) 251 | cv2.imshow("Deviation", steering_img) 252 | 253 | line_length = int(height / 2) 254 | 255 | angle_to_middle_vertical_rad = math.atan(x_deviation / line_length) 256 | angle_to_middle_vertical_deg = int(angle_to_middle_vertical_rad * 180.0 / math.pi) 257 | 258 | return angle_to_middle_vertical_deg 259 | 260 | 261 | 262 | 263 | 264 | # frame = cv2.imread('lane.jpg') 265 | # frame = cv2.resize(frame, (640, 480)) 266 | 267 | # # frame_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) 268 | # # frame_masked = mask_img(frame_hsv) 269 | 270 | 271 | # frame_edge = detect_edges(frame, show=True) 272 | # region_of_interest = crop_roi(frame_edge) 273 | # all_lines = detect_lines(region_of_interest) 274 | 275 | # draw_lines(frame, all_lines) 276 | # #lane_search_area(frame, boundary=1/3) 277 | 278 | # lane_markings = group_lines(frame, all_lines) 279 | # print("LANE : " , lane_markings) 280 | 281 | # steering = steering_angle(frame, lane_markings) 282 | # print(steering) 283 | 284 | # draw_lines(frame, lane_markings) 285 | 286 | 287 | 288 | 289 | # #print(all_lines) 290 | # # cv2.imshow("lane image", region_of_interest) 291 | # cv2.waitKey(0) 292 | 293 | 294 | --------------------------------------------------------------------------------