用于并行并发工作流中高效资源管理的分区池

pool = parpool("myCluster",16);

[gpuPool,cpuPool] = partition(pool,"MaxNumWorkersPerGPU",1);

cpuWorkers = cpuPool.Workers;
[cpuLidarPool,cpuOtherPool] = partition(cpuPool,"Workers",cpuWorkers(1:4));

params = initializeParameters;

plotScene(params);
view([-110.88 31.50])

stage2InputQueue = parallel.pool.PollableDataQueue(Destination="any"); % Pollable data queue 1
stage3SortQueue = parallel.pool.PollableDataQueue(Destination="any");  % Pollable data queue 2
stage4CloudQueue = parallel.pool.PollableDataQueue(Destination="any");  % Pollable data queue 3

stage2TriggerQueue = parallel.pool.DataQueue;
afterEach(stage2TriggerQueue,@(call) ...
    parfeval(gpuPool,@findIntersections,0,stage2InputQueue,stage3SortQueue,stage4CloudQueue));

[fig,s,rays] = prepareScanningPlot(params);

displayQueue = parallel.pool.DataQueue; 
afterEach(displayQueue,@(data) displayScanFrames(data,s,rays));

fgenerate = parfeval(cpuLidarPool,@addParamsToQueue,0,params,stage2InputQueue,stage2TriggerQueue);

fSort = parfeval(cpuLidarPool,@sortFrames,0,stage3SortQueue,displayQueue);

fCloud(1) = parfeval(cpuLidarPool,@collectCloudPointData,1,stage4CloudQueue);
fCloud(2) = parfeval(cpuLidarPool,@collectCloudPointData,1,stage4CloudQueue);

fig.Visible = "on";

startPos = [25 25];
targetPos = [-30 -20];
objectRadius = ones(size(params.objectPositions,1),1)*2;
obstacles = [params.objectPositions objectRadius];

numParticles = 1000;
numIterations = 300;
dim = 2;
numTrials = 100;

parfor(trial = 1:numTrials,cpuOtherPool)
[bestPath(trial,:,:),scores(trial)] = planPathPSO(startPos,targetPos, ...
    obstacles,numParticles,dim,numIterations);
end

[~,bestInd] = min(scores);

plotBestPath(obstacles,startPos,bestPath(bestInd,:,:),targetPos);

pointCloud = fetchOutputs(fCloud);

plotPointCloud(pointCloud,params);

function addParamsToQueue(params,stage2InputQueue,stage2TriggerQueue)
fN = 0;
 
numScans = size(params.lidarOrigins,1);

for scan = 1:numScans
    % Set lidar origin (position)
    lidarOrigin = params.lidarOrigins(scan,:);

    for t = 1:params.totalTimeSteps
        data = getScanParam(params,t,lidarOrigin);
        fN = fN + 1;

        % Send scan number and input data to the data queue
        data.fN = fN;
        send(stage2InputQueue,data);

        % Submit parfeval computation on the GPU pool
        send(stage2TriggerQueue,"findIntersections");

        pause(0.5); % Pause to improve visualization

        % Perform some queue management to reduce strain on queue
        while stage2InputQueue.QueueLength > 10
            pause(1);
        end
    end
end

% Close queue after generating all input parameters
close(stage2InputQueue);

% Submit parfeval computation to close stage 2 queues
send(stage2TriggerQueue,"findIntersections");
end

function findIntersections(stage2InputQueue,stage3SortQueue,stage4CloudQueue)
[data,OK] = poll(stage2InputQueue,inf);
if OK
    rayOriginsGPU = gpuArray(data.lidarOrigin);
    rayDirectionsGPU = gpuArray(data.rayDirections);

    % Convert to gpuArray and calculate ray triangle intersections on the GPU
    [Hits,intXs,intYs,intZs,intersectionDistances] = arrayfun(@rayTriangleIntersection, ...
        rayOriginsGPU(:,1),rayOriginsGPU(:,2),rayOriginsGPU(:,3), ...
        rayDirectionsGPU(:,1),rayDirectionsGPU(:,2),rayDirectionsGPU(:,3), ...
        data.A(:,1)',data.B(:,1)',data.C(:,1)',...
        data.A(:,2)',data.B(:,2)',data.C(:,2)', ...
        data.A(:,3)',data.B(:,3)',data.C(:,3)');

    data.rayDirections = [];

    % Find the closest intersection for each ray
    [~,minIndices] = min(abs(intersectionDistances),[],2,"omitnan");

    % Extract the first intersection points using the indices
    numRays = size(intXs,1);
    X = intXs(sub2ind(size(intXs),(1:numRays)',minIndices));
    Y = intYs(sub2ind(size(intYs),(1:numRays)',minIndices));
    Z = intZs(sub2ind(size(intZs),(1:numRays)',minIndices));

    % Add intersection values to data structure
    data.numHits = gather(sum(Hits,1));

    % Send scan number and scan data to next worker to sort for display
    output{1} = data.fN;
    output{2} = data;
    send(stage3SortQueue,output);

    % Send intersection points to next worker
    cloudData.intXs = gather(X);
    cloudData.intYs = gather(Y);
    cloudData.intZs = gather(Z);
    send(stage4CloudQueue,cloudData);

elseif ~OK
    close(stage3SortQueue);
    close(stage4CloudQueue);
end
end

function pointCloud = collectCloudPointData(stage4CloudQueue)
intersectionPoints = [];
OK = true;
while OK
    [cloudData,OK] = poll(stage4CloudQueue,inf);
    if ~isempty(cloudData)
        % Flatten the matrices
        X_flat = cloudData.intXs(:);
        Y_flat = cloudData.intYs(:);
        Z_flat = cloudData.intZs(:);

        % Create a logical mask for valid intersections
        validMask = ~isnan(X_flat);

        % Filter the points using the valid mask
        validX = X_flat(validMask);
        validY = Y_flat(validMask);
        validZ = Z_flat(validMask);

        % Combine the valid points into a single matrix
        thisIntersectionPoints = [validX validY validZ];
        % Downsample by selecting every nth point
        n = 5; % Downsample factor
        thisIntersectionPoints = thisIntersectionPoints(1:n:end,:);

        % Round coordinates to reduce precision
        precisionFactor = 0.01;
        thisIntersectionPoints = round(thisIntersectionPoints/precisionFactor)*precisionFactor;

        intersectionPoints = [intersectionPoints;thisIntersectionPoints];
        pointCloud = intersectionPoints;

    elseif ~OK
        pointCloud = intersectionPoints;
        return
    end
end
end

function params = initializeParameters
% Define the object and create the triangulated surface
params.surfProfile = 80;
params.objectPositions = [-20 -20;20 -20;-20 20;20 20;-10 -5;5 5];
[params.A,params.B,params.C,params.indices] = createTriangulatedSurfaces(params.surfProfile, ...
    params.objectPositions);

% LiDAR Parameters
params.numRaysPerRevolution = 1000; % Number of rays in one complete horizontal revolution
params.numLayers = 40; % Number of vertical layers
params.verticalFOV = 60; % Vertical field of view in degrees

% Simulation Parameters
params.angularSpeed = 10; % Degrees per time step
params.totalTimeSteps = 36;
params.lidarOrigins = single([0,0,1;0,0,2;0,0,3]); % Set lidar origin (position
end

function displayScanFrames(data,s,rays)
s(1).UserData = s(1).UserData + data.numHits;
indices = s(2).UserData;

% Normalize hits to range between 1 and 256
normalizedHits = rescale(s(1).UserData,1,256);

% Update surface plot with new intensities
colors = repmat(normalizedHits',1,3);

for idx = 1:length(indices)
rowStart = indices(idx,1);
rowEnd = indices(idx,2);
s(idx).CData = colors(rowStart:rowEnd,:);
end

% Update origin of light source
for idx = 1:size(data.rayX,1)
rays(idx).XData = data.rayX(idx,:);
rays(idx).YData = data.rayY(idx,:);
rays(idx).ZData = data.rayZ(idx,:);
end
drawnow limitrate nocallbacks;
end

function [fig,s,rays] = prepareScanningPlot(params)
fig = figure(Position=[933 313 582 483],Visible="off");

%  Create axes for surface
colors = zeros(size(params.A));

hold on
for idx = 1:length(params.indices)
rowStart = params.indices(idx,1);
rowEnd = params.indices(idx,2);
s(idx) = surf(params.A(rowStart:rowEnd,:), ...
    params.B(rowStart:rowEnd,:), ...
    params.C(rowStart:rowEnd,:), ...
    colors(rowStart:rowEnd,:), ...
    EdgeColor="none");
end
hold off
view([-110.88 31.50])

title("Intersections of Rays with Objects");
% Add title and axes labels
axis square;
axis off
colormap turbo;
c = colorbar;
c.Label.String = "Ray Intensity";
view(160,28);
s(1).UserData = zeros(1,size(params.A,1));
s(2).UserData = params.indices;

hold on
for idx = 1:3
rays(idx) = plot3([NaN NaN],[NaN NaN],[NaN NaN],"r",LineWidth=3);
end
rays(1).MarkerIndices = 1;
rays(1).Marker = "square";
rays(1).MarkerSize = 8;
rays(1).MarkerFaceColor = "r";
rays(1).DisplayName = "Sensor position";
hold off
axis off
legend(rays(1),Location="southwest")
end

function plotScene(params)
figure;
hold on
for idx = 1:length(params.indices)
    rowStart = params.indices(idx,1);
    rowEnd = params.indices(idx,2);
    surf(params.A(rowStart:rowEnd,:), ...
        params.B(rowStart:rowEnd,:), ...
        params.C(rowStart:rowEnd,:));
end
hold off
view(3)
axis square
end

function plotPointCloud(pointCloud,params)
figure;
scatter3(pointCloud(:,1),pointCloud(:,2),pointCloud(:,3),1,"filled");
hold on
scatter3(params.lidarOrigins(:,1),params.lidarOrigins(:,2),params.lidarOrigins(:,3),"filled");
hold off
xlabel("X");
ylabel("Y");
zlabel("Z");
title("Lidar Point Cloud");
legend("Point cloud","Sensor position",Location="bestoutside");
axis equal;
grid on;
end

function plotBestPath(obstacles,startPos,bestPath,targetPos)
figure;
hold on;
scatter(obstacles(:,1),obstacles(:,2),100);
plot(startPos(1),startPos(2),"go","MarkerSize",10);
plot(targetPos(1),targetPos(2),"rx","MarkerSize",10);
plot(bestPath(1,:,1),bestPath(1,:,2),"b--");
legend("Obstacles","Start","Target","Best Path",Location="bestoutside");
title("Robot Path using PSO");
xlabel("X Position");
ylabel("Y Position");
hold off;
end