scenario csv on both platforms
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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
|
||||
% ---- 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
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
function scenario = readScenarioCsv(obj, csvPath)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')}; %#ok<INUSA>
|
||||
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<ST2NM>
|
||||
end
|
||||
end
|
||||
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<AGROW>
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user