diff --git a/Detect/detect.py b/Detect/detect.py index dfcf070c33319ee8e4c11bcac6bdc03e4928fadc..506493a93dbabd04126e57694883386cc75d4b6f 100644 --- a/Detect/detect.py +++ b/Detect/detect.py @@ -41,7 +41,7 @@ def ROI(pic): roi = cv2.bitwise_and(pic, mask) return roi -# Define the region of in which the lanes will be in the cameras view +# Define the region of in which the lanes will be in the cameras view for the robot # params # pic: original image to apply the pre-set region of interest too def ROI_real(pic): @@ -86,14 +86,15 @@ def find_middle(leftPoints, rightPoints): middle_lines = [[],[]] - if(leftPoints[1] is None or rightPoints[1] is None - ): - print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~") + if rightPoints[1] is None: + print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty right list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~") + return 0 + elif leftPoints[1] is None : + print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty left list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~") return 0 - else: - print(leftPoints[1]) - print(rightPoints[1]) + #print(leftPoints[1]) + #print(rightPoints[1]) for x in range(150): #print("Right x point: " + str(rightPoints[0][x])) @@ -181,70 +182,6 @@ def find_poly_lane(pic, lines): return [right_x_new, right_y_new], [left_x_new, left_y_new] -# Find the two average lines given the set of lines -# params -# pic: the original image -# lines: An array of lines in the form (x1, x2, y1, y2) -# Deprecated??? -def find_average_lane(pic, lines): - - # Collections for the negative and positive sloped lines - left_lines = [] # Negative slope - left_lines_points = [[],[]] # Negative slope - right_lines = [] # Positive slope - right_lines_points = [[],[]] # Positive slope - - for line in lines: - x1, y1, x2, y2 = line[0] - parameters = np.polyfit((x1, x2), (y1, y2), 1) - #print("Slope, intercept") - #print(parameters) - slope = parameters[0] - intercept = parameters[1] - - if(slope < 0): - #print("Left insert") - left_lines.append((slope, intercept)) - left_lines_points[0].append(x1) - left_lines_points[0].append(x2) - left_lines_points[1].append(y1) - left_lines_points[1].append(y2) - - else: - #print("Right insert") - right_lines.append((slope, intercept)) - right_lines_points[0].append(x1) - right_lines_points[0].append(x2) - right_lines_points[1].append(y1) - right_lines_points[1].append(y2) - - if not left_lines: - #print("Left is empty") - left_line = [0, 0, 0, 0] - - else: - left_average = np.average(left_lines, axis=0) - left_line = make_coordinates(pic, left_average) - #print("Left Line: ") - #print(left_line) - - if not right_lines: - #print("Right is emtpy") - right_line = [0, 0, 0, 0] - - else: - right_average = np.average(right_lines, axis=0) - right_line = make_coordinates(pic, right_average) - #print("Right line : ") - #print(right_line) - - #print("Left fit") - #print(left_line) - - #print("\nRight fit") - #print(right_line) - return np.array([left_line, right_line]) - def make_coordinates(image, line_parameters): #print(line_parameters) slope, intercept = line_parameters @@ -283,14 +220,26 @@ def detectDeparture(left, car, right): print("On course") print("On course") +def detectDepartureNew(midPoints): + midx = 300 + midy = 400 + for x in range(150): + if(myround(midPoints[1][x]) == 400): + print(midPoints[0][x] - midx) + return midPoints[0][x] - midx + + +def myround(x): + return int(5 * round(float(x)/5)) + -video = cv2.VideoCapture("test2.mp4") +video = cv2.VideoCapture("midTest.avi") #video = cv2.VideoCapture("highway.mp4") -#video = cv2.VideoCapture(1) +#video = cv2.VideoCapture(0) plt.ion() while True: @@ -307,8 +256,8 @@ while True: wEdges = detect_edge(new_img) - #cropped = ROI_real(wEdges) - cropped = ROI(wEdges) + cropped = ROI_real(wEdges) + #cropped = ROI(wEdges) lines = getLines(cropped) @@ -329,11 +278,12 @@ while True: plt.scatter(Lpoints[0], Lpoints[1]) plt.scatter(Mpoints[0], Mpoints[1]) - #plt.scatter(310, 300) + plt.scatter(300, 400) plt.imshow(frame, zorder=0) - detectDeparture(Lpoints, 310, Rpoints) + #detectDeparture(Lpoints, 310, Rpoints) + detectDepartureNew(Mpoints) plt.pause(.001)