diff --git a/.github/worklflows/ci.yml b/.github/worklflows/ci.yml new file mode 100644 index 0000000..5a3075c --- /dev/null +++ b/.github/worklflows/ci.yml @@ -0,0 +1,30 @@ +name: TelloZune CI + +on: + push: + branches: [ "main", "master" ] + pull_request: + branches: [ "main", "master" ] + +jobs: + test: + runs-on: ubuntu-latest + + steps: + - name: Checkout do código + uses: actions/checkout@v4 + + - name: Configurar o Python + uses: actions/setup-python@v5 + with: + python-version: "3.14" + + - name: Instalar dependências + run: | + python -m pip install --upgrade pip + + pip install -r requirements.txt + + - name: Rodar testes unitários (unittest) + run: | + python -m unittest discover -s test -v \ No newline at end of file diff --git a/README.md b/README.md index 185fa31..6d36c66 100644 --- a/README.md +++ b/README.md @@ -1,27 +1,58 @@ -# tello_zune -Library used by the Zune Drones team to control the DJI Tello drone. In the examples folder, there are some simple usage examples of the tello_zune module, which is also present in the folder. +# Tello Zune (v0.7.7) +Uma biblioteca Python orientada a eventos thread-safe para o controle, leitura de telemetria e streaming de vídeo do drone DJI Tello. -To create the library, adaptations were made from two other existing libraries: -* [fvilmos](https://github.com/fvilmos/tello_object_tracking) -* [damiafuentes](https://github.com/damiafuentes/DJITelloPy) +A Tello Zune gerencia filas de comandos assíncronas, faz monitoramento contínuo de estado em background e bloqueios de segurança para garantir que o seu drone responda de forma previsível e estável, mesmo durante a execução de rotas complexas. -# Requeriments +## Funcionalidades Principais -* Python >= 3.9.0 +* **Comunicação Thread-Safe:** Uso de `Locks` e `Queues` para evitar colisões de pacotes UDP. +* **Telemetria Contínua:** Leitura de sensores (bateria, altura, velocidade, temperatura) passiva pela porta 8890, sem bloquear o envio de comandos de voo. +* **Streaming de Vídeo Integrado:** Captura e redimensionamento de frames em tempo real usando OpenCV. +* **Rotas e Eventos Periódicos:** Capacidade de programar sequências de movimentos (ex: patrulhas) com intervalos de tempo precisos usando o comando customizado `delay`. +* **Modo Interativo de Terminal:** Controle o drone enviando comandos de texto direto pelo console enquanto o script de vídeo roda em paralelo. +* **Parada de Emergência:** Limpeza instantânea da fila de comandos e corte de motores com o método `emergency_stop()`. -```bash -pip install numpy -``` +## Pré-requisitos e Instalação +Certifique-se de ter o Python 3 instalado. As dependências principais da biblioteca estão no requirements.txt. + +Instale a Tello Zune: ```bash -pip install opencv-python +pip install tello-zune ``` -# How to use +Conecte o seu computador à rede Wi-Fi do drone Tello antes de executar os scripts. + +--- + +## Como Usar (Exemplos) + +Na pasta `examples/` você encontra scripts prontos para testar as capacidades da Tello Zune. + +### 1. Controle por Terminal de Texto (`text_commands.py`) +Este exemplo inicia a câmera do Tello com um HUD de telemetria na tela. Ao mesmo tempo, ele habilita o terminal para que você digite comandos de voo (como `takeoff`, `forward 50`, `cw 90`) manualmente. -To use the library, install with: +### 2. Rotas e Comandos Periódicos (`periodic_commands.py`) +Neste exemplo, o drone é programado para executar uma rota periódica de "Vigilância". Ele se moverá para frente 50cm e girará 90 graus a cada intervalo de tempo determinado, operando de forma autônoma. + +--- + +## Métodos Principais da API + +Aqui estão algumas das funções mais úteis para controlar o Tello programaticamente: + +* `add_command(cmd: str)`: Enfileira um comando oficial do SDK do Tello (ex: `up 50`, `flip b`) para ser executado de forma segura na próxima janela disponível. +* `get_speed() -> tuple`: Retorna a velocidade atual em tempo real nos eixos X, Y e Z `(vx, vy, vz)` em cm/s. +* `get_battery() -> int`: Retorna a porcentagem atual da bateria (0-100). +* `emergency_stop()`: Esvazia a fila de comandos imediatamente e corta os motores do drone. Ideal para evitar colisões iminentes. +* `clear_command_queue()`: Limpa todos os comandos pendentes na fila sem derrubar o drone. +* `end_tello()`: Pousa o drone com retentativas automáticas em caso de falha na rede, encerra o streaming de vídeo e fecha os sockets de comunicação corretamente. + +## Estrutura e Testes + +A biblioteca possui cobertura de testes unitários para garantir a estabilidade das funções de rede e de concorrência. Para executar os testes na raiz do projeto, utilize: ```bash -pip install tello-zune -``` +python -m unittest discover -s test -v +``` \ No newline at end of file diff --git a/test/main.py b/examples/periodic_commands.py similarity index 76% rename from test/main.py rename to examples/periodic_commands.py index f89e4ba..4d00bae 100644 --- a/test/main.py +++ b/examples/periodic_commands.py @@ -1,18 +1,7 @@ import cv2 -import os -import sys - -current_dir = os.path.dirname(os.path.abspath(__file__)) - -project_root = os.path.dirname(current_dir) - -sys.path.insert(0, project_root) from tello_zune.tello_zune import TelloZune - -cap = cv2.VideoCapture(0) # Captura de vídeo da webcam -# tello.start_communication() # Este método deve ser chamado apenas se for usar a câmera do drone tello = TelloZune() # Cria objeto da classe TelloZune tello.start_tello() # Inicia a comunicação com o drone tello.add_periodic_event("forward 50 e cw 90", 100, "Vigilância", 10) # Adiciona evento periódico @@ -20,7 +9,6 @@ try: while True: # Captura - # ret, frame = cap.read() # Captura de vídeo da webcam frame = tello.get_frame() # Tratamento @@ -39,6 +27,5 @@ break finally: # Finalização - #cap.release() tello.end_tello() cv2.destroyAllWindows() diff --git a/examples/text_commands.py b/examples/text_commands.py new file mode 100644 index 0000000..543b544 --- /dev/null +++ b/examples/text_commands.py @@ -0,0 +1,31 @@ +import cv2 + +from tello_zune.tello_zune import TelloZune + +tello = TelloZune() # Cria objeto da classe TelloZune +tello.start_tello() # Inicia a comunicação com o drone +tello.enable_text_input = True # Habilita o input de texto para enviar comandos manualmente + +try: + while True: + # Captura + frame = tello.get_frame() + + # Tratamento + bat, height, temph, pres, time_elapsed = tello.get_info() + fps = tello.calc_fps() + cv2.putText(frame, f"FPS: {fps}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (10, 255, 0), 2) + cv2.putText(frame, f"Bat: {bat}%", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (10, 255, 0), 2) + cv2.putText(frame, f"Height: {height}cm", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 1, (10, 255, 0), 2) + cv2.putText(frame, f"Max. Temp.: {temph}C", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (10, 255, 0), 2) + cv2.putText(frame, f"Press.: {pres}", (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 1, (10, 255, 0), 2) + cv2.putText(frame, f"TOF: {time_elapsed}s", (10, 180), cv2.FONT_HERSHEY_SIMPLEX, 1, (10, 255, 0), 2) + + # Exibição + cv2.imshow('QR Code', frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + break +finally: + # Finalização + tello.end_tello() + cv2.destroyAllWindows() diff --git a/tello_zune/tello_zune.py b/tello_zune/tello_zune.py index b3867ad..06338ba 100644 --- a/tello_zune/tello_zune.py +++ b/tello_zune/tello_zune.py @@ -1,6 +1,8 @@ import time import threading import numpy as np +import socket +import cv2 from queue import Queue, Empty class SafeThread(threading.Thread): @@ -27,8 +29,6 @@ class TelloZune: Args: text_input (bool, optional): Se True, aceita comandos de texto via terminal. Padrão: False. """ - import socket - import cv2 def __init__( self, TELLOIP: str = '192.168.10.1', @@ -63,6 +63,9 @@ def __init__( # Fila de comandos self.command_queue: Queue[str] = Queue() + self.command_events: dict[str, threading.Event] = {} + self.current_command = None + self.cmd_lock = threading.Lock() # Eventos e contadores self.cmd_recv_ev = threading.Event() @@ -80,9 +83,9 @@ def __init__( ] # Sockets - self.sock_cmd = self.socket.socket(self.socket.AF_INET, self.socket.SOCK_DGRAM) + self.sock_cmd = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.sock_cmd.bind(self.localaddr) - self.sock_state = self.socket.socket(self.socket.AF_INET, self.socket.SOCK_DGRAM) + self.sock_state = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.sock_state.bind(self.stateaddr) # Threads cíclicas seguras @@ -91,13 +94,12 @@ def __init__( self.videoThread = SafeThread(target=self._video) # Thread de vídeo self.stateThread = SafeThread(target=self._state_receive) # Thread de estado self.movesThread = SafeThread(target=self._read_queue) # Thread de movimentos - self.periodicStateThread = SafeThread(target=self._periodic_state) # Thread de estado periódico self.textInputThread = SafeThread(target=self._text_input) # Thread de entrada de texto pelo terminal # Inicialização self.enable_text_input = text_input - def _execute_route(self, commands: list, interval: int) -> None: + def _execute_route(self, commands: list, interval: int=0) -> None: """ Executa uma sequência de comandos (rota) em sua própria thread. Args: @@ -110,11 +112,9 @@ def _execute_route(self, commands: list, interval: int) -> None: print(f"Executando passo {i + 1}/{len(commands)} da rota: '{cmd}'") self.add_command(cmd) - # Espera o intervalo definido antes do próximo comando - # Não executa a espera após o último comando - if i < len(commands) - 1: - print(f"Aguardando {interval} segundos...") - time.sleep(interval) + # Se não for o último comando e houver intervalo, manda o comando "delay" + if i < len(commands) - 1 and interval > 0: + self.add_command(f"delay {interval}") except Exception as e: print(f"Erro ao executar a rota: {e}") finally: @@ -128,7 +128,7 @@ def _video(self) -> None: if self.video is not None: ret, frame = self.video.read() if ret: - frame = self.cv2.resize(frame, self.image_size) + frame = cv2.resize(frame, self.image_size) self.frame = frame if not self.q.full(): self.q.put(frame) @@ -191,18 +191,6 @@ def _periodic_cmd(self) -> None: print(f"Erro na thread de comandos periódicos: {e}") time.sleep(1) - def _periodic_state(self) -> None: - """Atualiza valores em state_list periodicamente.""" - try: - for ev in self.state_list: - if self.state_count % ev['period'] == 0: - raw = self.get_state_field(ev['state']) or '' - ev['val'] = raw.rstrip() - self.timer_ev.wait(0.1) - self.state_count += 1 - except Exception as e: - print(f"Erro na thread de atualização de estado: {e}") - def _response_cmd_receive(self) -> None: """Recebe strings de resposta de comando via socket UDP.""" try: @@ -213,28 +201,37 @@ def _response_cmd_receive(self) -> None: print(f"Erro na thread de recebimento de comando: {e}") def _state_receive(self) -> None: - """Recebe strings de estado via socket UDP.""" + """Recebe strings de estado via socket UDP e atualiza state_value e state_list.""" try: data, _ = self.sock_state.recvfrom(512) val = data.decode("utf-8").rstrip() self.state_value = val.replace(';', ':').split(':') + for state in self.state_list: + if self.state_count % state['period'] == 0: + raw = self.get_state_field(state['state']) or '' + state['val'] = raw.rstrip() + self.state_count += 1 except Exception as e: print(f"Erro na thread de estado: {e}") - def _read_queue(self) -> None: + def _read_queue(self): """Lê comandos da fila, envia ao drone e exibe resposta.""" try: - cmd = self.command_queue.get(timeout=1) # Tenta pegar um comando; se não houver em 1s, dispara Empty + cmd = self.command_queue.get(timeout=1) + self.current_command = cmd + + if cmd.startswith("delay"): # Novo comando: "delay x" para pausar x segundos + seconds = float(cmd.split()[1]) + time.sleep(seconds) + return + + timeout = 8.0 if cmd.split()[0] in ['forward','back','left','right','up','down','cw','ccw'] else 2.0 + resp = self.send_cmd_return(cmd, timeout=timeout) + + print(f"{cmd}\t{resp}") + time.sleep(0.01) except Empty: - return # Sem comando, volta ao loop - self.send_cmd(cmd) # Envia o comando - if self.cmd_recv_ev.wait(timeout=2) and self.udp_cmd_ret: # Espera pela resposta - resp = self.udp_cmd_ret.rstrip() - self.cmd_recv_ev.clear() - else: - resp = '' # Timeout sem resposta - print(f"{cmd}\t{resp}") - time.sleep(0.1) # Pequeno intervalo para não sobrecarregar + return def _text_input(self) -> None: """Thread que lê comandos de texto do terminal.""" @@ -342,13 +339,17 @@ def set_image_size(self, image_size: tuple[int, int] = (960, 720)) -> None: """ self.image_size = image_size - def get_frame(self) -> np.ndarray: + def get_frame(self, timeout: float = 1.0) -> np.ndarray: """ Retorna próximo frame da fila. Returns: np.ndarray: Frame do vídeo (920x720) """ - return self.q.get() + try: + return self.q.get(timeout=timeout) + except Empty: + # Retorna um frame preto + return np.zeros((self.image_size[1], self.image_size[0], 3), dtype=np.uint8) def stop_communication(self) -> None: """Para threads e fecha sockets.""" @@ -358,7 +359,6 @@ def stop_communication(self) -> None: self.movesThread.stop() self.sock_cmd.close() self.sock_state.close() - self.periodicStateThread.stop() if self.textInputThread.is_alive(): self.textInputThread.stop() print("Comunicação finalizada") @@ -369,7 +369,6 @@ def start_communication(self) -> None: if self.periodicCmdThread.is_alive() is not True: self.periodicCmdThread.start() # Thread de comandos periódicos if self.stateThread.is_alive() is not True: self.stateThread.start() # Thread de estado if self.movesThread.is_alive() is not True: self.movesThread.start() # Thread de movimentos - if self.periodicStateThread.is_alive() is not True: self.periodicStateThread.start() # Thread de estado periódico print("Iniciando comunicação") def start_video(self) -> None: @@ -378,7 +377,7 @@ def start_video(self) -> None: time.sleep(1) - self.video = self.cv2.VideoCapture(self.video_source, self.cv2.CAP_FFMPEG) + self.video = cv2.VideoCapture(self.video_source, cv2.CAP_FFMPEG) if not self.videoThread.is_alive(): self.videoThread.start() @@ -420,21 +419,27 @@ def wait_till_connected(self, timeout: int = 10) -> bool: print(f"Falha na conexão: Tempo limite de {timeout}s excedido. Verifique se o drone está ligado") return False - def send_cmd_return(self, cmd: str) -> str: + def send_cmd_return(self, cmd: str, timeout: float = 1.0) -> str: """ Envia um comando para o drone Tello via UDP e espera pela resposta. O comando é enviado via UDP e a resposta é recebida na mesma conexão. Args: cmd (str): Comando a ser enviado para o drone. + timeout (float): Tempo máximo de espera em segundos. Padrão é 1.0 segundo. Returns: str: Resposta do drone. Verifique a documentação do SDK do Tello para os comandos válidos. """ - self.udp_cmd_ret = str() - cmd_bytes = cmd.encode("utf-8") - _ = self.sock_cmd.sendto(cmd_bytes, self.telloaddr) - self.cmd_recv_ev.wait(1) - self.cmd_recv_ev.clear() - return self.udp_cmd_ret + with self.cmd_lock: + self.cmd_recv_ev.clear() + self.udp_cmd_ret = "" + + cmd_bytes = cmd.encode("utf-8") + self.sock_cmd.sendto(cmd_bytes, self.telloaddr) + + # Espera a resposta (a thread recebedora vai dar .set() no evento) + self.cmd_recv_ev.wait(timeout) + + return self.udp_cmd_ret def send_cmd(self, cmd: str) -> None: """ @@ -469,10 +474,18 @@ def takeoff(self) -> None: def land(self) -> None: """Pousa o drone.""" print("Pousando") - while float(self.get_state_field('tof')) >= 30: - self.send_rc_control(0, 0, -70, 0) - self.add_command("land") - #time.sleep(4) + answer = self.send_cmd_return("land", timeout=5.0) # Bom dar um timeout maior no pouso + trys = 0 + max_trys = 3 + + while answer != 'ok' and trys < max_trys: + print(f"Resposta inesperada para 'land': '{answer}'. Tentando novamente...") + time.sleep(1) + answer = self.send_cmd_return("land", timeout=5.0) + trys += 1 + + if answer != 'ok': + print("Aviso: Falha ao confirmar pouso após várias tentativas.") def start_tello(self) -> bool: """ @@ -493,6 +506,20 @@ def start_tello(self) -> bool: print("Entrada de texto habilitada.") return True + + def is_vertical_moving(self, height_threshold: float = 5.0, sample_interval: float = 0.1) -> bool: + """ + Detecta movimento vertical comparando a altura em dois instantes. + Args: + height_threshold (float): Diferença mínima de altura (cm) para considerar que está se movendo verticalmente. + sample_interval (float): Tempo em segundos entre as duas amostras de altura. + Returns: + bool: True se o drone estiver se movendo verticalmente, False caso contrário. + """ + h1 = float(self.get_state_field('tof')) + time.sleep(sample_interval) + h2 = float(self.get_state_field('tof')) + return abs(h2 - h1) > height_threshold def end_tello(self) -> None: """Finaliza o drone Tello. Pousa se possivel, encerra o video e a comunicacao.""" @@ -549,3 +576,31 @@ def get_info(self) -> tuple: d.get('baro'), d.get('time'), ) + + def get_speed(self) -> tuple[float, float, float]: + """ + Retorna a velocidade atual do drone. + Returns: + tuple: (vx, vy, vz) velocidades em cm/s + """ + try: + vx = float(self.get_state_field('vgx')) + vy = float(self.get_state_field('vgy')) + vz = float(self.get_state_field('vgz')) + return vx, vy, vz + except (ValueError, TypeError): + return 0.0, 0.0, 0.0 + + def clear_command_queue(self): + """Limpa a fila de comandos""" + with self.command_queue.mutex: + self.command_queue.queue.clear() + print("Fila de comandos limpa.") + + def emergency_stop(self): + """ + Envia comando de emergência para o drone, parando-o imediatamente. Limpa a fila de comandos antes de enviar o comando de emergência. + """ + self.clear_command_queue() + self.send_cmd("emergency") + diff --git a/test/test_tello_zune.py b/test/test_tello_zune.py new file mode 100644 index 0000000..3a23d43 --- /dev/null +++ b/test/test_tello_zune.py @@ -0,0 +1,112 @@ +import unittest +from unittest.mock import patch, MagicMock +import threading +import time + +# Importa a sua classe (certifique-se de que o arquivo principal se chama tello_zune.py) +from tello_zune.tello_zune import TelloZune + +class TestTelloZune(unittest.TestCase): + + @patch('tello_zune.tello_zune.socket.socket') + def setUp(self, mock_socket): + """ + Configuração inicial antes de cada teste. + Mockamos o socket para evitar que a classe tente abrir portas UDP reais no PC. + """ + # Cria a instância sem habilitar o input de texto para não travar os testes + self.tello = TelloZune(text_input=False) + + # Facilita o acesso aos mocks dos sockets + self.mock_sock_cmd = self.tello.sock_cmd + self.mock_sock_state = self.tello.sock_state + + def tearDown(self): + """Limpeza após cada teste.""" + self.tello.stop_communication() + + def test_send_cmd(self): + """Testa se o envio assíncrono de comandos formata e envia os bytes corretamente.""" + comando = "forward 50" + self.tello.send_cmd(comando) + + # Verifica se o socket enviou os bytes corretos para o IP/Porta do Tello + self.mock_sock_cmd.sendto.assert_called_once_with( + b'forward 50', + ('192.168.10.1', 8889) + ) + + @patch('tello_zune.tello_zune.threading.Event.wait') + def test_send_cmd_return(self, mock_wait): + """Testa o envio síncrono simulando uma resposta do drone.""" + comando = "battery?" + resposta_simulada = "85" + + # Simulamos que a thread recebedora preencheu a variável de retorno + # antes do 'wait' ser liberado + def side_effect(*args, **kwargs): + self.tello.udp_cmd_ret = resposta_simulada + return True + mock_wait.side_effect = side_effect + + resultado = self.tello.send_cmd_return(comando) + + self.assertEqual(resultado, "85") + self.mock_sock_cmd.sendto.assert_called_once_with(b'battery?', ('192.168.10.1', 8889)) + + def test_get_speed_parsing(self): + """Testa se a extração de velocidade funciona e lida com erros.""" + # Cenário 1: Estado populado corretamente + self.tello.state_value = ['vgx', '15', 'vgy', '-10', 'vgz', '0'] + vx, vy, vz = self.tello.get_speed() + self.assertEqual(vx, 15.0) + self.assertEqual(vy, -10.0) + self.assertEqual(vz, 0.0) + + # Cenário 2: Estado vazio ou corrompido (deve retornar 0,0,0 sem quebrar) + self.tello.state_value = [] + vx, vy, vz = self.tello.get_speed() + self.assertEqual((vx, vy, vz), (0.0, 0.0, 0.0)) + + @patch.object(TelloZune, 'send_cmd_return') + @patch('time.sleep') # Evita que o teste fique lento por causa do sleep + def test_land_timeout_loop(self, mock_sleep, mock_send_cmd_return): + """Garante que a função land não entra em loop infinito se o drone não responder 'ok'.""" + # Simulamos o drone retornando 'error' continuamente + mock_send_cmd_return.return_value = 'error' + + # Executa o land + self.tello.land() + + # Verifica se tentou exatamente o número máximo de vezes (1 chamada inicial + 3 retentativas = 4) + self.assertEqual(mock_send_cmd_return.call_count, 4) + + def test_add_command_and_clear_queue(self): + """Testa se a fila de comandos enfileira e limpa corretamente.""" + self.tello.add_command("takeoff") + self.tello.add_command("forward 100") + + self.assertEqual(self.tello.command_queue.qsize(), 2) + + self.tello.clear_command_queue() + self.assertEqual(self.tello.command_queue.qsize(), 0) + + @patch.object(TelloZune, 'add_command') + def test_execute_route(self, mock_add_command): + """Testa se as rotas inserem comandos e delays adequadamente.""" + comandos = ["takeoff", "forward 50", "land"] + + self.tello._execute_route(comandos, interval=2) + + # Verifica as chamadas exatas que deveriam ir para a fila + expected_calls = [ + unittest.mock.call("takeoff"), + unittest.mock.call("delay 2"), + unittest.mock.call("forward 50"), + unittest.mock.call("delay 2"), + unittest.mock.call("land") + ] + mock_add_command.assert_has_calls(expected_calls, any_order=False) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file