basic implementation of client/server for AERPAW, whole lot of mess included

aerpaw/@scenario/initialize.m

@@ -0,0 +1,30 @@
+function obj = initialize(obj, initsPath)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'scenario')};
+        initsPath (1, 1) string;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'scenario')};
+    end
+
+    obj.inits = load(initsPath);
+
+    % Instantiate the correct number of UAVs
+    obj.uavs = cell(obj.inits.numAgents, 1);
+    [obj.uavs{:}] = deal(uav);
+
+    % Configure ports to broadcast to drones
+    obj.ports = repmat(obj.basePort, [obj.inits.numAgents, 1]) + (1:obj.inits.numAgents)';
+    obj.udp = cell(obj.inits.numAgents, 1);
+    for ii = 1:obj.inits.numAgents
+        obj.udp{ii} = udpport("IPV4", "LocalPort", obj.ports(ii), "Tag", sprintf("UAV %d", ii), "Timeout", obj.timeout);
+    end
+
+    % Initialize UAVs in scenario's knowledge
+    for ii = 1:obj.inits.numAgents
+        obj.uavs{ii} = obj.uavs{ii}.initialize(obj.ports(ii) + obj.portOffset, obj.timeout, sprintf("UAV %d", ii));
+    end
+
+    % Update opMode
+    obj.opMode = OPMODE.INITIALIZED;
+end

aerpaw/@scenario/run.m

@@ -0,0 +1,73 @@
+function obj = run(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'scenario')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'scenario')};
+    end
+    
+    count = 0;
+
+    while true
+        %% UAV operations (simulated only)
+        if coder.target("MATLAB")
+            % Iterate over UAVs
+            for ii = 1:size(obj.uavs, 1)
+                % Determine behavior from UAV opMode
+                if obj.uavs{ii}.opMode == OPMODE.INVALID
+                    error("Invalid UAV opMode");
+                elseif obj.uavs{ii}.opMode == OPMODE.INITIALIZED
+                    % Waiting for starting position to be sent by the controller
+                    if all(~(isnan(obj.uavs{ii}.commandPos)))
+                        % Teleport to starting position
+                        obj.uavs{ii}.pos = obj.uavs{ii}.commandPos;
+
+                        % reset to no command
+                        obj.uavs{ii}.commandPos = NaN(1, 3);
+
+                        % Acknowledge command receipt to controller by reporting a new opMode
+                        obj.uavs{ii}.commandOpMode = OPMODE.SET;
+                    end
+                else
+                    error("Unexpected UAV opMode");
+                end
+            end
+        elseif coder.target("C++")
+            coder.cinclude('udp_comm.h');
+            % Iterate over UAVs
+            for ii = 1:size(obj.uavs, 1)
+                mode = uint8(0);
+                coder.ceval('recvOpMode', coder.wref(mode));
+            end
+        end
+
+
+        %% Server operations
+        % Compute guidance for UAVs and issue commands
+        if obj.opMode == OPMODE.INVALID
+            error("Invalid controller opMode");
+        elseif obj.opMode == OPMODE.INITIALIZED
+            % On the first iteration, command UAVs to go to their starting positions
+            commandPos = obj.inits.pos;
+        elseif obj.opMode == OPMODE.SET
+            % begin experiment once the controller and UAVs are ready
+            if unique(cellfun(@(x) x.opMode.id, obj.uavs)) == OPMODE.SET.id
+                keyboard
+            end
+        else
+            error("Unexpected controller opMode");
+        end
+
+        % Transmit commands
+        % Command drones to their starting positions
+        for ii = 1:size(obj.uavs, 1)
+            obj.uavs{ii} = obj.sendTarget(obj.uavs{ii}, commandPos(ii, 1:3));
+        end
+        keyboard
+            
+
+
+        % tally iterations
+        count = count + 1;
+    end
+end

aerpaw/@scenario/scenario.m

@@ -0,0 +1,28 @@
+classdef scenario
+    properties (Access = public)
+        % Experiment
+        domain = rectangularPrism;
+        inits;
+
+        % State
+        opMode = OPMODE.INVALID;
+
+        % UAVs
+        uavs;
+
+        % Communications
+        timeout = 2; % seconds
+        basePort = 3330;
+        portOffset = 1110;
+        ports;
+        udp;
+
+    end
+
+    methods(Access = public)
+        [obj] = initialize(obj, initsPath);
+        [obj] = run(obj);
+        [obj] = setup(obj);
+        u = sendTarget(u, pos);
+    end
+end

aerpaw/@scenario/sendTarget.m

@@ -0,0 +1,24 @@
+function u = sendTarget(~, u, pos)
+    arguments (Input)
+        ~
+        u (1, 1) {mustBeA(u, 'uav')};
+        pos (1, 3) double;
+    end
+    arguments (Output)
+        u (1, 1) {mustBeA(u, 'uav')};
+    end
+
+    % Branch here depending on environment
+    if coder.target("MATLAB")
+        % Simulation - update target position
+        u.commandPos = pos;
+
+    elseif coder.target("C++")
+        % Codegen - TX starting position to UAV
+        coder.cinclude("udp_comm.h");
+        coder.ceval("sendTarget", coder.rref(pos));
+    end
+
+
+
+end

aerpaw/@scenario/setup.m

@@ -0,0 +1,17 @@
+function obj = setup(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'scenario')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'scenario')};
+    end
+    
+    % Command drones to their starting positions
+    for ii = 1:size(obj.uavs, 1)
+        obj.uavs{ii} = obj.sendTarget(obj.uavs{ii}, obj.inits.pos(ii, 1:3));
+    end
+
+    % Update opMode
+    obj.opMode = OPMODE.SET;
+    
+end