【MATLAB】ROS bag 激光雷达数据处理

基本步骤

rosbag record  /scan 打包数据
rosbag info 2021-11-12-16-54-23.bag  查看打包信息

clc,clear
filepath=fullfile('~/','2021-11-12-16-54-23.bag');
bag=rosbag(filepath);
geometry_message=select(bag,'MessageType','sensor_msgs/LaserScan');多个类型消息时选取雷达信息
data=readMessages(geometry_message);  
for i=1:1:360
    polarscatter(i,data{1,1}.Ranges(i),'.')极坐标画点
    hold on
end

problem:

rviz数据很规则,有聚堆现象为啥MATLAB,这么乱…原来MATLAB的极坐标画点函数是弧度制,修改后:

clc,clear
filepath=fullfile('~/','2021-11-13-16-22-08.bag');
bag=rosbag(filepath);
geometry_message=select(bag,'MessageType','sensor_msgs/LaserScan');
data=readMessages(geometry_message);  
for i=1:1:360
    polarscatter(i*0.0174,data{1,1}.Ranges(i),'.')
    hold on
end

nice!

与RVIZ odom坐标对应

clc,clear
filepath=fullfile('~/','2021-11-13-16-22-08.bag');
bag=rosbag(filepath);
geometry_message=select(bag,'MessageType','sensor_msgs/LaserScan');
data=readMessages(geometry_message);  
for i=1:1:360
    if i < 180
    polarscatter(-(180-i)*0.0174,data{1,1}.Ranges(i),'.')
    hold on
    elseif i>= 180
    polarscatter((i-180)*0.0174,data{1,1}.Ranges(i),'.')
    hold on
    end
end

两个包数据不一样,看最下面两个静态障碍物,可确定坐标已对齐
OK!

雷达聚类算法障碍物检测算法准备

clc,clear
%定义雷达分辨率
resolution=2*pi/360;
%读取ROS bag 数据包
filepath=fullfile('~/','2021-11-13-16-22-08.bag');
bag=rosbag(filepath);
%从bag数据包中选择指定类型读取
geometry_message=select(bag,'MessageType','sensor_msgs/LaserScan');
data=readMessages(geometry_message); 
%定义数组以极坐标形式保存雷达数据
LaserData=double(data{1,1}.Ranges(:));
%绘制雷达原始数据
for i=1:1:360
    if i < 180
    polarscatter(-(180-i)*resolution,LaserData(i),'.')
    LaserData(i,2)=-(180-i)*resolution;%在原始雷达数据中为点添加角度信息,用于后续计算点与点之间的距离
    hold on
    elseif i>= 180
    polarscatter((i-180)*resolution,LaserData(i),'.')    
    LaserData(i,2)=(i-180)*resolution;
    hold on
    end
end
%核心代码,雷达数据聚类TODO

NICE!什么都有了,可以利用原始数据,编写和验证自己的算法了,未完待续…
看了一篇文献,大致在MATLAB中粗略地尝试了,step1.
Real-Time Avoidance Strategy of Dynamic Obstacles via Half Model-Free Detection and Tracking With 2D Lidar for Mobile Robots二维雷达聚类算法

clc,clear
%定义雷达分辨率
resolution=2*pi/360;
%读取ROS bag 数据包
filepath=fullfile('~/','2021-11-13-16-22-08.bag');
bag=rosbag(filepath);
%从bag数据包中选择指定类型读取
geometry_message=select(bag,'MessageType','sensor_msgs/LaserScan');
data=readMessages(geometry_message); 
%定义数组以极坐标形式保存雷达数据
LaserData=double(data{1,1}.Ranges(:));
%绘制雷达原始数据
figure('Name','原始雷达信息','NumberTitle','off');

for i=1:1:360
    if i < 180
     polarscatter(-(180-i)*resolution,LaserData(i),'.')
    LaserData(i,2)=-(180-i)*resolution;%在原始雷达数据中为点添加角度信息,用于后续计算点与点之间的距离
     hold on
    elseif i>= 180
     polarscatter((i-180)*resolution,LaserData(i),'.')    
    LaserData(i,2)=(i-180)*resolution;
    hold on
    end
end
%////核心代码,雷达数据聚类////TODO
minNum = 2 ;
r = 0.10 ;
N =1;
flag =0;
tempN=1;
ncluster=[];
for i=1:1:359
    if (LaserData(i+1,1)*cos(LaserData(i+1,2))-LaserData(i,1)*cos(LaserData(i,2)))^2+(LaserData(i+1,1)*sin(LaserData(i+1,2))-LaserData(i,1)*sin(LaserData(i,2)))^2 <= (r)^2

        if flag==0
            N=N+1;
            ncluster(N,tempN)=i;
            flag=1;
        end
        ncluster(N,tempN)=i;
        tempN=tempN+1;
    
    else
        flag=0;
        tempN=1;
    end
end
[m,n]=size(ncluster)
color=['b*','y.','gv','r^','black*','b.','rv','y^',];
figure('Name','粗略聚类效果','NumberTitle','off');
for i=1:1:m
    for j=1:1:n
        if ncluster(i,j)~=0
           polarscatter(LaserData(ncluster(i,j),2),LaserData(ncluster(i,j),1),color(mod(i,8)+1))
           hold on
        end
    end
end

效果:
还行吧,不知道在ROS C++中效果怎么样,后续把检测框部分,滤波部分加上,然后在RVIZ显示,未完待续…