def start_right_cb(self, channel):
if self.start_dir_state is None:
self.start_dir_state = "RIGHT"
print("Starting at 0 degrees (right).")
go_to_angle(self.start_band)
GPIO.output(start_right_led, GPIO.HIGH)
# Give the servo motor time to get there
time.sleep(0.25)
def main():
time.sleep(2)
go_to_angle(0)
time.sleep(4)
e = Encoder()
time.sleep(4)
#
# try:
# while True:
# pass
# finally:
# e.close()
# test_angles = list(range(0, 190, 10))
# for a in test_angles:
# go_to_angle(a)
# time.sleep(1)
go_to_angle(180)
time.sleep(5)
e.close()
def main():
global e
global cap
global out
# video_write_proc = multiprocessing.Process(
# target=write_frame_process(e),
# args = (e,)
# )
# video_write_proc.start()
while True:
if hud.start_dir_state is None:
hud.flash_loading()
else:
break
while hud.running_state is False:
pass
hud.set_start_led("LOW")
# camera = jetson.utils.gstCamera(1280, 720, "dev/video0")
if hud.start_dir_state == "RIGHT":
prev_angle = start_band
desired_angle = start_band
else:
prev_angle = end_band
desired_angle = end_band
# Init control velocity
vel = 0
# load the object detection network
#TODO: Figure out the threshold that works best
net = jetson.inference.detectNet(network="facenet", threshold=0.5)
# take first frame of the video
ret, frame = cap.read()
#print("Frame Shape: ", frame.shape[1], frame.shape[0])
hud.set_start_led("HIGH")
detections = []
while len(detections) < 1:
img_np = cv.cvtColor(frame, cv.COLOR_BGR2RGBA)
img_cuda = jetson.utils.cudaFromNumpy(img_np)
detections = net.Detect(img_cuda, frame.shape[1], frame.shape[0])
ret, frame = cap.read()
hud.set_start_led("LOW")
# if len(detections) > 1:
# print('Detected.')
# if int(detections[0].Right - detections[0].Left) >= 20:
# break
jetson.utils.cudaDeviceSynchronize()
detected_obj = detections[0]
print("Detection Class: " + str(detected_obj.ClassID))
detect_top = int(detected_obj.Top)
detect_left = int(detected_obj.Left)
detect_width = int(detected_obj.Right - detected_obj.Left)
detect_height = int(detected_obj.Bottom - detected_obj.Top)
if detect_top < 0:
detect_top = detect_top + int(frame.shape[0] / 2)
if detect_left < 0:
detect_left = detect_left + int(frame.shape[1] / 2)
#print("Detection Left: " + str(detected_obj.Left))
#print("Detection Top: " + str(detected_obj.Top))
#print("Detection Width: " + str(detected_obj.Width))
#print("Detection Height: " + str(detected_obj.Height))
# setup initial location of window
# x_prev, y, w, h = detect_left, detect_top, detect_width, detect_height
x_prev, y, w, h = detect_left, detect_top, int(detect_width), int(
detect_height)
# x_prev, y, w, h = detect_left, 0, detect_width, frame.shape[0]
x_center = int(frame.shape[1] / 2)
#TODO ^ Figure out scaling down of width and height if they are too big for deadzone
# x, y, w, h = int(detect_left), int(detect_top), 100, 100
track_window = (x_prev, y, w, h)
# set up the ROI for tracking
roi = frame[y:(y + h), x_prev:(x_prev + w)]
#cv.imshow("roi", roi)
hsv_roi = cv.cvtColor(roi, cv.COLOR_BGR2HSV)
mask = cv.inRange(hsv_roi, np.array((0, 60, 32)), np.array(
(180, 255, 255)))
#cv.imshow("mask", mask)
roi_hist = cv.calcHist([hsv_roi], [0], mask, [180], [0, 180])
cv.normalize(roi_hist, roi_hist, 0, 255, cv.NORM_MINMAX)
# Setup the termination criteria, either 10 iteration or move by atleast 1 pt
term_crit = (cv.TERM_CRITERIA_EPS | cv.TERM_CRITERIA_COUNT, 10, 1)
pGain = 1 / 35
dGain = 0.05
# dGain = 0
while (1):
if hud.reset:
cap.release()
out.release()
cv.destroyAllWindows()
hud.start_dir_state = None
hud.running_state = False
video_string = "medianflow_" + str(
datetime.datetime.now()) + ".mp4"
out = cv.VideoWriter(video_string, cv.VideoWriter_fourcc(*'X264'),
24, (800, 600))
go_to_angle(90)
main()
start = time.time()
ret, frame = cap.read()
# cropped_frame =
if ret == True:
# roi = frame[y:(y + h), x_prev:(x_prev + w)]
# hsv_roi = cv.cvtColor(roi, cv.COLOR_BGR2HSV)
# roi_hist = cv.calcHist([hsv_roi], [0], mask, [180], [0, 180])
# cv.normalize(roi_hist, roi_hist, 0, 255, cv.NORM_MINMAX)
hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV)
dst = cv.calcBackProject([hsv], [0], roi_hist, [0, 180], 1)
# apply meanshift to get the new location
ret, track_window = cv.meanShift(dst, track_window, term_crit)
# Draw it on image
x, y, _, _ = track_window
y = clamp(y,
int(frame.shape[0] / 2) - 200,
int(frame.shape[0] / 2) + 50)
# if x > x_center + 75:
if x > x_center + 75:
vel = -((x - x_center) * pGain)
deriv = -dGain * abs((x - x_prev))
elif x < x_center - 75:
vel = abs((x - x_center) * pGain)
deriv = dGain * abs((x - x_prev))
else:
# desired_angle = desired_angle
vel = 0
deriv = 0
desired_angle = desired_angle + vel - deriv
#print("Desired Angle: " + str(desired_angle) + " Velocity: " + str(vel))
if desired_angle <= 0:
desired_angle = 0
vel = 0
elif desired_angle >= 180:
desired_angle = 180
vel = 0
go_to_angle(desired_angle)
x_prev = x
img2 = cv.rectangle(frame, (x, y), (x + w, y + h), 255, 2)
#cv.imshow('img2', img2)
out.write(img2)
loop = time.time() - start
print("FPS: ", str(1 / loop))
k = cv.waitKey(1) & 0xff
if k == 27:
break
else:
cap.release()
out.release()
cv.destroyAllWindows()
break
def main():
global e
global cap
global out
# video_write_proc = multiprocessing.Process(
# target=write_frame_process(e),
# args = (e,)
# )
# video_write_proc.start()
while True:
if hud.start_dir_state is None:
hud.flash_loading()
else:
break
while hud.running_state is False:
pass
hud.set_start_led("LOW")
# camera = jetson.utils.gstCamera(1280, 720, "dev/video0")
if hud.start_dir_state == "RIGHT":
prev_angle = start_band
desired_angle = start_band
else:
prev_angle = end_band
desired_angle = end_band
# Init control velocity
vel = 0
# load the object detection network
#TODO: Figure out the threshold that works best
net = jetson.inference.detectNet(network="facenet", threshold=0.5)
# take first frame of the video
ret, frame = cap.read()
#print("Frame Shape: ", frame.shape[1], frame.shape[0])
hud.set_start_led("HIGH")
detections = []
while len(detections) < 1:
img_np = cv.cvtColor(frame, cv.COLOR_BGR2RGBA)
img_cuda = jetson.utils.cudaFromNumpy(img_np)
detections = net.Detect(img_cuda, frame.shape[1], frame.shape[0])
ret, frame = cap.read()
hud.set_start_led("LOW")
# if len(detections) > 1:
# print('Detected.')
# if int(detections[0].Right - detections[0].Left) >= 20:
# break
jetson.utils.cudaDeviceSynchronize()
detected_obj = detections[0]
print("Detection Class: " + str(detected_obj.ClassID))
detect_top = int(detected_obj.Top)
detect_left = int(detected_obj.Left)
detect_width = int(detected_obj.Right - detected_obj.Left)
detect_height = int(detected_obj.Bottom - detected_obj.Top)
if detect_top < 0:
detect_top = detect_top + int(frame.shape[0] / 2)
if detect_left < 0:
detect_left = detect_left + int(frame.shape[1] / 2)
# setup initial location of window
x_prev, y, w, h = detect_left, detect_top, detect_width, detect_height
x = x_prev
x_center = int(frame.shape[1] / 2)
#TODO ^ Figure out scaling down of width and height if they are too big for deadzone
# x, y, w, h = int(detect_left), int(detect_top), 100, 100
track_window = (x_prev, y, w, h)
# set up the ROI for tracking
roi = frame[y:(y + h), x_prev:(x_prev + w)]
cv.imshow("roi", roi)
# roi = frame[y:(y + h), x:(x+ w)]
old_gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
roi_mask = np.full_like(old_gray, 0)
roi_mask[y:(y + h), x_prev:(x_prev + w)] = 1
# p0 = cv.goodFeaturesToTrack(old_gray[y:(y + h), x_prev:(x_prev + w)], mask=None, **feature_params)
# p0 = cv.goodFeaturesToTrack(get_roi(old_gray, x_prev, y, w, h), mask=None, **feature_params)
p0 = cv.goodFeaturesToTrack(old_gray, mask=roi_mask, **feature_params)
p_global = p0
# p_global = np.zeros(np.shape(p0), dtype=np.float32)
#
# print('p_global', p_global)
pGain = 1 / 45
dGain = 0.01
# dGain = 0
while (1):
if hud.reset:
cap.release()
out.release()
cv.destroyAllWindows()
hud.start_dir_state = None
hud.running_state = False
video_string = "optflow_" + str(datetime.datetime.now()) + ".avi"
out = cv.VideoWriter(video_string, cv.VideoWriter_fourcc(*'XVID'),
24, (800, 600))
go_to_angle(90)
main()
start = time.time()
ret, frame = cap.read()
# cropped_frame =
if ret == True:
frame_gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
# calculate optical flow
# p1, st, err = cv.calcOpticalFlowPyrLK(get_roi(old_gray, x_prev, y, w+200, h+100), get_roi(frame_gray, x_prev, y, w+200, h+100), p_global, None, **lk_params)
p1, st, err = cv.calcOpticalFlowPyrLK(old_gray, frame_gray,
p_global, None, **lk_params)
if p1 is None or (p1.shape[0] < 3):
print("Refresh")
count = 0
roi_mask = np.full_like(old_gray, 0)
roi_mask[y:(y + h), x_prev:(x_prev + w)] = 1
p_global = cv.goodFeaturesToTrack(frame_gray,
mask=roi_mask,
**feature_params)
# cv.imshow("Refresh", get_roi(frame_gray, x_prev, y, w, h))
# p1, st, err = cv.calcOpticalFlowPyrLK(get_roi(old_gray, x_prev, y, w+200, h+100), get_roi(frame_gray, x_prev, y, w+200, h+100), p_global, None, **lk_params)
p1, st, err = cv.calcOpticalFlowPyrLK(old_gray, frame_gray,
p_global, None,
**lk_params)
good_new = p1[st == 1]
x, y = get_bounding_box(good_new, w, h)
# if good_new.size != 0:
#
# count = 0
# gn_global = np.zeros(np.shape(good_new), dtype=np.float32)
# for p in good_new:
# p_x, p_y = p.ravel()
# print('p_x', p_x, 'p_y', p_y)
# gn_global[count] = [p_x + x, p_y + y]
# count = count + 1
# x, y = get_bounding_box(gn_global, w, h)
else:
# Select good points
good_new = p1[st == 1]
x, y = get_bounding_box(good_new, w, h)
# if good_new.size != 0:
#
# count = 0
# gn_global = np.zeros(np.shape(good_new), dtype=np.float32)
# for p in good_new:
# p_x, p_y = p.ravel()
# print('p_x', p_x, 'p_y', p_y)
# gn_global[count] = [p_x + x, p_y + y]
# count = count + 1
#
# x, y = get_bounding_box(gn_global, w, h)
frame = cv.rectangle(frame, (x, y), (x + w, y + h), 255, 2)
# draw the tracks
for i, (new, old) in enumerate(zip(good_new, good_new)):
a, b = new.ravel()
c, d = old.ravel()
# mask = cv.line(mask, (a, b), (c, d), color[i].tolist(), 2)
frame = cv.circle(frame, (a, b), 5, color[i].tolist(), -1)
# Servo Control
if x > x_center + 35:
vel = -((x - x_center) * pGain)
deriv = -dGain * abs((x - x_prev))
elif x < x_center - 35:
vel = abs((x - x_center) * pGain)
deriv = dGain * abs((x - x_prev))
else:
# desired_angle = desired_angle
vel = 0
deriv = 0
desired_angle = desired_angle + vel - deriv
#print("Desired Angle: " + str(desired_angle) + " Velocity: " + str(vel))
if desired_angle <= 0:
desired_angle = 0
vel = 0
elif desired_angle >= 180:
desired_angle = 180
vel = 0
go_to_angle(desired_angle)
x_prev = x
cv.imshow('Frame', frame)
out.write(frame)
loop = time.time() - start
print("FPS: ", str(1 / loop))
k = cv.waitKey(1) & 0xff
if k == 27:
break
# Now update the previous frame and previous points
old_gray = frame_gray.copy()
p_global = good_new.reshape(-1, 1, 2)
else:
cap.release()
out.release()
cv.destroyAllWindows()
break
#TODO ^ Figure out scaling down of width and height if they are too big for deadzone
# x, y, w, h = int(detect_left), int(detect_top), 100, 100
track_window = (x_prev, y, w, h)
# set up the ROI for tracking
roi = frame[y:(y + h), x_prev:(x_prev + w)]
cv.imshow("roi", roi)
hsv_roi = cv.cvtColor(roi, cv.COLOR_BGR2HSV)
mask = cv.inRange(hsv_roi, np.array((0, 60, 32)), np.array((180, 255, 255)))
roi_hist = cv.calcHist([hsv_roi], [0], mask, [180], [0, 180])
cv.normalize(roi_hist, roi_hist, 0, 255, cv.NORM_MINMAX)
# Setup the termination criteria, either 10 iteration or move by atleast 1 pt
term_crit = (cv.TERM_CRITERIA_EPS | cv.TERM_CRITERIA_COUNT, 10, 1)
go_to_angle(90)
while (1):
start = time.clock()
ret, frame = cap.read()
if ret == True:
hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV)
dst = cv.calcBackProject([hsv], [0], roi_hist, [0, 180], 1)
# apply meanshift to get the new location
ret, track_window = cv.meanShift(dst, track_window, term_crit)
# Draw it on image
x, y, _, _ = track_window