class FrontEnd(object):
def __init__(self):
# Init pygame
pygame.init()
# Create pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# 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, 50)
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
# TELLO EVENT REGION
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:
frame_read.stop()
break
self.updateOnFaces(frame_read)
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
# HAND TRACKING
pygame.display.update()
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
def updateOnFaces(self, frame_read):
gray = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2GRAY)
faces = faceCascade.detectMultiScale(
gray,
scaleFactor=1.1,
minNeighbors=5,
minSize=(30, 30),
)
# Draw a rectangle around the faces
index = 0
for (x, y, w, h) in faces:
if index != 0:
return
if x < 960 / 2:
image = cv2.putText(frame_read.frame, 'LEFT', org, font,
fontScale, color, thickness, cv2.LINE_AA)
self.left_right_velocity = -S
elif x > 960 / 2:
image = cv2.putText(frame_read.frame, 'RIGHT', org, font,
fontScale, color, thickness, cv2.LINE_AA)
self.left_right_velocity = S
if y > (720 / 2) - h:
image = cv2.putText(frame_read.frame, 'DOWN', (900, 50), font,
fontScale, color, thickness, cv2.LINE_AA)
self.up_down_velocity = -S
elif y < 720 / 2:
image = cv2.putText(frame_read.frame, 'UP', (900, 50), font,
fontScale, color, thickness, cv2.LINE_AA)
self.up_down_velocity = S
if (w * h) < faceWidth * faceHeight:
image = cv2.putText(frame_read.frame, 'FORWARD', (50, 100),
font, fontScale, color, thickness,
cv2.LINE_AA)
self.for_back_velocity = S
elif (w * h) > faceWidth * faceHeight:
image = cv2.putText(frame_read.frame, 'BACKWARD', (50, 100),
font, fontScale, color, thickness,
cv2.LINE_AA)
self.for_back_velocity = -S
cv2.rectangle(frame_read.frame, (faceX, faceY),
(faceX + faceWidth, faceY + faceHeight), (0, 0, 255),
2)
cv2.rectangle(frame_read.frame, (x, y), (x + w, y + h),
(0, 255, 0), 2)
index += 1
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
"""
keys = pygame.key.get_pressed()
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
self.tello.land()
self.send_rc_control = False
elif key == pygame.K_f:
self.tello.flip_back()
elif key == pygame.K_g:
self.tello.flip_forward()
elif key == pygame.K_h:
self.tello.flip_right()
elif key == pygame.K_j:
self.tello.flip_left()
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)
class FrontEnd(object):
def __init__(self):
# 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
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
imgCount = 0
OVERRIDE = False
oSpeed = args.override_speed
tDistance = args.distance
self.tello.get_battery()
# Safety Zone X
szX = args.saftey_x
# Safety Zone Y
szY = args.saftey_y
if args.debug:
print("DEBUG MODE ENABLED!")
while not should_stop:
self.update()
if frame_read.stopped:
frame_read.stop()
break
theTime = str(datetime.datetime.now()).replace(':', '-').replace(
'.', '_')
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frameRet = frame_read.frame
vid = self.tello.get_video_capture()
if args.save_session:
cv2.imwrite("{}/tellocap{}.jpg".format(ddir, imgCount),
frameRet)
frame = np.rot90(frame)
imgCount += 1
time.sleep(1 / FPS)
# Listen for key presses
k = cv2.waitKey(20)
# Press 0 to set distance to 0
if k == ord('0'):
if not OVERRIDE:
print("Distance = 0")
tDistance = 0
# Press 1 to set distance to 1
if k == ord('1'):
if OVERRIDE:
oSpeed = 1
else:
print("Distance = 1")
tDistance = 1
# Press 2 to set distance to 2
if k == ord('2'):
if OVERRIDE:
oSpeed = 2
else:
print("Distance = 2")
tDistance = 2
# Press 3 to set distance to 3
if k == ord('3'):
if OVERRIDE:
oSpeed = 3
else:
print("Distance = 3")
tDistance = 3
# Press 4 to set distance to 4
if k == ord('4'):
if not OVERRIDE:
print("Distance = 4")
tDistance = 4
# Press 5 to set distance to 5
if k == ord('5'):
if not OVERRIDE:
print("Distance = 5")
tDistance = 5
# Press 6 to set distance to 6
if k == ord('6'):
if not OVERRIDE:
print("Distance = 6")
tDistance = 6
# Press T to take off
if k == ord('t'):
if not args.debug:
print("Taking Off")
self.tello.takeoff()
self.tello.get_battery()
self.send_rc_control = True
# Press L to land
if k == ord('l'):
if not args.debug:
print("Landing")
self.tello.land()
self.send_rc_control = False
# Press Backspace for controls override
if k == 8:
if not OVERRIDE:
OVERRIDE = True
print("OVERRIDE ENABLED")
else:
OVERRIDE = False
print("OVERRIDE DISABLED")
if OVERRIDE:
# S & W to fly forward & back
if k == ord('w'):
self.for_back_velocity = int(S * oSpeed)
elif k == ord('s'):
self.for_back_velocity = -int(S * oSpeed)
else:
self.for_back_velocity = 0
# a & d to pan left & right
if k == ord('d'):
self.yaw_velocity = int(S * oSpeed)
elif k == ord('a'):
self.yaw_velocity = -int(S * oSpeed)
else:
self.yaw_velocity = 0
# Q & E to fly up & down
if k == ord('e'):
self.up_down_velocity = int(S * oSpeed)
elif k == ord('q'):
self.up_down_velocity = -int(S * oSpeed)
else:
self.up_down_velocity = 0
# c & z to fly left & right
if k == ord('c'):
self.left_right_velocity = int(S * oSpeed)
elif k == ord('z'):
self.left_right_velocity = -int(S * oSpeed)
else:
self.left_right_velocity = 0
# Quit the software
if k == 27:
should_stop = True
break
gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray,
scaleFactor=1.5,
minNeighbors=2)
# Target size
tSize = faceSizes[tDistance]
# These are our center dimensions
cWidth = int(dimensions[0] / 2)
cHeight = int(dimensions[1] / 2)
noFaces = len(faces) == 0
# if we've given rc controls & get face coords returned
if self.send_rc_control and not OVERRIDE:
for (x, y, w, h) in faces:
#
roi_gray = gray[y:y + h,
x:x + w] # (ycord_start, ycord_end)
roi_color = frameRet[y:y + h, x:x + w]
# setting Face Box properties
fbCol = (255, 0, 0) # BGR 0-255
fbStroke = 2
# end coords are the end of the bounding box x & y
end_cord_x = x + w
end_cord_y = y + h
end_size = w * 2
# these are our target coordinates
targ_cord_x = int((end_cord_x + x) / 2)
targ_cord_y = int((end_cord_y + y) / 2) + UDOffset
# This calculates the vector from your face to the center of the screen
vTrue = np.array((cWidth, cHeight, tSize))
vTarget = np.array((targ_cord_x, targ_cord_y, end_size))
vDistance = vTrue - vTarget
#
if not args.debug:
# for turning
if vDistance[0] < -szX:
self.yaw_velocity = S
# self.left_right_velocity = S2
elif vDistance[0] > szX:
self.yaw_velocity = -S
# self.left_right_velocity = -S2
else:
self.yaw_velocity = 0
# for up & down
if vDistance[1] > szY:
self.up_down_velocity = S
elif vDistance[1] < -szY:
self.up_down_velocity = -S
else:
self.up_down_velocity = 0
F = 0
if abs(vDistance[2]) > acc[tDistance]:
F = S
# for forward back
if vDistance[2] > 0:
self.for_back_velocity = S + F
elif vDistance[2] < 0:
self.for_back_velocity = -S - F
else:
self.for_back_velocity = 0
# Draw the face bounding box
cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y),
fbCol, fbStroke)
# Draw the target as a circle
cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10,
(0, 255, 0), 2)
# Draw the safety zone
cv2.rectangle(frameRet,
(targ_cord_x - szX, targ_cord_y - szY),
(targ_cord_x + szX, targ_cord_y + szY),
(0, 255, 0), fbStroke)
# Draw the estimated drone vector position in relation to face bounding box
cv2.putText(frameRet, str(vDistance), (0, 64),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255),
2)
# if there are no faces detected, don't do anything
if noFaces:
self.yaw_velocity = 0
self.up_down_velocity = 0
self.for_back_velocity = 0
print("NO TARGET")
# Draw the center of screen circle, this is what the drone tries to match with the target coords
cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2)
dCol = lerp(np.array((0, 0, 255)), np.array((255, 255, 255)),
tDistance + 1 / 7)
if OVERRIDE:
show = "OVERRIDE: {}".format(oSpeed)
dCol = (255, 255, 255)
else:
show = "AI: {}".format(str(tDistance))
# Draw the distance choosen
cv2.putText(frameRet, show, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1,
dCol, 2)
# Display the resulting frame
cv2.imshow(f'Tello Tracking...', frameRet)
# On exit, print the battery
self.tello.get_battery()
# When everything done, release the capture
cv2.destroyAllWindows()
# Call it always before finishing. I deallocate resources.
self.tello.end()
def battery(self):
return self.tello.get_battery()[:2]
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)
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
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# 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, 50)
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
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:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.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.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
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)
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
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
# Center of screen - 480,360
self.screen = pygame.display.set_mode([960, 720])
# 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, 50)
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
face_detected = False
should_stop = False
#initialise tracker type
tracker = cv2.TrackerKCF_create()
framespersecond = 0
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:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
originalframe = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
if face_detected == False:
self.faces = []
#------------FACIAL RECOGNITION-----------------------
if framespersecond % 10 == 0:
self.faceCascade = cv2.CascadeClassifier(
"haarcascade_frontalface_default.xml")
self.gray = cv2.cvtColor(originalframe, cv2.COLOR_BGR2GRAY)
self.faces = self.faceCascade.detectMultiScale(
self.gray,
scaleFactor=1.1,
minNeighbors=5,
minSize=(30, 30),
flags=cv2.CASCADE_SCALE_IMAGE)
#self.faces returns the faces it detects in a nested list [[face1],[face2]]
#each list contains[x-axis,y-axis,width,height]
#-----------------------------------------------------
frame = np.rot90(originalframe)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
if len(self.faces) > 0:
print("face detected")
face_detected = True
#-----------------DRAW BOUNDINGBOX ON FACE------------
self.colorofbox = pygame.Color(0, 255, 0)
for item in self.faces:
pygame.draw.rect(
self.screen, self.colorofbox,
pygame.Rect(item[0], item[1], item[2], item[3]), 2)
#select the closest face to track
self.selectedface = self.faces[0]
for item in self.faces:
if (item[2] * item[3]) > (self.selectedface[2] *
self.selectedface[3]):
self.selectedface = item
ok = tracker.init(
originalframe,
(self.selectedface[0], self.selectedface[1],
self.selectedface[2], self.selectedface[3]))
#-----------------------------------------------------
pygame.display.update()
else:
print("face tracking")
self.colorofbox = pygame.Color(255, 0, 0)
#updates the tracker with current frame, ok determines if tracking is successful, bbox is the bounding box
ok, bbox = tracker.update(originalframe)
#----CONTROL THE DRONE BASED ON THE BOUNDING BOX------
if ok:
frame = np.rot90(originalframe)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.draw.rect(
self.screen, self.colorofbox,
pygame.Rect(bbox[0], bbox[1], bbox[2], bbox[3]), 2)
self.centeredaxis = [
bbox[0] + (bbox[2] // 2), bbox[1] + (bbox[3] // 2)
]
pygame.display.update()
if self.centeredaxis[0] < 480:
print("left")
self.yawleft()
self.update()
elif self.centeredaxis[0] > 480:
print("right")
self.yawright()
self.update()
else:
print("face lost")
face_detected = False
#-----------------------------------------------------
if framespersecond == 30:
framespersecond = 0
else:
framespersecond += 1
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
def forward(self):
self.for_back_velocity = S
def backward(self):
self.for_back_velocity = -S
def left(self):
self.left_right_velocity = -S
def right(self):
self.left_right_velocity = S
def up(self):
self.up_down_velocity = S
def down(self):
self.up_down_velocity = -S
def yawleft(self):
self.yaw_velocity = -S
def yawright(self):
self.yaw_velocity = S
def takeoff(self):
self.tello.takeoff()
self.send_rc_control = True
def land(self):
self.tello.land()
self.send_rc_control = False
def emergencystop(self):
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
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
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)