932 lines
32 KiB
Python
932 lines
32 KiB
Python
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),
|
|
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),
|
|
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),
|
|
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),
|
|
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),
|
|
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 the latest FPV frame. Optionally save to disk if save_frames_to_disk is True."""
|
|
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
|
|
|
|
# Always increment frame count for tracking
|
|
self.frame_count += 1
|
|
print(f"[Capture] Frame {self.frame_count} captured (memory only)")
|
|
|
|
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 # type: ignore
|
|
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()
|
|
|
|
|