-
Notifications
You must be signed in to change notification settings - Fork 0
/
TDP_tello.py
179 lines (148 loc) · 6.3 KB
/
TDP_tello.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
from djitellopy import Tello
import cv2
import mediapipe as mp
import os
import time
import threading
# Suprime avisos do TensorFlow Lite
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
# Inicializa o MediaPipe para reconhecimento de mãos
mp_hands = mp.solutions.hands
hands = mp_hands.Hands(min_detection_confidence=0.7)
mp_draw = mp.solutions.drawing_utils
# Função para conectar ao drone Tello com tentativas de reconexão
def connect_tello():
tello = Tello()
while True:
try:
tello.connect()
tello.streamon() # Ativa o stream de vídeo
print("Conectado ao Tello")
return tello
except Exception as e:
print(f"Erro ao conectar ao Tello: {e}")
print("Tentando reconectar em 5 segundos...")
time.sleep(5)
# Conecta ao drone Tello
tello = connect_tello()
print("Decolar")
tello.takeoff()
tello.move_up(40)
# Variável para verificar se o comando terminou
command_is_over = True
current_gesture = None
# Função para detectar gestos
def detect_gesture(landmarks):
if landmarks:
thumb_tip = landmarks[4]
index_tip = landmarks[8]
middle_tip = landmarks[12]
ring_tip = landmarks[16]
pinky_tip = landmarks[20]
# Detecta o gesto de subir (indicador levantado)
if index_tip.y < thumb_tip.y and middle_tip.y > thumb_tip.y and ring_tip.y > thumb_tip.y and pinky_tip.y > thumb_tip.y:
return "move_up"
# Detecta o gesto de descer (indicador e médio levantados)
if index_tip.y < thumb_tip.y and middle_tip.y < thumb_tip.y and ring_tip.y > thumb_tip.y and pinky_tip.y > thumb_tip.y:
return "move_down"
# Detecta o gesto para girar para a esquerda (indicador, médio e anelar levantados)
if index_tip.y < thumb_tip.y and middle_tip.y < thumb_tip.y and ring_tip.y < thumb_tip.y and pinky_tip.y > thumb_tip.y:
return "rotate_left"
# Detecta o gesto para girar para a direita (indicador, médio, anelar e mindinho levantados)
if index_tip.y < thumb_tip.y and middle_tip.y < thumb_tip.y and ring_tip.y < thumb_tip.y and pinky_tip.y < thumb_tip.y:
return "rotate_right"
# Detecta o gesto de pouso (todos os dedos abaixados)
if index_tip.y > thumb_tip.y and middle_tip.y > thumb_tip.y and ring_tip.y > thumb_tip.y and pinky_tip.y > thumb_tip.y:
return "land"
# Detecta o gesto de mover para frente (indicador e mindinho levantados)
if index_tip.y < thumb_tip.y and middle_tip.y > thumb_tip.y and ring_tip.y > thumb_tip.y and pinky_tip.y < thumb_tip.y:
return "move_forward"
# Detecta o gesto de mover para trás (thumb longe do indicador no eixo x)
if abs(thumb_tip.x - index_tip.x) > 0.2: # Ajuste o valor conforme necessário
return "move_backward"
# Detecta o gesto de pousar definitivamente (apenas o mindinho levantado)
if index_tip.y > thumb_tip.y and middle_tip.y > thumb_tip.y and ring_tip.y > thumb_tip.y and pinky_tip.y < thumb_tip.y:
return "land and takeoff"
return None
# Funções de controle do Tello
def move_up():
print("Subir")
response = tello.send_command_with_return("up 20")
print(f"Resposta do drone: {response}")
return response
def move_down():
print("Descer")
response = tello.send_command_with_return("down 20")
print(f"Resposta do drone: {response}")
return response
def rotate_left():
print("Rotacionar para a esquerda")
response = tello.send_command_with_return("ccw 90")
print(f"Resposta do drone: {response}")
return response
def rotate_right():
print("Rotacionar para a direita")
response = tello.send_command_with_return("cw 90")
print(f"Resposta do drone: {response}")
return response
def move_forward():
print("Mover para frente")
response = tello.send_command_with_return("forward 40")
print(f"Resposta do drone: {response}")
return response
def move_backward():
print("Mover para trás")
response = tello.send_command_with_return("back 40")
print(f"Resposta do drone: {response}")
return response
def land():
print("Pousar")
response = tello.send_command_with_return("land")
print(f"Resposta do drone: {response}")
return response
def execute_command(gesture):
global command_is_over
if gesture == "move_up":
command_is_over = move_up() == "ok"
elif gesture == "move_down":
command_is_over = move_down() == "ok"
elif gesture == "rotate_left":
command_is_over = rotate_left() == "ok"
elif gesture == "rotate_right":
command_is_over = rotate_right() == "ok"
elif gesture == "move_forward":
command_is_over = move_forward() == "ok"
elif gesture == "move_backward":
command_is_over = move_backward() == "ok"
elif gesture == "land and takeoff":
command_is_over = land() == "ok"
time.sleep(5)
tello.takeoff()
tello.send_command_without_return('up 60')
elif gesture == "land":
command_is_over = land() == "ok"
# Loop principal para capturar o vídeo do Tello
while True:
frame = tello.get_frame_read().frame
img_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
results = hands.process(img_rgb)
if results.multi_hand_landmarks:
for hand_landmarks in results.multi_hand_landmarks:
mp_draw.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS)
gesture = detect_gesture(hand_landmarks.landmark)
if gesture and command_is_over:
print(f"Ação detectada: {gesture}")
cv2.putText(frame, f'Status: {gesture}', (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
command_is_over = False # Bloqueia novos comandos até o comando atual ser concluído
current_gesture = gesture
threading.Thread(target=execute_command, args=(gesture,)).start()
battery = tello.get_battery()
print("Bateria:", battery, "%")
cv2.putText(frame, f'Bateria: {battery}%', (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
cv2.imshow("Image", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
tello.streamoff()
cv2.destroyAllWindows()
# Desconecta o drone Tello
tello.end()