-
Notifications
You must be signed in to change notification settings - Fork 503
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Control the drone using pygame, python 2.7 and 3.6 compatibility
- Loading branch information
1 parent
3dc074a
commit 8c40625
Showing
5 changed files
with
270 additions
and
146 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,28 +1,42 @@ | ||
| import sys | ||
|
|
||
|
|
||
| # Decorator to check method param type, raise needed exception type | ||
| # http://code.activestate.com/recipes/578809-decorator-to-check-method-param-types/ | ||
| def accepts(**types): | ||
| def check_accepts(f): | ||
| argcount = f.func_code.co_argcount | ||
| if 'self' in f.func_code.co_varnames: | ||
| if sys.version_info >= (3, 0): | ||
| fun_code = f.__code__ | ||
| fun_name = f.__name__ | ||
| else: | ||
| fun_code = f.func_code | ||
| fun_name = f.func_name | ||
|
|
||
| argcount = fun_code.co_argcount | ||
| if 'self' in fun_code.co_varnames: | ||
| argcount -= 1 | ||
|
|
||
| s = "accept number of arguments not equal with function number of arguments in ", f.func_name, ", argcount ", argcount | ||
| s = "accept number of arguments not equal with function number of arguments in ", fun_name, ", argcount ", \ | ||
| argcount | ||
| assert len(types) == argcount, s | ||
|
|
||
| def new_f(*args, **kwds): | ||
| for i, v in enumerate(args): | ||
| if types.has_key(f.func_code.co_varnames[i]) and \ | ||
| not isinstance(v, types[f.func_code.co_varnames[i]]): | ||
| raise TypeError("arg '%s'=%r does not match %s" % \ | ||
| (f.func_code.co_varnames[i], v, types[f.func_code.co_varnames[i]])) | ||
| if fun_code.co_varnames[i] in types and \ | ||
| not isinstance(v, types[fun_code.co_varnames[i]]): | ||
| raise TypeError("arg '%s'=%r does not match %s" % (fun_code.co_varnames[i], v, | ||
| types[fun_code.co_varnames[i]])) | ||
|
|
||
| for k, v in kwds.iteritems(): | ||
| if types.has_key(k) and not isinstance(v, types[k]): | ||
| for k, v in kwds.items(): | ||
| if k in types and not isinstance(v, types[k]): | ||
| raise TypeError("arg '%s'=%r does not match %s" % (k, v, types[k])) | ||
|
|
||
| return f(*args, **kwds) | ||
|
|
||
| new_f.func_name = f.func_name | ||
| if sys.version_info >= (3, 0): | ||
| new_f.__name__ = fun_name | ||
| else: | ||
| new_f.func_name = fun_name | ||
| return new_f | ||
|
|
||
| return check_accepts |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,56 +1,158 @@ | ||
| from TelloSDKPy.tello import Tello | ||
| import time | ||
| import cv2 | ||
| import pygame | ||
| from pygame.locals import * | ||
| import numpy as np | ||
| import time | ||
|
|
||
| # Speed of the drone | ||
| S = 60 | ||
| # Frames per second of the pygame window display | ||
| FPS = 25 | ||
|
|
||
|
|
||
| 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(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 == USEREVENT + 1: | ||
| self.update() | ||
| elif event.type == QUIT: | ||
| should_stop = True | ||
| elif event.type == KEYDOWN: | ||
| if event.key == K_ESCAPE: | ||
| should_stop = True | ||
| else: | ||
| self.keydown(event.key) | ||
| elif event.type == 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 = pygame.surfarray.make_surface(frame) | ||
| self.screen.blit(frame, (0, 0)) | ||
| pygame.display.update() | ||
|
|
||
| time.sleep(1 / FPS) | ||
|
|
||
| # Call it always before finishing. I 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 clockwise velocity | ||
| self.yaw_velocity = -S | ||
| elif key == pygame.K_d: # set yaw counter 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) | ||
|
|
||
|
|
||
| def main(): | ||
| frontend = FrontEnd() | ||
|
|
||
| # run frontend | ||
| frontend.run() | ||
|
|
||
|
|
||
| def start(): | ||
| start_time = time.time() | ||
|
|
||
| tello = Tello() | ||
|
|
||
| if not tello.connect(): | ||
| print("%s, Not connected" % (time.time() - start_time)) | ||
| return | ||
|
|
||
| if not tello.streamon(): | ||
| print("%s, Could not start video stream" % (time.time() - start_time)) | ||
| return | ||
|
|
||
| frame_read = tello.get_frame_read() | ||
|
|
||
| while True: | ||
| if frame_read.stopped: | ||
| frame_read.stop() | ||
| break | ||
|
|
||
| cv2.imshow('Augmented reality', frame_read.frame) | ||
|
|
||
| ch = cv2.waitKey(1) | ||
| if ch == ord('t'): | ||
| tello.takeoff() | ||
| if ch == ord('l'): | ||
| tello.land() | ||
| if ch == ord('a'): | ||
| tello.move_left(20) | ||
| if ch == ord('d'): | ||
| tello.move_right(20) | ||
| if ch == ord('w'): | ||
| tello.move_forward(20) | ||
| if ch == ord('d'): | ||
| tello.move_back(20) | ||
| if ch == ord('e'): | ||
| tello.rotate_counter_clockwise(450) | ||
| if ch == ord('r'): | ||
| tello.rotate_clockwise(450) | ||
| if ch == ord('f'): | ||
| tello.move_up(20) | ||
| if ch == ord('g'): | ||
| tello.move_down(20) | ||
| if ch == 27: | ||
| frame_read.stopped = True | ||
| break | ||
|
|
||
| tello.end() | ||
|
|
||
|
|
||
| start() | ||
| if __name__ == '__main__': | ||
| main() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,3 @@ | ||
| numpy==1.15.4 | ||
| opencv-python==3.4.3.18 | ||
| pygame==1.9.4 |
Oops, something went wrong.