55 lines
2.1 KiB
Matlab
55 lines
2.1 KiB
Matlab
## Author: adril <adril@LAPTOP-EJ1AIJHT>
|
|
## Created: 2023-01-08
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function planPathPRM(mapFilePath, startPoint, endPoint)
|
|
%
|
|
% Task: Establish the path from the starting point to the goal, using the points defined with the PRM method
|
|
% and using the Dijkstra method.
|
|
%
|
|
% Inputs:
|
|
% - mapFilePath : the path to get the .mat file
|
|
% - startPoint : our starting point for the path planning
|
|
% - endPoint : our final goal
|
|
%
|
|
% Outputs:
|
|
% - None
|
|
%
|
|
% Adrien Lasserre (adrien.lasserre@ecam.fr) &
|
|
% Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
|
|
% 08/01/2023
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
function planPathPRM (mapFilePath, startPoint, endPoint)
|
|
|
|
mapFile = load(mapFilePath);
|
|
|
|
S = startPoint; %define the start as S
|
|
G = endPoint; %define the goal as G
|
|
|
|
n = columns(mapFile.MatrixOfLinks); %get the size of the MatrixOfLinks
|
|
connectionMatrix = zeros(n+2,n+2); %initialize a new connection matrix, with size n+2 to enter the S and G
|
|
connectionMatrix(2:n+1,2:n+1) = mapFile.MatrixOfLinks;%put the MatrixOfLinks inside
|
|
|
|
points2Dmap = mapFile.Points'; %get the Points, but transposed (to use the findClosestPoint function)
|
|
|
|
indexClosestPointS = findClosestPoint(S,points2Dmap) + 1 %+1 as in the first indexes of the connectionMatrix we put the S
|
|
indexClosestPointG = findClosestPoint(G,points2Dmap) + 1 %display those results
|
|
|
|
connectionMatrix(1, indexClosestPointS) = 1; %use the previous results to update the connectionMatrix
|
|
connectionMatrix(indexClosestPointS, 1) = 1;
|
|
connectionMatrix(1, 1) = 1;
|
|
connectionMatrix(n+2, indexClosestPointG) = 1;
|
|
connectionMatrix(indexClosestPointG, n+2) = 1;
|
|
connectionMatrix(n+2, n+2) = 1;
|
|
|
|
points2D = [S; points2Dmap; G] %store the points (with S and G)
|
|
connectionMatrix %debugging
|
|
|
|
[numberOfNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D) %create the visibilityGraph
|
|
|
|
|
|
dijkstra(numberOfNodes,visibilityGraph); %and finally use the dijkstra algorithm for path planning
|
|
|
|
endfunction
|