读.bag中指定的topic / 将.bag中的点云保存为.pcd

close all
clc
clear all

fid = fopen('Training.txt','w');

bag=rosbag('Training.bag');

BagSel1 = select(bag,'Topic','/NewTopic');
BagSel2 = select(bag,'Topic','/NewTopicCloud');
MsgStructs1 = readMessages(BagSel1,'DataFormat','struct');
MsgStructs2 = readMessages(BagSel2,'DataFormat','struct');


for i=1:39855
%     SingleBoxType{i} = MsgStructs1{i,1}.SingleBoxType;
    
    if(~isempty(MsgStructs1{i,1}.SingleBoxType) && i~=2514)
        fprintf(fid,'Number:%i\r\n',i);
        fprintf(fid,'Center:%f,%f,%f\r\n',MsgStructs1{i,1}.SingleBoxCenter(1),MsgStructs1{i,1}.SingleBoxCenter(2),MsgStructs1{i,1}.SingleBoxCenter(3));
        fprintf(fid,'Size:%f,%f,%f\r\n',MsgStructs1{i,1}.SingleBoxSize(1),MsgStructs1{i,1}.SingleBoxSize(2),MsgStructs1{i,1}.SingleBoxSize(3));
        fprintf(fid,'Velocity:%f\r\n',MsgStructs1{i,1}.SingleBoxVelocity);
        fprintf(fid,'Type:');
        fprintf(fid,'%c',MsgStructs1{i,1}.SingleBoxType);
        fprintf(fid,'\r\n');
        fprintf(fid,'\r\n');
        
       % 将该帧点云保存为pcd 
        str = strcat(int2str(i),'.pcd');
        xyz = rosReadXYZ(MsgStructs2{i,1});
        ptCloud = pointCloud(xyz);
        pcwrite(ptCloud,str);

    end
    

end


fclose(fid);