def main():
global timer
timer = SW('timer')
timer.start()
nt = init_nt()
cam = init_cv()
loop(cam, nt)
def find_valids(img_orig, calibration, rect_cnt1, rect_cnt2):
"""
Input: image from camera, calibration information, contours from generated rectangle
Output:
angle -> float, angle from camera center to target center
validUpdate -> boolean, valid target found
This function uses calibration information to create a mask of the target. It then
finds valid targets comparing to the rectangle contours, calculates the angle to target center,
and provides graphical representations for future use.
"""
debug = calibration['debug']
search = calibration['search']
angle = 1000
valid_update = False
img_copy = np.copy(img_orig)
lower_bound = np.array(calibration["green"]["green_lower"])
upper_bound = np.array(calibration["green"]["green_upper"])
if debug:
timer_ft = SW('ft')
timer_ft.start()
hsv = cv2.cvtColor(img_copy, cv2.COLOR_BGR2HSV)
if debug:
elapsed=timer_ft.get()
print("DEBUG: cvt took " + str(elapsed))
if debug:
timer_ft.start()
mask = cv2.inRange(hsv, lower_bound, upper_bound)
if debug:
elapsed = timer_ft.get()
print("DEBUG: inrange took "+ str(elapsed))
if debug:
timer_ft.start()
erode_and_diliate = MI.erodeAndDilate(mask)
if debug:
elapsed = timer_ft.get()
print("DEBUG: erode_and_dilate took " + str(elapsed))
if debug:
timer_ft.start()
ret, mask_thresh = cv2.threshold(erode_and_diliate, 127, 255, cv2.THRESH_BINARY)
if debug:
elapsed = timer_ft.get()
print("DEBUG: threshold took " + str(elapsed))
if debug:
time = datetime.now().strftime("%s")
cv2.imwrite("/home/linaro/test_images/"+time+"image_orig.png", img_orig)
cv2.imwrite("/home/linaro/test_images/"+time+"mask.png", mask)
cv2.imwrite("/home/linaro/test_images/"+time+"erode_and_diliate.png", erode_and_diliate)
if search:
valid, cnt, cx, cy = VT.find_valid_target(mask_thresh, rect_cnt1, rect_cnt2)
if valid:
valid_update = True
if debug:
cx_avg = int((cx[0]+cx[1])/2)
cy_avg = int((cy[0]+cy[1])/2)
line_img = MI.drawLine2Target(mask_thresh, cx_avg, cy_avg)
cv2.imwrite("/home/linaro/test_images/" + time + "target_lined.png", line_img)
angle = IC.findAngle(img_orig, cx[0], cx[1])
return angle, valid_update
def create_rect(debug):
"""
Creates a rectangle and performs appropriate processing to provide a target
returns the contour object of the rectangle
:return the two contours of the rectangle we want to validate targets
against (returned as a tuple)
"""
# Draw rectangles of the retro reflective tape (Find dimensions in the game manual)
# Camera dimensions: 320 x 240
# Rectangle dimensions: 40 x 110
if debug:
timer_rect = SW('rect')
timer_rect.start()
width = 40
length = 110
img_width = 175
img_length = 175
top_left_x = int(img_width - width / 2)
top_left_y = int(img_length - length / 2)
bottom_right_x = int(img_width + width / 2)
bottom_right_y = int(img_length + length / 2)
background = np.zeros((350, 350, 3), np.uint8)
rect1 = cv2.rectangle(background, (top_left_x, top_left_y), (bottom_right_x, bottom_right_y), (255, 255, 255), -1)
m = cv2.getRotationMatrix2D((350 / 2, 350 / 2), -14.5, 1)
rect1_rotated = cv2.warpAffine(rect1, m, (350, 350))
ret, thresh = cv2.threshold(rect1_rotated, 127, 255, cv2.THRESH_BINARY)
thresh = cv2.cvtColor(thresh, cv2.COLOR_BGR2GRAY)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnt1 = contours[0]
rect2 = rect1
m = cv2.getRotationMatrix2D((350 / 2, 350 / 2), 14.5, 1)
rect2_rotated = cv2.warpAffine(rect2, m, (350, 350))
ret, thresh = cv2.threshold(rect2_rotated, 127, 255, cv2.THRESH_BINARY)
thresh = cv2.cvtColor(thresh, cv2.COLOR_BGR2GRAY)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnt2 = contours[0]
if debug:
elapsed = timer_rect.get()
print('DEBUG: rectangles took ' + str(elapsed))
return cnt1, cnt2
def run(cap, camera_table, calibration, freqFramesNT, rect_cnt1, rect_cnt2):
"""
Run the main vision algorithm on each camera frame and update network table appropriately
:param cap: cap returned from cv2.VideoCapture
:param camera_table: the network table we are writing to
:param calibration: dictionary containing hsv thresholds and whether we are in debug mode or not
:param freqFramesNT: frequency of frames for data to be sent to network tables
:param rect_cnt: contour of the rectangle we want to validate targets against
:return: None
"""
valid_count = 0
n = 0
while cap.isOpened():
ret, frame = cap.read()
if ret:
try:
if calibration['debug']:
timer_fv=SW('FV')
timer_fv.start()
angle, valid_update = FT.find_valids(frame, calibration, rect_cnt1, rect_cnt2)
if calibration['debug']:
elapsed = timer_fv.get()
print("DEBUG: find_valids took " + str(elapsed))
print("DEBUG: angle: " + str(angle) + " valid_update: " + str(valid_update) + " valid_count: " + str(valid_count) )
if valid_update:
valid_count += 1
if n > freqFramesNT:
nt_send(camera_table, angle, valid_count, valid_update)
n = 0
else:
n += 1
except:
print("WARNING: There was an error with find_valids. Continuing.")
continue
else:
print("WARNING: Unable to read frame. Continuing.")
continue
else:
print("ERROR: Capture is not opened. Ending program.")
sys.exit()
# Only works with mac and Windows 10 currently see comment on line 6 for more instructions
import cv2
import numpy as np
from util.stopwatch import stopwatch as SW
cap = cv2.VideoCapture(
0) # Need to change the 0 to what ever the camera devices name is
timer = SW('timer')
display = "normal"
lastk = 48
while cap.isOpened:
k = cv2.waitKey(15) & 0xFF
print(k)
if k != 255:
lastk = k
else:
k = lastk
timer.start()
ret, frame = cap.read()
new_frame = frame
if ret:
print('Got a frame!!')
frame = cv2.UMat(frame)
display = "normal_frame"
cv2.imshow("display", frame)
if k == 48:
display = "normal_frame"
cv2.imshow("display", frame)
fps = 1.0 / timer.get()
print('frames per second: %.3f' % fps)
def run(cap, vision_table, calibration, freqFramesNT, desired_cnt):
"""[summary]
Arguments:
cap {[type]} -- [description]
vision_table {[type]} -- [description]
calibration {[type]} -- [description]
freqFramesNT {[type]} -- [description]
desired_cnt {[type]} -- [description]
"""
valid_count = 0
heartbeat = 0
n = 0
x = 0
recent_vals = []
while cap.isOpened():
heartbeat += 1
x += 1
ret, frame = cap.read()
if ret:
try:
if calibration['debug']:
print("DEBUG: heartbeat: " + str(heartbeat))
timer_fv = SW('FV')
timer_fv.start()
angle, valid_update = FT.find_valids(frame, calibration,
desired_cnt)
except Exception as e:
print("WARNING: find_valids threw an exception: " + str(e))
angle = 9999
valid_update: False
# Checking for anomalies
if calibration['debug']:
print("DEBUG: recent_vals: " + str(recent_vals) +
" last angle: " + str(angle))
if angle != 1000 and angle != 9999: # do not append the angle if something blew up
recent_vals.append(angle)
if len(
recent_vals
) > 5: # if the length of the recent_values is larger than 5, we want to make it 5
recent_vals.pop(0)
if len(
recent_vals
) > 0: # if there is no angle data, we want to NOT divide by zero
average_angle = sum(recent_vals) / len(recent_vals)
else:
average_angle = 0 # set the average angle to something sane until we get some values
if valid_update:
valid_count += 1
if calibration['debug']:
elapsed = timer_fv.get()
print("DEBUG: find_valids took " + str(elapsed))
print("DEBUG: average_angle: " + str(average_angle) +
" valid_update: " + str(valid_update) +
" valid_count: " + str(valid_count))
if n > freqFramesNT:
nt_send(vision_table, average_angle, valid_count, valid_update,
heartbeat)
n = 0
else:
n += 1
else:
print("WARNING: Unable to read frame. Continuing.")
continue
else:
print("ERROR: Capture is not opened. Ending program.")
sys.exit()
def find_valids(img_orig, calibration, desired_cnt):
"""[summary]
Arguments:
img_orig {[type]} -- [description]
calibration {[type]} -- [description]
desired_cnt {[type]} -- [description]
Returns:
[type] -- [description]
"""
path = "/" + "/".join(os.getcwd().split("/")[1:3]) + "/test_images/"
print("path: " + path)
debug = calibration['debug']
search = calibration['search']
angle = 1000
valid_update = False
img_copy = np.copy(img_orig)
lower_bound = np.array(calibration["green"]["green_lower"])
upper_bound = np.array(calibration["green"]["green_upper"])
if debug:
timer_ft = SW('ft')
timer_ft.start()
hsv = cv2.cvtColor(img_copy, cv2.COLOR_BGR2HSV)
if debug:
elapsed = timer_ft.get()
print("DEBUG: cvt took " + str(elapsed))
if debug:
timer_ft.start()
mask = cv2.inRange(hsv, lower_bound, upper_bound)
if debug:
elapsed = timer_ft.get()
print("DEBUG: inrange took " + str(elapsed))
if debug:
timer_ft.start()
erode_and_diliate = MI.erodeAndDilate(mask)
if debug:
elapsed = timer_ft.get()
print("DEBUG: erode_and_dilate took " + str(elapsed))
if debug:
timer_ft.start()
ret, mask_thresh = cv2.threshold(erode_and_diliate, 127, 255,
cv2.THRESH_BINARY)
if debug:
elapsed = timer_ft.get()
print("DEBUG: threshold took " + str(elapsed))
if debug:
time = datetime.now().strftime("%s")
cv2.imwrite(path + time + "_image_orig.png", img_orig)
cv2.imwrite(path + time + "_mask.png", mask)
cv2.imwrite(path + time + "_mask.png", mask_thresh)
cv2.imwrite(path + time + "_erode_and_diliate.png", erode_and_diliate)
if search:
valid, cnt = VT.find_valid_target(mask_thresh, desired_cnt)
if valid:
valid_update = True
hull = cv2.convexHull(cnt[0])
cx, cy = IC.findCenter(hull)
if debug:
line_img = MI.drawLine2Target(mask_thresh, cx, cy)
cv2.imwrite(path + time + "target_lined.png", line_img)
angle = IC.findAngle(img_orig, cx)
return angle, valid_update