kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

message type updates

Browse Source
This commit is contained in:
Kevin Dee
2026-02-01 16:02:18 -08:00
parent f6e1f13bb5
commit 330c1e5d54
10 changed files with 202 additions and 498 deletions
Download Patch File Download Diff File Expand all files Collapse all files

282
aerpaw/client/uav.py
View File

@@ -1,282 +0,0 @@
#!/usr/bin/env python3
"""
UAV Client for AERPAW Target Location Protocol
Protocol:
Server sends: TARGET:x,y,z (ENU coordinates in meters)
Client sends: ACK:TARGET
Client (after moving): READY
Server sends: FINISHED
Coordinates:
- Server sends local ENU (East, North, Up) coordinates in meters
- Client converts to lat/lon using shared origin from config/origin.txt
- Origin must match between controller and UAV
Auto-detection:
- If flight controller is available, uses aerpawlib to move drone
- If no flight controller, runs in test mode with placeholder movement
"""
import asyncio
import socket
import sys
import time
from pathlib import Path
from typing import Optional, Tuple
# Get the aerpaw directory (parent of client/)
AERPAW_DIR = Path(__file__).parent.parent
CONFIG_DIR = AERPAW_DIR / "config"
def load_origin(path: Path) -> Tuple[float, float, float]:
"""
Load ENU origin from config file.
Returns (lat, lon, alt) tuple.
"""
with open(path, 'r') as f:
for line in f:
line = line.strip()
if not line or line.startswith('#'):
continue
parts = line.split(',')
if len(parts) == 3:
return float(parts[0]), float(parts[1]), float(parts[2])
raise ValueError(f"No valid origin found in {path}")
def load_connection(path: Path) -> str:
"""
Load drone connection string from config file.
Returns connection string (e.g., 'udp:127.0.0.1:14550').
"""
with open(path, 'r') as f:
for line in f:
line = line.strip()
if not line or line.startswith('#'):
continue
return line
raise ValueError(f"No valid connection string found in {path}")
def load_server(path: Path) -> Tuple[str, int]:
"""
Load controller server address from config file.
Returns (ip, port) tuple.
"""
with open(path, 'r') as f:
for line in f:
line = line.strip()
if not line or line.startswith('#'):
continue
parts = line.split(',')
if len(parts) == 2:
return parts[0].strip(), int(parts[1].strip())
raise ValueError(f"No valid server address found in {path}")
def parse_target(message: str) -> Tuple[float, float, float]:
"""Parse TARGET:x,y,z message and return (x, y, z) ENU coordinates."""
if not message.startswith("TARGET:"):
raise ValueError(f"Invalid message format: {message}")
coords_str = message[7:] # Remove "TARGET:" prefix
parts = coords_str.split(",")
if len(parts) != 3:
raise ValueError(f"Expected 3 coordinates, got {len(parts)}")
x, y, z = float(parts[0]), float(parts[1]), float(parts[2])
return (x, y, z)
def enu_to_coordinate(origin_lat: float, origin_lon: float, origin_alt: float,
enu_x: float, enu_y: float, enu_z: float):
"""
Convert ENU (East, North, Up) coordinates to lat/lon/alt.
Args:
origin_lat, origin_lon, origin_alt: Origin in degrees/meters
enu_x: East offset in meters
enu_y: North offset in meters
enu_z: Up offset in meters (altitude above origin)
Returns:
aerpawlib Coordinate object
"""
from aerpawlib.util import Coordinate, VectorNED
origin = Coordinate(origin_lat, origin_lon, origin_alt)
# ENU to NED: north=enu_y, east=enu_x, down=-enu_z
offset = VectorNED(north=enu_y, east=enu_x, down=-enu_z)
return origin + offset
def try_connect_drone(conn_str: str, timeout: float = 10.0):
"""
Try to connect to drone flight controller.
Returns Drone object if successful, None if not available.
"""
try:
from aerpawlib.vehicle import Drone
print(f" Attempting to connect to flight controller: {conn_str}")
drone = Drone(conn_str)
print(f" Connected to flight controller")
return drone
except Exception as e:
print(f" Could not connect to flight controller: {e}")
return None
def try_connect_server(ip: str, port: int, timeout: float = 2.0) -> Optional[socket.socket]:
"""
Try to connect to controller server with timeout.
Returns socket if successful, None if connection fails.
"""
try:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.settimeout(timeout)
s.connect((ip, port))
s.settimeout(None) # Reset to blocking mode after connect
return s
except (socket.timeout, ConnectionRefusedError, OSError):
return None
def move_to_target_test(enu_x: float, enu_y: float, enu_z: float,
lat: float, lon: float, alt: float):
"""
Placeholder movement for test mode.
"""
print(f" [TEST MODE] Target ENU: ({enu_x}, {enu_y}, {enu_z}) meters")
print(f" [TEST MODE] Target lat/lon: ({lat:.6f}, {lon:.6f}, {alt:.1f})")
print(f" [TEST MODE] Simulating movement...")
time.sleep(1.0)
print(f" [TEST MODE] Arrived at target")
async def move_to_target_real(drone, target_coord, tolerance: float = 2.0):
"""
Move drone to target using aerpawlib.
"""
print(f" [REAL MODE] Moving to: ({target_coord.lat:.6f}, {target_coord.lon:.6f}, {target_coord.alt:.1f})")
await drone.goto_coordinates(target_coord, tolerance=tolerance)
print(f" [REAL MODE] Arrived at target")
async def run_uav_client(client_id: int):
"""Run a single UAV client."""
print(f"UAV {client_id}: Starting...")
# Load configuration
origin_path = CONFIG_DIR / "origin.txt"
connection_testbed_path = CONFIG_DIR / "connection_testbed.txt"
connection_local_path = CONFIG_DIR / "connection.txt"
server_testbed_path = CONFIG_DIR / "server_testbed.txt"
server_local_path = CONFIG_DIR / "server.txt"
print(f"UAV {client_id}: Loading origin from {origin_path}")
origin_lat, origin_lon, origin_alt = load_origin(origin_path)
print(f"UAV {client_id}: Origin: lat={origin_lat}, lon={origin_lon}, alt={origin_alt}")
# Load both connection configs
testbed_conn = load_connection(connection_testbed_path)
local_conn = load_connection(connection_local_path)
# Try testbed flight controller first (AERPAW MAVLink filter)
print(f"UAV {client_id}: Trying testbed flight controller: {testbed_conn}...")
drone = try_connect_drone(testbed_conn)
if not drone:
print(f"UAV {client_id}: Testbed not available, trying local: {local_conn}...")
drone = try_connect_drone(local_conn)
real_mode = drone is not None
if real_mode:
print(f"UAV {client_id}: Running in REAL mode")
else:
print(f"UAV {client_id}: Running in TEST mode (no flight controller)")
# Try testbed server first, fall back to local
testbed_ip, testbed_port = load_server(server_testbed_path)
local_ip, local_port = load_server(server_local_path)
print(f"UAV {client_id}: Trying testbed server at {testbed_ip}:{testbed_port}...")
s = try_connect_server(testbed_ip, testbed_port)
if s:
print(f"UAV {client_id}: Connected to testbed server")
else:
print(f"UAV {client_id}: Testbed server not available, trying local server at {local_ip}:{local_port}...")
s = try_connect_server(local_ip, local_port)
if s:
print(f"UAV {client_id}: Connected to local server")
else:
print(f"UAV {client_id}: ERROR - Could not connect to any server")
if drone:
drone.close()
return
try:
# Receive TARGET command
data = s.recv(1024).decode().strip()
print(f"UAV {client_id}: Received: {data}")
# Parse target coordinates (ENU)
enu_x, enu_y, enu_z = parse_target(data)
print(f"UAV {client_id}: Parsed ENU target: x={enu_x}, y={enu_y}, z={enu_z}")
# Convert ENU to lat/lon
target_coord = enu_to_coordinate(origin_lat, origin_lon, origin_alt,
enu_x, enu_y, enu_z)
print(f"UAV {client_id}: Target coordinate: lat={target_coord.lat:.6f}, lon={target_coord.lon:.6f}, alt={target_coord.alt:.1f}")
# Send acknowledgment
s.sendall(b"ACK:TARGET")
print(f"UAV {client_id}: Sent ACK:TARGET")
# Move to target
if real_mode:
await move_to_target_real(drone, target_coord)
else:
move_to_target_test(enu_x, enu_y, enu_z,
target_coord.lat, target_coord.lon, target_coord.alt)
# Signal ready
s.sendall(b"READY")
print(f"UAV {client_id}: Sent READY")
# Wait for FINISHED signal from server
data = s.recv(1024).decode().strip()
if data == "FINISHED":
print(f"UAV {client_id}: Received FINISHED - session ended normally")
else:
print(f"UAV {client_id}: Unexpected message: {data}")
except ValueError as e:
error_msg = f"ERROR:{str(e)}"
s.sendall(error_msg.encode())
print(f"UAV {client_id}: Error - {e}")
finally:
s.close()
if drone:
drone.close()
print(f"UAV {client_id}: Connection closed")
def main():
if len(sys.argv) != 2:
print(f"Usage: {sys.argv[0]} <client_id>")
print(" Run one instance per UAV, e.g.:")
print(" Terminal 1: ./uav.py 1")
print(" Terminal 2: ./uav.py 2")
sys.exit(1)
client_id = int(sys.argv[1])
asyncio.run(run_uav_client(client_id))
if __name__ == "__main__":
main()

140
aerpaw/client/uav_runner.py
View File

@@ -8,29 +8,40 @@ Run via:
Or use the auto-detecting wrapper:
./run_uav.sh
Protocol:
Server sends: TARGET:x,y,z (ENU coordinates in meters)
Client sends: ACK:TARGET
Client (after moving): READY
Server sends: RTL
Client sends: ACK:RTL
Client (after returning home): RTL_COMPLETE
Server sends: LAND
Client sends: ACK:LAND
Client (after landing): LAND_COMPLETE
Server sends: FINISHED
Binary Protocol:
Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles)
Client sends: ACK (1 byte)
Client (after moving): READY (1 byte)
Server sends: RTL (1 byte)
Client sends: ACK (1 byte)
Client (after returning home): READY (1 byte)
Server sends: LAND (1 byte)
Client sends: ACK (1 byte)
Client (after landing): READY (1 byte)
Server sends: READY (1 byte) - mission complete, disconnect
"""
from enum import IntEnum
from pathlib import Path
import asyncio
import os
import socket
import struct
import yaml
from aerpawlib.runner import BasicRunner, entrypoint
from aerpawlib.util import Coordinate, VectorNED
from aerpawlib.vehicle import Drone
# Message types - must match MESSAGE_TYPE.m enum
class MessageType(IntEnum):
TARGET = 1
ACK = 2
READY = 3
RTL = 4
LAND = 5
AERPAW_DIR = Path(__file__).parent.parent
CONFIG_FILE = AERPAW_DIR / "config" / "config.yaml"
@@ -56,15 +67,42 @@ def get_environment():
return env
def parse_target(message: str):
"""Parse TARGET:x,y,z message. Returns (x, y, z) ENU coordinates."""
if not message.startswith("TARGET:"):
raise ValueError(f"Invalid message format: {message}")
coords_str = message[7:]
parts = coords_str.split(",")
if len(parts) != 3:
raise ValueError(f"Expected 3 coordinates, got {len(parts)}")
return float(parts[0]), float(parts[1]), float(parts[2])
def recv_exactly(sock: socket.socket, n: int) -> bytes:
"""Receive exactly n bytes from socket."""
data = b''
while len(data) < n:
chunk = sock.recv(n - len(data))
if not chunk:
raise ConnectionError("Connection closed while receiving data")
data += chunk
return data
def recv_message_type(sock: socket.socket) -> MessageType:
"""Receive a single-byte message type."""
data = recv_exactly(sock, 1)
return MessageType(data[0])
def send_message_type(sock: socket.socket, msg_type: MessageType):
"""Send a single-byte message type."""
sock.sendall(bytes([msg_type]))
def recv_target(sock: socket.socket) -> tuple[float, float, float]:
"""Receive TARGET message (1 byte type + 3 doubles).
Returns (x, y, z) ENU coordinates.
"""
# First byte is message type (already consumed or check it)
msg_type = recv_message_type(sock)
if msg_type != MessageType.TARGET:
raise ValueError(f"Expected TARGET, got {msg_type.name}")
# Next 24 bytes are 3 doubles (little-endian)
data = recv_exactly(sock, 24)
x, y, z = struct.unpack('<ddd', data)
return x, y, z
class UAVRunner(BasicRunner):
@@ -117,12 +155,9 @@ class UAVRunner(BasicRunner):
await drone.takeoff(25)
print("[UAV] Takeoff complete, waiting for target...")
# Receive TARGET command
data = sock.recv(1024).decode().strip()
print(f"[UAV] Received: {data}")
enu_x, enu_y, enu_z = parse_target(data)
print(f"[UAV] Target ENU: x={enu_x}, y={enu_y}, z={enu_z}")
# Receive TARGET command (1 byte type + 24 bytes coords)
enu_x, enu_y, enu_z = recv_target(sock)
print(f"[UAV] Received TARGET: x={enu_x}, y={enu_y}, z={enu_z}")
# Convert ENU to lat/lon
# ENU: x=East, y=North, z=Up
@@ -131,8 +166,8 @@ class UAVRunner(BasicRunner):
print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
# Acknowledge
sock.sendall(b"ACK:TARGET")
print("[UAV] Sent ACK:TARGET")
send_message_type(sock, MessageType.ACK)
print(f"[UAV] Sent {MessageType.ACK.name}")
# Move to target
print("[UAV] Moving to target...")
@@ -140,52 +175,47 @@ class UAVRunner(BasicRunner):
print("[UAV] Arrived at target")
# Signal ready
sock.sendall(b"READY")
print("[UAV] Sent READY, waiting for commands...")
send_message_type(sock, MessageType.READY)
print(f"[UAV] Sent {MessageType.READY.name}, waiting for commands...")
# Command loop - handle RTL, LAND, FINISHED from controller
# Command loop - handle RTL, LAND, READY (mission complete) from controller
while True:
data = sock.recv(1024).decode().strip()
print(f"[UAV] Received: {data}")
msg_type = recv_message_type(sock)
print(f"[UAV] Received: {msg_type.name}")
if data == "RTL":
sock.sendall(b"ACK:RTL")
if msg_type == MessageType.RTL:
send_message_type(sock, MessageType.ACK)
print(f"[UAV] Sent {MessageType.ACK.name}")
print("[UAV] Returning to home...")
# Navigate to home lat/lon but stay at current altitude
# (AERPAW blocks goto_coordinates below 20m, but land() mode is allowed)
# Navigate to home lat/lon at safe altitude
home = drone.home_coords
current_alt = drone.position.alt
safe_alt = 25 # Set to 25m alt for RTL motion
rtl_target = Coordinate(home.lat, home.lon, safe_alt)
print(f"[UAV] RTL to {home.lat:.6f}, {home.lon:.6f} at {safe_alt:.1f}m")
await drone.goto_coordinates(rtl_target)
print("[UAV] Arrived at home position")
sock.sendall(b"RTL_COMPLETE")
print("[UAV] Sent RTL_COMPLETE")
send_message_type(sock, MessageType.READY)
print(f"[UAV] Sent {MessageType.READY.name}")
elif data == "LAND":
sock.sendall(b"ACK:LAND")
elif msg_type == MessageType.LAND:
send_message_type(sock, MessageType.ACK)
print(f"[UAV] Sent {MessageType.ACK.name}")
print("[UAV] Landing...")
await drone.land()
print("[UAV] Landed and disarmed")
sock.sendall(b"LAND_COMPLETE")
print("[UAV] Sent LAND_COMPLETE")
send_message_type(sock, MessageType.READY)
print(f"[UAV] Sent {MessageType.READY.name}")
elif data == "FINISHED":
print("[UAV] Received FINISHED - mission complete")
elif msg_type == MessageType.READY:
print("[UAV] Received READY from server - mission complete")
break
else:
print(f"[UAV] Unknown command: {data}")
print(f"[UAV] Unknown command: {msg_type}")
except ValueError as e:
error_msg = f"ERROR:{str(e)}"
try:
sock.sendall(error_msg.encode())
except Exception:
pass
except (ValueError, ConnectionError) as e:
print(f"[UAV] Error: {e}")
finally:
sock.close()
print("[UAV] Socket closed")
print("[UAV] Socket closed")