74 lines
2.4 KiB
Python
74 lines
2.4 KiB
Python
|
###############################
|
||
|
|
#### Stop Car at Stop Sign ####
|
||
|
|
#### --------------------- ####
|
||
|
|
#### Objective: ####
|
||
|
|
#### > Drive car forward ####
|
||
|
|
#### until a sotpsign is ####
|
||
|
|
#### spotted on the cam ####
|
||
|
|
###############################
|
||
|
|
|
||
|
|
from picamera.array import PiRGBArray
|
||
|
|
from picamera import PiCamera
|
||
|
|
from time import sleep
|
||
|
|
import cv2
|
||
|
|
|
||
|
|
from kuukar_motion import drive, stop
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
# Inializations: #
|
||
|
|
#Camera fps/size
|
||
|
|
camera=PiCamera()
|
||
|
|
camera.resolution=(640,480)
|
||
|
|
camera.framerate=20
|
||
|
|
rawCapture=PiRGBArray(camera,size=(640,480))
|
||
|
|
sleep(0.1)
|
||
|
|
stopsign_cascade=cv2.CascadeClassifier('/home/pi/Desktop/oldRPi/RPi/stopsign_good.xml')
|
||
|
|
|
||
|
|
""" Download Stopsign_good.xml to the RPi : https://github.com/chadrs2/Self-Driving_Car_Project/blob/master/stopsign_good.xml"""
|
||
|
|
|
||
|
|
|
||
|
|
# Begin Camera video and driving forward #
|
||
|
|
camera.start_preview()
|
||
|
|
camera.start_recording('stopsign_video.h264')
|
||
|
|
for frame in camera.capture_continuous(rawCapture,format="bgr",use_video_port=True):
|
||
|
|
# Begin driving at medium speed
|
||
|
|
drive(30) ###
|
||
|
|
|
||
|
|
# grab raw NumPy array representing image - 3D array
|
||
|
|
image=frame.array
|
||
|
|
#convert image to grayscale
|
||
|
|
gray_img=cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
|
||
|
|
# Wait and read key input
|
||
|
|
key=cv2.waitKey(1) & 0xFF
|
||
|
|
# Find stopsign in image
|
||
|
|
print("Before stop sign finding")
|
||
|
|
found_stopsigns=stopsign_cascade.detectMultiScale(gray_img,1.1,5)
|
||
|
|
print("Found "+str(len(found_stopsigns))+" stop sign(s)")
|
||
|
|
if len(found_stopsigns)>0:
|
||
|
|
for (x,y,w,h) in found_stopsigns:
|
||
|
|
cv2.rectangle(image,(x,y),(x+w,y+h),(255,255,0),2)
|
||
|
|
camera.add_overlay(image)
|
||
|
|
cv2.imwrite("found_stopsign_Jun_25_20.jpg",image)
|
||
|
|
sign_width=w
|
||
|
|
sign_height=h
|
||
|
|
print("width of stop sign:",w,"and height:",h)
|
||
|
|
if(sign_width>65 or sign_height>65):
|
||
|
|
print("Turn on brake lights")
|
||
|
|
print("Decrease motor speed")
|
||
|
|
print("stop car")
|
||
|
|
time.sleep(2) #delay for 2 seconds after found the stop sign
|
||
|
|
stop()
|
||
|
|
break
|
||
|
|
#clear the stream in preparation for the next frame
|
||
|
|
rawCapture.truncate(0)
|
||
|
|
# if the 'q' key was pressed or a stop sign was found
|
||
|
|
# break from the loop
|
||
|
|
if key == ord("q"): #or len(found_stopsigns)>0:
|
||
|
|
break
|
||
|
|
# End camera functions
|
||
|
|
camera.stop_recording()
|
||
|
|
camera.stop_preview()
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
camera.close()
|
||
|
|
# End car functions
|