add KI
This commit is contained in:
Sebastian Unterschütz
@@ -1,5 +1,4 @@
|
||||
from dataclasses import dataclass
|
||||
import logging
|
||||
import subprocess
|
||||
import platform
|
||||
import sys
|
||||
@@ -9,255 +8,141 @@ import cv2
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
from djitellopy import Tello
|
||||
HAS_TELLO_LIB = True
|
||||
except ImportError:
|
||||
HAS_TELLO_LIB = False
|
||||
|
||||
@dataclass
|
||||
class BackgroundFrameRead():
|
||||
frame: cv2.typing.MatLike
|
||||
|
||||
class TelloSimClient:
|
||||
def __init__(self, host='localhost', port=9999, auto_start_simulation=True):
|
||||
def __init__(self, host='127.0.0.1', port=9999, auto_start_simulation=True, use_real_tello=False):
|
||||
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()
|
||||
self.use_real_tello = use_real_tello
|
||||
self.real_drone = None
|
||||
|
||||
if self.use_real_tello:
|
||||
if HAS_TELLO_LIB:
|
||||
print("[Client] Connecting to REAL Tello Hardware...")
|
||||
self.real_drone = Tello()
|
||||
else:
|
||||
print("[Error] djitellopy not found! Falling back to Simulator.")
|
||||
self.use_real_tello = False
|
||||
|
||||
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)
|
||||
if not self.use_real_tello:
|
||||
if auto_start_simulation and not self._check_simulation_running():
|
||||
self._start_simulation()
|
||||
self._wait_for_simulation()
|
||||
|
||||
def connect(self):
|
||||
if self.real_drone: self.real_drone.connect()
|
||||
else: self._send_command('connect')
|
||||
|
||||
def takeoff(self):
|
||||
if self.real_drone: self.real_drone.takeoff()
|
||||
else: self._send_command('takeoff')
|
||||
|
||||
def land(self):
|
||||
if self.real_drone: self.real_drone.land()
|
||||
else: self._send_command('land')
|
||||
|
||||
def streamon(self):
|
||||
if self.real_drone: self.real_drone.streamon()
|
||||
else: self._send_command('streamon')
|
||||
|
||||
def streamoff(self):
|
||||
if self.real_drone: self.real_drone.streamoff()
|
||||
else: self._send_command('streamoff')
|
||||
|
||||
def get_frame_read(self):
|
||||
if self.real_drone: return self.real_drone.get_frame_read()
|
||||
return self # Simulator acts as frame provider via TCP in get_frame_read-style
|
||||
|
||||
@property
|
||||
def frame(self):
|
||||
"""Helper for simulator to match BackgroundFrameRead structure"""
|
||||
return self._receive_tcp_frame()
|
||||
|
||||
def send_rc_control(self, lr, fb, ud, yaw):
|
||||
if self.real_drone: self.real_drone.send_rc_control(int(lr), int(fb), int(ud), int(yaw))
|
||||
else: self._send_command(f"send_rc_control {lr} {fb} {ud} {yaw}")
|
||||
|
||||
def get_yaw(self):
|
||||
if self.real_drone: return self.real_drone.get_yaw()
|
||||
return float(self._request_data('get_yaw') or 0)
|
||||
|
||||
def get_height(self):
|
||||
if self.real_drone: return self.real_drone.get_height()
|
||||
return float(self._request_data('get_height') or 0)
|
||||
|
||||
def get_distance_tof(self):
|
||||
if self.real_drone: return self.real_drone.get_distance_tof()
|
||||
return float(self._request_data('get_distance_tof') or 0)
|
||||
|
||||
def get_battery(self):
|
||||
if self.real_drone: return self.real_drone.get_battery()
|
||||
return int(self._request_data('get_battery') or 0)
|
||||
|
||||
def end(self):
|
||||
if self.real_drone:
|
||||
self.real_drone.streamoff()
|
||||
else:
|
||||
raise OSError("Unsupported OS for launching terminal simulation.")
|
||||
self._send_command('end')
|
||||
|
||||
# --- Internal Simulator Logic ---
|
||||
def _start_simulation(self):
|
||||
sim_path = os.path.abspath(os.path.join(os.path.dirname(__file__), 'tello_sim', 'tello_drone_sim.py'))
|
||||
print(f"[Wrapper] Launching Simulator: {sim_path}")
|
||||
clean_env = os.environ.copy()
|
||||
# Set Qt platform to xcb for better compatibility on Linux/Wayland
|
||||
clean_env["QT_QPA_PLATFORM"] = "xcb"
|
||||
# Clean PyCharm debugger vars
|
||||
for k in list(clean_env.keys()):
|
||||
if k.startswith('PYDEVD'): del clean_env[k]
|
||||
subprocess.Popen([sys.executable, sim_path], cwd=os.path.dirname(sim_path),
|
||||
env=clean_env, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, start_new_session=True)
|
||||
|
||||
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
|
||||
with socket.create_connection((self.host, self.port), timeout=0.5): return True
|
||||
except: 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
|
||||
def _wait_for_simulation(self, timeout=15):
|
||||
start = time.time()
|
||||
while time.time() - start < timeout:
|
||||
if self._check_simulation_running(): return
|
||||
time.sleep(1)
|
||||
raise TimeoutError("[Error] Simulation did not become ready in time.")
|
||||
print("[Warning] Simulator not responding.")
|
||||
|
||||
def _send_command(self, command: str):
|
||||
def _send_command(self, cmd):
|
||||
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:
|
||||
"""Get the latest frame directly from the simulator over TCP."""
|
||||
s.connect((self.host, self.port)); s.send(cmd.encode())
|
||||
except: pass
|
||||
|
||||
def _request_data(self, cmd):
|
||||
try:
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
||||
s.connect((self.host, self.port)); s.send(cmd.encode())
|
||||
return s.recv(1024).decode()
|
||||
except: return None
|
||||
|
||||
def _receive_tcp_frame(self):
|
||||
try:
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
||||
s.connect((self.host, self.port))
|
||||
s.send(b'get_latest_frame')
|
||||
|
||||
# Receive frame size (4 bytes)
|
||||
size_data = s.recv(4)
|
||||
if len(size_data) != 4:
|
||||
print("[Error] Failed to receive frame size")
|
||||
return BackgroundFrameRead(frame=np.zeros([360, 640, 3], dtype=np.uint8))
|
||||
|
||||
frame_size = int.from_bytes(size_data, byteorder='big')
|
||||
|
||||
# If size is 0, no frame available
|
||||
if frame_size == 0:
|
||||
print("[Debug] No frame available from simulator")
|
||||
return BackgroundFrameRead(frame=np.zeros([360, 640, 3], dtype=np.uint8))
|
||||
|
||||
# Receive the frame data
|
||||
frame_data = b''
|
||||
bytes_received = 0
|
||||
while bytes_received < frame_size:
|
||||
chunk = s.recv(min(4096, frame_size - bytes_received))
|
||||
if not chunk:
|
||||
break
|
||||
frame_data += chunk
|
||||
bytes_received += len(chunk)
|
||||
|
||||
# Decode the frame from PNG bytes
|
||||
if len(frame_data) == frame_size:
|
||||
nparr = np.frombuffer(frame_data, np.uint8)
|
||||
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
|
||||
if image is not None:
|
||||
# Return frame in BGR format (OpenCV's native format)
|
||||
# Users should convert to RGB if needed for display
|
||||
return BackgroundFrameRead(frame=image)
|
||||
|
||||
print("[Error] Failed to decode frame data")
|
||||
return BackgroundFrameRead(frame=np.zeros([360, 640, 3], dtype=np.uint8))
|
||||
|
||||
except ConnectionRefusedError:
|
||||
print(f"[Error] Unable to connect to the simulation at {self.host}:{self.port}")
|
||||
return BackgroundFrameRead(frame=np.zeros([360, 640, 3], dtype=np.uint8))
|
||||
except Exception as e:
|
||||
print(f"[Error] Failed to get frame: {e}")
|
||||
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')
|
||||
if not size_data: return None
|
||||
size = int.from_bytes(size_data, 'big')
|
||||
data = b''
|
||||
while len(data) < size:
|
||||
chunk = s.recv(min(size - len(data), 4096))
|
||||
if not chunk: break
|
||||
data += chunk
|
||||
return cv2.imdecode(np.frombuffer(data, np.uint8), cv2.IMREAD_COLOR)
|
||||
except: return None
|
||||
|
||||
Reference in New Issue
Block a user