Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
T
telloCodeHunt
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
GitLab community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
uavVisionLab
telloCodeHunt
Commits
00b6f186
Commit
00b6f186
authored
Nov 26, 2020
by
Zsedrovits Tamás
Browse files
Options
Downloads
Patches
Plain Diff
Move tello in steps
parent
81d3c85d
No related branches found
No related tags found
No related merge requests found
Changes
1
Show whitespace changes
Inline
Side-by-side
Showing
1 changed file
tello_code_hunt_pygame_manual_control.py
+123
-49
123 additions, 49 deletions
tello_code_hunt_pygame_manual_control.py
with
123 additions
and
49 deletions
tello_code_hunt_pygame_manual_control.py
+
123
−
49
View file @
00b6f186
...
@@ -5,6 +5,8 @@
...
@@ -5,6 +5,8 @@
To control the drone and show the video, it uses pygame.
To control the drone and show the video, it uses pygame.
"""
"""
import
time
import
time
import
queue
import
threading
import
cv2
import
cv2
import
pygame
import
pygame
import
numpy
as
np
import
numpy
as
np
...
@@ -12,7 +14,9 @@ from djitellopy import Tello
...
@@ -12,7 +14,9 @@ from djitellopy import Tello
from
dbr
import
*
from
dbr
import
*
# Speed of the drone
# Speed of the drone
S
=
60
S
=
100
# Step size
s_S
=
100
# Frames per second of the pygame window display
# Frames per second of the pygame window display
# A low number also results in input lag, as input information is processed once per frame.
# A low number also results in input lag, as input information is processed once per frame.
FPS
=
120
FPS
=
120
...
@@ -28,6 +32,58 @@ class FrontEnd():
...
@@ -28,6 +32,58 @@ class FrontEnd():
- W and S: Up and down.
- W and S: Up and down.
"""
"""
class
CommandThread
(
threading
.
Thread
):
"""
Class tu send commands to Tello.
"""
def
__init__
(
self
,
command_queue
,
tello
):
threading
.
Thread
.
__init__
(
self
)
self
.
commands_to_run
=
command_queue
self
.
tello
=
tello
self
.
is_done
=
False
def
run
(
self
):
"""
Main thread.
"""
while
True
:
command
=
self
.
commands_to_run
.
get
(
block
=
True
,
timeout
=
None
)
if
command
==
"
e
"
:
# forward
success
=
self
.
tello
.
move_forward
(
s_S
)
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
move_forward
(
s_S
)
elif
command
==
"
h
"
:
# back
success
=
self
.
tello
.
move_back
(
s_S
)
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
move_back
(
s_S
)
elif
command
==
"
b
"
:
# left
success
=
self
.
tello
.
move_left
(
s_S
)
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
move_left
(
s_S
)
elif
command
==
"
j
"
:
# right
success
=
self
.
tello
.
move_right
(
s_S
)
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
move_right
(
s_S
)
elif
command
==
"
f
"
:
# up
success
=
self
.
tello
.
move_up
(
int
(
s_S
/
4
))
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
move_up
(
int
(
s_S
/
4
))
elif
command
==
"
l
"
:
# down
success
=
self
.
tello
.
move_down
(
int
(
s_S
/
4
))
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
move_down
(
int
(
s_S
/
4
))
elif
command
==
"
ccw
"
:
# turn left
success
=
self
.
tello
.
rotate_counter_clockwise
(
15
)
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
rotate_counter_clockwise
(
15
)
elif
command
==
"
cw
"
:
# turn right
success
=
self
.
tello
.
rotate_clockwise
(
15
)
while
not
success
:
time
.
sleep
(
1
)
success
=
self
.
tello
.
rotate_clockwise
(
15
)
def
__init__
(
self
):
def
__init__
(
self
):
# Init pygame
# Init pygame
pygame
.
init
()
pygame
.
init
()
...
@@ -43,13 +99,7 @@ class FrontEnd():
...
@@ -43,13 +99,7 @@ class FrontEnd():
self
.
tello
=
Tello
()
self
.
tello
=
Tello
()
# Drone velocities between -100~100
# Drone velocities between -100~100
self
.
for_back_velocity
=
0
self
.
speed
=
S
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
# create update timer
pygame
.
time
.
set_timer
(
pygame
.
USEREVENT
+
1
,
1000
//
FPS
)
pygame
.
time
.
set_timer
(
pygame
.
USEREVENT
+
1
,
1000
//
FPS
)
...
@@ -94,6 +144,13 @@ class FrontEnd():
...
@@ -94,6 +144,13 @@ class FrontEnd():
# store AR images for overlay to decrease flashing
# store AR images for overlay to decrease flashing
self
.
warped
=
[]
self
.
warped
=
[]
# queue to store commands
self
.
command_queue
=
queue
.
Queue
(
0
)
# thread to send commands
self
.
command_thread
=
self
.
CommandThread
(
self
.
command_queue
,
self
.
tello
)
self
.
command_thread
.
start
()
def
on_barcode_result
(
self
,
data
):
def
on_barcode_result
(
self
,
data
):
"""
The callback function for receiving barcode results.
"""
"""
The callback function for receiving barcode results.
"""
self
.
qrcodes
=
data
self
.
qrcodes
=
data
...
@@ -251,15 +308,15 @@ class FrontEnd():
...
@@ -251,15 +308,15 @@ class FrontEnd():
self
.
tello
.
streamoff
()
self
.
tello
.
streamoff
()
self
.
tello
.
streamon
()
self
.
tello
.
streamon
()
self
.
tello
.
send_control_command
(
"
mon
"
)
frame_read
=
self
.
tello
.
get_frame_read
()
frame_read
=
self
.
tello
.
get_frame_read
()
should_stop
=
False
should_stop
=
False
while
not
should_stop
:
while
not
should_stop
:
for
event
in
pygame
.
event
.
get
():
for
event
in
pygame
.
event
.
get
():
if
event
.
type
==
pygame
.
USEREVENT
+
1
:
if
event
.
type
==
pygame
.
QUIT
:
self
.
update
()
elif
event
.
type
==
pygame
.
QUIT
:
should_stop
=
True
should_stop
=
True
elif
event
.
type
==
pygame
.
KEYDOWN
:
elif
event
.
type
==
pygame
.
KEYDOWN
:
if
event
.
key
==
pygame
.
K_ESCAPE
:
if
event
.
key
==
pygame
.
K_ESCAPE
:
...
@@ -285,9 +342,9 @@ class FrontEnd():
...
@@ -285,9 +342,9 @@ class FrontEnd():
self
.
calculate_warped
()
self
.
calculate_warped
()
frame
=
self
.
decorate_frame
(
frame
)
frame
=
self
.
decorate_frame
(
frame
)
frame
=
frame
.
astype
(
'
float32
'
)
frame
=
frame
.
astype
(
'
float32
'
)
text
=
"
Battery: {}%
"
.
format
(
self
.
tello
.
get_battery
().
rstrip
())
#
text = "Battery: {}%".format(self.tello.get_battery()
) #
.rstrip())
cv2
.
putText
(
frame
,
text
,
(
5
,
self
.
im_height
-
5
),
#
cv2.putText(frame, text, (5, self.im_height - 5),
cv2
.
FONT_HERSHEY_SIMPLEX
,
1
,
(
0
,
0
,
255
),
2
)
#
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
frame
=
cv2
.
cvtColor
(
frame
,
cv2
.
COLOR_BGR2RGB
)
frame
=
cv2
.
cvtColor
(
frame
,
cv2
.
COLOR_BGR2RGB
)
frame
=
np
.
rot90
(
frame
)
frame
=
np
.
rot90
(
frame
)
frame
=
np
.
flipud
(
frame
)
frame
=
np
.
flipud
(
frame
)
...
@@ -307,49 +364,66 @@ class FrontEnd():
...
@@ -307,49 +364,66 @@ class FrontEnd():
Arguments:
Arguments:
key: pygame key
key: pygame key
"""
"""
if
key
==
pygame
.
K_UP
:
# set forward velocity
# if key == pygame.K_UP: # set forward velocity
self
.
for_back_velocity
=
S
# self.for_back_velocity = S
elif
key
==
pygame
.
K_DOWN
:
# set backward velocity
# elif key == pygame.K_DOWN: # set backward velocity
self
.
for_back_velocity
=
-
S
# self.for_back_velocity = -S
elif
key
==
pygame
.
K_LEFT
:
# set left velocity
# elif key == pygame.K_LEFT: # set left velocity
self
.
left_right_velocity
=
-
S
# self.left_right_velocity = -S
elif
key
==
pygame
.
K_RIGHT
:
# set right velocity
# elif key == pygame.K_RIGHT: # set right velocity
self
.
left_right_velocity
=
S
# self.left_right_velocity = S
elif
key
==
pygame
.
K_w
:
# set up velocity
# elif key == pygame.K_w: # set up velocity
self
.
up_down_velocity
=
S
# self.up_down_velocity = S
elif
key
==
pygame
.
K_s
:
# set down velocity
# elif key == pygame.K_s: # set down velocity
self
.
up_down_velocity
=
-
S
# self.up_down_velocity = -S
elif
key
==
pygame
.
K_a
:
# set yaw counter clockwise velocity
# elif key == pygame.K_a: # set yaw counter clockwise velocity
self
.
yaw_velocity
=
-
S
# self.yaw_velocity = -S
elif
key
==
pygame
.
K_d
:
# set yaw clockwise velocity
# elif key == pygame.K_d: # set yaw clockwise velocity
self
.
yaw_velocity
=
S
# self.yaw_velocity = S
def
keyup
(
self
,
key
):
def
keyup
(
self
,
key
):
"""
Update velocities based on key released
"""
Update velocities based on key released
Arguments:
Arguments:
key: pygame key
key: pygame key
"""
"""
if
key
==
pygame
.
K_UP
or
key
==
pygame
.
K_DOWN
:
# set zero forward/backward velocity
# if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self
.
for_back_velocity
=
0
# self.for_back_velocity = 0
elif
key
==
pygame
.
K_LEFT
or
key
==
pygame
.
K_RIGHT
:
# set zero left/right velocity
# elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self
.
left_right_velocity
=
0
# self.left_right_velocity = 0
elif
key
==
pygame
.
K_w
or
key
==
pygame
.
K_s
:
# set zero up/down velocity
# elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self
.
up_down_velocity
=
0
# self.up_down_velocity = 0
elif
key
==
pygame
.
K_a
or
key
==
pygame
.
K_d
:
# set zero yaw velocity
# elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self
.
yaw_velocity
=
0
# 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
if
key
==
pygame
.
K_UP
:
# forward
self
.
command_queue
.
put
(
"
e
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_DOWN
:
# back
self
.
command_queue
.
put
(
"
h
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_LEFT
:
# left
self
.
command_queue
.
put
(
"
b
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_RIGHT
:
# right
self
.
command_queue
.
put
(
"
j
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_w
:
# up
self
.
command_queue
.
put
(
"
f
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_s
:
# down
self
.
command_queue
.
put
(
"
l
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_d
:
# turn right
self
.
command_queue
.
put
(
"
cw
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_a
:
# turn left
self
.
command_queue
.
put
(
"
ccw
"
,
block
=
True
,
timeout
=
None
)
elif
key
==
pygame
.
K_t
:
# takeoff
elif
key
==
pygame
.
K_t
:
# takeoff
self
.
tello
.
takeoff
()
self
.
tello
.
takeoff
()
self
.
send_rc_control
=
True
elif
key
==
pygame
.
K_l
:
# land
elif
key
==
pygame
.
K_l
:
# land
not
self
.
tello
.
land
()
self
.
tello
.
land
()
self
.
send_rc_control
=
False
elif
key
==
pygame
.
K_e
:
# emergency land
self
.
tello
.
emergency
()
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
():
def
main
():
"""
Main class to create and start the frontend.
"""
"""
Main class to create and start the frontend.
"""
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment