From 387d6aea566df3b4294d2fb5e4d01ab24e1cc499 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Sat, 28 Feb 2026 19:36:58 -0800 Subject: [PATCH] scenario csv on both platforms --- @miSim/constrainMotion.m | 8 +- @miSim/initializeFromCsv.m | 112 +++++++- @miSim/readScenarioCsv.m | 79 ++++-- aerpaw/config/scenario.csv | 2 + aerpaw/config/server.yaml | 23 -- aerpaw/controller.coderprj | 251 ++++++++---------- aerpaw/controller.m | 65 +++-- aerpaw/guidance_step.m | 171 ++++++------ aerpaw/impl/controller_impl.cpp | 249 +++++++++++++++++ aerpaw/impl/controller_impl.h | 25 ++ aerpaw/results/plotGpsCsvs.m | 6 +- .../BIoAmslL9uGQPg5zxemTUHYw4Z4p.xml | 2 - ...d.xml => e9CQ_K5hshi9GcW6IwNtfuxqFYwd.xml} | 0 .../e9CQ_K5hshi9GcW6IwNtfuxqFYwp.xml | 2 + test/parametricTestSuite.m | 37 +++ 15 files changed, 739 insertions(+), 293 deletions(-) create mode 100644 aerpaw/config/scenario.csv delete mode 100644 aerpaw/config/server.yaml delete mode 100644 resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/BIoAmslL9uGQPg5zxemTUHYw4Z4p.xml rename resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/{BIoAmslL9uGQPg5zxemTUHYw4Z4d.xml => e9CQ_K5hshi9GcW6IwNtfuxqFYwd.xml} (100%) create mode 100644 resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/e9CQ_K5hshi9GcW6IwNtfuxqFYwp.xml diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 1d9da7c..55dc091 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -49,7 +49,13 @@ function [obj] = constrainMotion(obj) r_sum_ij = obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius; v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep; slack = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent); - b(kk) = obj.barrierGain * max(slack, h(ii, jj))^obj.barrierExponent; + if norm(A(kk, :)) < 1e-9 + % Agents are coincident: A-row is zero, so b < 0 would make + % 0 ≤ b unsatisfiable. Fall back to b = 0 (no correction possible). + b(kk) = 0; + else + b(kk) = obj.barrierGain * max(slack, h(ii, jj))^obj.barrierExponent; + end kk = kk + 1; end end diff --git a/@miSim/initializeFromCsv.m b/@miSim/initializeFromCsv.m index c1faa20..b296ea8 100644 --- a/@miSim/initializeFromCsv.m +++ b/@miSim/initializeFromCsv.m @@ -1,12 +1,106 @@ function obj = initializeFromCsv(obj, csvPath) - arguments (Input) - obj (1, 1) {mustBeA(obj, 'miSim')}; - csvPath (1, 1) string; - end - arguments (Output) - obj (1, 1) {mustBeA(obj, 'miSim')}; - end +% INITIALIZEFROMCSV Initialize miSim from an AERPAW scenario CSV file. +% +% Reads all guidance parameters, domain geometry, initial UAV positions, +% and obstacle definitions from the CSV, then builds and initialises the +% simulation. Ends by calling the standard obj.initialize(...) method. +% +% This is the MATLAB-path counterpart to the compiled path that unpacks a +% flat scenarioParams array in guidance_step.m. It is only ever called +% from within a coder.target('MATLAB') guard and is never compiled. +% +% Usage (inside guidance_step.m on MATLAB path): +% sim = sim.initializeFromCsv('aerpaw/config/scenario.csv'); +% +% Expected CSV columns (see scenario.csv): +% timestep, maxIter, minAlt, discretizationStep, protectedRange, +% initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, +% alphaDist, betaDist, alphaTilt, betaTilt, +% domainMin ("x,y,z"), domainMax ("x,y,z"), objectivePos ("x,y"), +% initialPositions (flat "x1,y1,z1, x2,y2,z2,..."), +% numObstacles, obstacleMin (flat), obstacleMax (flat) - params = obj.readScenarioCsv(tc.csvPath); +arguments (Input) + obj (1, 1) {mustBeA(obj, 'miSim')}; + csvPath (1, 1) string; +end +arguments (Output) + obj (1, 1) {mustBeA(obj, 'miSim')}; +end -end \ No newline at end of file +% ---- Parse CSV via readScenarioCsv --------------------------------------- +scenario = obj.readScenarioCsv(csvPath); + +TIMESTEP = scenario.timestep; +MAX_ITER = scenario.maxIter; +MIN_ALT = scenario.minAlt; +DISCRETIZATION_STEP = scenario.discretizationStep; +PROTECTED_RANGE = scenario.protectedRange; +INITIAL_STEP_SIZE = scenario.initialStepSize; +BARRIER_GAIN = scenario.barrierGain; +BARRIER_EXPONENT = scenario.barrierExponent; +COLLISION_RADIUS = scenario.collisionRadius; +COMMS_RANGE = scenario.comRange; +ALPHA_DIST = scenario.alphaDist; +BETA_DIST = scenario.betaDist; +ALPHA_TILT = scenario.alphaTilt; +BETA_TILT = scenario.betaTilt; + +DOMAIN_MIN = scenario.domainMin; % 1×3 +DOMAIN_MAX = scenario.domainMax; % 1×3 +OBJECTIVE_GROUND_POS = scenario.objectivePos; % 1×2 +SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar + +% Initial UAV positions: flat vector reshaped to N×3 +flatPos = scenario.initialPositions; % 1×(3*N) +assert(mod(numel(flatPos), 3) == 0, ... + "initialPositions must have a multiple of 3 values; got %d", numel(flatPos)); +positions = reshape(flatPos, 3, [])'; % N×3 +numAgents = size(positions, 1); + +numObstacles = scenario.numObstacles; + +% ---- Build domain -------------------------------------------------------- +dom = rectangularPrism; +dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain"); + +% ---- Build sensing objective (MATLAB path: objectiveFunctionWrapper) ----- +dom.objective = sensingObjective; +objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, 3 * eye(2)); +dom.objective = dom.objective.initialize(objFcn, dom, DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM); + +% ---- Build shared sensor model ------------------------------------------- +sensor = sigmoidSensor; +sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT); + +% ---- Initialise agents from scenario positions --------------------------- +agentList = cell(numAgents, 1); +for ii = 1:numAgents + pos = positions(ii, :); + geom = spherical; + geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ... + sprintf("UAV %d Collision", ii)); + ag = agent; + ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ... + INITIAL_STEP_SIZE, sprintf("UAV %d", ii)); + agentList{ii} = ag; +end + +% ---- Build obstacles from CSV -------------------------------------------- +obstacleList = cell(numObstacles, 1); +if numObstacles > 0 + obsMin = reshape(scenario.obstacleMin, 3, numObstacles)'; % N×3 + obsMax = reshape(scenario.obstacleMax, 3, numObstacles)'; + for ii = 1:numObstacles + obs = rectangularPrism; + obs = obs.initialize([obsMin(ii, :); obsMax(ii, :)], ... + REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii)); + obstacleList{ii} = obs; + end +end + +% ---- Initialise simulation (plots and video disabled) -------------------- +obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ... + MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false); + +end diff --git a/@miSim/readScenarioCsv.m b/@miSim/readScenarioCsv.m index c79a017..4f81ba1 100644 --- a/@miSim/readScenarioCsv.m +++ b/@miSim/readScenarioCsv.m @@ -1,6 +1,6 @@ function scenario = readScenarioCsv(obj, csvPath) arguments (Input) - obj (1, 1) {mustBeA(obj, 'miSim')}; + obj (1, 1) {mustBeA(obj, 'miSim')}; %#ok csvPath (1, 1) string; end arguments (Output) @@ -8,27 +8,62 @@ function scenario = readScenarioCsv(obj, csvPath) end % File input validation - assert(isfile(csvPath), "%s is not a valid filepath."); - assert(endsWith(csvPath, ".csv"), "%s is not a CSV file."); + assert(isfile(csvPath), "%s is not a valid filepath.", csvPath); + assert(endsWith(csvPath, ".csv"), "%s is not a CSV file.", csvPath); - % Read file - csv = readtable(csvPath, "TextType", "String", "NumHeaderLines", 0, "VariableNamingRule", "Preserve"); - csv.Properties.VariableNames = ["timestep", "maxIter", "minAlt", "discretizationStep", "protectedRange", "sensorPerformanceMinimum", "initialStepSize", "barrierGain", "barrierExponent", "numObstacles", "numAgents", "collisionRadius", "comRange", "alphaDist", "betaDist", "alphaTilt", "betaTilt"]; - - for ii = 1:size(csv.Properties.VariableNames, 2) - csv.(csv.Properties.VariableNames{ii}) = cell2mat(cellfun(@(x) str2num(x), csv.(csv.Properties.VariableNames{ii}), "UniformOutput", false)); - end - - % Put params into standard structure - scenario = struct("timestep", csv.timestep, "maxIter", csv.maxIter, "minAlt", csv.minAlt, "discretizationStep", csv.discretizationStep, ... - "protectedRange", csv.protectedRange, "sensorPerformanceMinimum", csv.sensorPerformanceMinimum, "initialStepSize", csv.initialStepSize, ... - "barrierGain", csv.barrierGain, "barrierExponent", csv.barrierExponent, "numObstacles", csv.numObstacles,... - "numAgents", csv.numAgents, "collisionRadius", csv.collisionRadius, "comRange", csv.comRange, "alphaDist", csv.alphaDist, ... - "betaDist", csv.betaDist, "alphaTilt", csv.alphaTilt, "betaTilt", csv.betaTilt); + % Read the first two lines directly — avoids readtable's quoting + % requirement that '"' must immediately follow ',' (no leading space). + fid = fopen(csvPath, 'r'); + headerLine = fgetl(fid); + dataLine = fgetl(fid); + fclose(fid); - % size check - fns = fieldnames(scenario); - for ii = 2:size(fns, 1) - assert(size(scenario.(fns{ii}), 1) == size(scenario.(fns{ii - 1}), 1), "Mismatched number of rows in scenario definition CSV"); + assert(ischar(headerLine) && ischar(dataLine), ... + "CSV must have a header row and at least one data row."); + + % Parse header: simple comma split + trim (no quoting expected in names) + headers = strtrim(strsplit(headerLine, ',')); + + % Parse data row: comma split that respects double-quoted fields + fields = splitQuotedCSV(dataLine); + + assert(numel(fields) == numel(headers), ... + "CSV data row has %d fields but header has %d columns.", ... + numel(fields), numel(headers)); + + % Build output struct: strip outer quotes, trim whitespace, convert to numeric. + % str2num handles scalar ("5") and vector ("1.0, 2.0, 3.0") fields alike. + % Empty fields ("") become [] via str2num('') == []. + scenario = struct(); + for ii = 1:numel(headers) + raw = strtrim(stripQuotes(fields{ii})); + scenario.(headers{ii}) = str2num(raw); %#ok end -end \ No newline at end of file +end + +% ------------------------------------------------------------------------- +function fields = splitQuotedCSV(line) +% Split a CSV row by commas, respecting double-quoted fields. +% Fields may have leading/trailing whitespace around quotes. + fields = {}; + inQuote = false; + start = 1; + for ii = 1:length(line) + if line(ii) == '"' + inQuote = ~inQuote; + elseif line(ii) == ',' && ~inQuote + fields{end+1} = line(start:ii-1); %#ok + start = ii + 1; + end + end + fields{end+1} = line(start:end); +end + +% ------------------------------------------------------------------------- +function s = stripQuotes(s) +% Trim whitespace then remove a single layer of enclosing double-quotes. + s = strtrim(s); + if length(s) >= 2 && s(1) == '"' && s(end) == '"' + s = s(2:end-1); + end +end diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv new file mode 100644 index 0000000..d42583b --- /dev/null +++ b/aerpaw/config/scenario.csv @@ -0,0 +1,2 @@ +timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax +5, 100, 25.0, 0.1, 1.0, 1.0, 100, 3, 3.0, 60.0, 60.0, 0.2, 10.0, 1.0, "0.0, 0.0, 0.0", "20.0, 20.0, 55.0", "10.0, 10.0", 1e-6, "5.0, 10.0, 45.0, 15.0, 10.0, 30.0", 0, "", "" \ No newline at end of file diff --git a/aerpaw/config/server.yaml b/aerpaw/config/server.yaml deleted file mode 100644 index 57a6db9..0000000 --- a/aerpaw/config/server.yaml +++ /dev/null @@ -1,23 +0,0 @@ -# AERPAW Controller (Server) Configuration - -# Target waypoints in ENU coordinates (meters: east, north, up) -# Grouped by client: first N entries = Client 1, next N = Client 2, etc. -# Each client must have the same number of waypoints. -# -# Flight pattern: -# UAV 1 (45m alt): straight line along x-axis, 4m intervals -# UAV 2 (30m alt): triangular path, diverges then converges -# -# WP1: x=0 (close, ~10m apart) -# WP2: x=4 (medium, ~20m apart) -# WP3: x=8 (far, ~25m apart) -# WP4: x=12 (medium, ~20m apart) -# WP5: x=16 (close, ~10m apart) -# -# Max distance from origin: ~22m (all waypoints within geofence) -# -targets: - # UAV 1 Initial Position — west side of guidance domain - - [5.0, 10.0, 45.0] - # UAV 2 Initial Position — east side of guidance domain - - [15.0, 10.0, 30.0] diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj index 5565058..73eb287 100644 --- a/aerpaw/controller.coderprj +++ b/aerpaw/controller.coderprj @@ -88,6 +88,11 @@ + + int32 + + + @@ -329,445 +334,445 @@ - /home/kdee/Desktop/miSim/aerpaw/codegen/cone.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/cone.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/cone.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h + /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h + /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h + /home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/dot.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/dot.h + /home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/driver.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/driver.h + /home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/exp.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/exp.h + /home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/eye.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/eye.h + /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h + /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.h + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h + /home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/find.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/find.h + /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h + /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h + /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h + /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h + /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h + /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h + /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h + /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h + /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h + /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h + /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h + /home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/norm.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/norm.h + /home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/partition.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/partition.h + /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h + /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h + /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h + /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h + /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h + /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h GENERATED_SOURCE @@ -833,245 +838,221 @@ - /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h + /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/string1.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp + /home/kdee/matlab/R2025a/extern/include/tmwtypes.h GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h - - GENERATED_SOURCE - - - - /home/kdee/matlab/R2025a/extern/include/tmwtypes.h - - GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp GENERATED_SOURCE - + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h @@ -1079,7 +1060,7 @@ true - 2026-02-25T11:42:50 + 2026-02-27T17:26:39 diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 35db136..3276c34 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -3,36 +3,55 @@ arguments (Input) numClients (1, 1) int32; end -coder.extrinsic('disp', 'loadTargetsFromYaml'); +coder.extrinsic('disp', 'readScenarioCsv'); -% Maximum clients and waypoints supported +% Maximum clients supported (one initial position per UAV) MAX_CLIENTS = 4; -MAX_WAYPOINTS = 10; -MAX_TARGETS = MAX_CLIENTS * MAX_WAYPOINTS; +MAX_TARGETS = MAX_CLIENTS; % Allocate targets array (MAX_TARGETS x 3) targets = zeros(MAX_TARGETS, 3); numWaypoints = int32(0); +totalLoaded = int32(0); % pre-declare type for coder.ceval %#ok -% Load targets from YAML config file +% Load initial UAV positions from scenario CSV if coder.target('MATLAB') - disp('Loading targets from server.yaml (simulation)...'); - targetsLoaded = loadTargetsFromYaml('aerpaw/config/server.yaml'); - totalLoaded = size(targetsLoaded, 1); - targets(1:totalLoaded, :) = targetsLoaded(1:totalLoaded, :); - numWaypoints = int32(totalLoaded / numClients); - disp(['Loaded ', num2str(numWaypoints), ' waypoints per client']); + disp('Loading initial positions from scenario.csv (simulation)...'); + tmpSim = miSim; + sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv'); + flatPos = double(sc.initialPositions); % 1×(3*N) flat vector + posMatrix = reshape(flatPos, 3, [])'; % N×3, same layout as initializeFromCsv + totalLoaded = int32(size(posMatrix, 1)); + targets(1:totalLoaded, :) = posMatrix; + numWaypoints = int32(1); + disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']); else coder.cinclude('controller_impl.h'); - % Define filename as null-terminated character array for C compatibility - filename = ['config/server.yaml', char(0)]; - % loadTargets fills targets array (column-major for MATLAB compatibility) - totalLoaded = int32(0); - totalLoaded = coder.ceval('loadTargets', coder.ref(filename), ... + filename = ['config/scenario.csv', char(0)]; + totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ... coder.ref(targets), int32(MAX_TARGETS)); numWaypoints = totalLoaded / int32(numClients); end +% Load guidance scenario from CSV (parameters for guidance_step) +NUM_SCENARIO_PARAMS = 23; +MAX_OBSTACLES_CTRL = int32(8); +scenarioParams = zeros(1, NUM_SCENARIO_PARAMS); +obstacleMin = zeros(MAX_OBSTACLES_CTRL, 3); +obstacleMax = zeros(MAX_OBSTACLES_CTRL, 3); +numObstacles = int32(0); +if ~coder.target('MATLAB') + coder.cinclude('controller_impl.h'); + scenarioFilename = ['config/scenario.csv', char(0)]; + coder.ceval('loadScenario', coder.ref(scenarioFilename), coder.ref(scenarioParams)); + numObstacles = coder.ceval('loadObstacles', coder.ref(scenarioFilename), ... + coder.ref(obstacleMin), coder.ref(obstacleMax), ... + int32(MAX_OBSTACLES_CTRL)); +end +% On MATLAB path, scenarioParams and obstacle arrays are left as zeros. +% guidance_step's MATLAB path loads parameters directly from scenario.csv +% via sim.initializeFromCsv and does not use these arrays. + % Initialize server if coder.target('MATLAB') disp('Initializing server (simulation)...'); @@ -105,8 +124,12 @@ positions = zeros(MAX_CLIENTS, 3); if ~coder.target('MATLAB') coder.ceval('sendRequestPositions', int32(numClients)); coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS)); +else + % Simulation: seed positions from CSV waypoints so agents don't start at origin + positions(1:totalLoaded, :) = targets(1:totalLoaded, :); end -guidance_step(positions(1:numClients, :), true); +guidance_step(positions(1:numClients, :), true, ... + scenarioParams, obstacleMin, obstacleMax, numObstacles); % Main guidance loop for step = 1:MAX_GUIDANCE_STEPS @@ -117,7 +140,8 @@ for step = 1:MAX_GUIDANCE_STEPS end % Run one guidance step: feed GPS positions in, get targets out - nextPositions = guidance_step(positions(1:numClients, :), false); + nextPositions = guidance_step(positions(1:numClients, :), false, ... + scenarioParams, obstacleMin, obstacleMax, numObstacles); % Send target to each client (no ACK/READY expected in guidance mode) for i = 1:numClients @@ -129,6 +153,11 @@ for step = 1:MAX_GUIDANCE_STEPS end end + % Simulation: advance positions to guidance outputs for closed-loop feedback + if coder.target('MATLAB') + positions(1:numClients, :) = nextPositions(1:numClients, :); + end + % Wait for the guidance rate interval before the next iteration if ~coder.target('MATLAB') coder.ceval('sleepMs', int32(GUIDANCE_RATE_MS)); diff --git a/aerpaw/guidance_step.m b/aerpaw/guidance_step.m index b8489b4..4ce650b 100644 --- a/aerpaw/guidance_step.m +++ b/aerpaw/guidance_step.m @@ -1,67 +1,52 @@ -function nextPositions = guidance_step(currentPositions, isInit) +function nextPositions = guidance_step(currentPositions, isInit, ... + scenarioParams, ... + obstacleMin, obstacleMax, numObstacles) % guidance_step One step of the miSim sensing coverage guidance algorithm. % % Wraps the miSim gradient-ascent + CBF motion algorithm for AERPAW. % Holds full simulation state in a persistent variable between calls. % % Usage (from controller.m): -% guidance_step(initPositions, true) % first call: initialise state -% nextPos = guidance_step(gpsPos, false) % subsequent calls: run one step +% guidance_step(initPositions, true, scenarioParams, obstacleMin, obstacleMax, numObstacles) +% nextPos = guidance_step(gpsPos, false, scenarioParams, obstacleMin, obstacleMax, numObstacles) % % Inputs: % currentPositions (MAX_CLIENTS × 3) double ENU [east north up] metres % isInit (1,1) logical true on first call only +% scenarioParams (1 × NUM_SCENARIO_PARAMS) double +% Flat array of guidance parameters (compiled path). +% On MATLAB path this is ignored; parameters are loaded +% from scenario.csv via initializeFromCsv instead. +% Index mapping (1-based): +% 1 timestep 9 collisionRadius +% 2 maxIter 10 comRange +% 3 minAlt 11 alphaDist +% 4 discretizationStep 12 betaDist +% 5 protectedRange 13 alphaTilt +% 6 initialStepSize 14 betaTilt +% 7 barrierGain 15-17 domainMin +% 8 barrierExponent 18-20 domainMax +% 21-22 objectivePos +% 23 sensorPerformanceMinimum +% obstacleMin (MAX_OBSTACLES × 3) double column-major obstacle corners (compiled path) +% obstacleMax (MAX_OBSTACLES × 3) double +% numObstacles (1,1) int32 actual obstacle count % % Output: % nextPositions (MAX_CLIENTS × 3) double guidance targets, ENU metres % % Codegen notes: % - Persistent variable 'sim' holds the miSim object between calls. -% - Plotting/video are disabled (makePlots=false, makeVideo=false) for -% deployment. The coder.target('MATLAB') guards in the miSim/agent -% class files must be in place before codegen will succeed. -% - objectiveFunctionWrapper returns a function handle which is not -% directly codegen-compatible; the MATLAB path uses it normally. The -% compiled path requires an equivalent C impl (see TODO below). +% - On MATLAB path isInit uses sim.initializeFromCsv (file I/O, not compiled). +% - On compiled path isInit uses scenarioParams + obstacle arrays directly. +% - Plotting/video are disabled (makePlots=false, makeVideo=false). -coder.extrinsic('disp', 'objectiveFunctionWrapper'); +coder.extrinsic('disp', 'objectiveFunctionWrapper', 'initializeFromCsv'); -% ========================================================================= -% Tunable guidance parameters — adjust here and recompile as needed. -% ========================================================================= MAX_CLIENTS = int32(4); % must match MAX_CLIENTS in controller.m -% Domain bounds in ENU metres [east, north, up] -DOMAIN_MIN = [ 0.0, 0.0, 25.0]; -DOMAIN_MAX = [ 20.0, 20.0, 55.0]; -MIN_ALT = 25.0; % hard altitude floor (m) - -% Sensing objective: bivariate Gaussian centred at [east, north] -OBJECTIVE_GROUND_POS = [10.0, 10.0]; -DISCRETIZATION_STEP = 0.1; % objective grid step (m) — coarser = faster -PROTECTED_RANGE = 1.0; % objective centre must be >this from domain edge - -% Agent safety geometry -COLLISION_RADIUS = 3.0; % spherical collision radius (m) -COMMS_RANGE = 60.0; % communications range (m) - -% Gradient-ascent parameters -INITIAL_STEP_SIZE = 1; % step size at iteration 0 (decays to 0 at MAX_ITER) -MAX_ITER = 100; % guidance steps (sets decay rate) - -% Sensor model (sigmoidSensor) -ALPHA_DIST = 60.0; % effective sensing distance (m) — set beyond max domain slant range (~53 m) -BETA_DIST = 0.2; % distance sigmoid steepness — gentle, tilt drives the coverage gradient -ALPHA_TILT = 10.0; % max useful tilt angle (degrees) -BETA_TILT = 1.0; % tilt sigmoid steepness - -% Safety filter — Control Barrier Function (CBF/QP) -BARRIER_GAIN = 100; -BARRIER_EXPONENT = 3; - -% Simulation timestep (s) — should match GUIDANCE_RATE_MS / 1000 in controller.m -TIMESTEP = 5.0; -% ========================================================================= +% Path to scenario CSV — used on MATLAB path only (not compiled) +SCENARIO_CSV = 'aerpaw/config/scenario.csv'; persistent sim; if isempty(sim) @@ -76,22 +61,42 @@ numAgents = int32(size(currentPositions, 1)); if isInit if coder.target('MATLAB') disp('[guidance_step] Initialising simulation...'); - end - % --- Build domain geometry --- - dom = rectangularPrism; - dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain"); + % MATLAB path: load all parameters and obstacles from scenario CSV. + % initializeFromCsv reads the file, builds domain/agents/obstacles, + % and calls sim.initialize internally. + sim = sim.initializeFromCsv(SCENARIO_CSV); - % --- Build sensing objective --- - dom.objective = sensingObjective; - if coder.target('MATLAB') - % objectiveFunctionWrapper returns a function handle — MATLAB only. - objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, 3*eye(2)); - dom.objective = dom.objective.initialize(objFcn, dom, ... - DISCRETIZATION_STEP, PROTECTED_RANGE); + disp('[guidance_step] Initialisation complete.'); else - % Evaluate bivariate Gaussian inline (codegen-compatible; no function handle). - % Must build the same grid that initializeWithValues uses internally. + % ================================================================ + % Compiled path: unpack scenarioParams array and obstacle arrays. + % ================================================================ + TIMESTEP = scenarioParams(1); + MAX_ITER = int32(scenarioParams(2)); + MIN_ALT = scenarioParams(3); + DISCRETIZATION_STEP = scenarioParams(4); + PROTECTED_RANGE = scenarioParams(5); + INITIAL_STEP_SIZE = scenarioParams(6); + BARRIER_GAIN = scenarioParams(7); + BARRIER_EXPONENT = scenarioParams(8); + COLLISION_RADIUS = scenarioParams(9); + COMMS_RANGE = scenarioParams(10); + ALPHA_DIST = scenarioParams(11); + BETA_DIST = scenarioParams(12); + ALPHA_TILT = scenarioParams(13); + BETA_TILT = scenarioParams(14); + DOMAIN_MIN = scenarioParams(15:17); + DOMAIN_MAX = scenarioParams(18:20); + OBJECTIVE_GROUND_POS = scenarioParams(21:22); + SENSOR_PERFORMANCE_MINIMUM = scenarioParams(23); + + % --- Build domain geometry --- + dom = rectangularPrism; + dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain"); + + % --- Build sensing objective (inline Gaussian; codegen-compatible) --- + dom.objective = sensingObjective; xGrid = unique([DOMAIN_MIN(1):DISCRETIZATION_STEP:DOMAIN_MAX(1), DOMAIN_MAX(1)]); yGrid = unique([DOMAIN_MIN(2):DISCRETIZATION_STEP:DOMAIN_MAX(2), DOMAIN_MAX(2)]); [gridX, gridY] = meshgrid(xGrid, yGrid); @@ -99,33 +104,39 @@ if isInit dy = gridY - OBJECTIVE_GROUND_POS(2); objValues = exp(-0.5 * (dx .* dx + dy .* dy)); dom.objective = dom.objective.initializeWithValues(objValues, dom, ... - DISCRETIZATION_STEP, PROTECTED_RANGE); - end + DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM); - % --- Build shared sensor model --- - sensor = sigmoidSensor; - sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT); + % --- Build shared sensor model --- + sensor = sigmoidSensor; + sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT); - % --- Initialise agents from GPS positions --- - agentList = cell(numAgents, 1); - for ii = 1:numAgents - pos = currentPositions(ii, :); - geom = spherical; - geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ... - sprintf("UAV %d Collision", ii)); - ag = agent; - ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ... - INITIAL_STEP_SIZE, sprintf("UAV %d", ii)); - agentList{ii} = ag; - end + % --- Initialise agents from GPS positions --- + agentList = cell(numAgents, 1); + for ii = 1:numAgents + pos = currentPositions(ii, :); + geom = spherical; + geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ... + sprintf("UAV %d Collision", ii)); + ag = agent; + ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ... + INITIAL_STEP_SIZE, sprintf("UAV %d", ii)); + agentList{ii} = ag; + end - % --- Initialise simulation (plots and video disabled) --- - sim = miSim; - sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ... - MIN_ALT, TIMESTEP, MAX_ITER, cell(0, 1), false, false); + % --- Build obstacle list from flat arrays --- + coder.varsize('obstacleList', [8, 1], [1, 0]); + obstacleList = cell(int32(0), 1); + for ii = 1:numObstacles + obs = rectangularPrism; + obs = obs.initialize([obstacleMin(ii, :); obstacleMax(ii, :)], ... + REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii)); + obstacleList{ii} = obs; + end - if coder.target('MATLAB') - disp('[guidance_step] Initialisation complete.'); + % --- Initialise simulation (plots and video disabled) --- + sim = miSim; + sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ... + MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false); end % On the init call return current positions unchanged diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index f8246b7..d6c4b75 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -113,6 +113,255 @@ int loadTargets(const char* filename, double* targets, int maxClients) { return count; } +// --------------------------------------------------------------------------- +// Scenario CSV loading +// --------------------------------------------------------------------------- + +// Split a CSV data row into fields, respecting quoted commas. +// Inserts NUL terminators into `line` in place. +// Returns the number of fields found. +static int splitCSVRow(char* line, char** fields, int maxFields) { + int n = 0; + bool inQuote = false; + if (n < maxFields) fields[n++] = line; + for (char* p = line; *p && *p != '\n' && *p != '\r'; ++p) { + if (*p == '"') { + inQuote = !inQuote; + } else if (*p == ',' && !inQuote && n < maxFields) { + *p = '\0'; + fields[n++] = p + 1; + } + } + // NUL-terminate the last field (strip trailing newline) + for (char* p = fields[n - 1]; *p; ++p) { + if (*p == '\n' || *p == '\r') { *p = '\0'; break; } + } + return n; +} + +// Trim leading/trailing ASCII whitespace and remove enclosing double-quotes. +// Modifies the string in place and returns the start of the trimmed content. +static char* trimField(char* s) { + // Trim leading whitespace + while (*s == ' ' || *s == '\t') ++s; + // Remove enclosing quotes + size_t len = strlen(s); + if (len >= 2 && s[0] == '"' && s[len - 1] == '"') { + s[len - 1] = '\0'; + ++s; + } + // Trim trailing whitespace + char* end = s + strlen(s) - 1; + while (end > s && (*end == ' ' || *end == '\t')) { *end-- = '\0'; } + return s; +} + +// Open scenario.csv, skip the header row, return the data row in `line`. +// Returns 1 on success, 0 on failure. +static int readScenarioDataRow(const char* filename, char* line, int lineSize) { + FILE* f = fopen(filename, "r"); + if (!f) { + std::cerr << "loadScenario: cannot open " << filename << "\n"; + return 0; + } + // Skip header row + if (!fgets(line, lineSize, f)) { fclose(f); return 0; } + // Read data row + int ok = (fgets(line, lineSize, f) != NULL); + fclose(f); + return ok ? 1 : 0; +} + +// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS]. +// Index mapping (0-based): +// 0-13 : timestep, maxIter, minAlt, discretizationStep, protectedRange, +// initialStepSize, barrierGain, barrierExponent, collisionRadius, +// comRange, alphaDist, betaDist, alphaTilt, betaTilt +// 14-16: domainMin (east, north, up) +// 17-19: domainMax (east, north, up) +// 20-21: objectivePos (east, north) +// Returns 1 on success, 0 on failure. +int loadScenario(const char* filename, double* params) { + char line[4096]; + if (!readScenarioDataRow(filename, line, sizeof(line))) return 0; + + char copy[4096]; + strncpy(copy, line, sizeof(copy) - 1); + copy[sizeof(copy) - 1] = '\0'; + + char* fields[32]; + int nf = splitCSVRow(copy, fields, 32); + if (nf < 21) { + fprintf(stderr, "loadScenario: expected >=21 columns, got %d\n", nf); + return 0; + } + + // Scalar fields (columns 0–13) + for (int i = 0; i < 14; i++) { + params[i] = atof(trimField(fields[i])); + } + + // domainMin: column 14, format "east, north, up" + { + char tmp[256]; strncpy(tmp, fields[14], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char* t = trimField(tmp); + if (sscanf(t, "%lf , %lf , %lf", ¶ms[14], ¶ms[15], ¶ms[16]) != 3) { + fprintf(stderr, "loadScenario: failed to parse domainMin: %s\n", t); + return 0; + } + } + + // domainMax: column 15 + { + char tmp[256]; strncpy(tmp, fields[15], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char* t = trimField(tmp); + if (sscanf(t, "%lf , %lf , %lf", ¶ms[17], ¶ms[18], ¶ms[19]) != 3) { + fprintf(stderr, "loadScenario: failed to parse domainMax: %s\n", t); + return 0; + } + } + + // objectivePos: column 16 + { + char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char* t = trimField(tmp); + if (sscanf(t, "%lf , %lf", ¶ms[20], ¶ms[21]) != 2) { + fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t); + return 0; + } + } + + // sensorPerformanceMinimum: column 17 + { + char tmp[64]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + params[22] = atof(trimField(tmp)); + } + + printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n", + params[14], params[15], params[16], params[17], params[18], params[19]); + return 1; +} + +// Load initial UAV positions from scenario.csv (column 17: initialPositions). +// targets is a column-major [maxClients x 3] array (same layout as loadTargets): +// targets[i + 0*maxClients] = east +// targets[i + 1*maxClients] = north +// targets[i + 2*maxClients] = up +// Returns number of positions loaded. +int loadInitialPositions(const char* filename, double* targets, int maxClients) { + char line[4096]; + if (!readScenarioDataRow(filename, line, sizeof(line))) return 0; + + char copy[4096]; + strncpy(copy, line, sizeof(copy) - 1); + copy[sizeof(copy) - 1] = '\0'; + + char* fields[32]; + int nf = splitCSVRow(copy, fields, 32); + if (nf < 19) { + fprintf(stderr, "loadInitialPositions: expected >=19 columns, got %d\n", nf); + return 0; + } + + // Column 18: initialPositions flat "x1,y1,z1, x2,y2,z2, ..." + char tmp[1024]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char* t = trimField(tmp); + + double vals[3 * 4]; // up to MAX_CLIENTS triples + int parsed = 0; + char* tok = strtok(t, ","); + while (tok && parsed < 3 * maxClients) { + vals[parsed++] = atof(tok); + tok = strtok(nullptr, ","); + } + + int count = parsed / 3; + for (int i = 0; i < count; i++) { + targets[i + 0 * maxClients] = vals[i * 3 + 0]; // east + targets[i + 1 * maxClients] = vals[i * 3 + 1]; // north + targets[i + 2 * maxClients] = vals[i * 3 + 2]; // up + } + + printf("Loaded %d initial position(s) from scenario\n", count); + return count; +} + +// Load obstacle bounding-box corners from scenario.csv. +// Columns used: 18 (numObstacles), 19 (obstacleMin flat), 20 (obstacleMax flat). +// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays: +// obstacleMin[obs + 0*maxObstacles] = east_min +// obstacleMin[obs + 1*maxObstacles] = north_min +// obstacleMin[obs + 2*maxObstacles] = up_min +// Returns number of obstacles loaded (0 if none or on error). +int loadObstacles(const char* filename, double* obstacleMin, double* obstacleMax, + int maxObstacles) { + char line[4096]; + if (!readScenarioDataRow(filename, line, sizeof(line))) return 0; + + char copy[4096]; + strncpy(copy, line, sizeof(copy) - 1); + copy[sizeof(copy) - 1] = '\0'; + + char* fields[32]; + int nf = splitCSVRow(copy, fields, 32); + if (nf < 22) return 0; + + // Column 19: numObstacles + int n = (int)atof(trimField(fields[19])); + if (n <= 0) return 0; + if (n > maxObstacles) { + fprintf(stderr, "loadObstacles: %d obstacles exceeds MAX_OBSTACLES=%d\n", n, maxObstacles); + n = maxObstacles; + } + + // Column 20: obstacleMin flat "x0,y0,z0, x1,y1,z1, ..." + { + char tmp[1024]; strncpy(tmp, fields[20], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char* t = trimField(tmp); + double vals[3 * 8]; // up to MAX_OBSTACLES triples + int parsed = 0; + char* tok = strtok(t, ","); + while (tok && parsed < 3 * n) { + vals[parsed++] = atof(tok); + tok = strtok(nullptr, ","); + } + if (parsed < 3 * n) { + fprintf(stderr, "loadObstacles: obstacleMin has fewer values than expected\n"); + return 0; + } + for (int obs = 0; obs < n; obs++) { + obstacleMin[obs + 0 * maxObstacles] = vals[obs * 3 + 0]; // east + obstacleMin[obs + 1 * maxObstacles] = vals[obs * 3 + 1]; // north + obstacleMin[obs + 2 * maxObstacles] = vals[obs * 3 + 2]; // up + } + } + + // Column 21: obstacleMax flat + { + char tmp[1024]; strncpy(tmp, fields[21], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char* t = trimField(tmp); + double vals[3 * 8]; + int parsed = 0; + char* tok = strtok(t, ","); + while (tok && parsed < 3 * n) { + vals[parsed++] = atof(tok); + tok = strtok(nullptr, ","); + } + if (parsed < 3 * n) { + fprintf(stderr, "loadObstacles: obstacleMax has fewer values than expected\n"); + return 0; + } + for (int obs = 0; obs < n; obs++) { + obstacleMax[obs + 0 * maxObstacles] = vals[obs * 3 + 0]; + obstacleMax[obs + 1 * maxObstacles] = vals[obs * 3 + 1]; + obstacleMax[obs + 2 * maxObstacles] = vals[obs * 3 + 2]; + } + } + + printf("Loaded %d obstacle(s) from scenario\n", n); + return n; +} + // Message type names for logging static const char* messageTypeName(uint8_t msgType) { switch (msgType) { diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index 2fac077..ed71d44 100644 --- a/aerpaw/impl/controller_impl.h +++ b/aerpaw/impl/controller_impl.h @@ -15,6 +15,31 @@ void closeServer(); // Configuration int loadTargets(const char* filename, double* targets, int maxClients); +// Scenario loading (scenario.csv) +// Number of elements in the flat params array passed to guidance_step. +// Indices: 0-13 scalars, 14-16 domainMin, 17-19 domainMax, 20-21 objectivePos. +#define NUM_SCENARIO_PARAMS 23 +// Maximum number of obstacles (upper bound for pre-allocated arrays). +#define MAX_OBSTACLES 8 + +// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS]. +// Returns 1 on success, 0 on failure. +int loadScenario(const char* filename, double* params); + +// Load initial UAV positions from scenario.csv (initialPositions column). +// targets is a column-major [maxClients x 3] array (same layout as loadTargets). +// Returns number of positions loaded. +int loadInitialPositions(const char* filename, double* targets, int maxClients); + +// Load obstacle bounding-box corners from scenario.csv. +// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays +// (same layout convention as loadTargets / recvPositions). +// Returns the number of obstacles loaded. +int loadObstacles(const char* filename, + double* obstacleMin, + double* obstacleMax, + int maxObstacles); + // User interaction void waitForUserInput(); diff --git a/aerpaw/results/plotGpsCsvs.m b/aerpaw/results/plotGpsCsvs.m index 8c193a8..11c1de7 100644 --- a/aerpaw/results/plotGpsCsvs.m +++ b/aerpaw/results/plotGpsCsvs.m @@ -6,9 +6,9 @@ c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer % Paths to logs -gpsCsvs = fullfile ("sandbox", "test1", ... - ["GPS_DATA_0c8d904aa159_2026-02-24_21:33:25.csv"; ... - "GPS_DATA_8e4f52dac04d_2026-02-24_21:33:25.csv"; ... +gpsCsvs = fullfile ("sandbox", "test2", ... + ["GPS_DATA_0c8d904aa159_2026-02-27_20:52:33.csv"; ... + "GPS_DATA_8e4f52dac04d_2026-02-27_20:52:33.csv"; ... ]); G = cell(size(gpsCsvs)); diff --git a/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/BIoAmslL9uGQPg5zxemTUHYw4Z4p.xml b/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/BIoAmslL9uGQPg5zxemTUHYw4Z4p.xml deleted file mode 100644 index c97d4d3..0000000 --- a/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/BIoAmslL9uGQPg5zxemTUHYw4Z4p.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/BIoAmslL9uGQPg5zxemTUHYw4Z4d.xml b/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/e9CQ_K5hshi9GcW6IwNtfuxqFYwd.xml similarity index 100% rename from resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/BIoAmslL9uGQPg5zxemTUHYw4Z4d.xml rename to resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/e9CQ_K5hshi9GcW6IwNtfuxqFYwd.xml diff --git a/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/e9CQ_K5hshi9GcW6IwNtfuxqFYwp.xml b/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/e9CQ_K5hshi9GcW6IwNtfuxqFYwp.xml new file mode 100644 index 0000000..04dda10 --- /dev/null +++ b/resources/project/vmVRNnXTZxyEiUtNp09z8POG4ww/e9CQ_K5hshi9GcW6IwNtfuxqFYwp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index f732413..b396112 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -27,6 +27,43 @@ classdef parametricTestSuite < matlab.unittest.TestCase methods (Test) % Test cases + function test_scenario(tc) + % Load scenario definition + tc.csvPath = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv"); + params = tc.testClass.readScenarioCsv(tc.csvPath); + + % Define scenario according to CSV specification + tc.domain = tc.domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain"); + tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum); + + agents = cell(size(params.initialPositions, 2) / 3, 1); + for ii = 1:size(agents, 1) + agents{ii} = agent; + + sensorModel = sigmoidSensor; + sensorModel = sensorModel.initialize(params.alphaDist, params.betaDist, params.alphaTilt, params.betaTilt); + + collisionGeometry = spherical; + collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius, REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii)); + + agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange, params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), tc.plotCommsGeometry); + end + + % TODO + obstacles = {}; + + % Set up simulation + tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, tc.makePlots, tc.makeVideo); + + % Save simulation parameters to output file + tc.testClass.writeInits(); + + % Run + tc.testClass = tc.testClass.run(); + + % Cleanup + tc.testClass = tc.testClass.teardown(); + end function csv_parametric_tests_random_agents(tc) % Read in parameters to iterate over params = tc.testClass.readScenarioCsv(tc.csvPath);