1、ROS下数据存储
可使用ros_bag
功能包进行数据记录,通过如下指令存储轨迹追踪相关话题
rosbag record -O subset /mavros/local_position/pose /prometheus/control/ref_pose_rviz
/mavros/local_position/pose
为无人机状态,包含了无人机位置、速度、姿态等信息
/prometheus/control/ref_pose_rviz
为无人机期望轨迹,包含了期望位置和期望姿态(期望姿态由控制器计算得到)
2、MATLAB绘图处理
若想在Matlab中对得到的bag数据进行处理,有两种方式:(比较推荐第一种)
方法一:
直接使用Matalab打开rosbag文件, 例子:
filepath=fullfile('c:','bag','subset_2021-06-06-18-12-19.bag');
bag=rosbag(filepath);
prometheus_msgs = select(bag,'MessageType','geometry_msgs/PoseStamped');
drone_pose = readMessages(prometheus_msgs,'DataFormat','struct');
drone_position=zeros(2180,2);
for i=1:2180
drone_position(i,1) = drone_pose{i,1}.Pose.Position.X;
drone_position(i,2) = drone_pose{i,1}.Pose.Position.Y;
end
real_x = drone_position(:,1);
real_y = drone_position(:,2);
plot(real_x,real_y);
方法二:
将bag文件存储为txt格式:
rostopic echo /mavros/local_position/pose
rostopic echo /prometheus/control/ref_pose_rviz
rostopic echo -b subset_2021-06-06-17-01-04.bag -p /prometheus/control/ref_pose_rviz > drone_ref_pose.txt
rostopic echo -b subset_2021-06-06-17-01-04.bag -p /mavros/local_position/pose > drone_pose.txt
rostopic echo -b file_name.bag -p /topic_name > Txt_name.txt
再通过Matlab读入数据,新建.m脚本文件,程序如下:
fidin=fopen('Txt_name.txt'); % 打开test2.txt文件
fidout=fopen('mkmatlab.txt','w'); % 创建MKMATLAB.txt文件
while ~feof(fidin) % 判断是否为文件末尾
tline=fgetl(fidin); % 从文件读行
if double(tline(1))>=48&&double(tline(1))<=57 % 判断首字符是否是数值
fprintf(fidout,'%s\n\n',tline); % 如果是数字行,把此行数据写入文件MKMATLAB.txt
continue % 如果是非数字继续下一次循环
end
end
fclose(fidout);
MK=importdata('MKMATLAB.txt'); % 将生成的MKMATLAB.txt文件导入工作空间,变量名为MK,实际上它不显示出来
date = MK(:,:); %将MK数据存储到date中
position=zeros(1311,2); %这边的1311是根据数据的行数确定的,如果随意写会报错
for i=1:1311
position(i,1)=date .data(i,2);
position(i,2)=date .data(i,3);
end
x = position(:,1);
y = position(:,2);
plot(x,y);
然后直接运行脚本,就可以绘制出在ros中相对应的数据
注:参考Prometheus位置控制器的开发