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
8692df68
Commit
8692df68
authored
Nov 25, 2020
by
Zsedrovits Tamás
Browse files
Options
Downloads
Patches
Plain Diff
Initial version
parents
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
manual-control-pygame.py
+331
-0
331 additions, 0 deletions
manual-control-pygame.py
with
331 additions
and
0 deletions
manual-control-pygame.py
0 → 100644
+
331
−
0
View file @
8692df68
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
()
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