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)
#elaborazione del frame per riconoscere oggetti
#impartire comandi al drone sulla base del riconoscimento dell'ogg
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
elif key == pygame.K_b: # flip_back
self.tello.flip_back()
self.send_rc_control = True
elif key == pygame.K_f: # flip_forward
self.tello.flip_forward()
self.send_rc_control = True
elif key == pygame.K_p: # battery
self.tello.get_battery()
self.send_rc_control = True
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)