Select Git revision
-
Jake Dreher authored
0-setup-initial-server - Create directory structure, .gitignore, etc. app.js is the entry point to the server, all actual logic should be contained within src.
Jake Dreher authored0-setup-initial-server - Create directory structure, .gitignore, etc. app.js is the entry point to the server, all actual logic should be contained within src.
detect.py NaN GiB
#!/usr/bin/python
import numpy as np
import rospy
import matplotlib.pyplot as plt
import cv2
from imutils.video import WebcamVideoStream
from imutils.video import FPS
from std_msgs.msg import String
from std_msgs.msg import Float64
pub = rospy.Publisher('direction', String, queue_size=10)
pub1 = rospy.Publisher('lane_distance', Float64, queue_size=10)
rospy.init_node('lane_status')
rate = rospy.Rate(10) # 100hz
width = 325
# Converts picture to grayscale and applies filter to picture
# params
# pic : a numpy array of pixel values to represent a picture
def formatImg(pic):
# Convert picture to gray scale
gray = cv2.cvtColor(pic, cv2.COLOR_BGR2GRAY)
# Apply filter to image
img_filter = cv2.GaussianBlur(gray, (5, 5), 0)
return img_filter
# pre-processes and performs edge detection on an image
# params
# pic: a numpy array of pixel values for an image in gray scale
def detect_edge(pic):
# Perform canny edge detection
img_edge = cv2.Canny(pic, 50, 150)
# return new edge image
return img_edge
# Define the region of in which the lanes will be in the cameras view
# params
# pic: original image to apply the pre-set region of interest too
def ROI(pic):
height = pic.shape[0]
triangle = np.array([[(250, height), (1100, height), (550, 250)]])
mask = np.zeros_like(pic)
cv2.fillPoly(mask, triangle, 255)
roi = cv2.bitwise_and(pic, mask)
return roi
# Define the region of in which the lanes will be in the cameras view
# params
# pic: original image to apply the pre-set region of interest too
def ROI_real(pic):
height = pic.shape[0]
triangle = np.array([[(0, height), (145, 300), (475, 300), (600, height)]], dtype=np.int32)
mask = np.zeros_like(pic)
cv2.fillPoly(mask, triangle, 255)
roi = cv2.bitwise_and(pic, mask)
return roi
# Get all possible HoughLines and return them
# params
# edge_pic: the image with the edge detection performed
def getLines(edge_pic):
return cv2.HoughLinesP(edge_pic, 1, np.pi / 180, 50, maxLineGap=80, minLineLength=10)
# Apply the passed in lines the the original picture
# params
# original_pic:
# lines: Array of lines in the form (x1, x2, y1, y2)
def applyLines(original_pic, lines):
# If there is no lines return the original photo
if lines is None:
return original_pic
# Loop through all possible lines
for line in lines:
# parse the array to individual variables
x1, y1, x2, y2 = line.reshape(4)
#print(line)
# Draw the lines on the original picture
cv2.line(original_pic, (x1, y1), (x2, y2), (255, 0, 0), 3)
# return the original picture with the lines drawn on
return original_pic
def find_middle(leftPoints, rightPoints):
middle_lines = [[],[]]
if leftPoints[1] == []:
print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
return 0
elif rightPoints[1] == []:
print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
return 0
else:
for x in range(150):
midPoint = (rightPoints[0][149-x] + leftPoints[0][x]) / 2
middle_lines[1].append(leftPoints[1][x])
middle_lines[0].append(midPoint)
return middle_lines
# 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)
def find_poly_lane(pic, lines):
# Collections for the negative and positive sloped lines
left_lines_points = [[],[]] # Negative slope
right_lines_points = [[],[]] # Positive slope
#print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Start~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
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 < 1 and slope > -1):
#print("Line ignored");
x = 1
elif(slope < 0):
#print("left insert")
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_points[0].append(x1)
right_lines_points[0].append(x2)
right_lines_points[1].append(y1)
right_lines_points[1].append(y2)
#print("POINTS")
#print(right_lines_points[0])
#print(left_lines_points[0])
if right_lines_points[0]:
z = np.polyfit(right_lines_points[0], right_lines_points[1], 1)
f = np.poly1d(z)
#print(f)
right_x_new = np.linspace(np.amin(right_lines_points[0]), np.amax(right_lines_points[0]), 150)
right_y_new = f(right_x_new)
else:
right_x_new = []
right_y_new = []
if left_lines_points[0]:
z = np.polyfit(left_lines_points[0], left_lines_points[1], 1)
f = np.poly1d(z)
# print(f)
left_x_new = np.linspace(np.amin(left_lines_points[0]), np.amax(left_lines_points[0]), 150)
left_y_new = f(left_x_new)
else:
left_x_new = []
left_y_new = []
#print("New left")
##print(left_x_new)
#print(left_y_new)
return [right_x_new, right_y_new], [left_x_new, left_y_new]
def make_coordinates(image, line_parameters):
#print(line_parameters)
slope, intercept = line_parameters
y1 = image.shape[0]
y2 = int(y1*(1/2))
x1 = int((y1 - intercept)/slope)
x2 = int((y2 - intercept) / slope)
return np.array([x1, y1, x2, y2])
def detectDeparture(left, car, right):
a = 9999999
b = 999999
try:
parametersLeft = np.polyfit((left[0][0], left[0][-1]), (left[1][0], left[1][-1]), 1)
leftEq = np.poly1d(parametersLeft)
leftPosition = (310 - parametersLeft[1]) / parametersLeft[0]
a = car - leftPosition
except IndexError:
print("Left lane does not exist")
try:
parametersRight = np.polyfit((right[0][0], right[0][-1]), (right[1][0], right[1][-1]), 1)
rightEq = np.poly1d(parametersRight)
rightPosition = (310 - parametersRight[1]) / parametersRight[0]
b = rightPosition - car
except IndexError:
print("Right lane does not exist")
area = width / 3
if(area > a):
print("Drift Left")
elif(area > b):
print("Drift right")
else:
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))
def lane_status(lane_distance):
rospy.loginfo(lane_distance)
pub.publish(lane_distance)
rate.sleep()
#video = cv2.VideoCapture("test2.mp4")
#video = cv2.VideoCapture("highway.mp4")
video = WebcamVideoStream(src=1).start()
plt.ion()
while not rospy.is_shutdown():
frame = video.read()
# gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# frame = cv2.imread("../frame.jpeg")
frame_edge = detect_edge(frame)
new_img = formatImg(frame)
wEdges = detect_edge(new_img)
cropped = ROI_real(wEdges)
#cropped = ROI(wEdges)
lines = getLines(cropped)
if lines is None:
x = 0
else:
Rpoints, Lpoints = find_poly_lane(new_img, lines)
Mpoints = find_middle(Lpoints, Rpoints)
#(type(Rpoints[0][0]))
plt.cla()
plt.clf()
plt.scatter(Rpoints[0], Rpoints[1])
plt.scatter(Lpoints[0], Lpoints[1])
if(Mpoints != 0):
plt.scatter(Mpoints[0], Mpoints[1])
lane_distance = detectDepartureNew(Mpoints)
lane_status(lane_distance)
else:
print("No midpoint calculated")
#plt.scatter(310, 300)
plt.imshow(frame, zorder=0)
plt.pause(.001)
#lane = applyLines(frame, lines)
#cv2.imshow("edges", wEdges)
cv2.imshow("cropped", cropped)
#cv2.imshow("frame", lane)
key = cv2.waitKey(25)
if key == 27:
break
video.release()
cv2.destroyAllWindows()