scanner_analysis.m 14.6 KB
Newer Older
Farid Alijani's avatar
Farid Alijani committed
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
clc;
close all;
clear all;

upper_angle = 135; % deg.
lower_angle = -135; % deg.
dist_to_obst = .1; % m.

%% docked robot for 5 different experiences...

S_mat = csvread('Scan_Data_5_exp.csv');

indx = S_mat(:,1);

s_exp_1 = S_mat(:,2);
s_exp_2 = S_mat(:,3);
s_exp_3 = S_mat(:,4);
s_exp_4 = S_mat(:,5);
s_exp_5 = S_mat(:,6);


points = size(indx,1); % to calculate the angle...
laser_beam = points -1;

angle = zeros(points,1);
X_exp_1 = zeros(points,1);
X_exp_2 = zeros(points,1);
X_exp_3 = zeros(points,1);
X_exp_4 = zeros(points,1);
X_exp_5 = zeros(points,1);
X_obst = zeros(points,1);

Y_exp_1 = zeros(points,1);
Y_exp_2 = zeros(points,1);
Y_exp_3 = zeros(points,1);
Y_exp_4 = zeros(points,1);
Y_exp_5 = zeros(points,1);
Y_obst = zeros(points,1);



for i = 1:points
    
    angle(i) = (lower_angle + (indx(i)* ((upper_angle - lower_angle)/(laser_beam))))* pi/180;
    
    X_exp_1(i) = s_exp_1(i) * cos(angle(i));
    X_exp_2(i) = s_exp_2(i) * cos(angle(i));
    X_exp_3(i) = s_exp_3(i) * cos(angle(i));
    X_exp_4(i) = s_exp_4(i) * cos(angle(i));
    X_exp_5(i) = s_exp_5(i) * cos(angle(i));
    X_obst(i) = dist_to_obst * cos(angle(i));
    
    Y_exp_1(i) = s_exp_1(i) * sin(angle(i));
    Y_exp_2(i) = s_exp_2(i) * sin(angle(i));
    Y_exp_3(i) = s_exp_3(i) * sin(angle(i));
    Y_exp_4(i) = s_exp_4(i) * sin(angle(i));
    Y_exp_5(i) = s_exp_5(i) * sin(angle(i));
    Y_obst(i) = dist_to_obst * sin(angle(i));    
end

% theta vs. scanner_value & reference
figure;
set(gcf,'color','white');
subplot(1,2,1);
Farid Alijani's avatar
Farid Alijani committed
65

Farid Alijani's avatar
Farid Alijani committed
66
67
68
69
70
71
72
73
74
75
% plot(angle,s_exp_1,'m--','LineWidth',1.1);
% hold on;
plot(angle,s_exp_2,'m--','LineWidth',1.9);
hold on;
plot(angle,s_exp_3,'b--','LineWidth',1.9);
hold on;
plot(angle,s_exp_4,'g--','LineWidth',1.1);
hold on;
% plot(angle,s_exp_5,'b--','LineWidth',.8);
% hold on;
Farid Alijani's avatar
Farid Alijani committed
76
plot(angle,dist_to_obst,'rx','LineWidth',1.05);
Farid Alijani's avatar
Farid Alijani committed
77
title('');
Farid Alijani's avatar
Farid Alijani committed
78
79
% xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',20);
% ylabel('${Distance}$ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
80
axis([-.09 .42 .08 .2]);
Farid Alijani's avatar
Farid Alijani committed
81
% set(gca,'box','off','fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
82
83
84
85
86
87
88
89
90
91
92
93
% legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve');
subplot(1,2,2);
% plot(X_exp_1,Y_exp_1,'m--','LineWidth',.9);
% hold on
plot(X_exp_2,Y_exp_2,'m--','LineWidth',1.9);
hold on
plot(X_exp_3,Y_exp_3,'b--','LineWidth',1.9);
hold on
plot(X_exp_4,Y_exp_4,'g--','LineWidth',1.1);
hold on
% plot(X_exp_5,Y_exp_5,'b--','LineWidth',.1);
% hold on
Farid Alijani's avatar
Farid Alijani committed
94
plot(X_obst,Y_obst,'r','LineWidth',2.05);
Farid Alijani's avatar
Farid Alijani committed
95
title('');
Farid Alijani's avatar
Farid Alijani committed
96
97
% xlabel('${X}$ [m]','interpreter','latex','FontSize',20);
% ylabel('${Y}$ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
98
axis([-.1 .16 -.18 .18]);
Farid Alijani's avatar
Farid Alijani committed
99
% set(gca,'box','off','fontsize',20);
Farid Alijani's avatar
Farid Alijani committed
100
101
l = legend('Trial 1','Trial 2','Trial 3','Safety Curve','Orientation','vertical');
set(l,'interpreter','latex','FontSize',16);
Farid Alijani's avatar
Farid Alijani committed
102
% legend('box','off')
Farid Alijani's avatar
Farid Alijani committed
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
%% docked robot with marker vs. docked robot without marker!
S_Mat_DockedwithMar = csvread('scanner_data_Docked_WITH_Marker.txt',0,0);
S_Mat_DockedwithNOmar = csvread('scanner_data_no_cylinder.txt',0,0);

indx_DockedwithMar = S_Mat_DockedwithMar(:,1);
s_val_DockedwithMar = S_Mat_DockedwithMar(:,2);

indx_DockedwithNOmar = S_Mat_DockedwithNOmar(:,1);
s_val_DockedwithNOmar = S_Mat_DockedwithNOmar(:,2);

points_DockedwithMar = size(indx_DockedwithMar,1);
points_DockedwithNOmar = size(indx_DockedwithNOmar,1);

laser_beam_DockedwithMar = points_DockedwithMar - 1;
laser_beam_DockedwithNOmar = points_DockedwithNOmar - 1;

s_angle_DockedwithMar = zeros(points_DockedwithMar,1);
s_angle_DockedwithNOmar = zeros(points_DockedwithNOmar,1);

X_DockedwithMar = zeros(points_DockedwithMar,1); Y_DockedwithMar = zeros(points_DockedwithMar,1);
X_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_DockedwithNOmar = zeros(points_DockedwithNOmar,1);

X_obst_DockedwithMar = zeros(points_DockedwithMar,1); Y_obst_DockedwithMar = zeros(points_DockedwithMar,1);
X_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1);


for i = 1:points_DockedwithMar
    s_angle_DockedwithMar(i) = (lower_angle + (indx_DockedwithMar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithMar))))* pi/180;
    X_DockedwithMar(i) = s_val_DockedwithMar(i) * cos(s_angle_DockedwithMar(i));
    Y_DockedwithMar(i) = s_val_DockedwithMar(i) * sin(s_angle_DockedwithMar(i));
    X_obst_DockedwithMar(i) = dist_to_obst * cos(s_angle_DockedwithMar(i));
    Y_obst_DockedwithMar(i)= dist_to_obst * sin(s_angle_DockedwithMar(i));
end


for i = 1:points_DockedwithNOmar
    s_angle_DockedwithNOmar(i) = (lower_angle + (indx_DockedwithNOmar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithNOmar))))* pi/180;
    X_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * cos(s_angle_DockedwithNOmar(i));
    Y_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * sin(s_angle_DockedwithNOmar(i));
    X_obst_DockedwithNOmar(i) = dist_to_obst * cos(s_angle_DockedwithNOmar(i));
    Y_obst_DockedwithNOmar(i)= dist_to_obst * sin(s_angle_DockedwithNOmar(i));
end

Farid Alijani's avatar
Farid Alijani committed
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169


% The robot is totally docked and a box is besides of laser scanner!
S_Mat_DockedwithBOX = csvread('scanner_data_box.txt',0,0);
indx_DockedwithBOX = S_Mat_DockedwithBOX(:,1);
s_val_DockedwithBOX = S_Mat_DockedwithBOX(:,2);

points_DockedwithBOX = size(indx_DockedwithBOX,1);
laser_beam_DockedwithBOX = points_DockedwithBOX - 1;
s_angle_DockedwithBOX = zeros(points_DockedwithBOX,1);

X_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_DockedwithBOX = zeros(points_DockedwithBOX,1);
X_obst_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_obst_DockedwithBOX = zeros(points_DockedwithBOX,1);

for i = 1:points_DockedwithBOX
    s_angle_DockedwithBOX(i) = (lower_angle + (indx_DockedwithBOX(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithBOX))))* pi/180;
    X_DockedwithBOX(i) = s_val_DockedwithBOX(i) * cos(s_angle_DockedwithBOX(i));
    Y_DockedwithBOX(i) = s_val_DockedwithBOX(i) * sin(s_angle_DockedwithBOX(i));
    
    
    X_obst_DockedwithBOX(i) = dist_to_obst * cos(s_angle_DockedwithBOX(i));
    Y_obst_DockedwithBOX(i)= dist_to_obst * sin(s_angle_DockedwithBOX(i));
end

Farid Alijani's avatar
Farid Alijani committed
170
171
172
173
% checking accuracy in robot coordinate system
figure;
set(gcf,'color','white');

Farid Alijani's avatar
Farid Alijani committed
174
175
subplot(321);
plot(s_angle_DockedwithNOmar,s_val_DockedwithNOmar,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
176
hold on
Farid Alijani's avatar
Farid Alijani committed
177
plot(s_angle_DockedwithNOmar,dist_to_obst,'r','LineWidth',3);
Farid Alijani's avatar
Farid Alijani committed
178
179
% title('No marker on docking platform');
axis([-2.2 2.2 -.5 10]);
Farid Alijani's avatar
Farid Alijani committed
180
181
% set(gca,'box','off','fontsize',20);
set(gca,'box','off');
Farid Alijani's avatar
Farid Alijani committed
182

Farid Alijani's avatar
Farid Alijani committed
183
184
subplot(322);
plot(X_DockedwithNOmar,Y_DockedwithNOmar,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
185
186
hold on;
plot(X_obst_DockedwithNOmar,Y_obst_DockedwithNOmar,'r','LineWidth',3);
Farid Alijani's avatar
Farid Alijani committed
187
% title('No marker on docking platform, TOP VIEW');
Farid Alijani's avatar
Farid Alijani committed
188
% legend('Obstacles','Laser scanner safety curve (r = 10 cm)');
Farid Alijani's avatar
Farid Alijani committed
189
% grid on
Farid Alijani's avatar
Farid Alijani committed
190
axis([-2 4.5 -4 1.5]);
Farid Alijani's avatar
Farid Alijani committed
191
192
% set(gca,'box','off','fontsize',20);
set(gca,'box','off');
Farid Alijani's avatar
Farid Alijani committed
193

Farid Alijani's avatar
Farid Alijani committed
194
subplot(323);
Farid Alijani's avatar
Farid Alijani committed
195
orig_coordinate_cir = gca; % create axes
Farid Alijani's avatar
Farid Alijani committed
196
plot(orig_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
197
hold on
Farid Alijani's avatar
Farid Alijani committed
198
plot(orig_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
199
% ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
200
axis([-2.3 2.3 -.5 10]);
Farid Alijani's avatar
Farid Alijani committed
201
202
% set(orig_coordinate_cir,'box','off','fontsize',20);
set(orig_coordinate_cir,'box','off');
Farid Alijani's avatar
Farid Alijani committed
203
204
205
206
207
208
209
210
211
212

% ----- Zoomed in ------
hkids_cir = get(orig_coordinate_cir, 'child'); % get original coordinate properties

set(hkids_cir(1),'marker','o','Color','r')
set(hkids_cir(2),'marker','.')

zoomed_coordinate_cir = axes('Position',get(orig_coordinate_cir,'Position'),...
           'XAxisLocation','bottom',...
           'YAxisLocation','left',...
Farid Alijani's avatar
Farid Alijani committed
213
           'fontsize',10,...
Farid Alijani's avatar
Farid Alijani committed
214
215
216
           'Color','none',...
           'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings

Farid Alijani's avatar
Farid Alijani committed
217
plot(zoomed_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
218
hold on;
Farid Alijani's avatar
Farid Alijani committed
219
plot(zoomed_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r-','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
220

Farid Alijani's avatar
Farid Alijani committed
221
222
set(zoomed_coordinate_cir,'xlim',[-.15 .32])% axes limit for zoom out,
set(zoomed_coordinate_cir,'ylim',[.08 .2])% axes limit for zoom out,
Farid Alijani's avatar
Farid Alijani committed
223
224
225
set(zoomed_coordinate_cir, 'Units', 'normalized', 'Position', [.35 0.48 0.15 0.18])
% ----- Zoomed in ------

Farid Alijani's avatar
Farid Alijani committed
226
227
228
229
230
231
232
233
234
% subplot(324);
% orig_coordinate_cir_T = gca; % create axes
% plot(orig_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar,'b','LineWidth',2.7);
% hold on;
% plot(orig_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
% % ylabel('$ {Y} $ [m]','interpreter','latex','FontSize',20);
% axis([-2 4.5 -4 1.5]);
% % set(orig_coordinate_cir_T,'box','off','fontsize',20);
% set(orig_coordinate_cir_T,'box','off');
Farid Alijani's avatar
Farid Alijani committed
235
% 
Farid Alijani's avatar
Farid Alijani committed
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
% % ----- Zoomed in ------
% hkids_cir_T = get(orig_coordinate_cir_T, 'child'); % get original coordinate properties
% % 
% % set(hkids_cir_T(1),'marker','.','Color','r')
% % set(hkids_cir_T(2),'marker','*')
% 
% set(hkids_cir_T(1),'Color','r')
% set(hkids_cir_T(2),'Color','b')
% 
% 
% zoomed_coordinate_cir_T = axes('Position',get(orig_coordinate_cir_T,'Position'),...
%            'XAxisLocation','bottom',...
%            'YAxisLocation','left',...
%            'fontsize',10,...
%            'Color','none',...
%            'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
% 
% plot(zoomed_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar,'b','LineWidth',2.7);
% hold on;
% plot(zoomed_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
% 
% set(zoomed_coordinate_cir_T,'xlim',[.08 .13])% axes limit for zoom out,
% set(zoomed_coordinate_cir_T,'ylim',[-.06 .06])% axes limit for zoom out,
% set(zoomed_coordinate_cir_T, 'Units', 'normalized', 'Position', [.84 0.485 0.15 0.18])
% % ----- Zoomed in ------
Farid Alijani's avatar
Farid Alijani committed
261

Farid Alijani's avatar
Farid Alijani committed
262

Farid Alijani's avatar
Farid Alijani committed
263
subplot(325);
Farid Alijani's avatar
Farid Alijani committed
264
orig_coordinate_box = gca; % create axes
Farid Alijani's avatar
Farid Alijani committed
265
plot(orig_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
266
hold on
Farid Alijani's avatar
Farid Alijani committed
267
plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',3);
Farid Alijani's avatar
Farid Alijani committed
268
% xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
269
axis([-2.3 2.3 -.15 5]);
Farid Alijani's avatar
Farid Alijani committed
270
271
% set(orig_coordinate_box,'box','off','fontsize',20);
set(orig_coordinate_box,'box','off');
Farid Alijani's avatar
Farid Alijani committed
272
273
274
275
276
277
278
279
280
281

% ----- Zoomed in BOX------
hkids_box = get(orig_coordinate_box, 'child'); % get original coordinate properties

set(hkids_box(1),'marker','o','Color','r')
set(hkids_box(2),'marker','.')

zoomed_coordinate_box = axes('Position',get(orig_coordinate_box,'Position'),...
           'XAxisLocation','bottom',...
           'YAxisLocation','left',...
Farid Alijani's avatar
Farid Alijani committed
282
           'fontsize',10,...
Farid Alijani's avatar
Farid Alijani committed
283
284
285
           'Color','none',...
           'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings

Farid Alijani's avatar
Farid Alijani committed
286
plot(zoomed_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
287
hold on;
Farid Alijani's avatar
Farid Alijani committed
288
plot(zoomed_coordinate_box,s_angle_DockedwithBOX,dist_to_obst,'r','LineWidth',3);
Farid Alijani's avatar
Farid Alijani committed
289
290

set(zoomed_coordinate_box,'xlim',[-1.6 -.3])% axes limit for zoom out,
Farid Alijani's avatar
Farid Alijani committed
291
292
set(zoomed_coordinate_box,'ylim',[.08 .2])% axes limit for zoom out,
set(zoomed_coordinate_box, 'Units', 'normalized', 'Position', [.35 0.18 0.15 0.18])
Farid Alijani's avatar
Farid Alijani committed
293
294
% ----- Zoomed in BOX------

Farid Alijani's avatar
Farid Alijani committed
295

Farid Alijani's avatar
Farid Alijani committed
296
subplot(326);
Farid Alijani's avatar
Farid Alijani committed
297
orig_coordinate_box_T = gca; % create axes
Farid Alijani's avatar
Farid Alijani committed
298
plot(orig_coordinate_box_T,X_DockedwithBOX,Y_DockedwithBOX,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
299
hold on
Farid Alijani's avatar
Farid Alijani committed
300
plot(orig_coordinate_box_T,X_obst_DockedwithMar,Y_obst_DockedwithMar, 'r','LineWidth',3);
Farid Alijani's avatar
Farid Alijani committed
301
% xlabel('$ {X} $ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
302
axis([-.53 .9 -.8 1.5]);
Farid Alijani's avatar
Farid Alijani committed
303
304
% set(orig_coordinate_box_T,'box','off','fontsize',20);
set(orig_coordinate_box_T,'box','off');
Farid Alijani's avatar
Farid Alijani committed
305
306
307
308
309
310
311
312
313
314

% ----- Zoomed in BOX_T------
hkids_box_T = get(orig_coordinate_box_T, 'child'); % get original coordinate properties

set(hkids_box_T(1),'Color','r')
set(hkids_box_T(2),'Color','b')

zoomed_coordinate_box_T = axes('Position',get(orig_coordinate_box_T,'Position'),...
           'XAxisLocation','bottom',...
           'YAxisLocation','left',...
Farid Alijani's avatar
Farid Alijani committed
315
           'fontsize',10,...
Farid Alijani's avatar
Farid Alijani committed
316
317
318
           'Color','none',...
           'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings

Farid Alijani's avatar
Farid Alijani committed
319
plot(zoomed_coordinate_box_T,X_DockedwithBOX,Y_DockedwithBOX,'b','LineWidth',2.7);
Farid Alijani's avatar
Farid Alijani committed
320
hold on;
Farid Alijani's avatar
Farid Alijani committed
321
plot(zoomed_coordinate_box_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
Farid Alijani's avatar
Farid Alijani committed
322
323
324
325
326
327
328

set(zoomed_coordinate_box_T,'xlim',[-.16 .2])% axes limit for zoom out,
set(zoomed_coordinate_box_T,'ylim',[-.22 .22])% axes limit for zoom out,
set(zoomed_coordinate_box_T, 'Units', 'normalized', 'Position', [.84 0.2 .15 .18])
% ----- Zoomed in BOX_T------


Farid Alijani's avatar
Farid Alijani committed
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
%% The robot is moving Towards docking...!
Scanner_Matrix_Moving = csvread('scanner_data_Moving_Towards_Docking_WITH_Marker.txt',0,0);

index_Moving = Scanner_Matrix_Moving(:,1);
scanner_value_Moving = Scanner_Matrix_Moving(:,2);

index_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),1);
scanner_value_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),2);

% figure;
% plot(index_Moving,scanner_value_Moving,'m',index_Moving_Final,scanner_value_Moving_Final,'k*');
% legend('Distance to obstacle','Reference Position');
% grid on;
% xlabel('Index');
% ylabel('Distance to Obstacle [m]');
% title('Scanner Analysis for a moving robot!');
% axis([0 540 0 31]);


%% The robot is in the docking area, while NOT docked!
S_Mat_Out = csvread('scanner_data_outside.txt',0,0);
indx_out = S_Mat_Out(:,1);
s_val_out = S_Mat_Out(:,2);

points_out = size(indx_out,1);
laser_beam_out = points_out - 1;
s_angle_out = zeros(points_out,1);
X_out = zeros(points_out,1); Y_out = zeros(points_out,1);
X_obst_out = zeros(points_out,1); Y_obst_out = zeros(points_out,1);

for i = 1:points_out
    s_angle_out(i) = (lower_angle + (indx_out(i)* ((upper_angle - lower_angle)/(laser_beam_out))))* pi/180;
    X_out(i) = s_val_out(i) * cos(s_angle_out(i));
    Y_out(i) = s_val_out(i) * sin(s_angle_out(i));
    
    X_obst_out(i) = dist_to_obst * cos(s_angle_out(i));
    Y_obst_out(i)= dist_to_obst * sin(s_angle_out(i));
end

Farid Alijani's avatar
Farid Alijani committed
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
% figure;
% set(gcf,'color','white');
% subplot(1,2,1);
% plot(s_angle_out,s_val_out,'b','LineWidth',2);
% hold on
% plot(s_angle_out,dist_to_obst,'r','LineWidth',10);
% xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',14);
% ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14);
% grid on;
% % axis([-2.23 2.23 -1 10]);
% 
% subplot(1,2,2)
% plot(X_out,Y_out,'b','LineWidth',2);
% hold on
% plot(X_obst_out,Y_obst_out,'r-','LineWidth',4)
% xlabel('$X$ [m] ','interpreter','latex','FontSize',14);
% ylabel('$Y$ [m]','interpreter','latex','FontSize',14);
% 
% l2 = legend('${Obstacles}$', '${Safety Margin (r = 10 cm)}$');
% set(l2,'interpreter','latex','FontSize',12);
% % legend('boxoff');
% grid on;
% % axis([-1.2 5 -3.5 1.5]);
Farid Alijani's avatar
Farid Alijani committed
391
392


Farid Alijani's avatar
Farid Alijani committed
393
394
395
396
397
398
399
400
401
402
403
404
405

% figure;
% plot(X_DockedwithMar,Y_DockedwithMar,'b');
% hold on;
% plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',.3);
% xlabel('${X}$ [m]','interpreter','latex','FontSize',14);
% % ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
% % grid on
% title('Detected marker (zoomed area from middle plot), TOP VIEW');
% axis([.04 .16 -.12 .12]);