在Matlab中捕获和更新激光雷达扫描数据

时间:2016-02-05 15:51:14

标签: matlab sensor lidar

我想获得一台Hokuyo UBG-04LX-F01激光雷达来扫描和读取范围数据到matlab。我的代码只返回一次扫描的数据,有人可以提供有关如何让代码连续返回扫描数据的帮助(在激光雷达扫描时不断更新)。代码适用于matlab。感谢。

%This is the function to capture and display scan data
%It displays range data for one scan for 682 steps

function [rangescan] = CaptureData(Urg_device)

proceed=0;
while (proceed==0)
    fprintf(Urg_device, 'GD0044072500\n');
    pause(0.1);
    data = fscanf(Urg_device);

    if numel(data) == 2134
        proceed = 1;
    end 
end

i = find(data == data(13));
rangedata = data(i(3)+1:end-1);
onlyrangedata = [];
for j=0:31
    onlyrangedata((64*j)+1:(64*j)+64) = rangedata( 1+(66*j):64+(66*j));
end

j=0;
for i=1:floor(numel(onlyrangedata)/3)
    encodeddist(i,:)=[onlyrangedata((3*j)+1) onlyrangedata((3*j)+2) onlyrangedata((3*j)+3)];      
    j=j+1;
end
for k=1:size(encodeddist,1)
    rangescan(k)=decodeSCIP(encodeddist(k,:));
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This function decodes the data, works for 2 or 3 bit character encoding

function rangeval=decodeSCIP(rangeenc)
% Check for 2 or 3 Character Encoding
if rangeenc(1)=='0' && rangeenc(2)=='0' && rangeenc(3)=='0'
    rangeval=0;
    return;
end
if rangeenc(1)=='0'
    dig1=((rangeenc(2)-'!')+33);
    dig2=((rangeenc(3)-'!')+33);
    dig1sub=dig1-48;
    dig2sub=dig2-48;
    dig1bin=dec2bin(dig1sub,6);
    dig2bin=dec2bin(dig2sub,6);
    rangeval=bin2dec([dig1bin dig2bin]);
    return;
else
    dig1=((rangeenc(1)-'!')+33);
    dig2=((rangeenc(2)-'!')+33);
    dig3=((rangeenc(3)-'!')+33);
    dig1sub=dig1-48;
    dig2sub=dig2-48;
    dig3sub=dig3-48;
    dig1bin=dec2bin(dig1sub,6);
    dig2bin=dec2bin(dig2sub,6);
    dig3bin=dec2bin(dig3sub,6);
    rangeval=bin2dec([dig1bin dig2bin dig3bin]);
    return;

end

end

0 个答案:

没有答案