def flight_data_handler(event, sender, data, **args):
global prev_flight_data
drone = sender
if event is drone.EVENT_FLIGHT_DATA:
if prev_flight_data != str(data):
# Disable flight_data print so that I can see the low battery warning better
# thread_print("\r" + str(data))
prev_flight_data = str(data)
if data.battery_percentage < 10:
thread_print("DRONE MUST LAND NOW")
else:
thread_print('event="%s" data=%s' % (event.getname(), str(data)))
def videofile_control(frame_q, frame_l, bbox_q, bbox_l, kill_q, done_q,
in_videofile, out_videofile):
thread_print("Videofile Control Process Starts")
width = 1024
height = 768
bbox = []
font = cv2.FONT_HERSHEY_SIMPLEX
cam = cv2.VideoCapture(in_videofile)
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out = cv2.VideoWriter(out_videofile, fourcc, 20.0, (width, height))
prev_time = 0.0
y2t_fps_str = "YOLO2-TINY FPS: 0.0"
done_printed = False
sleep(5)
while kill_q.empty():
start = time()
retval, cur_frame = cam.read()
if retval == False:
if not done_printed:
print("Done")
done_printed = True
continue
cur_frame = cv2.resize(cur_frame, (width, height))
if frame_q.empty():
frame_q.put(cur_frame)
sleep(0.005)
if not bbox_q.empty():
bbox, fps = bbox_q.get()
y2t_fps_str = "YOLO2-TINY FPS: %.1f" % fps
for tup in bbox:
label, l, r, t, b = tup
cv2.rectangle(cur_frame, (l, b), (r, t), (0, 255, 0), 2)
cv2.putText(cur_frame, label, (l, b), font, 1, (255, 255, 255), 2,
cv2.LINE_AA)
end = time()
webcam_fps_str = "VIDEOFILE FPS: %.1f" % (1.0 / (end - start))
cv2.putText(cur_frame, webcam_fps_str, (50, 50), font, 1,
(51, 255, 255), 2, cv2.LINE_AA)
cv2.putText(cur_frame, y2t_fps_str, (50, 100), font, 1, (51, 255, 255),
2, cv2.LINE_AA)
out.write(cur_frame)
out.release()
done_q.put(True)
thread_print("Videofile Control Process Ends")
def video_control(drone, frame_q, frame_l, bbox_q, bbox_l, capture_q, capture_l, kill_q, done_q):
thread_print ("Video Control Thread Starts")
video_stream = drone.get_video_stream()
container = av.open(video_stream)
bbox = []
frame_count = 0
font = cv2.FONT_HERSHEY_SIMPLEX
frame_skip = 600
out_break = False
prev_time = 0.0
y2t_fps_str = "YOLO2-TINY FPS: 0.0"
while True:
for frame in container.decode(video=0):
start = time()
if 0 < frame_skip:
frame_skip -= 1
continue
frame_count = frame_count + 1
if frame_count % 3 == 0 or frame_count % 3 == 1: # Dropping 33% of frames
im = np.array(frame.to_image()) # im.shape = (row=720, column=960, rgb=3)
cur_frame = cv2.cvtColor(im, cv2.COLOR_RGB2BGR)
with frame_l:
if frame_q.empty():
frame_q.put(cur_frame)
with bbox_l:
if not bbox_q.empty():
bbox, fps = bbox_q.get()
y2t_fps_str = "YOLO2-TINT FPS: %.1f" % fps
for tup in bbox:
label, l, r, t, b = tup
cv2.rectangle(cur_frame, (l, b), (r, t), (0, 255, 0), 2)
cv2.putText(cur_frame, label, (l, b), font, 1, (255, 255, 255), 2, cv2.CV_AA)
end = time()
# drone_fps_str = "WEBCAM FPS: %.1f" % (1.0 / (end - start))
# cv2.putText(cur_frame, drone_fps_str, (50, 50), font, 1, (51, 255, 255), 2, cv2.CV_AA)
# cv2.putText(cur_frame, y2t_fps_str, (50, 100), font, 1, (51, 255, 255), 2, cv2.CV_AA)
with capture_l:
if not capture_q.empty():
capture_q.get()
cv2.imwrite('capture-' + str(time()) + '.png', cur_frame)
cv2.imshow('Drone Demo', cur_frame)
cv2.waitKey(1)
def webcam_control(frame_q, frame_l, bbox_q, bbox_l, kill_q, done_q):
thread_print("Webcam Control Process Starts")
bbox = []
font = cv2.FONT_HERSHEY_SIMPLEX
cam = cv2.VideoCapture(0)
prev_time = 0.0
y2t_fps_str = "YOLO2-TINY FPS: 0.0"
while kill_q.empty():
start = time()
_, cur_frame = cam.read()
cur_frame = cv2.resize(cur_frame, (1024, 768))
if frame_q.empty():
frame_q.put(cur_frame)
if not bbox_q.empty():
bbox, fps = bbox_q.get()
y2t_fps_str = "YOLO2-TINY FPS: %.1f" % fps
for tup in bbox:
label, l, r, t, b = tup
cv2.rectangle(cur_frame, (l, b), (r, t), (0, 255, 0), 2)
cv2.putText(cur_frame, label, (l, b), font, 1, (255, 255, 255), 2,
cv2.CV_AA)
end = time()
webcam_fps_str = "WEBCAM FPS: %.1f" % (1.0 / (end - start))
cv2.putText(cur_frame, webcam_fps_str, (50, 50), font, 1,
(51, 255, 255), 2, cv2.CV_AA)
cv2.putText(cur_frame, y2t_fps_str, (50, 100), font, 1, (51, 255, 255),
2, cv2.CV_AA)
cv2.imshow('Webcam Demo', cur_frame)
cv2.waitKey(1)
cv2.destroyAllWindows()
done_q.put(True)
thread_print("Webcam Control Process Ends")
def detection(yolo_engine, tf_w_pickle, dnnweaver2_w_pickle, frame_q, frame_l, bbox_q, bbox_l, kill_q, done_q, proc="cpu", debug=False):
options = {"model": "conf/tiny-yolo-voc.cfg", "load": "weights/tiny-yolo-voc.weights", "threshold": 0.25}
tfnet = TFNet(options)
if yolo_engine == "dnnweaver2":
fpga_manager = dnn_fpga.initialize_yolo_graph(dnnweaver2_w_pickle)
if debug:
y2t_tf_whole = YOLO2_TINY_TF([1, 416, 416, 3], tf_w_pickle, proc)
elif yolo_engine == "tf-cpu" or yolo_engine == "tf-gpu":
y2t_tf = YOLO2_TINY_TF([1, 416, 416, 3], tf_w_pickle, proc)
cnt = 0
h = None
w = None
thread_print ("Detection Process Starts")
cur_frame = []
while kill_q.empty():
with frame_l:
if not frame_q.empty():
cur_frame = frame_q.get()
if cur_frame != []:
if h == None and w == None:
h, w, _ = cur_frame.shape
start = time()
if yolo_engine == "tf-cpu" or yolo_engine == "tf-gpu":
im = resize_input((416, 416, 3), cur_frame)
im = np.expand_dims(im, 0)
tout = y2t_tf.inference(im)
result = get_bbox(tfnet, tout[0], h, w)
elif yolo_engine == "dnnweaver2":
im = resize_input((416, 416, 3), cur_frame)
im = np.expand_dims(im, 0)
_im = yolo_demo.fp32tofxp16_tensor(im, 8)
intermediate_tout = dnn_fpga.fpga_inference(fpga_manager, _im)
intermediate_tout = yolo_demo.fxp16tofp32_tensor(intermediate_tout, fpga_manager.get_tout_frac_bits())
tout = intermediate_tout
if not debug:
result = get_bbox(tfnet, tout[0], h, w)
else:
_tout = copy.deepcopy(tout)
result = get_bbox(tfnet, _tout[0], h, w)
wnodes, wtout = y2t_tf_whole._inference(im)
final_tout = tout
final_wtout = wtout[len(wtout)-1]
error = np.sqrt(np.mean((final_tout - final_wtout) ** 2)) / (final_wtout.max() - final_wtout.min()) * 100.0
thread_print ("Final NRMSE = %.4f%%" % error)
end = time()
fps = 1.0 / (end - start)
out = []
for det in result:
label, l, r, t, b = det['label'], det['topleft']['x'], det['bottomright']['x'], det['topleft']['y'], det['bottomright']['y']
out.append((label, l, r, t, b))
with bbox_l:
if bbox_q.empty():
bbox_q.put([out, fps])
cnt += 1
done_q.put(True)
thread_print("Detection Process Ends..")
def drone_control(frame_q, frame_l, bbox_q, bbox_l, key_q, key_l, kill_q,
done_q):
drone = tellopy.Tello()
drone.connect()
drone.wait_for_connection(5.0)
drone.subscribe(drone.EVENT_FLIGHT_DATA, flight_data_handler)
capture_q = Queue(maxsize=1)
capture_l = Lock()
videoThread = threading.Thread(target=video_control,
args=(
drone,
frame_q,
frame_l,
bbox_q,
bbox_l,
capture_q,
capture_l,
kill_q,
done_q,
))
videoThread.daemon = True
videoThread.start()
thread_print("Drone Control Process Starts")
drone.set_loglevel(drone.LOG_DEBUG)
key = None
while True:
with key_l:
if not key_q.empty():
key = key_q.get()
else:
key = None
if key == 27: # key = esc; ignore esc inserted by python
continue
if key == 91: # key = arrows; ignore 91 inserted before actual arrow's ascii value
continue
if key == 101: # key = 'e'
break
if key == 112: # key = 'p'
with capture_l:
capture_q.put(True)
if key == 116: # key = t
drone.takeoff()
if key == 108: # key = l
drone.land()
if key == 122: # key = z
drone.left(25)
if key == 99: # key = c
drone.right(25)
if key == 115: # key = s
drone.forward(25)
if key == 120: # key = x
drone.backward(25)
if key == 46: # key = .
drone.clockwise(35)
if key == 117: # key = u
drone.up(25)
if key == 100: # key = d
drone.down(25)
if key == 44: # key = ,
drone.counter_clockwise(35)
if key == 49: # key = 1
drone.flip_forward()
if key == 50: # key = 2
drone.flip_back()
if key == 51: # key = 3
drone.flip_right()
if key == 52: # key = 4
drone.flip_left()
if key == 48:
drone.left(0)
drone.right(0)
drone.forward(0)
drone.backward(0)
drone.clockwise(0)
drone.up(0)
drone.down(0)
drone.counter_clockwise(0)
if key is not None:
thread_print(key)
thread_print(
str(drone.left_y) + " " + str(drone.right_y) + " " +
str(drone.right_x))
key = None
sleep(0.1)
done_q.put(True)
thread_print("Drone Control Process Ends")
def run(cam_source, yolo_engine, tf_weight_pickle, dnnweaver2_weight_pickle, in_videofile, out_videofile):
# Synchronous queues
frame_q = Queue(maxsize=1)
bbox_q = Queue(maxsize=1)
kill_q = Queue(maxsize=1)
key_q = Queue(maxsize=1)
num_processes = 2
done_q = Queue(maxsize=num_processes)
# Multiprocessing locks
frame_l = Lock()
bbox_l = Lock()
key_l = Lock()
# Drone management process
if cam_source == "drone":
droneProcess = Process(target=drone_control, args=(frame_q, frame_l, bbox_q, bbox_l, key_q, key_l, kill_q, done_q, ))
droneProcess.start()
elif cam_source == "webcam":
webcamProcess = Process(target=webcam_control, args=(frame_q, frame_l, bbox_q, bbox_l, kill_q, done_q, ))
webcamProcess.start()
elif cam_source == "videofile":
videofileProcess = Process(target=videofile_control, args=(frame_q, frame_l, bbox_q, bbox_l, kill_q, done_q, in_videofile, out_videofile, ))
videofileProcess.start()
# Object detection process using YOLO algorithm
if yolo_engine == "tf-cpu":
proc = "cpu"
elif yolo_engine == "tf-gpu":
proc = "gpu"
elif yolo_engine == "dnnweaver2":
proc = "gpu"
detectionProcess = Process(target=detection, args=(yolo_engine, tf_weight_pickle, dnnweaver2_weight_pickle, frame_q, frame_l, bbox_q, bbox_l, kill_q, done_q, proc, ))
# detectionProcess = Process(target=detection, args=(yolo_engine, tf_weight_pickle, dnnweaver2_weight_pickle, frame_q, frame_l, bbox_q, bbox_l, kill_q, done_q, proc, True, ))
detectionProcess.start()
# Keyboard input handler
inkey = GetKey()
thread_print ("Keyboard Input Handler Starts")
while True:
try:
key = inkey()
key = ord(key)
with key_l:
if key_q.empty():
key_q.put(key)
if key == 101: # key = 'e'
break
except KeyboardInterrupt:
break
thread_print ("Keyboard Input Handler Ends")
# Notifying all processes/threads to die
kill_q.put(True)
print ("Sent KILL Signal")
# Wait the processes to end
while done_q.qsize() != num_processes:
sleep(0.5)
# Flush all entires in queueus
drain_queue([frame_q, bbox_q, kill_q, key_q])
if cam_source == "drone":
droneProcess.join()
elif cam_source == "webcam":
webcamProcess.join()
detectionProcess.join()