-
Notifications
You must be signed in to change notification settings - Fork 0
/
path_finder_complex_route.m
49 lines (37 loc) · 1.08 KB
/
path_finder_complex_route.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
clc
clear all
% Probabilistic Roadmap (PRM) path planner
% The imported maps are : simpleMap, complexMap and ternaryMap.
% Example Maps for Planning a Path
load exampleMaps.mat
map = binaryOccupancyMap(simpleMap,2)
show(map)
robotRadius = 0.2
%load a more complex map
map = binaryOccupancyMap(complexMap,1)
show(map)
mapInflated = copy(map)
inflate(mapInflated, robotRadius);
show(mapInflated)
prm = mobileRobotPRM;
prm.Map = mapInflated;
prm.NumNodes = 100;
prm.ConnectionDistance = 10;
startLocation = [4 2]
endLocation = [24 20]
%final path
path = findpath(prm, startLocation, endLocation)
show(prm)
% For complex maps, there may not be a feasible
% path for a given number of nodes (returns an empty path).
path = findpath(prm, startLocation, endLocation);
while isempty(path)
% No feasible path found yet, increase the number of nodes
prm.NumNodes = prm.NumNodes + 10;
% Use the |update| function to re-create the PRM roadmap with the changed
% attribute
update(prm);
% Search for a feasible path with the updated PRM
path = findpath(prm, startLocation, endLocation);
end
show(prm)