-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathgenerate_wp_path.m
114 lines (107 loc) · 4.44 KB
/
generate_wp_path.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
function [wp_smart, index_min, wp_path, Tend, flag] = generate_wp_path(waypoints, x, y, index_prev, kk, obs, Rmaxt0)
flag = 0;
L_calculus = Rmaxt0;%1 % the largest curvilinear distance from the wp, i.e., max preview lenght
L_min = Rmaxt0;%1 % minimum preview lenght
max_distance = Rmaxt0; % if the closest wp is further than this, return it as wp_smart
% angle_max = pi/3; % max tolerated angle to compute varying preview
Tend = kk;
distance_wp_path = .1; % a wp per distance_wp_path meters
length_window_search = 1.5; % meters around which I look for the minimum
n_waypoints_main = size(waypoints,2); % munber of wp inserted manually
n_sub_path = n_waypoints_main - 1; % number of subpath
length_sub_path = zeros(n_sub_path,1); % lenght of each piece of the path
n_wp_sub_path = zeros(n_sub_path,1); % number of wp per sub path
for i = 1:length(n_wp_sub_path)
length_sub_path(i) = norm(waypoints(:,i+1) - waypoints(:,i));
n_wp_sub_path(i) = floor(length_sub_path(i) / distance_wp_path);
end
wp_path = zeros(2,sum(n_wp_sub_path));
%% Fill the subpath
inserted_wp = 0;
for i = 1:n_sub_path % move on the subpath
P1 = waypoints(:,i);
P2 = waypoints(:,i+1);
s = 0;
for j = 1:n_wp_sub_path(i)
P = P1 + s * (P2 - P1) / norm(P2 - P1);
inserted_wp = inserted_wp + 1;
wp_path(:,inserted_wp) = P;
s = min(s + distance_wp_path, length_sub_path(i));
end
end
%% Compute the closest point of the path to me
min_dist = norm(wp_path(:,index_prev) - [x;y]);
index_min = index_prev;
for i = index_prev:min(size(wp_path,2),index_prev + floor(length_window_search/distance_wp_path))% 2:size(wp_path,2)
dist = norm(wp_path(:,i)- [x;y]);
if min_dist > dist
min_dist = dist;
index_min = i;
end
end
%
if min_dist > max_distance
index_wp_smart = min(floor(index_min + L_min/distance_wp_path), size(wp_path,2));
wp_smart = wp_path(:, index_wp_smart);
return;
end
%% Compute the length of the preview depending on the path curvature
% I will process the wp comprised in the following indexes:
index_wp_far = min(floor(index_min + L_calculus/distance_wp_path), size(wp_path,2));
% index_wp_close = min(floor(index_min + L_min/distance_wp_path), size(wp_path,2));
% index_wp_close = min(index_min, size(wp_path,2));
% n_points_processed = index_wp_far - index_wp_close + 1;
% unit_vec_store = zeros(2,n_points_processed - 1); % each column is a unit vector from a wp to the following one
% Compute unit vectors
% for i = 1:(n_points_processed-1)
% unit_vec_store(:, i) = wp_path(:, i + index_wp_close) - wp_path(:, i - 1 + index_wp_close);
% unit_vec_store(:, i) = unit_vec_store(:, i)/norm(unit_vec_store(:, i));
% end
% Compute the increments of angles
% angles_vec = zeros(n_points_processed - 1, 1); % the element i-th is the angle traveled from the beginning to wp i-th
% for i = 2:length(angles_vec)
% %% Compute the dot prod between each unit vec and the following and augment the angle
% angles_vec(i) = angles_vec(i-1) + abs(acos(dot(unit_vec_store(:, i),unit_vec_store(:, i-1))));
% end
% i = length(angles_vec);
% if i == 0
% flag = 1;
% wp_smart = wp_path(:, index_prev);
% return
% % error('SIMULAZIONE TERMINATA, kk = %d', kk)
% end
%
% while angles_vec(i) > angle_max
% i = i - 1;
% end
% index_wp_smart = i + index_wp_close - 1;
index_wp_smart = index_wp_far;
% Compute the curvilinear distance from wp_smart
% distance = 0;
% for i = index_min:(index_wp_smart - 1)
% distance = distance + norm( wp_path(:,i+1) - wp_path(:,i));
% end
%
% if distance < L_min
% index_wp_smart = min(floor(index_min + L_min/distance_wp_path), size(wp_path,2));
% end
% if wp_path(:, index_wp_smart) == wp_path(:, end)
% wp_smart = wp_path(:, end);
% else
wp_smart = wp_path(:, index_wp_smart); % this is the wp_smart with the variable preview in the standard case
% if kk==14
% keyboard
% end
% end
while( noCollision([x,y]',[wp_smart(1),wp_smart(2)]',obs)==0 )
index_wp_smart = index_wp_smart-1;
wp_smart = wp_path(:, index_wp_smart);
if index_wp_smart == 0
index_wp_smart = 1;
wp_smart = wp_path(:, index_wp_smart);
return
end
end
%%
% index_wp_smart = min(floor(index_min + length_preview/distance_wp_path), size(wp_path,2));
% wp_smart = wp_path(:, index_wp_smart);