-
Notifications
You must be signed in to change notification settings - Fork 2
/
hokuyoExampleSingleRequest.m
executable file
·127 lines (99 loc) · 3.38 KB
/
hokuyoExampleSingleRequest.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
%Sample script that uses the hokuyoAPI interface to Hokuyo C++ driver
%Sensor must be upgraded to SCIP2.0 format
%Compile the API with the mexHokuyoAPI script.
%Note: in order to shutdown the device cleanly, type "clear hokuyoAPI" or "clear all"
%Aleksandr Kushleyev
%University of Pennsylvania, 2008
%{
General info (with Hokuyo URG-04LX):
The sensor provides three types of data:
-range (0-5500 mm)
-intensity (reflectivity) 0-10000 or 0-40000 (for some reason older sensors have different range)
-AGC (Automatic Gain Control) 0-1000 (somewhat inverse proportional (but nonlinear..) relationship to reflectivity of the surface)
%}
%provide the device name and baud rate
%(if you use USB connection, then the baud rate setting does not matter
% - the device will work at full speed regardless of the setting)
%dev='/dev/cu.usbmodem0000103D1';
dev='/dev/ttyACM0';
baud=115200; %does not matter for USB
if ~hokuyoAPI('open',dev,baud)
return;
end
disp('press any key to start reading data');
pause;
%{
maximum number of points for hokuyo URG 04-LX is 769
however, if you request range, reflectivity, and agc data,
(all three at the same time), only 1/3 of points will be returned -
every 2 out of 3 points will be thrown out to maintain the same
total number of points - see hokuyo protocol...
%}
n_points=769; %max for URG-04LX
%n_points=1081; %max for URG-30LX
%may need to play with resolution - the one provided in the manual produces distorted (bent) walls.
%just try changing it manually
res=0.356; %URG-04LX
%res=0.25; %URG-30LX
%the vector of angles
angles=((0:res:(n_points-1)*res)-135)'*pi/180;
%setup figures
figure(1);
clf(gcf);
hXY=plot(0,0,'b.');
axis([-5500 5500 -5500 5500]);
figure(2);
clf(gcf);
hR=plot(0,0,'b.');hold on;
hRef=plot(0,0,'g.');
hAGC=plot(0,0,'r.'); hold off;
start=0; %start step of the scan
stop=n_points-1; %end step of the scan
encoding=3; %2 or 3 character data encoding (data coming from sensor to the driver)
%all possible command modes
cmd='range'; %both URG-04LX and URG-30LX
%cmd='top_urg_range+intensity'; %only for URG-30LX
%the commands below are not supporoted by URG-30LX
%cmd='range+intensityAv+AGCAv';
%cmd='range+intensity0+AGC0';
%cmd='range+intensity1+AGC1';
%cmd='range+intensityAv';
%cmd='range+intensity0';
%cmd='range+intensity1';
%cmd='intensityAv';
%cmd='intensity0';
%cmd='intensity1';
%cmd='AGC0';
%cmd='AGC1';
%only for range measurements (04LX), and both range and intensity (30LX),
%you can specify how many points to skip:
% skip=1: return all points, skip=2: return every other point,etc
skip=1;
while(1)
tic
%get the scan
[r ref agc]=hokuyoAPI('getScan',cmd,start,stop,skip,encoding);
toc
%continue;
if ~isempty(r)
%only plot the x,y coordinates if 'r' is the only thing you requested
%otherwise, you have to calculate the angles differently, because
%points get thrown out - this is just a quick demo..
if size(r)==size(angles)
x=r.*cos(angles);
y=r.*sin(angles);
set(hXY,'xdata',x,'ydata',y);
end
%plot the radii
set(hR,'xdata',1:length(r),'ydata',r);
end
if ~isempty(ref)
%plot the reflectivity data
set(hRef,'xdata',1:length(ref),'ydata',ref);
end
if ~isempty(agc)
%plot the AGC (automatic gain correction) data
set(hAGC,'xdata',1:length(agc),'ydata',agc);
end
drawnow;
end