def main():
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
#undistort
undistorted_image = undistort(image)
#--------------motor control--------------
#AR marker
markers = detect_markers(undistorted_image)[0]
print(detect_markers(undistorted_image)[1])
for marker in markers:
marker.highlite_marker(undistorted_image)
# show the frame
cv2.imshow("Frame", undistorted_image)
key = cv2.waitKey(1) & 0xFF
rawCapture.truncate(0)
# q : break, tap : capture
if key == ord("q"):
break
elif key == ord("\t"):
captured(undistorted_image)
def get_markers():
#captures two frames then returns the ids of markers in both frames
cap = cv2.VideoCapture("https://*****:*****@10.50.250.152/api/holographic/stream/live_high.mp4?holo=false&pv=true&mic=false&loopback=false")
ret, frame = cap.read()
markers, ids = detect_markers(frame)
ids_op = []
for i in ids:
ids_op.append(i)
ord = []
for a in markers:
ord.append(get_coordinates(a, meters=True))
return ids_op, ord
def main():
#for capture every second
checktimeBefore = int(time.strftime('%S'))
FPS_list = []
stop_sign = 0
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
#undistort
undistorted_image = undistort(image)
#brightness
M = np.ones(undistorted_image.shape, dtype="uint8") * 25
undistorted_image = cv2.add(undistorted_image, M)
#--------------motor control--------------
#decision (action, round(m,4), forward, left_line, right_line, center, direction)
masked_image = select_white(undistorted_image, 170)
result = set_path3(masked_image)
#line marker
line_left = []
line_right = []
for j in range(result[2]):
#left
left_coord = (result[5] + 1 - result[4][j], 239 - j)
line_left.append(left_coord)
undistorted_image = cv2.line(undistorted_image, left_coord,
left_coord, (0, 255, 0), 4)
#right
right_coord = (+result[5] + 1 + result[3][j], 239 - j)
line_right.append(right_coord)
undistorted_image = cv2.line(undistorted_image, right_coord,
right_coord, (0, 255, 0), 4)
#slope
try:
undistorted_image = cv2.line(undistorted_image, result[6][0],
result[6][1], (0, 0, 255), 4)
except:
pass
#straight
if result[0] == 'w' and line_left != [] and line_right != []:
straight_factor = 30
if line_left[0][0] > straight_factor:
result_direction = '2'
elif line_right[0][0] < 320 - straight_factor:
result_direction = '1'
else:
result_direction = result[0]
else:
result_direction = result[0]
#motor ON!
direction = motor(result_direction, result[1])
#1st U-turn
if result[2] < 30 and abs(result[1]) < 0.2:
motor('a', result[1])
time.sleep(0.5)
#----------------------------
#ultrasonic
ultra = ultrasonic()
if ultra > 0 and ultra < 12:
#print('stop')
direction = motor('s', 0)
print(ultra)
time.sleep(1)
#cascade
cas = cascade(undistorted_image)
cas_detect = len(cas)
#if detected
if cas_detect != 0:
cas_distance = cas[0][2]
if cas_distance > 20:
if stop_sign == 3:
direction = motor('s', result[1])
print('stop sign')
time.sleep(5)
else:
print('Non stop', stop_sign)
stop_sign = stop_sign + 1
#AR marker
distance = detect_markers(undistorted_image)[1]
markers = detect_markers(undistorted_image)[0]
for marker in markers:
#highlight
marker.highlite_marker(undistorted_image)
print("distance :", distance)
if distance > 10:
#finish, stop
if marker.id == 2537:
direction = motor('w', 0)
time.sleep(1)
direction = motor('s', 0)
print('stop', marker.id)
time.sleep(5)
if distance > 30:
#left
if marker.id == 114:
direction = motor('a', result[1])
print('left', marker.id)
time.sleep(0.5)
#right
elif marker.id == 922:
direction = motor('d', result[1])
print('right', marker.id)
time.sleep(0.5)
#----------------------------
#putText
try:
#slope
cv2.putText(undistorted_image, 'm = ' + str(result[1]), (10, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1)
#direction
cv2.putText(undistorted_image, direction, (10, 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1)
except:
pass
#----------------------------
# show the frame
cv2.imshow("Frame", undistorted_image)
key = cv2.waitKey(1) & 0xFF
rawCapture.truncate(0)
FPS_list.append(1)
checktime = int(time.strftime('%S'))
if checktime - checktimeBefore == 1 or checktime - checktimeBefore == -59:
print("FPS :", len(FPS_list))
FPS_list = []
checktimeBefore = checktime
# q : break, tap : capture
if key == ord("q"):
break
elif key == ord("\t"):
captured(undistorted_image)
def main():
#for capture every second
checktimeBefore = int(time.strftime('%S'))
FPS_list = []
ultra_switch = 1
ultra_detect = 0
ultra_cnt = 0
cas_switch = 0
cas_cnt = 0
left_once = 0
right_once = 0
left_2nd = 0
U_turn_switch = 1
mode = "start"
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
FPS_list.append(1)
checktime = int(time.strftime('%S'))
#print(checktime, checktimeBefore)
if checktime - checktimeBefore == 1 or checktime - checktimeBefore == -59:
print("FPS :", len(FPS_list))
FPS_list = []
checktimeBefore = checktime
image = frame.array
#undistort
undistorted_image = undistort(image)
#brightness
M = np.ones(undistorted_image.shape, dtype="uint8") * 50
undistorted_image = cv2.add(undistorted_image, M)
#--------------motor control--------------
#decision (action, round(m,4), forward, left_line, right_line, center, direction)
masked_image = select_white(undistorted_image, 150)
result = set_path3(masked_image)
#motor ON!
motor(result[0], result[1])
#1st U-turn
if result[2] < 30 and abs(result[1]) < 0.2 and ultra_cnt <= 2:
motor('a', result[1])
time.sleep(0.5)
checktimeBefore = checktime
print("other")
if U_turn_switch == 1:
left_once = 0
left_2nd = 1
MOTOR_SPEEDS['q'] = left_motor_marker
MOTOR_SPEEDS['e'] = right_motor_marker
mode = "U-turn"
#right Re-try
#print(result[2])
if result[2] < 60 and abs(
result[1]) < 0.15 and ultra_cnt == 3 and right_once == 0:
motor('x', result[1])
time.sleep(2)
checktimeBefore = checktime
#----------------------------
#ultrasonic
if ultra_switch == 1:
ultra = ultrasonic()
if ultra > 0 and ultra < 12:
if ultra_cnt == 0 or ultra_cnt == 2:
motor('s', 0)
print(ultra)
time.sleep(1)
checktimeBefore = checktime
elif ultra_cnt == 1:
motor('a', 0)
print(ultra)
time.sleep(0.5)
checktimeBefore = checktime
MOTOR_SPEEDS['q'] = left_motor_marker
MOTOR_SPEEDS['e'] = right_motor_marker
U_turn_switch = 1
ultra_detect = 1
if ultra_detect == 1:
if ultra >= 12 or ultra == -1:
ultra_cnt += 1
if ultra_cnt == 1:
ultra_switch = 1
U_turn_switch = 0
mode = "first ultra"
elif ultra_cnt == 2:
ultra_switch = 0
mode = "first left (ultra)"
elif ultra_cnt == 3:
MOTOR_SPEEDS['q'] = left_motor_marker
MOTOR_SPEEDS['e'] = right_motor_marker
ultra_switch = 0
mode = "third ultra"
ultra_detect = 0
print('ultra stop')
#cascade
if cas_switch == 1:
cas = cascade(undistorted_image)
cas_detect = len(cas)
#if detected
if cas_detect != 0:
cas_distance = cas[0][2]
if cas_distance > 20:
if cas_cnt == 2:
motor('s', result[1])
print('stop sign')
mode = "stop sign"
ultra_switch = 1
ultra_cnt = 2
cas_switch = 0
time.sleep(5)
checktimeBefore = checktime
else:
print('Non stop', cas_cnt)
cas_cnt += 1
#AR marker
distance = detect_markers(undistorted_image)[1]
markers = detect_markers(undistorted_image)[0]
for marker in markers:
#highlight
marker.highlite_marker(undistorted_image)
#print("distance :", distance)
#finish, stop
if marker.id == 2537:
if distance > 10:
motor('w', 0)
time.sleep(1)
checktimeBefore = checktime
motor('s', 0)
print('stop', marker.id)
time.sleep(5)
checktimeBefore = checktime
right_once = 0
mode = "finish"
#left
if marker.id == 114 and left_once == 0:
if distance > 35:
motor('w', result[1])
time.sleep(0.6)
checktimeBefore = checktime
motor('a', result[1])
print('left', marker.id)
time.sleep(0.5)
checktimeBefore = checktime
left_once = 1
ultra_switch = 0
if left_2nd == 1:
cas_switch = 1
mode = "second left"
else:
MOTOR_SPEEDS['q'] = left_motor_marker
MOTOR_SPEEDS['e'] = right_motor_marker
U_turn_switch = 1
mode = "first left"
#right
print(distance)
if marker.id == 922 and right_once == 0 and ultra_cnt == 3:
if distance < 40:
pass
else:
if distance >= 40 and distance < 55:
motor('w', result[1])
time.sleep(0.6)
checktimeBefore = checktime
elif distance >= 55 and distance < 80:
motor('w', result[1])
time.sleep(0.2)
checktimeBefore = checktime
elif distance >= 80:
motor('x', result[1])
time.sleep(0.5)
mode = "right"
motor('d', result[1])
print('right', marker.id)
time.sleep(0.5)
checktimeBefore = checktime
right_once = 1
print("distance :", distance)
MOTOR_SPEEDS['q'] = left_motor
MOTOR_SPEEDS['e'] = right_motor
#----------------------------
# show the frame
cv2.imshow("Frame", undistorted_image)
cv2.imshow("masked", masked_image)
key = cv2.waitKey(1) & 0xFF
rawCapture.truncate(0)
# q : break, tap : capture
if key == ord("q"):
break
elif key == ord("\t"):
captured(undistorted_image)
print("left_2nd", left_2nd, "cas_switch", cas_switch, "ultra_switch",
ultra_switch, "ultra_cnt", ultra_cnt, "right_once", right_once,
"U_turn_switch", U_turn_switch, MOTOR_SPEEDS['q'],
MOTOR_SPEEDS['e'], "mode", mode)
cv2.imwrite(
"../SavedResults/drawCylinder/cylinder" + str(imgnum) + ".jpg",
img_cylinder)
"""
MAIN CODE
Purpose: Iterates through all the test cases and calls the function written by
participants to draw images, cubes, cylinders on all the test case images.
Saves
"""
if __name__ == "__main__":
cam, dist = d.getCameraMatrix()
for imgnum in range(1, 9):
image = cv2.imread("../TestCases/image_" + str(imgnum) + ".jpg")
ar = d.detect_markers(image, cam, dist)
print(ar)
print(" ")
ret = check_ar_list_type(ar)
if ret == True:
draw_and_save(image, ar, cam, dist)
np_arr = np.array(ar, dtype=object)
results.append(np_arr)
np.savez("../SavedResults/Results.npz",
image1=results[0],
image2=results[1],
image3=results[2],
image4=results[3],
image5=results[4],
image6=results[5],
image7=results[6],
camera.resolution = (1280,1024)
camera.framerate = 60
camera.exposure_mode = 'sports'
rawCapture = PiRGBArray(camera, size=(1280,1024))
#camera warm up
time.sleep(2.5)
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
# Detect Markers
markers = detect_markers(image)
os.system('clear')
# Draw contour and log positions
for m in markers:
print ('Found marker {} at {}'.format(m.id, m.center))
m.draw_contour(image)
# show the frame
cv2.imshow("Markers", image)
key = cv2.waitKey(1) & 0xFF
# clear the stream in preparation for the next frame
rawCapture.truncate(0)
capture = cv2.VideoCapture(1)
# Connect a client socket to my_server:8000 (change my_server to the
# hostname of your server)
client_socket = socket.socket()
client_socket.connect(('169.254.130.102', 50001))
>>>>>>> 285369fc13fcd97aad977fe5e14a6f2a116106b9
# Make a file-like object out of the connection
connection = client_socket.makefile('wb')
if capture.isOpened(): # try to get the first frame
frame_captured, frame = capture.read()
else:
frame_captured = False
while frame_captured:
markers = detect_markers(frame)
diagonal_vectors = []
angles = []
positions = []
ids = []
for m in markers:
diff_vec = m.contours[3] - m.contours[1]
norm_diff = diff_vec/np.linalg.norm(diff_vec)
diagonal_vectors.append(norm_diff)
abs_angle = math.atan2(norm_diff[0][1], norm_diff[0][0])
rel_angle = angle_diff(math.pi/4.0, abs_angle)
angles.append(rel_angle)
transform = pixel_to_angle_transform(m.center[1])
def main():
motor_key = 115
#for capture every second
checktimeBefore = int(time.strftime('%S'))
cas_cnt = 0
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
#undistort
undistorted_image = undistort(image)
#brightness
M = np.ones(undistorted_image.shape, dtype="uint8") * 50
undistorted_image = cv2.add(undistorted_image, M)
#----motor control----
#cascade
cas = len(cascade(undistorted_image))
if cas != 0:
cas_cnt += 1
print(cas, cas_cnt)
#motor('s')
#AR marker
distance = detect_markers(undistorted_image)[1]
markers = detect_markers(undistorted_image)[0]
for marker in markers:
marker.highlite_marker(undistorted_image)
if distance > 60:
print("distance :", distance)
if marker.id == 114:
print('left', marker.id)
elif marker.id == 922:
print('right', marker.id)
elif marker.id == 2537:
print('stop', marker.id)
# show the frame
cv2.imshow("Frame", undistorted_image)
key = cv2.waitKey(1) & 0xFF
rawCapture.truncate(0)
# 1 : capture negative images every second
if switch == 1:
checktime = int(time.strftime('%S'))
if checktime - checktimeBefore >= 1:
captured(undistorted_image)
checktimeBefore = checktime
#print(key)
if key == 27:
break
elif key == ord("\t"):
captured(undistorted_image)
elif key in MOTOR_SPEEDS:
motor_key = key
motor(motor_key)
elif key == 255:
motor(motor_key)
elif key == 32: #space bar
pass
