Skip to content
Snippets Groups Projects
Commit 8692df68 authored by Zsedrovits Tamás's avatar Zsedrovits Tamás
Browse files

Initial version

parents
No related branches found
No related tags found
No related merge requests found
from djitellopy import Tello
import cv2
import pygame
import numpy as np
import time
import os
import sys
from dbr import *
# Speed of the drone
S = 60
# Frames per second of the pygame window display
# A low number also results in input lag, as input information is processed once per frame.
FPS = 120
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations (yaw)
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
self.im_width = 320
self.im_height = 240
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([self.im_width, self.im_height])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
# create update timer
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
# create qr code reader
self.license_key = "t0068NQAAAI5b3yhlP8v8IboD8CuVw36iT7KUwyfpZ8UYxAMAovEGNXYkJ815aDT4dQ8dv4/5V48UxD9fJP5wBlL7urlPeHQ="
self.json_file = "settings.json"
self.reader = BarcodeReader()
self.arIms = [cv2.imread('minta1.png', cv2.IMREAD_UNCHANGED), cv2.imread('minta2.png', cv2.IMREAD_UNCHANGED),
cv2.imread('minta3.png', cv2.IMREAD_UNCHANGED), cv2.imread('minta4.png', cv2.IMREAD_UNCHANGED)]
# Keeps the detected qr code between frames for a while to lessen the flashing effect
self.detection_smooth_counter = 0
self.qrcodes = None
self.capture_time = None
self.reader.init_license(self.license_key)
error = self.reader.init_runtime_settings_with_file(self.json_file)
if error[0] != EnumErrorCode.DBR_OK:
print(error[1])
self.parameters = self.reader.init_frame_decoding_parameters()
# you can modify these following parameters.
self.parameters.max_queue_length = 30
self.parameters.max_result_queue_length = 30
self.parameters.width = self.im_width
self.parameters.height = self.im_height
self.parameters.stride = self.im_width*3
self.parameters.image_pixel_format = EnumImagePixelFormat.IPF_RGB_888
self.parameters.region_top = 0
self.parameters.region_bottom = 100
self.parameters.region_left = 0
self.parameters.region_right = 100
self.parameters.region_measured_by_percentage = 1
self.parameters.threshold = 0.01
self.parameters.fps = 0
self.parameters.auto_filter = 1
self.reader.start_video_mode(self.parameters, self.on_barcode_result)
# store AR images for overlay to decrease flashing
self.warped = []
# The callback function for receiving barcode results
def on_barcode_result(self,data):
self.qrcodes = data
self.capture_time = self.get_time
def get_time(self):
localtime = time.localtime()
capturetime = time.strftime("%Y%m%d%H%M%S", localtime)
return capturetime
def decorate_frame(self,frame):
frame = frame.astype(float)
if self.detection_smooth_counter >= 0:
for warped in self.warped:
background = frame
(h,w) = background.shape[:2]
foreground = warped[:,:,:3]
alpha1 = warped[...,-1]
alpha = cv2.merge((alpha1,alpha1,alpha1))
# Display results
# Convert uint8 to float
foreground = foreground.astype(float)
background = background.astype(float)
# Normalize the alpha mask to keep intensity between 0 and 1
alpha = alpha.astype(float)/255
# Multiply the foreground with the alpha matte
foreground = cv2.multiply(alpha, foreground)
# Multiply the background with ( 1 - alpha )
background = cv2.multiply(1.0 - alpha, background)
# Add the masked foreground and background.
frame = cv2.add(foreground, background)
self.detection_smooth_counter = self.detection_smooth_counter - 1
return frame
def calculate_warped(self):
if self.qrcodes != None:
capture_time = self.capture_time
self.warped = []
self.detection_smooth_counter = 60
for qrcode in self.qrcodes:
text_result = TextResult(qrcode)
# print("Barcode Format :")
# print(text_result.barcode_format_string)
# print("Barcode Text :")
# print(text_result.barcode_text)
# print("Localization Points : ")
# print(text_result.localization_result.localization_points)
# print("-------------")
points = text_result.localization_result.localization_points
if text_result.barcode_text == '1':
arIm = self.arIms[0]
elif text_result.barcode_text == '2':
arIm = self.arIms[1]
elif text_result.barcode_text == '3':
arIm = self.arIms[2]
elif text_result.barcode_text == '4':
arIm = self.arIms[3]
else:
arIm = np.zeros([1,1,3])
# Calculate transform
dst = self.order_points(points)
convHullSize = np.max(arIm.shape)
scale = 4
verticalDisplacement = 0
horizontalDisplacement = -(1.5*convHullSize/scale)
rect = np.array([
[-horizontalDisplacement, -verticalDisplacement],
[-horizontalDisplacement + convHullSize/scale - 1, -verticalDisplacement],
[-horizontalDisplacement + convHullSize/scale - 1, -verticalDisplacement + convHullSize/scale - 1],
[-horizontalDisplacement, -verticalDisplacement + convHullSize/scale - 1]], dtype = "float32")
M = cv2.getPerspectiveTransform(rect, dst)
self.warped.append(cv2.warpPerspective(arIm, M, (self.im_width, self.im_height)))
if self.capture_time == capture_time:
self.qrcodes = None
else:
self.detection_smooth_counter = self.detection_smooth_counter-1
def ResizeWithAspectRatio(self,image, width=None, height=None, inter=cv2.INTER_AREA):
dim = None
(h, w) = image.shape[:2]
if width is None and height is None:
return image
if width is None:
r = height / float(h)
dim = (int(w * r), height)
else:
r = width / float(w)
dim = (width, int(h * r))
return cv2.resize(image, dim, interpolation=inter)
def order_points(self,pts):
# if points are in a list, construct an array
pts = np.array(pts, dtype="float32")
# initialzie a list of coordinates that will be ordered
# such that the first entry in the list is the top-left,
# the second entry is the top-right, the third is the
# bottom-right, and the fourth is the bottom-left
rect = np.zeros((4, 2), dtype = "float32")
# the top-left point will have the smallest sum, whereas
# the bottom-right point will have the largest sum
s = pts.sum(axis = 1)
rect[0] = pts[np.argmin(s)]
rect[2] = pts[np.argmax(s)]
# now, compute the difference between the points, the
# top-right point will have the smallest difference,
# whereas the bottom-left will have the largest difference
diff = np.diff(pts, axis = 1)
rect[1] = pts[np.argmin(diff)]
rect[3] = pts[np.argmax(diff)]
# return the ordered coordinates
return rect
def run(self):
self.tello.connect()
self.tello.set_speed(self.speed)
# In case streaming is on. This happens when we quit this program without the escape key.
self.tello.streamoff()
self.tello.streamon()
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
for event in pygame.event.get():
if event.type == pygame.USEREVENT + 1:
self.update()
elif event.type == pygame.QUIT:
should_stop = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == pygame.KEYUP:
self.keyup(event.key)
if frame_read.stopped:
break
self.screen.fill([0, 0, 0])
frame = frame_read.frame
frame = self.ResizeWithAspectRatio(frame, height=self.im_height)
try:
ret = self.reader.append_video_frame(frame)
except:
pass
self.calculate_warped()
frame = self.decorate_frame(frame)
frame = frame.astype('float32')
text = "Battery: {}%".format(self.tello.get_battery().rstrip())
cv2.putText(frame, text, (5, self.im_height - 5),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.reader.stop_video_mode()
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw counter clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land
not self.tello.land()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity,
self.up_down_velocity, self.yaw_velocity)
def main():
frontend = FrontEnd()
# run frontend
frontend.run()
if __name__ == '__main__':
main()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment