def cap_read_write(w,h,fps,long):
image = frame.array
start = time.time()
image,offset,Radius = auto_lane(image)
print('Time:',time.time()-start)
print('offset: ',offset,'R :',Radius)
rawCapture.truncate(0)
#return image,offset,Radius
return image,offset,Radius
def cap_read_write(w,h,fps):
stream = io.BytesIO()
camera = picamera.PiCamera()
camera.resolution = (w,h)
camera.framerate = 30
rawCapture = PiRGBArray(camera,size=(w,h))
size = (w,h)
for frame in camera.capture_continuous(rawCapture,format='bgr',use_video_port = True):
image = frame.array
start = time.time()
image,offset,Radius = auto_lane(image)
print('Time:',time.time()-start)
print('offset: ',offset,'R :',Radius)
rawCapture.truncate(0)
def main_function():
date_recv = '0'
last_data_recv = '0'
stream = io.BytesIO()
camera = picamera.PiCamera()
w, h = 640, 360
camera.resolution = (w, h)
camera.framerate = 30
rawCapture = PiRGBArray(camera, size=(w, h))
size = (w, h)
#first_recv = sock.recv(1024)
# print('first_recv:',first_recv)
#error = 0 # camera piancha
###excutive module
curve_durationtime = 2.8
right_angle_durationtime = 2.0
hire_durationtime = 10.9
triple_angle_durationtime = 9.2
stop_durationtime = 1.5
changelane_durationtime = 4.6
hire_trigetimes = 0
triple_trigetimes = 0
park_trigetimes = 0
steer_middle_position = 90
zebracross_times = 0
'''
steer = 90 # duo ji
speed = 25 # motor speed
direc = 1 # motor direction
buzzer = 0
a = 1
'''
last_error = 0
last_steer = steer_middle_position
last_flag = 1
flag = 1
right_angle_starttime = datetime.datetime.now()
for frame in camera.capture_continuous(rawCapture,
format='bgr',
use_video_port=True):
st = time.time()
image = frame.array
start = time.time()
result, error, Radius, lanetype, SpeedLimit, RedLight, GreenLight, HireSign, ParkSign, zebracrossing = auto_lane(
image)
print "VisionTotleTime:", time.time() - start
print 'Lane_type:', lanetype
# if error=="None"and abs(error)>60:
# sss=60
#### decision module
#r_0, target_distance1, target_distance2, inf_01, inf_02, inf_03, inf_04, yaw, r_7 = ardiuno_to_pi()
#data_from_ardiuno = ardiuno_to_pi() #get data from ardiuno
#print 'jghfdjkfhdkjfjd00000000',target_distance1
#b = str(b[0])+str(b[1])+str(b[2])+str(b[3])+str(b[4])+str(b[5])+str(b[6])+str(b[7])+str(b[8])
#q_2.put(b)
# connection.send(data_from_ardiuno) #send data to client
target_detect = 0
target_distance1 = 100
#print 'dfhjghhhffhggfggdgd78889',target_distance1
if target_distance1 < 30:
target_detect = 1
if (last_flag == 1 or last_flag == 0
or last_flag == 2) and lanetype == 1:
#and (HireSign + ParkSign + RedLight + ZebraCrossing) == 0
flag = 1
zebracross_times = 0
if (last_flag == 1 or last_flag == 0
or last_flag == 2) and lanetype == 2:
flag = 2
#if last_flag != 2:
#flag = 2
#curve_starttime = datetime.datetime.now()
if (last_flag == 1 or last_flag == 0
or last_flag == 2) and lanetype == 3:
if last_flag != 3:
flag = 3
right_angle_starttime = datetime.datetime.now()
if HireSign == 1: #HireSign hire_trigetimes ==0
if last_flag != 4:
flag = 4
hire_starttime = datetime.datetime.now()
hire_trigetimes = 1
if ParkSign == 1:
if last_flag != 5:
flag = 5
park_starttime = datetime.datetime.now()
park_trigetimes = 1
#if TriangleSign == 1:
#if last_flag != 6:
#flag = 6
#triple_angle_starttime = datetime.datetime.now()
if zebracrossing == 1 and zebracross_times == 0:
if last_flag != 7:
flag = 7
zebracross_times = 1
stop_starttime = datetime.datetime.now()
## if RedLight == 1:#
## if last_flag != 8:
## flag = 8
## changelane_starttime = datetime.datetime.now()
if RedLight == 1 or target_detect == 1:
flag = 0
currenttime = datetime.datetime.now()
print 'final_flag7878778878flagflag', flag
#if flag == 2:
# timediff = currenttime - curve_starttime
# dt = timediff.total_seconds()
# if dt <= curve_durationtime:
# flag = 2
# elif dt > curve_durationtime:
# flag = 1
if flag == 3:
timediff = currenttime - right_angle_starttime
dt = timediff.total_seconds()
print "lalalalalalalalalalal", dt
if dt <= right_angle_durationtime:
flag = 3
elif dt > right_angle_durationtime:
flag = 1
if flag == 4:
timediff = currenttime - hire_starttime
dt = timediff.total_seconds()
if dt <= hire_durationtime:
flag = 4
elif dt > hire_durationtime:
flag = 1
# if flag == 5:
# timediff = currenttime - right_angle_starttime
# dt = timediff.total_seconds()
# if dt <= park_durationtime:
# flag = 3
# elif dt > park_durationtime:
# flag = 1
if flag == 6:
timediff = currenttime - triple_angle_starttime
dt = timediff.total_seconds()
if dt <= triple_angle_durationtime:
flag = 6
elif dt > triple_angle_durationtime:
flag = 1
if flag == 7:
timediff = currenttime - stop_starttime
dt = timediff.total_seconds()
if dt <= stop_durationtime:
flag = 7
elif dt > stop_durationtime:
flag = 1
## if flag == 8:
## timediff = currenttime - changelane_starttime
## dt = timediff.total_seconds()
## if dt <= changelane_durationtime:
## flag = 8
## elif dt > changelane_durationtime:
## flag = 1
last_flag = flag
cv2.putText(result,
str('final_flage:') + str(flag), (80, 80),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 5), 1)
cv2.imshow('flage', result)
cv2.waitKey(1)
print('offset: ', error, 'R :', Radius)
rawCapture.truncate(0)
##
## if q.empty():
## data_recv=last_data_recv
##
## else:
## data_recv = q.get()
#if Lane_type==1:
# data_recv=1
#elif Lane_type==2:
# data_recv=2
print 'cur -------time:', datetime.datetime.now()
#print data_recv,'1111111111111111145511111'
## if flag==3:
## flag=1
if flag == 0:
steer, speed, direc, buzzer = stop_function_1(90)
elif flag == 1:
steer, speed, direc, buzzer, last_error = lane_keeping(
error, last_error)
elif flag == 2:
steer, speed, direc, buzzer, last_steer = lane_wandao(
error, last_steer)
last_steer = steer
elif flag == 3:
steer, speed, direc, buzzer = right_angle_function(
right_angle_starttime, right_angle_durationtime)
elif flag == 4:
steer, speed, direc, buzzer = hire_function(
hire_starttime, hire_durationtime)
# elif flag == 5:
# steer, speed, dir, buzzer = park_function(park_starttime , park_durationtime)
elif flag == 6:
steer, speed, direc, buzzer = triple_angle_function(
hire_starttime, hire_durationtime)
elif flag == 7:
steer, speed, direc, buzzer = stop_function_2(
last_steer, stop_starttime, stop_durationtime)
elif flag == 8:
steer, speed, direc, buzzer = changelane_function(
changelane_starttime, changelane_durationtime)
last_data_recv = data_recv
last_steer = steer
data_ardiuno = drive_program(steer, speed, direc, buzzer)
print 'MainFunctionTime:', time.time() - st
threshold = 10 # yuzhi
depth = 280 #biaoding depth
steer = 90 # duo ji
speed = 25 # motor speed
direc = 1 # motor direction
buzzer = 0
a=1
last_error=0
for frame in camera.capture_continuous(rawCapture,format='bgr',use_video_port = True):
image = frame.array
start = time.time()
image = frame.array
start = time.time()
image,error,Radius = auto_lane(image)
#if error=="None"and abs(error)>60:
#sss=60
print('Time:',time.time()-start)
print('offset: ',error,'R :',Radius)
rawCapture.truncate(0)
while 1:
a += 1
print ('!!!!!!!!!',a)
#image alogthrim start(output +-,error)
#image alogthrim start(output +-,error)
if error=="None":#left lane lose
theta=90-15