-
Notifications
You must be signed in to change notification settings - Fork 4
/
show_sensor_data.m
239 lines (184 loc) · 9.31 KB
/
show_sensor_data.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
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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
% ----------------------------------------------------------------
% --- show_sensor_data ----
%
% This function displaies the ROBOX robot data.
%
%
% Developed by Marcelo Becker - EPFL ASL September, 2005
%
% BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
% FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
% OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
% PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
% OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
% TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
% PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
% REPAIR OR CORRECTION.
%
% 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
% WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
% REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
% INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
% OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
% TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
% YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
% PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGES.
% -----------------------------------------------------------------
function show_sensor_data(L)
% Global variables declaration
global SwitchProc1 SwitchProc2 SwitchProc3 % used for the Selection of the Procedures
global minNr % Initial File Number
global maxNr % Final File Number
global GRAF % Radio Button
global EDIT % Edit Text on Robox Window
global L
global InitialFileName % Name of the Initial File
global FinalFileName % Name of the Final File
global InitialFilePath % Path of the Initial File
global FinalFilePath % Path of the Final File
global Root_FN % File Name Root
global Folder_Path % File Path
global TEXTOX TEXTOY % Figure2 parameters
global TITULO EIXOS LEGENDA % Figure2 parameters
global FIGURA FIGU FIGU2 % Figure parameters
global obstQuantity obstBeginning obstEnd dist2Obst ang2Obst % Sensor data derivation Approach
global der1 der2 der3 SSMedian1 % Sensor data derivation Approach variables
global LINHA XLIMI YLIMI
global Observation_radius positionROBOX cellQuantity % Figure Parameters
global xxc yyc scans_p_sensor CGx CGy maxNrsets % Figure Parameters
global obstBeginning obstEnd angulo distancia nrset % Figure Parameters
global sizegrid robotradius contmob setcells Ka ncells
global xrealoccup yrealoccup OldSetCells time2GetNewScan
global NewMoMatched OldMoMatched VelObst ThetaObst
global minimumdist nrcellp
% -------------------------------------------------------------------------
%========================
% Building the Interface
%========================
% Screen Resolution
TMP=get(0,'ScreenSize');
RSTELA=TMP(3:4);
FOLGA=20;
COR_JANELA=[1 1 1];
TAM_FIG=[600 500];
FIGU2 = figure('NumberTitle','off',...
'MenuBar','figure',...
'Resize','on',...
'NextPlot','replacechildren',...
'Color',COR_JANELA,...
'Position',[(RSTELA-TAM_FIG)/2 TAM_FIG]);
% Checkbox to use grid on figure
GRADE=uicontrol('Style','checkbox','Value',0,'Position',[450 230 110 20],...
'BackgroundColor',COR_JANELA,'String','Enable Grid Lines','Callback',['TMP=get(gcbo,''Value'');',...
'if TMP, set(EIXOS,''XGrid'',''on'',''YGrid'',''on'');',...
'else, set(EIXOS,''XGrid'',''off'',''YGrid'',''off''); end;']);
% Checkbox to show the data uncertainties on figure
Uncert=uicontrol('Style','checkbox','Value',0,'Position',[450 200 110 20],...
'BackgroundColor',COR_JANELA,'String','Data Uncertainties','Callback',['TMP=get(gcbo,''Value'');',...
'if TMP, set(EIXOS,''XGrid'',''on'',''YGrid'',''on'');',...
'else, set(EIXOS,''XGrid'',''off'',''YGrid'',''off''); end;']);
POS_EIXOS=[80 100];
TAM_EIXOS=[350 350];
EIXOS=axes('Box','on','Units','pixels','Position',[POS_EIXOS TAM_EIXOS]);
TEXTOX=xlabel('X axis [m]','Parent',EIXOS);
TEXTOY=ylabel('Y axis [m]','Parent',EIXOS);
TITULO=title('ROBOX Sensor Data in Cartesian Plane','Parent',EIXOS,...
'FontSize',12,'FontWeight','bold');
% Restore Figure Button
BOT_REST=uicontrol('Position',[450 350 100 20],'String','Restore Figure',...
'BackgroundColor',COR_JANELA,'Callback',...
['set(EIXOS,''XLimMode'',''manual''); set(EIXOS,''YLimMode'',''manual'');',...
'XLIMI=[-10 10];','YLIMI=[-10 10];','set(EIXOS,''XLim'',XLIMI);',...
'set(EIXOS,''YLim'',YLIMI);',...
'set(EDIT(3),''String'',num2str(XLIMI(1)));',...
'set(EDIT(4),''String'',num2str(XLIMI(2)));',...
'set(EDIT(5),''String'',num2str(YLIMI(1)));',...
'set(EDIT(6),''String'',num2str(YLIMI(2)));']);
% SAVE Figura Button
BOT_SAVE=uicontrol('Position',[450 310 100 20],'String','Save Figure',...
'BackgroundColor',COR_JANELA,'Callback','savegraph(FIGU);');
% Return Button
L=1;
BOT_SAIDA=uicontrol('Position',[450 270 100 20],'String','Return',...
'BackgroundColor',COR_JANELA,'Callback','close; robox;');
% Controle para ZOOM da Figura
uicontrol('Style','text','Position',[60 15 40 20],'BackgroundColor',COR_JANELA,...
'String','Xmin:');
uicontrol('Style','text','Position',[180 15 40 20],'BackgroundColor',COR_JANELA,...
'String','Xmax:');
uicontrol('Style','text','Position',[300 15 40 20],'BackgroundColor',COR_JANELA,...
'String','Ymin:');
uicontrol('Style','text','Position',[420 15 40 20],'BackgroundColor',COR_JANELA,...
'String','Ymax:');
% counters initialization
cont1 = 0; % Total data storaged
% Data input
% ----------
for nr=minNr:1:maxNr,
cont1 = cont1 + 1;
% Read the sensor data
if nr < 10
name=[strcat(Root_FN, sprintf('_0000%1d_scan.dat',nr))];
elseif nr < 100
name=[strcat(Root_FN, sprintf('_000%2d_scan.dat',nr))];
elseif nr < 1000
name=[strcat(Root_FN, sprintf('_00%3d_scan.dat',nr))];
elseif nr < 10000
name=[strcat(Root_FN, sprintf('_0%4d_scan.dat',nr))];
elseif nr < 100000
name=[strcat(Root_FN, sprintf('_%5d_scan.dat',nr))];
end
scan=importdata([Folder_Path name]);
disp('Openning Files...')
% Read the Robox data
if nr < 10
name=[strcat(Root_FN, sprintf('_0000%1d_misc.dat',nr))];
elseif nr < 100
name=[strcat(Root_FN, sprintf('_000%2d_misc.dat',nr))];
elseif nr < 1000
name=[strcat(Root_FN, sprintf('_00%3d_misc.dat',nr))];
elseif nr < 10000
name=[strcat(Root_FN, sprintf('_0%4d_misc.dat',nr))];
elseif nr < 100000
name=[strcat(Root_FN, sprintf('_%5d_misc.dat',nr))];
end
[position] = textread([Folder_Path name],'%n','delimiter','\n','whitespace','');
% position' = [1 time x y theta]
time(cont1) = position(2); % processing time [msec]
posXROBOX(cont1) = position(3);
posYROBOX(cont1) = position(4);
thetaROBOX(cont1) = position(5);
positionROBOX = [posXROBOX' posYROBOX' thetaROBOX'];
% input data storage: polar data
% One should observe that we are using a local and polar coordenate system
distancia = scan(:,2);
angulo = thetaROBOX(cont1) + scan(:,1);
[xxc, yyc] = polar2cart(distancia,angulo);
[s1 s2] = size(distancia);
cla; % Clear de axes area
set(FIGU2,'Name',sprintf('Sensor Data - [%s]',name));
set(EIXOS,'XLimMode','manual');
set(EIXOS,'YLimMode','manual');
XLIMI=[-10 10];
YLIMI=[-10 10];
set(EIXOS,'XLim',XLIMI);
set(EIXOS,'YLim',YLIMI);
efe=patch(xxc, yyc,[0.95 0.95 0.95],'LineStyle','-');
LINHA=line(xxc,yyc,'Parent',EIXOS,'Marker','.','Color','blue', 'LineStyle', 'none');
drawROBOX2D(0,0,positionROBOX(cont1,3));
pause(0.5);
end
EDIT(3)=uicontrol('Style','edit','Position',[100 20 60 20],'BackgroundColor',[1 1 1],...
'String',num2str(XLIMI(1)),'Callback',['XLIMI(1)=str2num(get(gcbo,''String''));',...
'set(EIXOS,''XLim'',XLIMI);']);
EDIT(4)=uicontrol('Style','edit','Position',[220 20 60 20],'BackgroundColor',[1 1 1],...
'String',num2str(XLIMI(2)),'Callback',['XLIMI(2)=str2num(get(gcbo,''String''));',...
'set(EIXOS,''XLim'',XLIMI);']);
EDIT(5)=uicontrol('Style','edit','Position',[340 20 60 20],'BackgroundColor',[1 1 1],...
'String',num2str(YLIMI(1)),'Callback',['YLIMI(1)=str2num(get(gcbo,''String''));',...
'set(EIXOS,''YLim'',YLIMI);']);
EDIT(6)=uicontrol('Style','edit','Position',[460 20 60 20],'BackgroundColor',[1 1 1],...
'String',num2str(YLIMI(2)),'Callback',['YLIMI(2)=str2num(get(gcbo,''String''));',...
'set(EIXOS,''YLim'',YLIMI);']);