From 1475d9e7d1673f62a1d65b04f0a1b41517ee2be0 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Sun, 1 Feb 2026 11:08:04 -0800 Subject: [PATCH] refactor experiment config --- aerpaw/client/uav_runner.py | 78 +++++++++++++--------------- aerpaw/config/config.yaml | 36 +++++++++++++ aerpaw/config/connection.txt | 7 --- aerpaw/config/connection_testbed.txt | 2 - aerpaw/config/origin.txt | 4 -- aerpaw/config/server.txt | 3 -- aerpaw/config/server_testbed.txt | 3 -- aerpaw/config/targets.txt | 2 - aerpaw/controller.m | 10 ++-- aerpaw/impl/controller_impl.cpp | 49 ++++++++++++----- aerpaw/loadTargetsFromYaml.m | 42 +++++++++++++++ aerpaw/run_uav.sh | 41 ++++++++------- 12 files changed, 180 insertions(+), 97 deletions(-) create mode 100644 aerpaw/config/config.yaml delete mode 100644 aerpaw/config/connection.txt delete mode 100644 aerpaw/config/connection_testbed.txt delete mode 100644 aerpaw/config/origin.txt delete mode 100644 aerpaw/config/server.txt delete mode 100644 aerpaw/config/server_testbed.txt delete mode 100644 aerpaw/config/targets.txt create mode 100644 aerpaw/loadTargetsFromYaml.m diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index dd4e1e5..f6a9918 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -22,40 +22,41 @@ Protocol: """ from pathlib import Path import asyncio +import os import socket +import yaml + from aerpawlib.runner import BasicRunner, entrypoint from aerpawlib.util import Coordinate, VectorNED from aerpawlib.vehicle import Drone AERPAW_DIR = Path(__file__).parent.parent -CONFIG_DIR = AERPAW_DIR / "config" +CONFIG_FILE = AERPAW_DIR / "config" / "config.yaml" -def load_origin(path: Path): - """Load ENU origin from config file. Returns (lat, lon, alt).""" - 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_config(): + """Load configuration from YAML file.""" + with open(CONFIG_FILE, 'r') as f: + return yaml.safe_load(f) -def load_server(path: Path): - """Load server address from config file. Returns (ip, port).""" - 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 detect_environment(): + """Detect whether we're on the AERPAW testbed or running locally.""" + # Check for AERPAW_ENV environment variable first + env = os.environ.get('AERPAW_ENV') + if env in ('local', 'testbed'): + return env + + # Auto-detect based on network (testbed uses 192.168.122.x) + import subprocess + try: + result = subprocess.run(['ip', 'addr'], capture_output=True, text=True, timeout=5) + if '192.168.122' in result.stdout: + return 'testbed' + except Exception: + pass + return 'local' def parse_target(message: str): @@ -71,26 +72,21 @@ def parse_target(message: str): class UAVRunner(BasicRunner): def initialize_args(self, extra_args): - """Load configuration from config files.""" + """Load configuration from YAML config file.""" + config = load_config() + env = detect_environment() + print(f"[UAV] Environment: {env}") + # Load origin - origin_lat, origin_lon, origin_alt = load_origin(CONFIG_DIR / "origin.txt") - self.origin = Coordinate(origin_lat, origin_lon, origin_alt) - print(f"[UAV] Origin: {origin_lat}, {origin_lon}, {origin_alt}") + origin = config['origin'] + self.origin = Coordinate(origin['lat'], origin['lon'], origin['alt']) + print(f"[UAV] Origin: {origin['lat']}, {origin['lon']}, {origin['alt']}") - # Load server address - try testbed first, fall back to local - testbed_path = CONFIG_DIR / "server_testbed.txt" - local_path = CONFIG_DIR / "server.txt" - - if testbed_path.exists(): - try: - self.server_ip, self.server_port = load_server(testbed_path) - print(f"[UAV] Loaded testbed server config: {self.server_ip}:{self.server_port}") - except ValueError: - self.server_ip, self.server_port = load_server(local_path) - print(f"[UAV] Loaded local server config: {self.server_ip}:{self.server_port}") - else: - self.server_ip, self.server_port = load_server(local_path) - print(f"[UAV] Loaded local server config: {self.server_ip}:{self.server_port}") + # Load controller address for this environment + env_config = config['environments'][env] + self.server_ip = env_config['controller']['ip'] + self.server_port = env_config['controller']['port'] + print(f"[UAV] Controller: {self.server_ip}:{self.server_port}") @entrypoint async def run_mission(self, drone: Drone): diff --git a/aerpaw/config/config.yaml b/aerpaw/config/config.yaml new file mode 100644 index 0000000..f73742f --- /dev/null +++ b/aerpaw/config/config.yaml @@ -0,0 +1,36 @@ +# AERPAW UAV Guidance System Configuration +# This file contains all settings for both simulation and testbed environments + +# ENU coordinate system origin (AERPAW Lake Wheeler Road Field) +origin: + lat: 35.727436 + lon: -78.696471 + alt: 0.0 # Alt=0 means ENU z directly becomes target altitude above home + +# Target positions in ENU coordinates (meters) +# Each target is assigned to a client in order: client 1 gets targets[0], etc. +targets: + - [10.5, 20.3, 45.0] # Client 1: east, north, up + - [25.0, 30.0, 30.0] # Client 2: east, north, up + +# Environment-specific settings +environments: + local: + # MAVLink connection for SITL simulation (UDP) + mavlink: + ip: "127.0.0.1" + port: 14550 + # Controller server address + controller: + ip: "127.0.0.1" + port: 5000 + + testbed: + # AERPAW testbed: E-VM listens, MAVLink Filter connects TO us (UDP) + mavlink: + ip: "192.168.32.26" + port: 14550 + # Controller runs on host machine (192.168.122.1 from E-VM perspective) + controller: + ip: "192.168.122.1" + port: 5000 \ No newline at end of file diff --git a/aerpaw/config/connection.txt b/aerpaw/config/connection.txt deleted file mode 100644 index 4472ee4..0000000 --- a/aerpaw/config/connection.txt +++ /dev/null @@ -1,7 +0,0 @@ -# Drone connection string for aerpawlib/dronekit -# Examples: -# tcp:127.0.0.1:5760 - SITL TCP -# udp:127.0.0.1:14550 - SITL UDP (default MAVProxy output) -# /dev/ttyUSB0 - Serial connection -# /dev/ttyACM0 - Serial connection (Pixhawk USB) -udp:127.0.0.1:14550 diff --git a/aerpaw/config/connection_testbed.txt b/aerpaw/config/connection_testbed.txt deleted file mode 100644 index 4c4795d..0000000 --- a/aerpaw/config/connection_testbed.txt +++ /dev/null @@ -1,2 +0,0 @@ -# AERPAW testbed - connect to MAVLink filter on E-VM -192.168.32.26:14550 diff --git a/aerpaw/config/origin.txt b/aerpaw/config/origin.txt deleted file mode 100644 index 35b7540..0000000 --- a/aerpaw/config/origin.txt +++ /dev/null @@ -1,4 +0,0 @@ -# Origin for ENU coordinate system (AERPAW Lake Wheeler Road Field) -# Format: lat,lon,alt (degrees, degrees, meters) -# Alt=0 means ENU z directly becomes the target altitude above home -35.727436,-78.696471,0.0 diff --git a/aerpaw/config/server.txt b/aerpaw/config/server.txt deleted file mode 100644 index ff725ff..0000000 --- a/aerpaw/config/server.txt +++ /dev/null @@ -1,3 +0,0 @@ -# Local/fallback controller server address -# Format: ip,port -127.0.0.1,5000 \ No newline at end of file diff --git a/aerpaw/config/server_testbed.txt b/aerpaw/config/server_testbed.txt deleted file mode 100644 index 21228da..0000000 --- a/aerpaw/config/server_testbed.txt +++ /dev/null @@ -1,3 +0,0 @@ -# AERPAW testbed controller server address -# Format: ip,port -192.168.122.1,5000 \ No newline at end of file diff --git a/aerpaw/config/targets.txt b/aerpaw/config/targets.txt deleted file mode 100644 index 3165440..0000000 --- a/aerpaw/config/targets.txt +++ /dev/null @@ -1,2 +0,0 @@ -10.5,20.3,45.0 -25.0,30.0,30.0 \ No newline at end of file diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 955e1e2..e7f51c9 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -3,7 +3,7 @@ arguments (Input) numClients (1, 1) int32; end -coder.extrinsic('disp', 'readmatrix'); +coder.extrinsic('disp', 'loadTargetsFromYaml'); % Maximum clients supported MAX_CLIENTS = 4; @@ -11,17 +11,17 @@ MAX_CLIENTS = 4; % Allocate targets array (MAX_CLIENTS x 3) targets = zeros(MAX_CLIENTS, 3); -% Load targets from file +% Load targets from YAML config file if coder.target('MATLAB') - disp('Loading targets from file (simulation)...'); - targetsLoaded = readmatrix('aerpaw/config/targets.txt'); + disp('Loading targets from config.yaml (simulation)...'); + targetsLoaded = loadTargetsFromYaml('aerpaw/config/config.yaml'); numTargets = min(size(targetsLoaded, 1), numClients); targets(1:numTargets, :) = targetsLoaded(1:numTargets, :); disp(['Loaded ', num2str(numTargets), ' targets']); else coder.cinclude('controller_impl.h'); % Define filename as null-terminated character array for C compatibility - filename = ['config/targets.txt', char(0)]; + filename = ['config/config.yaml', char(0)]; % loadTargets fills targets array (column-major for MATLAB compatibility) coder.ceval('loadTargets', coder.ref(filename), ... coder.ref(targets), int32(MAX_CLIENTS)); diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index 3736371..aba545c 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -82,22 +82,47 @@ void closeServer() { int loadTargets(const char* filename, double* targets, int maxClients) { FILE* file = fopen(filename, "r"); if (!file) { - std::cerr << "Failed to open targets file: " << filename << "\n"; + std::cerr << "Failed to open config file: " << filename << "\n"; return 0; } int count = 0; - double x, y, z; - // MATLAB uses column-major order, so for a maxClients x 3 matrix: - // Column 1 (x): indices 0, 1, 2, ... - // Column 2 (y): indices maxClients, maxClients+1, ... - // Column 3 (z): indices 2*maxClients, 2*maxClients+1, ... - while (count < maxClients && fscanf(file, "%lf,%lf,%lf", &x, &y, &z) == 3) { - targets[count + 0 * maxClients] = x; // Column 1 - targets[count + 1 * maxClients] = y; // Column 2 - targets[count + 2 * maxClients] = z; // Column 3 - std::cout << "Loaded target " << (count + 1) << ": " << x << "," << y << "," << z << "\n"; - count++; + char line[256]; + bool inTargets = false; + + // Simple YAML parser for targets section + // Expects format: + // targets: + // - [x, y, z] + // - [x, y, z] + while (fgets(line, sizeof(line), file) && count < maxClients) { + // Check if we've entered the targets section + if (strstr(line, "targets:") != nullptr) { + inTargets = true; + continue; + } + + // If we hit another top-level key (no leading whitespace), exit targets section + if (inTargets && line[0] != ' ' && line[0] != '\t' && line[0] != '\n' && line[0] != '#') { + break; + } + + // Parse target entries: " - [x, y, z]" + if (inTargets) { + double x, y, z; + // Try to match the array format + if (sscanf(line, " - [%lf, %lf, %lf]", &x, &y, &z) == 3) { + // MATLAB uses column-major order, so for a maxClients x 3 matrix: + // Column 1 (x): indices 0, 1, 2, ... + // Column 2 (y): indices maxClients, maxClients+1, ... + // Column 3 (z): indices 2*maxClients, 2*maxClients+1, ... + targets[count + 0 * maxClients] = x; + targets[count + 1 * maxClients] = y; + targets[count + 2 * maxClients] = z; + std::cout << "Loaded target " << (count + 1) << ": " << x << "," << y << "," << z << "\n"; + count++; + } + } } fclose(file); diff --git a/aerpaw/loadTargetsFromYaml.m b/aerpaw/loadTargetsFromYaml.m new file mode 100644 index 0000000..c68d8f9 --- /dev/null +++ b/aerpaw/loadTargetsFromYaml.m @@ -0,0 +1,42 @@ +function targets = loadTargetsFromYaml(filename) +%LOADTARGETSFROMYAML Load target coordinates from YAML config file +% targets = loadTargetsFromYaml(filename) reads the targets section +% from a YAML config file and returns an Nx3 matrix of [x, y, z] coordinates. + +targets = []; +fid = fopen(filename, 'r'); +if fid == -1 + error('Could not open file: %s', filename); +end + +inTargets = false; +while ~feof(fid) + line = fgetl(fid); + if ~ischar(line) + break; + end + + % Check if we've entered the targets section + if contains(line, 'targets:') + inTargets = true; + continue; + end + + % If we hit another top-level key, exit targets section + if inTargets && ~isempty(line) && line(1) ~= ' ' && line(1) ~= '#' + break; + end + + % Parse target entries: " - [x, y, z]" + if inTargets + % Extract numbers from array format + match = regexp(line, '\[\s*([-\d.]+)\s*,\s*([-\d.]+)\s*,\s*([-\d.]+)\s*\]', 'tokens'); + if ~isempty(match) + coords = str2double(match{1}); + targets = [targets; coords]; %#ok + end + end +end + +fclose(fid); +end diff --git a/aerpaw/run_uav.sh b/aerpaw/run_uav.sh index 7cbd0f2..e263bda 100755 --- a/aerpaw/run_uav.sh +++ b/aerpaw/run_uav.sh @@ -17,39 +17,44 @@ if [ -d "venv" ]; then source venv/bin/activate fi -# Connection strings -# AERPAW testbed: E-VM LISTENS on port 14550, MAVLink Filter connects TO us -# See: https://sites.google.com/ncsu.edu/aerpaw-user-manual/6-sample-experiments-repository/6-2-vehicle-control-software/vcs1-preplanned-trajectory -# "the IP address corresponds to the E-VM IP address and the port is where -# the MAVLink filter expects to find the vehicle control script" -TESTBED_CONN="udp:0.0.0.0:14550" -LOCAL_CONN="udp:127.0.0.1:14550" - # Function to check if we're in testbed environment check_testbed() { - # Check if we're on the AERPAW testbed network (192.168.122.X) ip addr | grep -q "192.168.122" return $? } -# Determine connection based on argument or auto-detection +# Determine environment based on argument or auto-detection if [ "$1" = "testbed" ]; then - CONN="$TESTBED_CONN" - echo "[run_uav] Forced testbed mode: $CONN" + ENV="testbed" + echo "[run_uav] Forced testbed mode" elif [ "$1" = "local" ]; then - CONN="$LOCAL_CONN" - echo "[run_uav] Forced local mode: $CONN" + ENV="local" + echo "[run_uav] Forced local mode" else echo "[run_uav] Auto-detecting environment..." if check_testbed; then - CONN="$TESTBED_CONN" - echo "[run_uav] Testbed detected, using: $CONN" + ENV="testbed" + echo "[run_uav] Testbed detected" else - CONN="$LOCAL_CONN" - echo "[run_uav] Local mode (testbed not reachable), using: $CONN" + ENV="local" + echo "[run_uav] Local mode" fi fi +# Export environment for Python to use +export AERPAW_ENV="$ENV" + +# Read MAVLink connection from config.yaml using Python +CONN=$(python3 -c " +import yaml +with open('config/config.yaml') as f: + cfg = yaml.safe_load(f) +env = cfg['environments']['$ENV']['mavlink'] +print(f\"udp:{env['ip']}:{env['port']}\") +") + +echo "[run_uav] MAVLink connection: $CONN" + # Run via aerpawlib echo "[run_uav] Starting UAV runner..." python3 -m aerpawlib \