Code covered by the BSD License  

Highlights from
HOKUYO UTM-30LX-EW Matlab API

image thumbnail

HOKUYO UTM-30LX-EW Matlab API

by

 

Adapted to support the Hokuyo to request HE measurements, i.e. multiecho distance and intensity.

[PC t]=utmTest(n,t)
function [PC t]=utmTest(n,t)

%clear all;
%clc;
%close all;

%%  hokuyo utm30-lx-ew

s='192.168.147.65';  % IP address of device (Hokuyo laser scanner)

% if ~exist('t','var')
%     t=utmOpen;
% end

if (n==1)
    t=utmOpen;
end

start=0;
stop=1080;
skip=0;

% Angular resolution (360/1440) of model UTM-30LX-EW
res=0.25;

% Wide scan angle 270 degrees
angleVector=((res*start:res:stop*res)-135)'.*pi/180;

nscans=1; % number of required scans

% Contains all received data - values of multi distance and intensity
dataVector=cell(nscans,1);

% Collect data from the laser scanner
for u=1:nscans
    dataVector{u}= utmGetScan(t,start,stop);
    %    disp('******************************');
end

% Disconnect and clean up the connection with laser scanner
% fclose(t);
% delete(t);
% clear t;

for fr=1:nscans
    columnVector=dataVector{fr};
end

% Detecting the delimiter symbol '&' which appears between multiechoes during reading data from the scanner (in ASCII table is expressed as number 38)
Ind38=find(columnVector==38); % find symbol '&' and store it as index 

colV_no38=columnVector(setdiff(1:length(columnVector),Ind38)); % remove '&', where colV=columnVector

% Decode received data from the scanner
DataDecoded=utmDecode(colV_no38); % utmDecode script takes as input 6 raw readings for each point (3 characters encoding for distance + 3 for intensity) and gives as output meaningful values of distance-intensity pair

% Getting rid of the delimiter symbol (next steps consider shifting the rows of the matrix inserting the new 'empty' ones)
emp=zeros(size(colV_no38)); % first, create empty zero matrix
emp=emp-999;           % fill it with some random values (e.g. -999) in order not to mix it later with other meaningful 0 values
emp(1:3:(length(DataDecoded)*3),:)=DataDecoded; % fill every third row of empty matrix with values of previous decoded data
emp_temp=zeros(length(columnVector),1);
emp_temp=emp_temp-999;
emp_temp((setdiff(1:length(columnVector),Ind38))')=emp; % this line contains part of the code from an upper line
emp_temp(Ind38,:)=38; % insert delimiter to places based on the initial origin index
Ind999=find(emp_temp==-999); % assistant index
K=emp_temp(setdiff(1:length(emp_temp),Ind999)); % K is data decoded vector with 38 ('&')
idx38_K=find(K==38); % index of symbol '&' within column vector K (performed action had goal to "Bring back symbol '&'" between decoded data, i.e. distance-intensity pairs)

% Index of 2nd and 3rd multi distance-intensity pairs (copyright Boris Jutzi)
PC.idx_mult=int16(((idx38_K(1:length(idx38_K))-(1:length(idx38_K))')/2)+1);
     
    no_mult_pulses=numel(PC.idx_mult); % amount of 2nd and 3rd multi pulses

% Index of only 3rd multiecho pairs (copyright Ana Djuricic)
% PC.idx_3rd=PC.idx_mult(find(PC.idx_mult(2:(length(PC.idx_mult)))-PC.idx_mult(1:(length(PC.idx_mult)-1))==1)+1); % one line solution or... 
% next few lines are the same solution like line above but in more details
a=length(PC.idx_mult)-1;
b=PC.idx_mult(1:a);
c=length(PC.idx_mult);
d=PC.idx_mult(2:c);
e=d-b;
PC.idx_3rd=int16(PC.idx_mult(find(e==1))+1);
    
    PC.n3rd=numel(PC.idx_3rd); % amount of 3rd multi pulses

% Index of 2nd multiecho pairs
idx_temp_new=[];
for i=1:length(PC.idx_3rd)
    %idx_temp_new(i,1)=find(PC.idx_mult==PC.idx_3rd(i));
    idx_temp_new=int16(find(PC.idx_mult==PC.idx_3rd(i))); % index of elements which should be excluded
end

% In case of returning an empty index, consider next steps
if isempty(idx_temp_new)
    %disp('no third echo');
    PC.idx_2nd=int16(PC.idx_mult);
else
    PC.idx_2nd=int16(PC.idx_mult(setdiff(1:length(PC.idx_mult),idx_temp_new)));
end
    PC.n2nd=numel(PC.idx_2nd); % amount of 2nd multi pulses

% Organizing data and results within structure 
PC.all_number_of_pulses = floor(length(DataDecoded)/2);
    
        PC.no_1st_pulses=PC.all_number_of_pulses - no_mult_pulses; % amount of all 1st pulses

    % Create a matrix of distance-intensity pairs
    PC.range_intens_pairs=[DataDecoded(1:2:2*PC.all_number_of_pulses) ((DataDecoded(2:2:2*PC.all_number_of_pulses)-min(DataDecoded(2:2:2*PC.all_number_of_pulses)))/(max(DataDecoded(2:2:2*PC.all_number_of_pulses))-min(DataDecoded(2:2:2*PC.all_number_of_pulses))))*255];

    range_intens(1:length(PC.range_intens_pairs),:)=1;
    range_intens(PC.idx_2nd,:)=0;
    range_intens(PC.idx_3rd,:)=0;
    C=cumsum(range_intens); % returns an array containing the cumulative sum
    % C_new=C(1:end-3); % in order to get 1084-3=1081

% Create angleVector
end_point=length(C)-1;

res=0.25;
start_point=0;
angleVector=((res*start_point:res:end_point*res))'.*pi/180;

PC.X=single(PC.range_intens_pairs(:,1).*cos(angleVector(C(:,1))));
PC.Y=single(PC.range_intens_pairs(:,1).*sin(angleVector(C(:,1))));
PC.Z=single(PC.range_intens_pairs(:,1).*0);
PC.XYZ=[PC.X PC.Y PC.Z];

% % 2-D colorful plot of multiechoes
% plot (PC.X(setdiff(1:length(PC.X),PC.idx_mult)),PC.Y(setdiff(1:length(PC.Y),PC.idx_mult)), 'b*', PC.X(PC.idx_2nd), PC.Y(PC.idx_2nd), 'g*', PC.X(PC.idx_3rd), PC.Y(PC.idx_3rd), 'r*')
% axis equal

%clear PC;
clear angleVector; clear emp; clear K; clear range_intens; clear a; clear b; clear e; clear d; clear c;
end



Contact us