from LaneDetection import pipeline, gaussian_blur, combined_color_gradient, perspective_transform, detect_cross, detect_snow, hls_select
from CarAndSignDetection import get_car_boundary, findIntersection
from CarControll import CarControll
import time
cap = cv2.VideoCapture('output.avi')
car = CarControll()
while (cap.isOpened()):
start_time = time.time()
ret, image = cap.read()
S_binary_img = hls_select(image)
image = gaussian_blur(image, 3)
combine = combined_color_gradient(image)
Is_cross = detect_cross(combine)
Is_snow = detect_snow(S_binary_img)
warp_image, Minv, M = perspective_transform(
combine) if not Is_cross and not Is_snow else perspective_transform(
combine, np.float32([[0, 200], [80, 40], [240, 40], [320, 240]]),
np.float32([[0, 240], [80, 0], [240, 0], [320, 240]]))
leftx, lefty, rightx, righty, img_left_fit, img_right_fit, lre = pipeline(
warp_image, 0, image, Minv, Is_cross, Is_snow)
boundary_cars, confirmlr = [], 'no'
if len(leftx) > 10 and len(rightx) > 10:
boundary_cars, confirmlr = get_car_boundary(image, leftx, lefty,
rightx, righty)
if Is_cross or Is_snow:
boundary_cars, confirmlr = [], 'no'
cte, speed, left_x_point, left_y_point, right_x_point, right_y_point, mid_x, mid_y = car.driveCar(
leftx, lefty, rightx, righty, img_left_fit, img_right_fit,
boundary_cars, lre, confirmlr)
if Is_snow:
# if len(sign_derections) > 0:
# print '--------------------'
#
# cv2.imshow("image", image)
# cv2.waitKey(0)
path = 'output.avi' #'/home/tl/catkin_ws/outputm3.avi'
cap = cv2.VideoCapture(path)
#i=30
while (cap.isOpened()):
ret, image = cap.read()
if ret == True:
image_copy = image.copy()
start_time = time.time()
image = cv2.resize(image, (320, 240), interpolation=cv2.INTER_AREA)
combine = combined_color_gradient(image)
warp_image, Minv, _ = perspective_transform(combine)
leftx, lefty, rightx, righty, img_left_fit, img_right_fit, lre = pipeline(
warp_image, 0, image, Minv)
if len(leftx) > 0 and len(rightx) > 0:
bouding_cars, _, sign_derections, signs = get_car_boundary_and_sign_direction(
image, leftx, lefty, rightx, righty)
for (x, y, w, h) in bouding_cars:
cv2.rectangle(image, (x, y), (x + w, y + h), (0, 0, 255),
2)
cv2.circle(image, (x + w / 2, y + h / 2), 3, (255, 255, 0))
for direction in sign_derections:
print 'Box found at ' + str(direction)
if len(sign_derections) > 0:
print '--------------------'
else:
def rgb_image_callback(img_msg):
try:
start_time = time.time()
np_arr = np.fromstring(img_msg.data, np.uint8)
image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
S_binary_img = hls_select(image)
image = gaussian_blur(image, 3)
combine = combined_color_gradient(image)
Is_cross = detect_cross(combine)
Is_snow = detect_snow(S_binary_img)
warp_image, Minv, M = perspective_transform(
combine
) if not Is_cross and not Is_snow else perspective_transform(
combine, np.float32([[0, 200], [80, 40], [240, 40], [320, 240]]),
np.float32([[0, 240], [80, 0], [240, 0], [320, 240]]))
leftx, lefty, rightx, righty, img_left_fit, img_right_fit, lre = pipeline(
warp_image, 0, image, Minv, Is_cross, Is_snow)
boundary_cars, confirmlr = [], 'no'
sign_direction = []
if len(leftx) > 10 and len(rightx) > 10:
boundary_cars, confirmlr, sign_direction = get_car_boundary_and_sign_direction(
image, leftx, lefty, rightx, righty)
if Is_cross or Is_snow:
boundary_cars, confirmlr = [], 'no'
cte, speed, left_x_point, left_y_point, right_x_point, right_y_point, mid_x, mid_y = car.driveCar(
leftx, lefty, rightx, righty, img_left_fit, img_right_fit,
boundary_cars, lre, confirmlr, Is_cross, sign_direction)
if Is_snow:
speed = np.float32(10)
if len(boundary_cars) > 0:
x, y, w, h = boundary_cars[0]
cv2.rectangle(image, (x, y), (x + w, y + h), (0, 0, 255), 2)
if len(leftx) > 0:
for x, y in zip(leftx, lefty):
cv2.circle(image, (x, y), 3, (0, 255, 0))
else:
for y in range(100, 200, 1):
x = np.float32(car.left_fit[0] * y**2 + car.left_fit[1] * y +
car.left_fit[2])
cv2.circle(image, (x, y), 3, (0, 255, 0))
if len(rightx) > 0:
for x, y in zip(rightx, righty):
cv2.circle(image, (x, y), 3, (0, 255, 0))
else:
for y in range(100, 200, 1):
x = np.float32(car.right_fit[0] * y**2 + car.right_fit[1] * y +
car.right_fit[2])
cv2.circle(image, (x, y), 3, (0, 255, 0))
cv2.circle(image, (left_x_point, left_y_point), 10, (255, 0, 0))
cv2.circle(image, (right_x_point, right_y_point), 10, (255, 0, 0))
cv2.circle(image, (mid_x, mid_y), 10, (255, 0, 0))
lx1, ly1, lx2, ly2, rx1, ry1, rx2, ry2 = leftx[0], lefty[0], leftx[int(
2 * len(leftx) / 3)], lefty[int(
2 * len(lefty) / 3)], rightx[0], righty[0], rightx[int(
2 * len(rightx) / 3)], righty[int(2 * len(righty) / 3)]
intersec_x, intersec_y = findIntersection(lx1, ly1, lx2, ly2, rx1, ry1,
rx2, ry2)
pt1 = tuple(np.asarray((leftx[0], lefty[0]), dtype=np.int))
pt2 = tuple(np.asarray((rightx[0], righty[0]), dtype=np.int))
pt3 = tuple(np.asarray((intersec_x, intersec_y), dtype=np.int))
cv2.line(image, pt1, pt3, (0, 255, 0), 2)
cv2.line(image, pt2, pt3, (0, 255, 0), 2)
cv2.imshow('Frame', image)
if cv2.waitKey(1) & 0xFF == ord('q'):
return
out.write(image)
ang_pub.publish(cte)
spe_pub.publish(np.float32(speed))
#print("frame: " +str (1/(time.time() - start_time)))
except CvBridgeError, e:
print 'error'