-
Notifications
You must be signed in to change notification settings - Fork 13
/
path_planner.m
executable file
·120 lines (111 loc) · 4.05 KB
/
path_planner.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
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
% path planner
%
% Modified:
% - 4/06/2010 - RWB
%
% output is a vector containing P.num_waypoints waypoints
%
% input is the map of the environment and desired goal location
function out = path_planner(in,P,map)
NN = 0;
pn = in(1+NN);
pe = in(2+NN);
h = in(3+NN);
% Va = in(4+NN);
% alpha = in(5+NN);
% beta = in(6+NN);
% phi = in(7+NN);
% theta = in(8+NN);
chi = in(9+NN);
% p = in(10+NN);
% q = in(11+NN);
% r = in(12+NN);
% Vg = in(13+NN);
% wn = in(14+NN);
% we = in(15+NN);
% psi = in(16+NN);
flag_new_waypoints = in(17+NN);
NN = NN + 17;
t = in(1+NN);
persistent num_waypoints
persistent wpp
if (t==0) || flag_new_waypoints,
% format for each point is [pn, pe, pd, chi, Va^d] where the position
% of the waypoint is (pn, pe, pd), the desired course at the waypoint
% is chi, and the desired airspeed between waypoints is Va
% if chi!=-9999, then Dubins paths will be used between waypoints.
switch 5,
case 1,
num_waypoints = 4;
wpp = [...
0, 0, -100, -9999, P.Va0;...
300, 0, -100, -9999, P.Va0;...
0, 300, -100, -9999, P.Va0;...
300, 300, -100, -9999, P.Va0;...
];
case 2, % Dubins
num_waypoints = 4;
wpp = [...
0, 0, -100, 0, P.Va0;...
300, 0, -100, 45*pi/180, P.Va0;...
0, 300, -100, 45*pi/180, P.Va0;...
300, 300, -100, -135*pi/180, P.Va0;...
];
case 3, % path through city using Dubin's paths
% current configuration
wpp_start = [pn, pe, -h, chi, P.Va0];
% desired end waypoint
if norm([pn; pe; -h]-[map.width; map.width; -h])<map.width/2,
wpp_end = [0, 0, -h, 0, P.Va0];
else
wpp_end = [map.width, map.width, P.pd0, 0, P.Va0];
end
waypoints = planRRTDubins(wpp_start, wpp_end, P.R_min, map);
num_waypoints = size(waypoints,1);
wpp = [];
for i=1:num_waypoints,
wpp = [...
wpp;...
waypoints(i,1), waypoints(i,2), waypoints(i,3), waypoints(i,4), P.Va0;...
];
end
case 4, % path through city using straight-line RRT
% current configuration
wpp_start = [pn, pe, -h, chi, P.Va0];
% desired end waypoint
if norm([pn; pe; -h]-[map.width; map.width; -h])<map.width/2,
wpp_end = [0, 0, -h, 0, P.Va0];
else
wpp_end = [map.width, map.width, P.pd0, 0, P.Va0];
end
waypoints = planRRT(wpp_start, wpp_end, map);
num_waypoints = size(waypoints,1);
wpp = [];
for i=1:num_waypoints,
wpp = [...
wpp;...
waypoints(i,1), waypoints(i,2), waypoints(i,3), waypoints(i,4), P.Va0;...
];
end
case 5, % cover path through city using Dubin's paths
% current configuration
wpp_start = [pn, pe, -h, chi, P.Va0];
waypoints = planCoverRRTDubins(wpp_start, P.R_min, map);
num_waypoints = size(waypoints,1);
wpp = [];
for i=1:num_waypoints,
wpp = [...
wpp;...
waypoints(i,1), waypoints(i,2), waypoints(i,3), waypoints(i,4), P.Va0;...
];
end
end
for i=num_waypoints+1:P.size_waypoint_array,
wpp = [...
wpp;...
-9999, -9999, -9999, -9999, -9999;...
];
end
end
out = [num_waypoints; reshape(wpp', 5*P.size_waypoint_array, 1)];
end