From 26b19a909fa4c8d516bf977f443d43df36146e25 Mon Sep 17 00:00:00 2001 From: Andrew Johnson Date: Sat, 22 Mar 2025 16:06:45 +0100 Subject: [PATCH] First Version (#1) Co-authored-by: Anujith Muraleedharan <140189296+AnujithM@users.noreply.github.com> --- .devcontainer/Dockerfile | 7 + .devcontainer/devcontainer.json | 9 + .github/dependabot.yml | 12 + .gitignore | 87 +- .vscode/launch.json | 15 + .vscode/settings.json | 1 + README.md | 19 +- example_exercises/10_throw_takeoff.py | 34 + example_exercises/11_emergency_stop.py | 33 + example_exercises/12_flips.py | 59 ++ example_exercises/13_rc_movement.py | 158 +++ example_exercises/14_speed.py | 42 + example_exercises/1_check_import.py | 6 + example_exercises/2_simple_takeoff_land.py | 34 + example_exercises/3_drone_information.py | 52 + example_exercises/4_camera_sanity_check.py | 32 + example_exercises/5_take_a_picture.py | 31 + example_exercises/6_record_video.py | 82 ++ example_exercises/7_movement.py | 60 ++ example_exercises/8_rotate.py | 47 + example_exercises/9_advanced_movement.py | 61 ++ example_exercises/README.md | 9 + example_exercises/navigate_route.py | 38 + main.py | 56 -- requirements.txt | 5 +- src/tello_drone.py | 511 ---------- tello_drone.py | 458 --------- tello_sim/__init__.py | 0 tello_sim/command_server.py | 224 +++++ {src => tello_sim/entities}/bos_standing.glb | Bin {src => tello_sim/entities}/business_man.glb | Bin {src => tello_sim/entities}/cobblestone.glb | Bin .../entities}/construction_barrier.glb | Bin {src => tello_sim/entities}/dirty_car.glb | Bin .../entities}/dirty_leaking_concrete_wall.glb | Bin {src => tello_sim/entities}/dji_mavic_3.glb | Bin .../entities}/gas_station_-_gta_v.glb | Bin {src => tello_sim/entities}/highway.glb | Bin {src => tello_sim/entities}/pig.glb | Bin .../entities}/pipeline_construction_site.glb | Bin {src => tello_sim/entities}/road.glb | Bin {src => tello_sim/entities}/road_closed.glb | Bin {src => tello_sim/entities}/road_roller.glb | Bin .../entities}/stone_ground_01.glb | Bin {src => tello_sim/entities}/street_light.glb | Bin {src => tello_sim/entities}/tello.glb | Bin {src => tello_sim/entities}/tunnel_3.glb | Bin tello_sim/run_sim.py | 13 + tello_sim/tello_drone_sim.py | 21 + tello_sim/tello_sim_client.py | 222 +++++ tello_sim/ursina_adapter.py | 940 ++++++++++++++++++ 51 files changed, 2348 insertions(+), 1030 deletions(-) create mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/devcontainer.json create mode 100644 .github/dependabot.yml create mode 100644 .vscode/launch.json create mode 100755 example_exercises/10_throw_takeoff.py create mode 100755 example_exercises/11_emergency_stop.py create mode 100755 example_exercises/12_flips.py create mode 100644 example_exercises/13_rc_movement.py create mode 100644 example_exercises/14_speed.py create mode 100755 example_exercises/1_check_import.py create mode 100755 example_exercises/2_simple_takeoff_land.py create mode 100755 example_exercises/3_drone_information.py create mode 100755 example_exercises/4_camera_sanity_check.py create mode 100755 example_exercises/5_take_a_picture.py create mode 100755 example_exercises/6_record_video.py create mode 100755 example_exercises/7_movement.py create mode 100755 example_exercises/8_rotate.py create mode 100755 example_exercises/9_advanced_movement.py create mode 100644 example_exercises/README.md create mode 100755 example_exercises/navigate_route.py delete mode 100644 main.py delete mode 100644 src/tello_drone.py delete mode 100644 tello_drone.py create mode 100644 tello_sim/__init__.py create mode 100644 tello_sim/command_server.py rename {src => tello_sim/entities}/bos_standing.glb (100%) rename {src => tello_sim/entities}/business_man.glb (100%) rename {src => tello_sim/entities}/cobblestone.glb (100%) rename {src => tello_sim/entities}/construction_barrier.glb (100%) rename {src => tello_sim/entities}/dirty_car.glb (100%) rename {src => tello_sim/entities}/dirty_leaking_concrete_wall.glb (100%) rename {src => tello_sim/entities}/dji_mavic_3.glb (100%) rename {src => tello_sim/entities}/gas_station_-_gta_v.glb (100%) rename {src => tello_sim/entities}/highway.glb (100%) rename {src => tello_sim/entities}/pig.glb (100%) rename {src => tello_sim/entities}/pipeline_construction_site.glb (100%) rename {src => tello_sim/entities}/road.glb (100%) rename {src => tello_sim/entities}/road_closed.glb (100%) rename {src => tello_sim/entities}/road_roller.glb (100%) rename {src => tello_sim/entities}/stone_ground_01.glb (100%) rename {src => tello_sim/entities}/street_light.glb (100%) rename {src => tello_sim/entities}/tello.glb (100%) rename {src => tello_sim/entities}/tunnel_3.glb (100%) create mode 100644 tello_sim/run_sim.py create mode 100644 tello_sim/tello_drone_sim.py create mode 100644 tello_sim/tello_sim_client.py create mode 100644 tello_sim/ursina_adapter.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..c87c715 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,7 @@ +FROM python:3.9 +RUN apt-get update && apt-get install -y \ + fluxbox \ + x11vnc \ + python3-tk \ + && rm -rf /var/lib/apt/lists/* +CMD ["x11vnc", "-forever", "-nopw", "-create"] \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..6ceec73 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,9 @@ +{ + "name": "Python GUI Devcontainer", + "build": { + "dockerfile": "Dockerfile" + }, + "forwardPorts": [5901], + "runArgs": ["--env", "DISPLAY=host.docker.internal:0"], + "mounts": ["source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind"] + } \ No newline at end of file diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..f33a02c --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,12 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for more information: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates +# https://containers.dev/guide/dependabot + +version: 2 +updates: + - package-ecosystem: "devcontainers" + directory: "/" + schedule: + interval: weekly diff --git a/.gitignore b/.gitignore index eba74f4..b9a518a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,86 @@ -venv/ \ No newline at end of file +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +pip-wheel-metadata/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IDEs and editors +.idea/ +*.swp +*.swo +*~ + +# Operating System files +.DS_Store +Thumbs.db + +# Pyre type checker +.pyre/ + +# Pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# Logs +*.log + +output/ diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..6b76b4f --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,15 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Python Debugger: Current File", + "type": "debugpy", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 67f867b..af3432e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,4 +1,5 @@ { + "python.defaultInterpreterPath": "./venv/lib/python", "python.analysis.extraPaths": [ "./venv/lib/python3.10/site-packages" ] diff --git a/README.md b/README.md index c10bf52..4d9ae38 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,9 @@ # Tello Drone Sim +This is a simple simulation of a Tello drone using Ursina. The drone can be controlled via tcp calls. + +in the repo there is the simulation server along with a client class that can be used to interact with the sim server + ## Setup 1. Create the virtual environment by running: @@ -20,8 +24,17 @@ source venv/bin/activate pip install -r requirements.txt ``` -4. Run the program by running: +4. Export the python path by running: ```bash -python tello_drone.py -``` # tello-sim +export PYTHONPATH=$PWD +``` + + +## Running the simulation + +To run the simulation, run the following command: + +```bash +python tello_sim/run_sim.py +``` diff --git a/example_exercises/10_throw_takeoff.py b/example_exercises/10_throw_takeoff.py new file mode 100755 index 0000000..458bef9 --- /dev/null +++ b/example_exercises/10_throw_takeoff.py @@ -0,0 +1,34 @@ +from tello_sim.tello_sim_client import TelloSimClient + + +import time + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Get ready to throw in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + +print("Throw the drone within 5 seconds!") +tello.initiate_throw_takeoff() + +for i in range(5, 0, -1): + print(i) + time.sleep(1) + +print("Waiting for...") +for i in range(10, 0, -1): + print(i) + time.sleep(1) + +print("Landing") +# Land +tello.land() + +# End the connection +tello.end() diff --git a/example_exercises/11_emergency_stop.py b/example_exercises/11_emergency_stop.py new file mode 100755 index 0000000..2f4591f --- /dev/null +++ b/example_exercises/11_emergency_stop.py @@ -0,0 +1,33 @@ +from tello_sim.tello_sim_client import TelloSimClient + +import time + +from tello_sim.tello_sim_client import TelloSimClient + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + +# Takeoff +print("Take off") +tello.takeoff() + + +for i in range(10, 0, -1): + print("Get Ready to catch drone!") + print("Emergency stop in", i) + time.sleep(1) + +print("Emergency stop now!") +# Land +tello.emergency() + +# End the connection +tello.end() diff --git a/example_exercises/12_flips.py b/example_exercises/12_flips.py new file mode 100755 index 0000000..5d2c9ee --- /dev/null +++ b/example_exercises/12_flips.py @@ -0,0 +1,59 @@ +from tello_sim.tello_sim_client import TelloSimClient + + +import time + +ROTATION_DEGREES = 90 +HIGHT_CM = 100 + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + + +def pause(): + print("Hovering for...") + for i in range(3, 0, -1): + print(i) + time.sleep(1) + + +def warn_of_flip(direction: str): + print("WARNING!") + print(f"Flipping {direction} in ...") + for i in range(3, 0, -1): + print(i) + time.sleep(1) + flip_func = getattr(tello, f"flip_{direction}") + flip_func() + + +# Takeoff +print("Take off") +tello.takeoff() + +tello.move_up(HIGHT_CM) + +warn_of_flip("left") + +warn_of_flip("right") + +warn_of_flip("forward") + +warn_of_flip("back") + +pause() + +print("Landing") +# Land +tello.land() + +# End the connection +tello.end() diff --git a/example_exercises/13_rc_movement.py b/example_exercises/13_rc_movement.py new file mode 100644 index 0000000..b91914a --- /dev/null +++ b/example_exercises/13_rc_movement.py @@ -0,0 +1,158 @@ +from typing import Callable +from tello_sim.tello_sim_client import TelloSimClient + + +import time + +SPEED_SETTING_CM_S = 30 +MOVEMENT_MAGNITUDE = 30 +TIME_PER_ACTION_SECS = 3 + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + +# Takeoff +print("Take off") +tello.takeoff() + +print("Setting speed to", SPEED_SETTING_CM_S) +tello.set_speed(SPEED_SETTING_CM_S) + + +def do_action_for_time(label: str, action: Callable, time_in_seconds: int): + print(label) + action() + time.sleep(time_in_seconds) + + +left_right_velocity = 0 +forward_backward_velocity = 0 +up_down_velocity = 0 +yaw_velocity = 0 +do_action_for_time( + "Staying still", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +# Move forward +left_right_velocity = 0 +forward_backward_velocity = MOVEMENT_MAGNITUDE +up_down_velocity = 0 +yaw_velocity = 0 +do_action_for_time( + "Moving Forward", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +# Move Backwards +left_right_velocity = 0 +forward_backward_velocity = -MOVEMENT_MAGNITUDE +up_down_velocity = 0 +yaw_velocity = 0 +do_action_for_time( + "Moving backwards", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + + +# Move Left +left_right_velocity = MOVEMENT_MAGNITUDE +forward_backward_velocity = 0 +up_down_velocity = 0 +yaw_velocity = 0 +do_action_for_time( + "Moving left", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +# Move Right +left_right_velocity = -MOVEMENT_MAGNITUDE +forward_backward_velocity = 0 +up_down_velocity = 0 +yaw_velocity = 0 +do_action_for_time( + "Moving right", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +# Move Up +left_right_velocity = 0 +forward_backward_velocity = 0 +up_down_velocity = MOVEMENT_MAGNITUDE +yaw_velocity = 0 +do_action_for_time( + "Moving up", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +# Move down +left_right_velocity = 0 +forward_backward_velocity = 0 +up_down_velocity = -MOVEMENT_MAGNITUDE +yaw_velocity = 0 +do_action_for_time( + "Moving down", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +# Turn Left +left_right_velocity = 0 +forward_backward_velocity = 0 +up_down_velocity = 0 +yaw_velocity = MOVEMENT_MAGNITUDE +do_action_for_time( + "Turning left", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +# Turn Right +left_right_velocity = 0 +forward_backward_velocity = 0 +up_down_velocity = 0 +yaw_velocity = -MOVEMENT_MAGNITUDE +do_action_for_time( + "Turning right", + lambda: tello.send_rc_control( + left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity + ), + TIME_PER_ACTION_SECS, +) + +print("Landing") +# Land +tello.land() + +# End the connection +tello.end() diff --git a/example_exercises/14_speed.py b/example_exercises/14_speed.py new file mode 100644 index 0000000..6ab3d41 --- /dev/null +++ b/example_exercises/14_speed.py @@ -0,0 +1,42 @@ +from tello_sim.tello_sim_client import TelloSimClient + +import time + +TEST_DISTANCE = 500 + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +def pause(next_action: str): + print(f"Hovering before {next_action}") + for i in range(3, 0, -1): + print(i) + time.sleep(1) + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + +# Takeoff +print("Take off") +tello.takeoff() + +pause("ascending") +tello.move_up(100) + +for speed in range(10, 101, 10): + pause(f"speed change to {speed} cm/s") + print("Setting speed to", speed, "cm/s") + tello.set_speed(speed) + print(f"Testing forward and back at {speed} cm/s") + tello.move_forward(1000) + tello.move_back(TEST_DISTANCE) + time.sleep(1) + +pause("landing") +print("Finished Landing") +tello.land() \ No newline at end of file diff --git a/example_exercises/1_check_import.py b/example_exercises/1_check_import.py new file mode 100755 index 0000000..7afae71 --- /dev/null +++ b/example_exercises/1_check_import.py @@ -0,0 +1,6 @@ +from tello_sim.tello_sim_client import TelloSimClient + + +print(TelloSimClient) + +print("Tello import works fine") diff --git a/example_exercises/2_simple_takeoff_land.py b/example_exercises/2_simple_takeoff_land.py new file mode 100755 index 0000000..748b370 --- /dev/null +++ b/example_exercises/2_simple_takeoff_land.py @@ -0,0 +1,34 @@ + +import time + +from tello_sim.tello_sim_client import TelloSimClient + + +# Create a Tello instance +tello = TelloSimClient() + +print("Attempting to connect to drone ...") + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + +# Takeoff +print("Take off") +tello.takeoff() + +print("Hovering for...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + +print("Landing") +# Land +tello.land() + +# End the connection +tello.end() diff --git a/example_exercises/3_drone_information.py b/example_exercises/3_drone_information.py new file mode 100755 index 0000000..7fd17a6 --- /dev/null +++ b/example_exercises/3_drone_information.py @@ -0,0 +1,52 @@ + +import time + +from tello_sim.tello_sim_client import TelloSimClient + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Get some information from the drone") + +# Print battery capacity +battery_capacity = tello.get_battery() +print("Battery Capacity:", battery_capacity, "%") + + + + + +# Print distance from time-of-flight sensor +print("Distance (TOF):", tello.get_distance_tof(), "cm") + +# Print height +print("Height:", tello.get_height(), "cm") + +# Print flight time +print("Flight Time:", tello.get_flight_time(), "seconds") + +# Print speed in the x, y, z directions +print("Speed X:", tello.get_speed_x(), "cm/s") +print("Speed Y:", tello.get_speed_y(), "cm/s") +print("Speed Z:", tello.get_speed_z(), "cm/s") + +# Print acceleration in the x, y, z directions +print("Acceleration X:", tello.get_acceleration_x(), "cm/s²") +print("Acceleration Y:", tello.get_acceleration_y(), "cm/s²") +print("Acceleration Z:", tello.get_acceleration_z(), "cm/s²") + + + +print("Pitch", tello.get_pitch(), "degrees") + +print("Roll", tello.get_roll(), "degrees") + +print("Yaw", tello.get_yaw(), "degrees") + +print("IMU data", tello.query_attitude()) + + +print("State", tello.get_current_state()) diff --git a/example_exercises/4_camera_sanity_check.py b/example_exercises/4_camera_sanity_check.py new file mode 100755 index 0000000..1882957 --- /dev/null +++ b/example_exercises/4_camera_sanity_check.py @@ -0,0 +1,32 @@ +from tello_sim.tello_sim_client import TelloSimClient +import cv2 + +# Create a Tello instance +tello = TelloSimClient() +# Desired window size +WINDOW_WIDTH = 640 +WINDOW_HEIGHT = 360 +# Connect to Tello +tello.connect() + +tello.streamon() + +# Create a normal, resizable window +cv2.namedWindow("frame", cv2.WINDOW_NORMAL) +cv2.resizeWindow("frame", WINDOW_WIDTH, WINDOW_HEIGHT) + +try: + while True: + img = tello.get_frame_read().frame + if img is not None: + img_bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + # Resize the image to fit the window size + img_resized = cv2.resize(img_bgr, (WINDOW_WIDTH, WINDOW_HEIGHT), interpolation=cv2.INTER_AREA) + cv2.imshow("frame", img_resized) + if cv2.waitKey(1) == ord('q'): + break +except KeyboardInterrupt: + pass +finally: + print("fin") + cv2.destroyAllWindows() diff --git a/example_exercises/5_take_a_picture.py b/example_exercises/5_take_a_picture.py new file mode 100755 index 0000000..212dab3 --- /dev/null +++ b/example_exercises/5_take_a_picture.py @@ -0,0 +1,31 @@ +import os +import cv2 +import time +import numpy as np +from tello_sim.tello_sim_client import TelloSimClient + +# Initialize and connect +tello = TelloSimClient() +tello.connect() +tello.streamon() + +# Takeoff +tello.takeoff() +time.sleep(5) # Let drone stabilize after takeoff + +# Get frame object +frame_read = tello.get_frame_read() + +tello.streamoff() + +# Prepare directory to save +script_dir = os.path.dirname(__file__) +artifact_folder_path = os.path.join(script_dir, "../../artifacts/images") +os.makedirs(artifact_folder_path, exist_ok=True) + +# Save the frame +save_path = os.path.join(artifact_folder_path, "picture.png") +cv2.imwrite(save_path, np.array(frame_read.frame)) + +# Land +tello.land() \ No newline at end of file diff --git a/example_exercises/6_record_video.py b/example_exercises/6_record_video.py new file mode 100755 index 0000000..26446c2 --- /dev/null +++ b/example_exercises/6_record_video.py @@ -0,0 +1,82 @@ +import os +from threading import Thread +import time +from typing import cast +from tello_sim.tello_sim_client import TelloSimClient + +import cv2 + +# Configuration +FPS = 30 + +# Initialize Tello connector +tello = TelloSimClient() +tello.connect() + +tello.streamon() +time.sleep(2) +# Define paths to save inside tello_recording +recording_folder = os.path.join(os.getcwd(), "output", "tello_recording") +images_folder_path = os.path.join(recording_folder, "frames") +video_file_path = os.path.join(recording_folder, "video.mp4") +os.makedirs(images_folder_path, exist_ok=True) + +# Recording control +keep_recording = True + + +def capture_images(): + frame_count = 0 + while keep_recording: + frame = tello.get_frame_read().frame + if frame is not None: + image_file_path = os.path.join(images_folder_path, f"frame_{frame_count:04d}.jpg") + cv2.imwrite(image_file_path, cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)) + frame_count += 1 + print(f"[Capture] Saved frame {frame_count}") + else: + print("[Capture] No frame received.") + time.sleep(1 / FPS) + + print("[Capture] Finished capturing frames.") + + +# Start recording thread +recorder_thread = Thread(target=capture_images) +recorder_thread.start() + +# Drone mission +tello.takeoff() +time.sleep(5) +tello.move_up(50) +time.sleep(5) +tello.rotate_counter_clockwise(360) +time.sleep(5) +tello.land() + +# Stop recording +keep_recording = False +recorder_thread.join() + +# Create video from frames +frame_files = sorted(os.listdir(images_folder_path)) +if frame_files: + first_frame = cv2.imread(os.path.join(images_folder_path, frame_files[0])) + height, width, _ = first_frame.shape + + video_writer = cv2.VideoWriter( + video_file_path, cv2.VideoWriter_fourcc(*"mp4v"), FPS, (width, height) # type: ignore + ) + + for frame_file in frame_files: + frame_path = os.path.join(images_folder_path, frame_file) + frame = cv2.imread(frame_path) + video_writer.write(frame) + + video_writer.release() + print(f"[Video] Video saved at {video_file_path}") +else: + print("[Video] No frames captured. Video not created.") + +print("Finished creating video") +tello.streamoff() diff --git a/example_exercises/7_movement.py b/example_exercises/7_movement.py new file mode 100755 index 0000000..a829400 --- /dev/null +++ b/example_exercises/7_movement.py @@ -0,0 +1,60 @@ +from tello_sim.tello_sim_client import TelloSimClient + +import time + +MOVEMENT_DISTANCE_CM = 70 + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + +# Takeoff +print("Take off") +tello.takeoff() + + +def pause(next_action: str): + print(f"Hovering before {next_action}") + for i in range(3, 0, -1): + print(i) + time.sleep(1) + + +pause("forward") +print(f"Moving forward {MOVEMENT_DISTANCE_CM}cm") +tello.move_forward(MOVEMENT_DISTANCE_CM) + +pause("backwards") +print(f"Moving backwards {MOVEMENT_DISTANCE_CM}cm") +tello.move_back(MOVEMENT_DISTANCE_CM) + +pause("left") +print(f"Moving left {MOVEMENT_DISTANCE_CM}cm") +tello.move_left(MOVEMENT_DISTANCE_CM) + +pause("right") +print(f"Moving right {MOVEMENT_DISTANCE_CM}cm") +tello.move_right(MOVEMENT_DISTANCE_CM) + +pause("up") +print(f"Moving up {MOVEMENT_DISTANCE_CM}cm") +tello.move_up(MOVEMENT_DISTANCE_CM) + +pause("down") +print(f"Moving down {MOVEMENT_DISTANCE_CM}cm") +tello.move_down(MOVEMENT_DISTANCE_CM) + +pause("landing") +print("Landing") +# Land +tello.land() + +# End the connection +tello.end() diff --git a/example_exercises/8_rotate.py b/example_exercises/8_rotate.py new file mode 100755 index 0000000..7bc14d5 --- /dev/null +++ b/example_exercises/8_rotate.py @@ -0,0 +1,47 @@ +from tello_sim.tello_sim_client import TelloSimClient + +import time + +ROTATION_DEGREES = 180 + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + + +def pause(): + print("Hovering for...") + for i in range(3, 0, -1): + print(i) + time.sleep(1) + + +# Takeoff +print("Take off") +tello.takeoff() + +pause() + +print(f"Rotating clockwise {ROTATION_DEGREES} degrees") +tello.rotate_clockwise(ROTATION_DEGREES) + +pause() + +print(f"Rotating counter clockwise {ROTATION_DEGREES} degrees") +tello.rotate_counter_clockwise(ROTATION_DEGREES) + +pause() + +print("Landing") +# Land +tello.land() + +# End the connection +tello.end() diff --git a/example_exercises/9_advanced_movement.py b/example_exercises/9_advanced_movement.py new file mode 100755 index 0000000..fb441ea --- /dev/null +++ b/example_exercises/9_advanced_movement.py @@ -0,0 +1,61 @@ +# Not verified working on normal Tello! + +from tello_sim.tello_sim_client import TelloSimClient + + +import time + +ROTATION_DEGREES = 90 + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(3, 0, -1): + print(i) + time.sleep(1) + + +def pause(): + print("Hovering for...") + for i in range(3, 0, -1): + print(i) + time.sleep(1) + + +# Takeoff +print("Take off") +tello.takeoff() + +pause() + +tello.curve_xyz_speed(100, 0, 0, 200, 200, 0, 20) +tello.wait_until_motion_complete()# Use this instead of pause for longer duration curve_xyz_speed/go_xyz_speed,to prevent break in animation. +#tello.go_xyz_speed(100, 0, 0, 10) +#tello.wait_until_motion_complete() +# Should go backwards 1m at 10 cm/s +#tello.go_xyz_speed(-100, 0, 0, 10) + +# Should go forward and up at 10 cm/s +# tello.go_xyz_speed(10, 10, 10, 10) +# Should go backward left and up at 10 cm/s +# tello.go_xyz_speed(-80, 10, 10, 20) + + +#pause() + +# Should go forward to the right and up at with rotations 10 cm/s +# tello.curve_xyz_speed(10, 10, 10, 20, 20, 120, 10) +#tello.curve_xyz_speed(40, 10, 140, -200, 80, 40, 20) + + + +print("Landing") +# Land +tello.land() + +# End the connection +tello.end() diff --git a/example_exercises/README.md b/example_exercises/README.md new file mode 100644 index 0000000..3788936 --- /dev/null +++ b/example_exercises/README.md @@ -0,0 +1,9 @@ +# Example Exercises + +This folder contains a collection of sample scripts and exercises designed to help you get started with and understand the functionality of the DJI Tello playground. These examples demonstrate how to use various components of the project—from basic drone commands to more advanced features like computer vision integrations. + +Make sure to always execute the scripts from the `src` directory to ensure proper module imports and file references. + +The numbered exercises are ordered from basic to advanced, with each script building upon the concepts introduced in the previous ones. Feel free to explore and modify the scripts to suit your needs or experiment with new ideas. + +The other scripts are more advaned and demonstrate additional capabilities of the Tello drone, such as face tracking, object detection, and autonomous flight. \ No newline at end of file diff --git a/example_exercises/navigate_route.py b/example_exercises/navigate_route.py new file mode 100755 index 0000000..b81e6ab --- /dev/null +++ b/example_exercises/navigate_route.py @@ -0,0 +1,38 @@ +"Here try to create a script that navigates the drone through a course." +from tello_sim.tello_sim_client import TelloSimClient + +import time + +# Create a Tello instance +tello = TelloSimClient() + +# Connect to Tello +tello.connect() + +print("Starting flying in ...") +for i in range(10, 0, -1): + print(i) + time.sleep(1) + + +# Takeoff +tello.takeoff() + +# Here change the commands to navigate though the course + +# # Go forward 100 cm +tello.move_forward(100) + +# # Turn right +# tello.rotate_clockwise(90) + +# # Go forward 150 cm +# tello.move_forward(150) + +##### + +# Land +tello.land() + +# End the connection +tello.end() diff --git a/main.py b/main.py deleted file mode 100644 index bcea3e0..0000000 --- a/main.py +++ /dev/null @@ -1,56 +0,0 @@ -from tello_drone import DroneSimulator, TelloSimulator -from ursina import * - -app = Ursina() -window.color = color.rgb(135, 206, 235) -window.fullscreen = True - -window.borderless = False -window.fps_counter.enabled = False -window.render_mode = 'default' - - -Sky(texture='sky_sunset') - -tello_sim = TelloSimulator() -drone_sim = DroneSimulator(tello_sim) - -def input(key): - if key == 'h': - drone_sim.help_text.visible = not drone_sim.help_text.visible - if key == 'c': - drone_sim.toggle_camera_view() -def update(): - moving = False - rolling = False - - if held_keys['w']: - drone_sim.move("forward") - moving = True - if held_keys['s']: - drone_sim.move("backward") - moving = True - if held_keys['a']: - drone_sim.move("left") - rolling = True - if held_keys['d']: - drone_sim.move("right") - rolling = True - if held_keys['j']: - drone_sim.rotate(-drone_sim.rotation_speed) - if held_keys['l']: - drone_sim.rotate(drone_sim.rotation_speed) - if held_keys['shift']: - drone_sim.change_altitude("up") - if held_keys['control']: - drone_sim.change_altitude("down") - if not moving: - drone_sim.pitch_angle = 0 # Reset pitch when not moving - - if not rolling: - drone_sim.roll_angle = 0 # Reset roll when not rolling - - drone_sim.update_movement() - drone_sim.update_pitch_roll() - -app.run() diff --git a/requirements.txt b/requirements.txt index e2c237d..fff3f56 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1 +1,4 @@ -ursina \ No newline at end of file +ursina +PyOpenGL +numpy +opencv-python \ No newline at end of file diff --git a/src/tello_drone.py b/src/tello_drone.py deleted file mode 100644 index 13d2dea..0000000 --- a/src/tello_drone.py +++ /dev/null @@ -1,511 +0,0 @@ -from ursina import * -from time import time - -def lerp_color(start_color, end_color, factor): - """Custom color interpolation function""" - return Color( - start_color.r + (end_color.r - start_color.r) * factor, - start_color.g + (end_color.g - start_color.g) * factor, - start_color.b + (end_color.b - start_color.b) * factor, - 1 # Alpha channel - ) - - -class TelloSimulator: - - def __init__(self): - self.battery_level = 100 - self.altitude = 3 - self.speed = 0 - self.rotation_angle = 0 - self.is_flying = False - self.start_time = time() - - def connect(self): - print("Tello Simulator: Drone connected") - return True - - def get_battery(self): - elapsed_time = time() - self.start_time - self.battery_level = max(100 - int((elapsed_time / 3600) * 100), 0) # Reduce battery over 60 min - return self.battery_level - - def takeoff(self): - if not self.is_flying: - print("Tello Simulator: Drone taking off") - self.is_flying = True - return "Taking off" - return "Already in air" - - def land(self): - if self.is_flying: - print("Tello Simulator: Drone landing") - self.is_flying = False - return "Landing" - return "Already on ground" - - def move(self, direction, distance=1): - print(f"Tello Simulator: Moving {direction} by {distance} meters") - - def rotate(self, angle): - self.rotation_angle += angle - print(f"Tello Simulator: Rotating {angle} degrees") - - #def start_video_stream(self): - #print("Tello Simulator: Starting video stream...") - - # def get_video_frame(self): - #return "Simulated video frame" - - -class DroneSimulator(Entity): - def __init__(self, tello_api, **kwargs): - super().__init__() - - self.help_text = Text( - text="Controls:\nW - Forward\nS - Backward\nA - Left\nD - Right\nShift - Launch/Up\nCtrl - Down\nJ - Rotate Left\nL - Rotate Right\nC - FPV\nH - Toggle Help", - position=(-0.85, 0.43), # Top-left position - scale=1.2, - color=color.white, - visible=True - ) - - self.tello = tello_api - self.drone = Entity( - model='tello.glb', - scale=0.06, - position=(-15.4, 3, 5), - collider='box', - cast_shadow=True - ) - - - self.car = Entity( - model='dirty_car.glb', - scale=0.085, - position=(10, 2.45, 155), - rotation=(0, 0, 0), - collider='box', - cast_shadow=True - ) - - - self.truck = Entity( - model='road_roller.glb', - scale=4.0, - position=(-150, 2.45, 155), - rotation=(0, -90, 0), - collider='box', - cast_shadow=True - ) - - self.road_closed = Entity( - model='road_closed.glb', - scale=7.0, - position=(-15, 2, 315), - rotation=(0, 90, 0), - collider='box', - cast_shadow=True - ) - - - self.business_man = Entity( - model='business_man.glb', - scale=7.3, - position=(23, 12, 155), - rotation=(0, 55, 0), - collider='box', - cast_shadow=True - ) - - self.man = Entity( - model='bos_standing.glb', - scale=10.3, - position=(-83, 2.8, 165), - rotation=(0, 120, 0), - collider='box', - cast_shadow=True - ) - - self.patch = Entity( - model='pipeline_construction_site.glb', - scale=(15, 15, 12), - position=(-123, 0.0, 260), - rotation=(0, 0, 0), - collider='none', - cast_shadow=True - ) - - self.police_man = Entity( - model='pig.glb', - scale=10.0, - position=(-35, 1.7, 230), - rotation=(0, -70, 0), - collider='box', - cast_shadow=True - ) - - self.light1 = Entity( - model='street_light.glb', - scale=(4, 6.5, 5), - position=(-55, 2.5, 260), - rotation=(0, -90, 0), - collider='none', - cast_shadow=True - ) - - - self.light2 = Entity( - model='street_light.glb', - scale=(4, 6.5, 5), - position=(25, 2.5, 95), - rotation=(0, 90, 0), - collider='none', - cast_shadow=True - ) - - self.light3 = Entity( - model='street_light.glb', - scale=(4, 6.5, 5), - position=(-55, 2.5, -70), - rotation=(0, -90, 0), - collider='none', - cast_shadow=True - ) - - - - for i in range(3): - Entity( - model='cobblestone.glb', - scale=(5, 10, 20), - position=(30, 0, i * 158.5), - ) - for i in range(3): - Entity( - model='cobblestone.glb', - scale=(5, 10, 20), - position=(-58, 0, i * 158.5), - ) - - self.tunnel_road = Entity( - model='tunnel_3.glb', - scale=(63, 45, 45), - position=(-199, 3.0, 380), - rotation=(0, 0, 0), - collider='None', - cast_shadow=True - ) - - self.highway_road = Entity( - model='highway.glb', - scale=(20, 20, 5), - position=(-14, 2.5, 120), - rotation=(0, 90, 0), - collider='box', - cast_shadow=True - ) - - - self.barrier1 = Entity( - model='construction_barrier.glb', - scale=(3, 3, 3), - position=(24, 2.5, 315), - rotation=(0, 0, 0), - collider='box', - cast_shadow=True - ) - - self.barrier2 = Entity( - model='construction_barrier.glb', - scale=(3, 3, 3), - position=(-54, 2.5, 315), - rotation=(0, 0, 0), - collider='box', - cast_shadow=True - ) - - self.station = Entity( - model='gas_station_-_gta_v.glb', - scale=(12.7, 10, 10), - position=(-210, 2.5, 77), - rotation=(0, -90, 0), - ) - - - Entity( - model='dirty_leaking_concrete_wall.glb', - scale=(25, 20, 30), - position=(34.2, 2.5, 25), - rotation=(0, 90.5, 0), - collider='box', - cast_shadow=True - ) - - - Entity( - model='dirty_leaking_concrete_wall.glb', - scale=(25, 20, 30), - position=(34, 2.5, 227), - rotation=(0, 91, 0), - collider='box', - cast_shadow=True - ) - - self.first_person_view = False - # Create a separate entity to hold the camera - self.camera_holder = Entity(position=self.drone.position) - - self.drone_camera = EditorCamera() - self.drone_camera.parent = self.camera_holder - self.third_person_position = (0, 5, -15) - self.third_person_rotation = (10, 0, 0) - self.first_person_position = (0, 0.5, 22) - self.drone_camera.position = self.third_person_position - self.drone_camera.rotation = self.third_person_rotation - self.is_flying = False - - self.velocity = Vec3(0, 0, 0) - self.acceleration = Vec3(0, 0, 0) - self.drag = 0.93 - self.rotation_speed = 5 - self.max_speed = 1.8 - self.accel_force = 0.65 - - self.pitch_angle = 0 - self.roll_angle = 0 - self.max_pitch = 20 - self.max_roll = 20 - self.tilt_smoothness = 0.05 - - self.create_meters() - - def create_meters(self): - - # Main battery container - self.battery_container = Entity( - parent=camera.ui, - model=Quad(radius=0.01), - color=color.gray, - scale=(0.12, 0.04), - position=(0.74, 0.41), - z=-1 - ) - - # Battery cap - self.battery_cap = Entity( - parent=self.battery_container, - model=Quad(radius=0.004), - color=color.gray, - position=(0.52, 0), - scale=(0.05, 0.3), - rotation_z=0 - ) - - # Battery fill - self.battery_fill = Entity( - parent=self.battery_container, - model=Quad(radius=0.01), - color=color.green, - scale=(0.9, 0.7), - position=(-0.46, 0), - origin=(-0.5, 0), - z=-0.1 - ) - - # Altitude meter - self.altitude_meter = Text( - text=f"Altitude: {self.tello.altitude}m", - position=(0.67, 0.38), - scale=0.94, - color=color.white - ) - - # Warning text - self.warning_text = Text( - text="", - position=(-0.25, 0), - scale=3, - color=color.red - ) - - def update_meters(self): - """Update telemetry meters""" - battery = self.tello.get_battery() - - # Update battery fill width with padding - fill_width = 0.92 * (battery / 100) - self.battery_fill.scale_x = fill_width - - # color transitions (green → yellow → orange → red) - if battery > 60: - factor = (battery - 60) / 40 # 100-60%: green to yellow - col = lerp_color(color.yellow, color.green, factor) - elif battery > 30: - factor = (battery - 30) / 30 # 60-30%: yellow to orange - col = lerp_color(color.orange, color.yellow, factor) - else: - factor = battery / 30 # 30-0%: orange to red - col = lerp_color(color.red, color.orange, factor) - - self.battery_fill.color = col - - # Update altitude - self.altitude_meter.text = f"Altitude: {((self.drone.y) / 10 - 3/10):.1f}m" - - # Battery warning - current_time = time() % 1 - if battery <= 10 and battery > 0: - if current_time < 0.5: - self.warning_text.text = "Battery Low!" - else: - self.warning_text.text = "" - print("\n========== Battery Low! ==========\n") - elif battery == 0: - self.warning_text.text = "Battery Depleted!" - print("\n========== Battery Depleted! ==========\n") - else: - self.warning_text.text = "" - - def update_movement(self): - self.velocity += self.acceleration - - if self.velocity.length() > self.max_speed: - self.velocity = self.velocity.normalized() * self.max_speed - - self.velocity *= self.drag - new_position = self.drone.position + self.velocity - hit_info = raycast(self.drone.position, self.velocity.normalized(), distance=self.velocity.length(), ignore=(self.drone,)) - - if not hit_info.hit: - self.drone.position = new_position - - if self.drone.y < 3: - self.drone.y = 3 - - self.acceleration = Vec3(0, 0, 0) - - # Apply pitch and roll to the drone - self.drone.rotation_x = lerp(self.drone.rotation_x, self.pitch_angle, self.tilt_smoothness) - self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness) - - if self.first_person_view: - - self.camera_holder.position = self.drone.position - self.camera_holder.rotation_x = 0 # Keep horizon level - self.camera_holder.rotation_z = 0 # Prevent roll tilting - self.camera_holder.rotation_y = self.drone.rotation_y # yaw only - else: - # Third-person view - self.camera_holder.position = lerp(self.camera_holder.position, self.drone.position, 0.1) - self.camera_holder.rotation_y = self.drone.rotation_y # yaw only - self.drone_camera.rotation_x = 0 # Prevent pitch tilting - self.drone_camera.rotation_z = 0 # Prevent roll tilting - - self.update_meters() - - - def move(self, direction): - self.tello.move(direction, 1) - - if direction == "forward": - self.acceleration += self.drone.forward * self.accel_force - self.pitch_angle = self.max_pitch - elif direction == "backward": - self.acceleration -= self.drone.forward * self.accel_force - self.pitch_angle = -self.max_pitch - elif direction == "left": - self.acceleration -= self.drone.right * self.accel_force - self.roll_angle = -self.max_roll - elif direction == "right": - self.acceleration += self.drone.right * self.accel_force - self.roll_angle = self.max_roll - def toggle_camera_view(self): - self.first_person_view = not self.first_person_view - if self.first_person_view: - # First-person view - self.drone_camera.position = self.first_person_position - self.drone_camera.rotation = (0, 0, 0) - else: - # Third-person view - self.drone_camera.position = self.third_person_position - self.drone_camera.rotation = self.third_person_rotation - - def change_altitude(self, direction): - if direction == "up": - self.drone.y += 0.27 - self.tello.altitude += 0.27 - elif direction == "down" and self.drone.y > 3: - self.drone.y -= 0.20 - self.tello.altitude -= 0.20 - - def rotate(self, angle): - self.tello.rotate(angle) - self.drone.rotation_y = lerp(self.drone.rotation_y, self.drone.rotation_y + angle, 0.2) - - def update_pitch_roll(self): - self.drone.rotation_x = lerp(self.drone.rotation_x, self.pitch_angle, self.tilt_smoothness) - self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness) - - #def land_drone(self): - # """ Land the drone when battery reaches zero """ - # while self.drone.y > 0: - # self.drone.y -= 0.1 - # self.update_meters() - # self.drone.y = 0 - # self.warning_text.text = "Landed." - - -app = Ursina() -window.color = color.rgb(135, 206, 235) -window.fullscreen = True - -window.borderless = False -window.fps_counter.enabled = False -window.render_mode = 'default' - - -Sky(texture='sky_sunset') - -tello_sim = TelloSimulator() -drone_sim = DroneSimulator(tello_sim) - -def input(key): - if key == 'h': - drone_sim.help_text.visible = not drone_sim.help_text.visible - if key == 'c': - drone_sim.toggle_camera_view() -def update(): - moving = False - rolling = False - - if held_keys['w']: - drone_sim.move("forward") - moving = True - if held_keys['s']: - drone_sim.move("backward") - moving = True - if held_keys['a']: - drone_sim.move("left") - rolling = True - if held_keys['d']: - drone_sim.move("right") - rolling = True - if held_keys['j']: - drone_sim.rotate(-drone_sim.rotation_speed) - if held_keys['l']: - drone_sim.rotate(drone_sim.rotation_speed) - if held_keys['shift']: - drone_sim.change_altitude("up") - if held_keys['control']: - drone_sim.change_altitude("down") - if not moving: - drone_sim.pitch_angle = 0 # Reset pitch when not moving - - if not rolling: - drone_sim.roll_angle = 0 # Reset roll when not rolling - - drone_sim.update_movement() - drone_sim.update_pitch_roll() - -app.run() diff --git a/tello_drone.py b/tello_drone.py deleted file mode 100644 index 287c2ea..0000000 --- a/tello_drone.py +++ /dev/null @@ -1,458 +0,0 @@ -from ursina import * -from time import time - -def lerp_color(start_color, end_color, factor): - """Custom color interpolation function""" - return Color( - start_color.r + (end_color.r - start_color.r) * factor, - start_color.g + (end_color.g - start_color.g) * factor, - start_color.b + (end_color.b - start_color.b) * factor, - 1 # Alpha channel - ) - - -class TelloSimulator: - - def __init__(self): - self.battery_level = 100 - self.altitude = 3 - self.speed = 0 - self.rotation_angle = 0 - self.is_flying = False - self.start_time = time() - - def connect(self): - print("Tello Simulator: Drone connected") - return True - - def get_battery(self): - elapsed_time = time() - self.start_time - self.battery_level = max(100 - int((elapsed_time / 3600) * 100), 0) # Reduce battery over 60 min - return self.battery_level - - def takeoff(self): - if not self.is_flying: - print("Tello Simulator: Drone taking off") - self.is_flying = True - return "Taking off" - return "Already in air" - - def land(self): - if self.is_flying: - print("Tello Simulator: Drone landing") - self.is_flying = False - return "Landing" - return "Already on ground" - - def move(self, direction, distance=1): - print(f"Tello Simulator: Moving {direction} by {distance} meters") - - def rotate(self, angle): - self.rotation_angle += angle - print(f"Tello Simulator: Rotating {angle} degrees") - - #def start_video_stream(self): - #print("Tello Simulator: Starting video stream...") - - # def get_video_frame(self): - #return "Simulated video frame" - - -class DroneSimulator(Entity): - def __init__(self, tello_api, **kwargs): - super().__init__() - - self.help_text = Text( - text="Controls:\nW - Forward\nS - Backward\nA - Left\nD - Right\nShift - Launch/Up\nCtrl - Down\nJ - Rotate Left\nL - Rotate Right\nC - FPV\nH - Toggle Help", - position=(-0.85, 0.43), # Top-left position - scale=1.2, - color=color.white, - visible=True - ) - - self.tello = tello_api - self.drone = Entity( - model='tello.glb', - scale=0.06, - position=(-15.4, 3, 5), - collider='box', - cast_shadow=True - ) - - - self.car = Entity( - model='dirty_car.glb', - scale=0.085, - position=(10, 2.45, 155), - rotation=(0, 0, 0), - collider='box', - cast_shadow=True - ) - - - self.truck = Entity( - model='road_roller.glb', - scale=4.0, - position=(-150, 2.45, 155), - rotation=(0, -90, 0), - collider='box', - cast_shadow=True - ) - - self.road_closed = Entity( - model='road_closed.glb', - scale=7.0, - position=(-15, 2, 315), - rotation=(0, 90, 0), - collider='box', - cast_shadow=True - ) - - - self.business_man = Entity( - model='business_man.glb', - scale=7.3, - position=(23, 12, 155), - rotation=(0, 55, 0), - collider='box', - cast_shadow=True - ) - - self.man = Entity( - model='bos_standing.glb', - scale=10.3, - position=(-83, 2.8, 165), - rotation=(0, 120, 0), - collider='box', - cast_shadow=True - ) - - self.patch = Entity( - model='pipeline_construction_site.glb', - scale=(15, 15, 12), - position=(-123, 0.0, 260), - rotation=(0, 0, 0), - collider='none', - cast_shadow=True - ) - - self.police_man = Entity( - model='pig.glb', - scale=10.0, - position=(-35, 1.7, 230), - rotation=(0, -70, 0), - collider='box', - cast_shadow=True - ) - - self.light1 = Entity( - model='street_light.glb', - scale=(4, 6.5, 5), - position=(-55, 2.5, 260), - rotation=(0, -90, 0), - collider='none', - cast_shadow=True - ) - - - self.light2 = Entity( - model='street_light.glb', - scale=(4, 6.5, 5), - position=(25, 2.5, 95), - rotation=(0, 90, 0), - collider='none', - cast_shadow=True - ) - - self.light3 = Entity( - model='street_light.glb', - scale=(4, 6.5, 5), - position=(-55, 2.5, -70), - rotation=(0, -90, 0), - collider='none', - cast_shadow=True - ) - - - - for i in range(3): - Entity( - model='cobblestone.glb', - scale=(5, 10, 20), - position=(30, 0, i * 158.5), - ) - for i in range(3): - Entity( - model='cobblestone.glb', - scale=(5, 10, 20), - position=(-58, 0, i * 158.5), - ) - - self.tunnel_road = Entity( - model='tunnel_3.glb', - scale=(63, 45, 45), - position=(-199, 3.0, 380), - rotation=(0, 0, 0), - collider='None', - cast_shadow=True - ) - - self.highway_road = Entity( - model='highway.glb', - scale=(20, 20, 5), - position=(-14, 2.5, 120), - rotation=(0, 90, 0), - collider='box', - cast_shadow=True - ) - - - self.barrier1 = Entity( - model='construction_barrier.glb', - scale=(3, 3, 3), - position=(24, 2.5, 315), - rotation=(0, 0, 0), - collider='box', - cast_shadow=True - ) - - self.barrier2 = Entity( - model='construction_barrier.glb', - scale=(3, 3, 3), - position=(-54, 2.5, 315), - rotation=(0, 0, 0), - collider='box', - cast_shadow=True - ) - - self.station = Entity( - model='gas_station_-_gta_v.glb', - scale=(12.7, 10, 10), - position=(-210, 2.5, 77), - rotation=(0, -90, 0), - ) - - - Entity( - model='dirty_leaking_concrete_wall.glb', - scale=(25, 20, 30), - position=(34.2, 2.5, 25), - rotation=(0, 90.5, 0), - collider='box', - cast_shadow=True - ) - - - Entity( - model='dirty_leaking_concrete_wall.glb', - scale=(25, 20, 30), - position=(34, 2.5, 227), - rotation=(0, 91, 0), - collider='box', - cast_shadow=True - ) - - self.first_person_view = False - # Create a separate entity to hold the camera - self.camera_holder = Entity(position=self.drone.position) - - self.drone_camera = EditorCamera() - self.drone_camera.parent = self.camera_holder - self.third_person_position = (0, 5, -15) - self.third_person_rotation = (10, 0, 0) - self.first_person_position = (0, 0.5, 22) - self.drone_camera.position = self.third_person_position - self.drone_camera.rotation = self.third_person_rotation - self.is_flying = False - - self.velocity = Vec3(0, 0, 0) - self.acceleration = Vec3(0, 0, 0) - self.drag = 0.93 - self.rotation_speed = 5 - self.max_speed = 1.8 - self.accel_force = 0.65 - - self.pitch_angle = 0 - self.roll_angle = 0 - self.max_pitch = 20 - self.max_roll = 20 - self.tilt_smoothness = 0.05 - - self.create_meters() - - def create_meters(self): - - # Main battery container - self.battery_container = Entity( - parent=camera.ui, - model=Quad(radius=0.01), - color=color.gray, - scale=(0.12, 0.04), - position=(0.74, 0.41), - z=-1 - ) - - # Battery cap - self.battery_cap = Entity( - parent=self.battery_container, - model=Quad(radius=0.004), - color=color.gray, - position=(0.52, 0), - scale=(0.05, 0.3), - rotation_z=0 - ) - - # Battery fill - self.battery_fill = Entity( - parent=self.battery_container, - model=Quad(radius=0.01), - color=color.green, - scale=(0.9, 0.7), - position=(-0.46, 0), - origin=(-0.5, 0), - z=-0.1 - ) - - # Altitude meter - self.altitude_meter = Text( - text=f"Altitude: {self.tello.altitude}m", - position=(0.67, 0.38), - scale=0.94, - color=color.white - ) - - # Warning text - self.warning_text = Text( - text="", - position=(-0.25, 0), - scale=3, - color=color.red - ) - - def update_meters(self): - """Update telemetry meters""" - battery = self.tello.get_battery() - - # Update battery fill width with padding - fill_width = 0.92 * (battery / 100) - self.battery_fill.scale_x = fill_width - - # color transitions (green → yellow → orange → red) - if battery > 60: - factor = (battery - 60) / 40 # 100-60%: green to yellow - col = lerp_color(color.yellow, color.green, factor) - elif battery > 30: - factor = (battery - 30) / 30 # 60-30%: yellow to orange - col = lerp_color(color.orange, color.yellow, factor) - else: - factor = battery / 30 # 30-0%: orange to red - col = lerp_color(color.red, color.orange, factor) - - self.battery_fill.color = col - - # Update altitude - self.altitude_meter.text = f"Altitude: {int(self.drone.y - 3)}m" - - # Battery warning - current_time = time() % 1 - if battery <= 10 and battery > 0: - if current_time < 0.5: - self.warning_text.text = "Battery Low!" - else: - self.warning_text.text = "" - print("\n========== Battery Low! ==========\n") - elif battery == 0: - self.warning_text.text = "Battery Depleted!" - print("\n========== Battery Depleted! ==========\n") - else: - self.warning_text.text = "" - - def update_movement(self): - self.velocity += self.acceleration - - if self.velocity.length() > self.max_speed: - self.velocity = self.velocity.normalized() * self.max_speed - - self.velocity *= self.drag - new_position = self.drone.position + self.velocity - hit_info = raycast(self.drone.position, self.velocity.normalized(), distance=self.velocity.length(), ignore=(self.drone,)) - - if not hit_info.hit: - self.drone.position = new_position - - if self.drone.y < 3: - self.drone.y = 3 - - self.acceleration = Vec3(0, 0, 0) - - # Apply pitch and roll to the drone - self.drone.rotation_x = lerp(self.drone.rotation_x, self.pitch_angle, self.tilt_smoothness) - self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness) - - if self.first_person_view: - - self.camera_holder.position = self.drone.position - self.camera_holder.rotation_x = 0 # Keep horizon level - self.camera_holder.rotation_z = 0 # Prevent roll tilting - self.camera_holder.rotation_y = self.drone.rotation_y # yaw only - else: - # Third-person view - self.camera_holder.position = lerp(self.camera_holder.position, self.drone.position, 0.1) - self.camera_holder.rotation_y = self.drone.rotation_y # yaw only - self.drone_camera.rotation_x = 0 # Prevent pitch tilting - self.drone_camera.rotation_z = 0 # Prevent roll tilting - - self.update_meters() - - - def move(self, direction): - self.tello.move(direction, 1) - - if direction == "forward": - self.acceleration += self.drone.forward * self.accel_force - self.pitch_angle = self.max_pitch - elif direction == "backward": - self.acceleration -= self.drone.forward * self.accel_force - self.pitch_angle = -self.max_pitch - elif direction == "left": - self.acceleration -= self.drone.right * self.accel_force - self.roll_angle = -self.max_roll - elif direction == "right": - self.acceleration += self.drone.right * self.accel_force - self.roll_angle = self.max_roll - - def toggle_camera_view(self): - self.first_person_view = not self.first_person_view - if self.first_person_view: - # First-person view - self.drone_camera.position = self.first_person_position - self.drone_camera.rotation = (0, 0, 0) - else: - # Third-person view - self.drone_camera.position = self.third_person_position - self.drone_camera.rotation = self.third_person_rotation - - def change_altitude(self, direction): - if direction == "up": - self.drone.y += 0.27 - self.tello.altitude += 0.27 - elif direction == "down" and self.drone.y > 3: - self.drone.y -= 0.20 - self.tello.altitude -= 0.20 - - def rotate(self, angle): - self.tello.rotate(angle) - self.drone.rotation_y = lerp(self.drone.rotation_y, self.drone.rotation_y + angle, 0.2) - - def update_pitch_roll(self): - self.drone.rotation_x = lerp(self.drone.rotation_x, self.pitch_angle, self.tilt_smoothness) - self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness) - - #def land_drone(self): - # """ Land the drone when battery reaches zero """ - # while self.drone.y > 0: - # self.drone.y -= 0.1 - # self.update_meters() - # self.drone.y = 0 - # self.warning_text.text = "Landed." - diff --git a/tello_sim/__init__.py b/tello_sim/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tello_sim/command_server.py b/tello_sim/command_server.py new file mode 100644 index 0000000..17f2366 --- /dev/null +++ b/tello_sim/command_server.py @@ -0,0 +1,224 @@ +import os +import socket +from ursina import * +from cv2.typing import MatLike +from time import time +import cv2 +from ursina_adapter import UrsinaAdapter + +class CommandServer: + """ + Serves a TCP connections to receive and parse commands and forward controls to the simulator. + """ + + def __init__(self, ursina_adapter: UrsinaAdapter): + self._ursina_adapter = ursina_adapter + self.latest_frame = None + self.stream_active = False + self.last_altitude = 0 + self._recording_folder = "output/recordings" + + if not os.path.exists(self._recording_folder): + os.makedirs(self._recording_folder) + + def streamon(self): + """Start capturing screenshots and enable FPV video preview.""" + if not self.stream_active: + self.stream_active = True + self._ursina_adapter.stream_active = True + self.frame_count = 0 + self.saved_frames = [] + self.last_screenshot_time = time() + 3 # First capture after 3 sec + + if self._ursina_adapter: + self._ursina_adapter.toggle_camera_view() + + print("Tello Simulator: Video streaming started, FPV mode activated.") + + + def streamoff(self): + """Stop capturing screenshots""" + if self.stream_active: + self.stream_active = False + self._ursina_adapter.stream_active = False + cv2.destroyAllWindows() + print(f"[FPV] Video streaming stopped. Frames captured: {len(self.saved_frames)}") + + if self._ursina_adapter: + self._ursina_adapter.toggle_camera_view() + + def curve_xyz_speed(self, x1: float, y1: float, z1: float, x2: float, y2: float, z2: float, speed: float) -> None: + self._ursina_adapter.curve_xyz_speed(x1, y1, z1, x2, y2, z2, speed) + + def send_rc_control(self, left_right_velocity_ms: float, forward_backward_velocity_ms: float, up_down_velocity_ms: float, yaw_velocity_ms: float) -> None: + self._ursina_adapter.send_rc_control(left_right_velocity_ms, forward_backward_velocity_ms, up_down_velocity_ms, yaw_velocity_ms) + + def end(self): + self._ursina_adapter.end() + + def listen(self) -> None: + """ + Listens for commands to send to the Simulator + """ + server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + server.bind(('localhost', 9999)) # Port number for communication + server.listen(5) + print("[Command Listener] Listening on port 9999...") + + while True: + conn, _ = server.accept() + data = conn.recv(1024).decode() + if data: + print(f"[Command Listener] Received command: {data}") + + if data == "connect": + self._ursina_adapter.connect() + elif data == "takeoff": + self._ursina_adapter.takeoff() + elif data == "land": + self._ursina_adapter.land() + elif data == "flip_forward": + self._ursina_adapter.animate_flip(direction="forward") + elif data == "flip_back": + self._ursina_adapter.animate_flip(direction="back") + elif data == "flip_left": + self._ursina_adapter.animate_flip(direction="left") + elif data == "flip_right": + self._ursina_adapter.animate_flip(direction="right") + elif data == "streamon": + self.streamon() + elif data == "streamoff": + self.streamoff() + elif data == "emergency": + self._ursina_adapter.emergency() + elif data.startswith("forward"): + parts = data.split() + distance = float(parts[1]) if len(parts) > 1 else 10 + self._ursina_adapter.move("forward", distance) + + elif data.startswith("backward"): + parts = data.split() + distance = float(parts[1]) if len(parts) > 1 else 10 + self._ursina_adapter.move("backward", distance) + + elif data.startswith("left"): + parts = data.split() + distance = float(parts[1]) if len(parts) > 1 else 10 + self._ursina_adapter.move("left", distance) + + elif data.startswith("right"): + parts = data.split() + distance = float(parts[1]) if len(parts) > 1 else 10 + self._ursina_adapter.move("right", distance) + + elif data.startswith("up"): + parts = data.split() + distance = float(parts[1]) if len(parts) > 1 else 10 + self._ursina_adapter.change_altitude_smooth("up", distance) + + elif data.startswith("down"): + parts = data.split() + distance = float(parts[1]) if len(parts) > 1 else 10 + self._ursina_adapter.change_altitude_smooth("down", distance) + + + elif data.startswith("rotate_cw"): + parts = data.split() + angle = float(parts[1]) if len(parts) > 1 else 90 + self._ursina_adapter.rotate_smooth(angle) + + elif data.startswith("rotate_ccw"): + parts = data.split() + angle = float(parts[1]) if len(parts) > 1 else 90 + self._ursina_adapter.rotate_smooth(-angle) + + elif data.startswith("go"): + try: + _, x, y, z, speed = data.split() + self._ursina_adapter.go_xyz_speed(float(x), float(y), float(z), float(speed)) + except ValueError: + print("[Error] Invalid go command format") + + elif data.startswith("send_rc_control"): + try: + _, lr, fb, ud, yaw = data.split() + self.send_rc_control(float(lr), float(fb), float(ud), float(yaw)) + conn.send(b"RC control applied") + except ValueError: + conn.send(b"Invalid RC control parameters") + + elif data == "get_is_moving": + conn.send(str(self._ursina_adapter.is_moving).encode()) + + elif data.startswith("curve"): + try: + _, x1, y1, z1, x2, y2, z2, speed = data.split() + self.curve_xyz_speed(float(x1), float(y1), float(z1), + float(x2), float(y2), float(z2), + float(speed)) + except ValueError: + print("[Error] Invalid curve command format") + + elif data == "get_battery": + conn.send(str(self._ursina_adapter.get_battery()).encode()) + elif data == "get_distance_tof": + conn.send(str(100).encode()) + elif data == "get_height": + height = (self._ursina_adapter.drone.y / 10) - 0.3 + conn.send(f"{height:.1f}".encode()) + elif data == "get_flight_time": + conn.send(str(self._ursina_adapter.get_flight_time()).encode()) + elif data == "get_speed_x": + conn.send(str(self._ursina_adapter.get_speed_x()).encode()) + elif data == "get_speed_y": + conn.send(str(self._ursina_adapter.get_speed_y()).encode()) + elif data == "get_speed_z": + conn.send(str(self._ursina_adapter.get_speed_z()).encode()) + elif data == "get_acceleration_x": + conn.send(str(self._ursina_adapter.calculated_acceleration.x * 100).encode()) + elif data == "get_acceleration_y": + conn.send(str(self._ursina_adapter.calculated_acceleration.y * 100).encode()) + elif data == "get_acceleration_z": + conn.send(str(self._ursina_adapter.calculated_acceleration.z * 100).encode()) + elif data == "get_pitch": + conn.send(str(self._ursina_adapter.get_pitch()).encode()) + elif data == "get_roll": + conn.send(str(self._ursina_adapter.get_roll()).encode()) + elif data == "get_yaw": + raw_yaw = self._ursina_adapter.drone.rotation_y + yaw = ((raw_yaw + 180) % 360) - 180 + conn.send(str(yaw).encode()) + elif data == "query_attitude": + raw_yaw = self._ursina_adapter.drone.rotation_y + yaw = ((raw_yaw + 180) % 360) - 180 + attitude = { + "pitch": self._ursina_adapter.get_pitch(), + "roll": self._ursina_adapter.get_roll(), + "yaw": yaw + } + conn.send(str(attitude).encode()) + elif data == "get_current_state": + state = "flying" if self._ursina_adapter.is_flying else "landed" + conn.send(state.encode()) + + elif data == "get_latest_frame": + # Save the frame to disk first + frame_path = os.path.join(self._recording_folder, "latest_frame.png") + if self._ursina_adapter.latest_frame is not None: + cv2.imwrite(frame_path, self._ursina_adapter.latest_frame) + conn.send(frame_path.encode()) + else: + conn.send(b"N/A") + elif data == "capture_frame": + self._ursina_adapter.capture_frame() + elif data.startswith("set_speed"): + try: + _, speed = data.split() + self._ursina_adapter.set_speed(int(speed)) + except ValueError: + print("[Error] Invalid set_speed command format") + + elif data == "end": + self.end() + + conn.close() diff --git a/src/bos_standing.glb b/tello_sim/entities/bos_standing.glb similarity index 100% rename from src/bos_standing.glb rename to tello_sim/entities/bos_standing.glb diff --git a/src/business_man.glb b/tello_sim/entities/business_man.glb similarity index 100% rename from src/business_man.glb rename to tello_sim/entities/business_man.glb diff --git a/src/cobblestone.glb b/tello_sim/entities/cobblestone.glb similarity index 100% rename from src/cobblestone.glb rename to tello_sim/entities/cobblestone.glb diff --git a/src/construction_barrier.glb b/tello_sim/entities/construction_barrier.glb similarity index 100% rename from src/construction_barrier.glb rename to tello_sim/entities/construction_barrier.glb diff --git a/src/dirty_car.glb b/tello_sim/entities/dirty_car.glb similarity index 100% rename from src/dirty_car.glb rename to tello_sim/entities/dirty_car.glb diff --git a/src/dirty_leaking_concrete_wall.glb b/tello_sim/entities/dirty_leaking_concrete_wall.glb similarity index 100% rename from src/dirty_leaking_concrete_wall.glb rename to tello_sim/entities/dirty_leaking_concrete_wall.glb diff --git a/src/dji_mavic_3.glb b/tello_sim/entities/dji_mavic_3.glb similarity index 100% rename from src/dji_mavic_3.glb rename to tello_sim/entities/dji_mavic_3.glb diff --git a/src/gas_station_-_gta_v.glb b/tello_sim/entities/gas_station_-_gta_v.glb similarity index 100% rename from src/gas_station_-_gta_v.glb rename to tello_sim/entities/gas_station_-_gta_v.glb diff --git a/src/highway.glb b/tello_sim/entities/highway.glb similarity index 100% rename from src/highway.glb rename to tello_sim/entities/highway.glb diff --git a/src/pig.glb b/tello_sim/entities/pig.glb similarity index 100% rename from src/pig.glb rename to tello_sim/entities/pig.glb diff --git a/src/pipeline_construction_site.glb b/tello_sim/entities/pipeline_construction_site.glb similarity index 100% rename from src/pipeline_construction_site.glb rename to tello_sim/entities/pipeline_construction_site.glb diff --git a/src/road.glb b/tello_sim/entities/road.glb similarity index 100% rename from src/road.glb rename to tello_sim/entities/road.glb diff --git a/src/road_closed.glb b/tello_sim/entities/road_closed.glb similarity index 100% rename from src/road_closed.glb rename to tello_sim/entities/road_closed.glb diff --git a/src/road_roller.glb b/tello_sim/entities/road_roller.glb similarity index 100% rename from src/road_roller.glb rename to tello_sim/entities/road_roller.glb diff --git a/src/stone_ground_01.glb b/tello_sim/entities/stone_ground_01.glb similarity index 100% rename from src/stone_ground_01.glb rename to tello_sim/entities/stone_ground_01.glb diff --git a/src/street_light.glb b/tello_sim/entities/street_light.glb similarity index 100% rename from src/street_light.glb rename to tello_sim/entities/street_light.glb diff --git a/src/tello.glb b/tello_sim/entities/tello.glb similarity index 100% rename from src/tello.glb rename to tello_sim/entities/tello.glb diff --git a/src/tunnel_3.glb b/tello_sim/entities/tunnel_3.glb similarity index 100% rename from src/tunnel_3.glb rename to tello_sim/entities/tunnel_3.glb diff --git a/tello_sim/run_sim.py b/tello_sim/run_sim.py new file mode 100644 index 0000000..521216a --- /dev/null +++ b/tello_sim/run_sim.py @@ -0,0 +1,13 @@ +from tello_drone_sim import TelloDroneSim + + +sim = TelloDroneSim() + +def update(): + """ + This function must be global and is called every frame by Ursina. + """ + sim.update() + + +sim.start() diff --git a/tello_sim/tello_drone_sim.py b/tello_sim/tello_drone_sim.py new file mode 100644 index 0000000..6eb6e5a --- /dev/null +++ b/tello_sim/tello_drone_sim.py @@ -0,0 +1,21 @@ +from command_server import CommandServer +from ursina_adapter import UrsinaAdapter +import threading + +class TelloDroneSim: + def __init__(self): + self._ursina_adapter = UrsinaAdapter() + self._server = CommandServer(self._ursina_adapter) + + @property + def state(self): + return self._ursina_adapter + + def start(self): + server_thread = threading.Thread(target=self._server.listen) + server_thread.daemon = True + server_thread.start() + self._ursina_adapter.run() + + def update(self) -> None: + self._ursina_adapter.tick() \ No newline at end of file diff --git a/tello_sim/tello_sim_client.py b/tello_sim/tello_sim_client.py new file mode 100644 index 0000000..079e943 --- /dev/null +++ b/tello_sim/tello_sim_client.py @@ -0,0 +1,222 @@ +from dataclasses import dataclass +import logging +import subprocess +import platform +import sys +import time +import socket +import cv2 +import os +import numpy as np + +@dataclass +class BackgroundFrameRead(): + frame: cv2.typing.MatLike + +class TelloSimClient: + def __init__(self, host='localhost', port=9999, auto_start_simulation=True): + self.host = host + self.port = port + self.latest_frame = None + if auto_start_simulation and not self._check_simulation_running(): + self._start_simulation() + print("[Wrapper] Starting Tello Simulation...") + self._wait_for_simulation() + + def _start_simulation(self): + sim_path = os.path.abspath(os.path.join(os.path.dirname(__file__), 'tello_drone_sim.py')) + if platform.system() == "Windows": + command = f'start cmd /k python "{sim_path}"' + print("[DEBUG] Launching simulation command:", command) + subprocess.Popen(command, shell=True) + elif platform.system() == "Linux": + subprocess.Popen(['gnome-terminal', '--', 'python3', 'tello_drone_sim.py']) + elif platform.system() == "Darwin": + subprocess.Popen(['ls']) + subprocess.Popen(['pwd']) + python_path = os.path.join(os.path.dirname(sys.executable), 'python3') + print("Running python3 from path:", python_path) + subprocess.Popen([python_path, './tello_drone_sim.py'], cwd=os.getcwd(), + stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, + start_new_session=True) + else: + raise OSError("Unsupported OS for launching terminal simulation.") + + def _check_simulation_running(self): + try: + with socket.create_connection((self.host, self.port), timeout=1): + return True + except (ConnectionRefusedError, socket.timeout, OSError) as ex: + logging.error("[Wrapper] Simulation is not running.", ex) + return False + + def _wait_for_simulation(self, timeout=30): + print("[Wrapper] Waiting for simulation to become ready...") + start_time = time.time() + while time.time() - start_time < timeout: + if self._check_simulation_running(): + print("[Wrapper] Simulation is now ready!") + return + time.sleep(1) + raise TimeoutError("[Error] Simulation did not become ready in time.") + + def _send_command(self, command: str): + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.connect((self.host, self.port)) + s.send(command.encode()) + except ConnectionRefusedError: + print(f"[Error] Unable to connect to the simulation at {self.host}:{self.port}") + + def get_frame_read(self) -> BackgroundFrameRead: + frame_path = self._request_data('get_latest_frame') + if frame_path != "N/A" and os.path.exists(frame_path): + image = cv2.imread(frame_path) + if image is not None: + return BackgroundFrameRead(frame=cv2.cvtColor(image, cv2.COLOR_BGR2RGB)) + return BackgroundFrameRead(frame=np.zeros([360, 640, 3], dtype=np.uint8)) + + def _request_data(self, command): + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.connect((self.host, self.port)) + s.send(command.encode()) + return s.recv(4096).decode() + except ConnectionRefusedError: + print(f"[Error] Unable to retrieve '{command}' from {self.host}:{self.port}") + return "N/A" + + def wait_until_motion_complete(self): + while self._request_data("get_is_moving") == "True": + time.sleep(0.1) + + def get_battery(self): + return self._request_data('get_battery') + + def get_distance_tof(self): + return self._request_data('get_distance_tof') + + def get_height(self): + return self._request_data('get_height') + + def get_flight_time(self): + return self._request_data('get_flight_time') + + def get_speed_x(self): + return self._request_data('get_speed_x') + + def get_speed_y(self): + return self._request_data('get_speed_y') + + def get_speed_z(self): + return self._request_data('get_speed_z') + + def get_acceleration_x(self): + return self._request_data('get_acceleration_x') + + def get_acceleration_y(self): + return self._request_data('get_acceleration_y') + + def get_acceleration_z(self): + return self._request_data('get_acceleration_z') + + def get_pitch(self): + return self._request_data('get_pitch') + + def get_roll(self): + return self._request_data('get_roll') + + def get_yaw(self): + return self._request_data('get_yaw') + + def query_attitude(self): + return self._request_data('query_attitude') + + def get_current_state(self): + return self._request_data('get_current_state') + + def connect(self): + self._send_command('connect') + + def takeoff(self): + self._send_command('takeoff') + + def land(self): + self._send_command('land') + + def rotate_clockwise(self, degrees): + self._send_command(f'rotate_cw {degrees}') + + def rotate_counter_clockwise(self, degrees): + self._send_command(f'rotate_ccw {degrees}') + + def streamon(self): + self._send_command('streamon') + + def streamoff(self): + self._send_command('streamoff') + + def capture_frame(self): + self._send_command('capture_frame') + + def emergency(self): + self._send_command('emergency') + + def move_forward(self, distance): + self._send_command(f'forward {distance}') + + def move_back(self, distance): + self._send_command(f'backward {distance}') + + def move_left(self, distance): + self._send_command(f'left {distance}') + + def move_right(self, distance): + self._send_command(f'right {distance}') + + def move_up(self, distance): + self._send_command(f'up {distance}') + + def move_down(self, distance): + self._send_command(f'down {distance}') + + def flip_left(self): + self._send_command('flip_left') + + def flip_right(self): + self._send_command('flip_right') + + def flip_forward(self): + self._send_command('flip_forward') + + def flip_back(self): + self._send_command('flip_back') + + def go_xyz_speed(self, x, y, z, speed): + self._send_command(f"go {x} {y} {z} {speed}") + + def curve_xyz_speed(self, x1, y1, z1, x2, y2, z2, speed): + self._send_command(f"curve {x1} {y1} {z1} {x2} {y2} {z2} {speed}") + + def set_speed(self, speed): + self._send_command(f"set_speed {speed}") + + def send_rc_control(self, left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity): + self._send_command(f"send_rc_control {left_right_velocity} {forward_backward_velocity} {up_down_velocity} {yaw_velocity}") + + + def end(self): + self._send_command('end') + + def get_info(self): + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.connect((self.host, self.port)) + s.send(b'get_info') + return s.recv(4096).decode() + except ConnectionRefusedError: + print(f"[Error] Unable to retrieve info from {self.host}:{self.port}") + return "{}" + + def initiate_throw_takeoff(self): + self._send_command('throw_takeoff') \ No newline at end of file diff --git a/tello_sim/ursina_adapter.py b/tello_sim/ursina_adapter.py new file mode 100644 index 0000000..a44aad9 --- /dev/null +++ b/tello_sim/ursina_adapter.py @@ -0,0 +1,940 @@ +import os +from PIL import Image +from OpenGL.GL import glReadPixels, GL_RGBA, GL_UNSIGNED_BYTE +import numpy as np +from typing import Literal +import cv2 +import numpy as np +from ursina import ( + Ursina, + window, + color, + Entity, + camera, + Quad, + Circle, + sin, + EditorCamera, + Vec3, + Text, + invoke, + curve, + Color, + Sky, + raycast, + lerp, +) +from time import sleep, time +from cv2.typing import MatLike + + +class UrsinaAdapter(): + """ + A wrapper for managing the simulator state and drone controls in Ursina. + """ + + def __init__(self): + super().__init__() + + self.app = Ursina() + window.color = color.rgb(135, 206, 235) + window.fullscreen = False + window.borderless = False + window.fps_counter.enabled = False + window.render_mode = 'default' + self.command_queue = [] + self.is_moving = False + Sky(texture='sky_sunset') + + self.is_flying = False + self.battery_level = 100 + self.altitude = 0 + self.speed = 0 + self.rotation_angle = 0 + self.last_keys = {} + self.start_time = time() + self.last_time = self.start_time + self.stream_active = False + self.is_connected = False + self.recording_folder = "tello_recording" + self.frame_count = 0 + self.saved_frames = [] + self.screenshot_interval = 3 + self.latest_frame = None + self.last_screenshot_time = None + self.last_altitude = self.altitude + self.bezier_path = [] + self.bezier_duration = 0 + self.bezier_start_time = None + self.bezier_mode = False + self.dynamic_island = Entity( + parent=camera.ui, + model=Quad(radius=0.09), # Rounded rectangle + color=color.black50, # Slightly transparent black + scale=(0.5, 0.065), # Elongated shape + position=(0, 0.45), # Center top position + z=0 + ) + + # Takeoff Indicator UI + self.takeoff_indicator_left = Entity( + parent=camera.ui, + model=Circle(resolution=30), + color=color.green, + scale=(0.03, 0.03, 1), + position=(0.07, 0.45), + z=-1 + ) + + self.takeoff_indicator_middle = Entity( + parent=camera.ui, + model=Circle(resolution=30), + color=color.green, + scale=(0.03, 0.03, 1), + position=(0.12, 0.45), + z=-1 + ) + + self.takeoff_indicator_right = Entity( + parent=camera.ui, + model=Circle(resolution=30), + color=color.green, + scale=(0.03, 0.03, 1), + position=(0.17, 0.45), + z=-1 + ) + + self.drone = Entity( + model='entities/tello.glb', + scale=0.06, + position=(-15.4, 2.6, 5), + collider='box', + cast_shadow=True + ) + + self.car = Entity( + model='entities/dirty_car.glb', + scale=0.085, + position=(10, 2.45, 155), + rotation=(0, 0, 0), + collider='box', + cast_shadow=True + ) + + self.truck = Entity( + model='entities/road_roller.glb', + scale=4.0, + position=(-150, 2.45, 155), + rotation=(0, -90, 0), + collider='box', + cast_shadow=True + ) + + self.road_closed = Entity( + model='entities/road_closed.glb', + scale=7.0, + position=(-15, 2, 315), + rotation=(0, 90, 0), + collider='box', + cast_shadow=True + ) + + + self.business_man = Entity( + model='entities/business_man.glb', + scale=7.3, + position=(23, 12, 155), + rotation=(0, 55, 0), + collider='box', + cast_shadow=True + ) + + self.man = Entity( + model='entities/bos_standing.glb', + scale=10.3, + position=(-83, 2.8, 165), + rotation=(0, 120, 0), + collider='box', + cast_shadow=True + ) + + self.patch = Entity( + model='entities/pipeline_construction_site.glb', + scale=(15, 15, 12), + position=(-123, 0.0, 260), + rotation=(0, 0, 0), + collider='none', + cast_shadow=True + ) + + self.police_man = Entity( + model='entities/pig.glb', + scale=10.0, + position=(-35, 1.7, 230), + rotation=(0, -70, 0), + collider='box', + cast_shadow=True + ) + + self.light1 = Entity( + model='entities/street_light.glb', + scale=(4, 6.5, 5), + position=(-55, 2.5, 260), + rotation=(0, -90, 0), + collider='none', + cast_shadow=True + ) + + + self.light2 = Entity( + model='entities/street_light.glb', + scale=(4, 6.5, 5), + position=(25, 2.5, 95), + rotation=(0, 90, 0), + collider='none', + cast_shadow=True + ) + + self.light3 = Entity( + model='entities/street_light.glb', + scale=(4, 6.5, 5), + position=(-55, 2.5, -70), + rotation=(0, -90, 0), + collider='none', + cast_shadow=True + ) + + for i in range(3): + Entity( + model='entities/cobblestone.glb', + scale=(5, 10, 20), + position=(30, 0, i * 158.5), + ) + for i in range(3): + Entity( + model='entities/cobblestone.glb', + scale=(5, 10, 20), + position=(-58, 0, i * 158.5), + ) + + self.tunnel_road = Entity( + model='entities/tunnel_3.glb', + scale=(63, 45, 45), + position=(-199, 3.0, 380), + rotation=(0, 0, 0), + collider='None', + cast_shadow=True + ) + + self.highway_road = Entity( + model='entities/highway.glb', + scale=(20, 20, 5), + position=(-14, 2.5, 120), + rotation=(0, 90, 0), + collider='box', + cast_shadow=True + ) + + + self.barrier1 = Entity( + model='entities/construction_barrier.glb', + scale=(3, 3, 3), + position=(24, 2.5, 315), + rotation=(0, 0, 0), + collider='box', + cast_shadow=True + ) + + self.barrier2 = Entity( + model='entities/construction_barrier.glb', + scale=(3, 3, 3), + position=(-54, 2.5, 315), + rotation=(0, 0, 0), + collider='box', + cast_shadow=True + ) + + self.station = Entity( + model='entities/gas_station_-_gta_v.glb', + scale=(12.7, 10, 10), + position=(-210, 2.5, 77), + rotation=(0, -90, 0), + ) + + Entity( + model='entities/dirty_leaking_concrete_wall.glb', + scale=(25, 20, 30), + position=(34.2, 2.5, 25), + rotation=(0, 90.5, 0), + collider='box', + cast_shadow=True + ) + + Entity( + model='entities/dirty_leaking_concrete_wall.glb', + scale=(25, 20, 30), + position=(34, 2.5, 227), + rotation=(0, 91, 0), + collider='box', + cast_shadow=True + ) + + self.first_person_view = False + # Create a separate entity to hold the camera + self.camera_holder = Entity(position=self.drone.position, rotation=self.drone.rotation) + + self.drone_camera = EditorCamera() + self.drone_camera.parent = self.camera_holder + self.third_person_position = (0, 5, -15) + self.third_person_rotation = (10, 0, 0) + self.first_person_position = (0, 0.5, 22) + self.drone_camera.position = self.third_person_position + self.drone_camera.rotation = self.third_person_rotation + self.is_flying = False + + self.velocity: Vec3 = Vec3(0, 0, 0) + self.acceleration: Vec3 = Vec3(0, 0, 0) + self.calculated_acceleration: Vec3 = Vec3(0, 0, 0) + self.last_velocity_accel: Vec3 = Vec3(0, 0, 0) + self.last_time_accel = time() + self.drag: float = 0.93 + self.rotation_speed: float = 5 + self.max_speed = 1.8 + self.accel_force = 0.65 + + self.pitch_angle = 0 + self.roll_angle = 0 + self.max_pitch = 20 + self.max_roll = 20 + self.tilt_smoothness = 0.05 + + self.create_meters() + + def run(self): + self.app.run() + + def connect(self): + """Simulate connecting to the drone.""" + if not self.is_connected: + print("Tello Simulator: Connecting...") + sleep(1) + self.is_connected = True + print("Tello Simulator: Connection successful! Press 'Shift' to take off.") + + def get_current_fpv_view(self): + """ Capture the current FPV camera view and return it as a texture """ + return camera.texture # Get the current screen texture + + def update_takeoff_indicator(self): + """Blinking effect for takeoff status""" + pulse = (sin(time() * 5) + 1) / 2 + + if self.is_flying: + # Sky Blue Glow after Takeoff + glow_color = color.rgba(100/255, 180/255, 225/255, pulse * 0.6 + 0.4) + else: + # Green Glow before Takeoff + glow_color = color.rgba(0/255, 255/255, 0/255, pulse * 0.6 + 0.4) + + # Apply color changes to all three indicators + self.takeoff_indicator_left.color = glow_color + self.takeoff_indicator_middle.color = glow_color + self.takeoff_indicator_right.color = glow_color + + def animate_flip(self, direction: Literal["forward", "back", "left", "right"]) -> None: + + if direction == "forward": + self.drone.animate('rotation_x', 360, duration=0.6, curve=curve.linear) + elif direction == "back": + self.drone.animate('rotation_x', -360, duration=0.6, curve=curve.linear) + elif direction == "left": + self.drone.animate('rotation_z', -360, duration=0.6, curve=curve.linear) + elif direction == "right": + self.drone.animate('rotation_z', 360, duration=0.6, curve=curve.linear) + + # Reset rotation after flip + invoke(self.reset_rotation, delay=0.62) + + def reset_rotation(self): + + self.drone.rotation_x = 0 + self.drone.rotation_z = 0 + + + def _rotate(self, angle): + self.rotation_angle += angle + print(f"Tello Simulator: Rotating {angle} degrees") + + def create_meters(self): + + # Main battery container + self.battery_container = Entity( + parent=camera.ui, + model=Quad(radius=0.01), + color=color.gray, + scale=(0.14, 0.04), + position=(-0.12, 0.45), + z=-1 + ) + + # Battery cap + self.battery_cap = Entity( + parent=self.battery_container, + model=Quad(radius=0.004), + color=color.gray, + position=(0.52, 0), + scale=(0.05, 0.3), + rotation_z=0 + ) + + # Battery fill + self.battery_fill = Entity( + parent=self.battery_container, + model=Quad(radius=0.01), + color=color.green, + scale=(0.9, 0.7), + position=(-0.46, 0), + origin=(-0.5, 0), + z=-0.1 + ) + + metrics_x_position = 0.51 + + # Altitude meter + self.altitude_meter = Text( + text=f"Altitude: {self.altitude}m", + position=(metrics_x_position, 0.44), + scale=1.21, + color=color.white + ) + + # Warning text + self.warning_text = Text( + text="", + position=(-0.25, 0), + scale=3, + color=color.red + ) + + self.orientation_text = Text( + text="Pitch: 0° Roll: 0°", + position=(metrics_x_position, 0.41), # Below altitude meter + scale=0.97, + color=color.white + ) + + self.flight_time_text = Text( + text="Flight Time: 0s", + position=(metrics_x_position, 0.38), # Below Pitch, Roll, Yaw + scale=0.97, + color=color.white + ) + + self.speed_x_text = Text( + text="Speed X: 0 km/h", + position=(metrics_x_position, 0.35), # Below Flight Time + scale=0.94, + color=color.white + ) + + self.speed_y_text = Text( + text="Speed Y: 0 km/h", + position=(metrics_x_position, 0.32), # Below Speed X + scale=0.94, + color=color.white + ) + + self.speed_z_text = Text( + text="Speed Z: 0 km/h", + position=(metrics_x_position, 0.29), # Below Speed Y + scale=0.94, + color=color.white + ) + + @staticmethod + def lerp_color(start_color, end_color, factor): + """Custom color interpolation function""" + return Color( + start_color.r + (end_color.r - start_color.r) * factor, + start_color.g + (end_color.g - start_color.g) * factor, + start_color.b + (end_color.b - start_color.b) * factor, + 1 # Alpha channel + ) + + def get_battery(self) -> float: + elapsed_time = time() - self.start_time + self.battery_level = max(100 - int((elapsed_time / 3600) * 100), 0) # Reduce battery over 60 min + return self.battery_level + + + def get_flight_time(self) -> int: + """Return total flight time in seconds.""" + if self.is_flying: + return int(time() - self.start_time) + return 0 # Not flying + + def get_pitch(self) -> int: + return int(self.drone.rotation_x) + + def get_roll(self) -> int: + return int(self.drone.rotation_z) + + def get_speed_x(self) -> int: + return int(self.velocity.x * 3.6) + + def get_speed_y(self) -> int: + current_time = time() + elapsed_time = current_time - self.last_time + + if elapsed_time > 0: + current_altitude = (self.drone.y * 0.1) - 0.3 + vertical_speed = (current_altitude - self.last_altitude) / elapsed_time + self.last_altitude = current_altitude + self.last_time = current_time + return int(vertical_speed * 3.6) + return 0 + + def get_speed_z(self) -> int: + return int(self.velocity.z * 3.6) + + def get_acceleration_x(self) -> float: + """Return the current acceleration in the X direction.""" + return self.acceleration.x * 100 + + def get_acceleration_y(self) -> float: + """Return the current acceleration in the Y direction.""" + return self.acceleration.y * 100 + + def get_acceleration_z(self) -> float: + """Return the current acceleration in the Z direction.""" + return self.acceleration.z * 100 + + def rotate_smooth(self, angle): + current_yaw = self.drone.rotation_y + target_yaw = current_yaw + angle + duration = abs(angle) / 90 + self.drone.animate('rotation_y', target_yaw, duration=duration, curve=curve.linear) + print(f"Tello Simulator: Smoothly rotating {angle} degrees over {duration:.2f} seconds.") + + def change_altitude_smooth(self, direction: str, distance: float): + delta = distance / 20 + current_altitude = self.drone.y + + if direction == "up": + target_altitude = current_altitude + delta + elif direction == "down": + target_altitude = max(3, current_altitude - delta) + else: + print(f"Invalid altitude direction: {direction}") + return + + duration = abs(delta) * 1 + self.drone.animate('y', target_altitude, duration=duration, curve=curve.in_out_quad) + self.altitude = target_altitude + + def update_meters(self): + """Update telemetry meters""" + battery = self.get_battery() + + # Update battery fill width with padding + fill_width = 0.92 * (battery / 100) + self.battery_fill.scale_x = fill_width + + # color transitions (green → yellow → orange → red) + if battery > 60: + factor = (battery - 60) / 40 # 100-60%: green to yellow + col = UrsinaAdapter.lerp_color(color.yellow, color.green, factor) + elif battery > 30: + factor = (battery - 30) / 30 # 60-30%: yellow to orange + col = UrsinaAdapter.lerp_color(color.orange, color.yellow, factor) + else: + factor = battery / 30 # 30-0%: orange to red + col = UrsinaAdapter.lerp_color(color.red, color.orange, factor) + + self.battery_fill.color = col + + # Update altitude + self.altitude_meter.text = f"Altitude: {((self.drone.y) / 10 - 3/10):.1f}m" + + pitch = self.get_pitch() + roll = self.get_roll() + self.orientation_text.text = f"Pitch: {pitch}° Roll: {roll}°" + + flight_time = self.get_flight_time() + self.flight_time_text.text = f"Flight Time: {flight_time}s" + + # Update Speed X, Y, Z + speed_x = self.get_speed_x() + speed_y = self.get_speed_y() + speed_z = self.get_speed_z() + + self.speed_x_text.text = f"Speed X: {speed_x} km/h" + self.speed_y_text.text = f"Speed Y: {speed_y} km/h" + self.speed_z_text.text = f"Speed Z: {speed_z} km/h" + + + # Battery warning + current_time = time() % 1 + if battery <= 10 and battery > 0: + if current_time < 0.5: + self.warning_text.text = "Battery Low!" + else: + self.warning_text.text = "" + print("\n========== Battery Low! ==========\n") + + elif battery == 0: + self.warning_text.text = "Battery Depleted!" + print("\n========== Battery Depleted! ==========\n") + + # **Trigger Emergency Landing** + self.emergency() + + def update_movement(self) -> None: + self.velocity += self.acceleration + + if self.velocity is None: + raise Exception("Velocity is None") + + if self.velocity.length() > self.max_speed: + self.velocity = self.velocity.normalized() * self.max_speed + + self.velocity *= self.drag + new_position = self.drone.position + self.velocity + hit_info = raycast(self.drone.position, self.velocity.normalized(), distance=self.velocity.length(), ignore=(self.drone,)) # type: ignore + + if not hit_info.hit: + self.drone.position = new_position + + if self.drone.y < 3: + self.drone.y = 3 + + self.acceleration = Vec3(0, 0, 0) + + # Apply pitch and roll to the drone + self.drone.rotation_x = lerp(self.drone.rotation_x, self.pitch_angle, self.tilt_smoothness) + self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness) + current_time = time() + dt = current_time - self.last_time_accel + + if dt > 0: + velocity_change = self.velocity - self.last_velocity_accel + self.calculated_acceleration = velocity_change / dt # type: ignore + + self.last_velocity_accel = Vec3(self.velocity.x, self.velocity.y, self.velocity.z) + self.last_time_accel = current_time + if self.first_person_view: + + self.camera_holder.position = self.drone.position + self.camera_holder.rotation_x = 0 # Keep horizon level + self.camera_holder.rotation_z = 0 # Prevent roll tilting + self.camera_holder.rotation_y = self.drone.rotation_y # yaw only + else: + # Third-person view + self.camera_holder.position = lerp(self.camera_holder.position, self.drone.position, 0.1) + self.camera_holder.rotation_y = self.drone.rotation_y # yaw only + self.drone_camera.rotation_x = 10 # Prevent pitch tilting + self.drone_camera.rotation_z = 0 # Prevent roll tilting + + self.update_meters() + + def enqueue_command(self, command_func, *args, **kwargs): + self.command_queue.append((command_func, args, kwargs)) + if not self.is_moving: + self._execute_next_command() + + def _execute_next_command(self): + if not self.command_queue: + return + self.is_moving = True + command_func, args, kwargs = self.command_queue.pop(0) + command_func(*args, **kwargs) + + def move(self, direction: Literal["forward", "backward", "left", "right"], distance: float) -> None: + scale_factor = distance/10 + if direction == "forward": + forward_vector = self.drone.forward * self.accel_force * scale_factor + forward_vector.y = 0 + self.acceleration += forward_vector + self.pitch_angle = self.max_pitch + elif direction == "backward": + backward_vector = -self.drone.forward * self.accel_force * scale_factor + backward_vector.y = 0 + self.acceleration += backward_vector + self.pitch_angle = -self.max_pitch + elif direction == "left": + left_vector = -self.drone.right * self.accel_force * scale_factor + left_vector.y = 0 + self.acceleration += left_vector + self.roll_angle = -self.max_roll + elif direction == "right": + right_vector = self.drone.right * self.accel_force * scale_factor + right_vector.y = 0 + self.acceleration += right_vector + self.roll_angle = self.max_roll + + def toggle_camera_view(self) -> None: + self.first_person_view = not self.first_person_view + if self.first_person_view: + # First-person view + self.drone_camera.position = self.first_person_position + self.drone_camera.rotation = (0, 0, 0) + else: + # Third-person view + self.drone_camera.position = self.third_person_position + self.drone_camera.rotation = self.third_person_rotation + + def change_altitude(self, direction: Literal["up", "down"], distance: float=5) -> None: + delta = distance / 20 + if direction == "up": + self.drone.y += delta + self.altitude += delta + elif direction == "down" and self.drone.y > 3: + self.drone.y -= delta + self.altitude -= delta + + # TODO: Is this Radians or Degrees? We should put a suffix in the argument name + def rotate(self, angle: float) -> None: + self._rotate(angle) + self.drone.rotation_y = lerp(self.drone.rotation_y, self.drone.rotation_y + angle, 0.2) + + def update_pitch_roll(self) -> None: + self.drone.rotation_x = lerp(self.drone.rotation_x, self.pitch_angle, self.tilt_smoothness) + self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness) + + def send_rc_control(self, left_right_velocity_ms: float, forward_backward_velocity_ms: float, up_down_velocity_ms: float, yaw_velocity_ms: float): + + self.velocity = Vec3( + -left_right_velocity_ms / 100, # LEFT/RIGHT mapped to X + up_down_velocity_ms / 100, # UP/DOWN mapped to Y + forward_backward_velocity_ms / 100 # FORWARD/BACKWARD mapped to Z + ) + + self.drone.rotation_y += -yaw_velocity_ms * 0.05 # Smooth yaw rotation + print(f"[RC Control] Velocities set -> LR: {left_right_velocity_ms}, FB: {forward_backward_velocity_ms}, UD: {up_down_velocity_ms}, Yaw: {yaw_velocity_ms}") + + @staticmethod + def map_coords(x: float, y: float, z: float) -> Vec3: + """ + Maps the differences between normal robotics coordinates system to the Ursina Coordinate system + """ + # Simulator Z-axis is positive forwards, while Tello Z-axis is positive upwards + sim_z = x + # Simulator X-axis is positive right, while Tello Y-axis is positive left + sim_x = -y + # Simulator Y-axis is positive upwards, while Tello Z-axis is positive forwards + sim_y = z + return Vec3( + sim_x, + sim_y, + sim_z + ) + + def go_xyz_speed(self, x: float, y: float, z: float, speed_ms: float) -> None: + """ + Moves in a linear path to the specified coordinates at the given speed. + """ + def command(): + print(f"Tello Simulator: GO command to X:{x}, Y:{y}, Z:{z} at speed {speed_ms} cm/s") + + target_position = self.drone.position + self.map_coords(x / 10, y / 10, z / 10) + direction_vector = self.map_coords(x, 0, z) + if direction_vector.length() != 0: + direction_vector = direction_vector.normalized() + target_yaw = np.degrees(np.arctan2(direction_vector.x, direction_vector.z)) + else: + target_yaw = self.drone.rotation_y + + distance_cm = self.map_coords(x, y, z).length() + duration = max(0.5, distance_cm / speed_ms) + + self.drone.animate_position(target_position, duration=duration, curve=curve.in_out_cubic) + self.drone.animate('rotation_y', target_yaw, duration=duration, curve=curve.in_out_cubic) + invoke(self._motion_complete_callback, delay=duration) + + self.enqueue_command(command) + + # TODO: Is this Radians or Degrees? We should put a suffix in the argument name + + def start_bezier_motion(self, x1, y1, z1, x2, y2, z2, speed): + + # Define start, control and end points + start = self.drone.position + control = self.drone.position + self.map_coords(x1 / 10, y1 / 10, z1 / 10) + end = self.drone.position + self.map_coords(x2 / 10, y2 / 10, z2 / 10) + + self.bezier_path = [start, control, end] + + chord = (end - start).length() + cont_net = (control - start).length() + (end - control).length() + approx_length = (chord + cont_net) / 2 + self.bezier_duration = max(1.0, approx_length / speed) + self.bezier_start_time = time() + self.bezier_mode = True + + def curve_xyz_speed(self, x1: float, y1: float, z1: float, x2: float, y2: float, z2: float, speed: float) -> None: + def command(): + self.start_bezier_motion(x1, y1, z1, x2, y2, z2, speed) + self.enqueue_command(command) + + def takeoff(self) -> None: + if not self.is_flying: + print("Tello Simulator: Taking off...") + + self.is_flying = True + target_altitude = self.drone.y + 2 # Target altitude + self.drone.animate('y', target_altitude, duration=1, curve=curve.in_out_quad) + + print("Tello Simulator: Takeoff successful! You can now control the drone.") + else: + print("Tello Simulator: Already in air.") + + def _motion_complete_callback(self): + self.is_moving = False + self._execute_next_command() + def land(self) -> None: + if self.is_moving: + print("Tello Simulator: Movement in progress. Deferring landing...") + invoke(self.land, delay=1.0) + return + if self.is_flying: + print("Tello Simulator: Drone landing...") + current_altitude = self.drone.y + self.drone.animate('y', 2.6, duration=current_altitude * 0.5, curve=curve.in_out_quad) + self.is_flying = False + print("Landing initiated") + else: + print("Already on ground") + + def emergency(self) -> None: + if self.is_flying: + print(" Emergency! Stopping all motors and descending immediately!") + # Stop movement + self.velocity = Vec3(0, 0, 0) + self.acceleration = Vec3(0, 0, 0) + + # descent to altitude = 3 + self.drone.animate('y', 2.6, duration=1.5, curve=curve.linear) + + self.is_flying = False + print("Emergency landing initiated") + + print("Drone is already on the ground") + + def get_latest_frame(self) -> MatLike: + """Return the latest frame directly""" + if self.latest_frame is None: + raise Exception("No latest frame available.") + return cv2.cvtColor(self.latest_frame, cv2.COLOR_BGR2RGB) + + + def capture_frame(self): + """Capture and save the latest FPV frame from update()""" + if not self.stream_active: + print("[Capture] Stream not active. Cannot capture frame.") + return + + if self.latest_frame is None: + print("[Capture] No latest frame available.") + return + + frame_path = os.path.join(self.recording_folder, f"frame_{self.frame_count}.png") + cv2.imwrite(frame_path, self.latest_frame) + self.saved_frames.append(frame_path) + self.frame_count += 1 + print(f"[Capture] Screenshot {self.frame_count} saved: {frame_path}") + + def set_speed(self, x: int): + """Set drone speed by adjusting acceleration force. + + Arguments: + x (int): Speed in cm/s (10-100) + """ + if not (10 <= x <= 100): + print(" Invalid speed! Speed must be between 10 and 100 cm/s.") + return + + + self.accel_force = (x / 100) * 1.5 + print(f" Speed set to {x} cm/s. Acceleration force: {self.accel_force}") + + def end(self) -> None: + print("Tello Simulator: Ending simulation session...") + self.land() + self.is_connected = False + + + def tick(self) -> None: + """ + Update the simulator state + """ + if not self.is_connected: + return + + self.update_takeoff_indicator() + if self.stream_active: + width, height = int(window.size[0]), int(window.size[1]) + try: + pixel_data = glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE) + if pixel_data: + image = Image.frombytes("RGBA", (width, height), pixel_data) # type: ignore + image = image.transpose(Image.FLIP_TOP_BOTTOM) # type: ignore + frame = cv2.cvtColor(np.array(image), cv2.COLOR_RGBA2BGR) + + self.latest_frame = frame.copy() + #cv2.imshow("Tello FPV Stream", frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + self.stream_active = False + cv2.destroyAllWindows() + print("[FPV] FPV preview stopped.") + except Exception as e: + print(f"[FPV] OpenGL read error: {e}") + + if not self.is_flying: + self.camera_holder.position = self.drone.position + Vec3(0, 3, -7) + + return + + moving = False + rolling = False + + if self.bezier_mode: + t_now = time() + elapsed = t_now - self.bezier_start_time + t = min(1.0, elapsed / self.bezier_duration) + + # Bézier point + start, control, end = self.bezier_path + pos = (1 - t)**2 * start + 2 * (1 - t)*t * control + t**2 * end + self.drone.position = pos + + # Update yaw + if t < 0.99: + pos2 = (1 - t - 0.01)**2 * start + 2 * (1 - t - 0.01)*(t + 0.01) * control + (t + 0.01)**2 * end + dir_vec = pos2 - pos + if dir_vec.length() > 0: + yaw = np.degrees(np.arctan2(dir_vec.x, dir_vec.z)) + self.drone.rotation_y = lerp(self.drone.rotation_y, yaw, 0.1) + + # Update camera + self.camera_holder.position = pos + self.camera_holder.rotation_y = self.drone.rotation_y + + if t >= 1.0: + self.bezier_mode = False + self._motion_complete_callback() + + if self.stream_active: + self.capture_frame() + + + + if not moving: + self.pitch_angle = 0 # Reset pitch when not moving + + if not rolling: + self.roll_angle = 0 # Reset roll when not rolling + + self.update_movement() + self.update_pitch_roll() + + \ No newline at end of file