机器人操作系统ROS（五）：rosbag与读取图片与点云数据_r如何读取rosbag

rosbag基本应用

rosbag录制数据

录制所有话题：

rosbag record -a
1

录制指定话题，设置 bag 包名：

rosbag record -O xxx.bag /topic
1

rosbag回放数据

以暂停的方式启动，防止跑掉数据

rosbag play --pause xxx.bag
1

设置以 0.5 倍速回放，也就是以录制频率的一半回放

rosbag play -r 0.5 xxx.bag
1

rosbag获取bag文件的信息

rosbag info xxx.bag
1

rosbag压缩

如果录制的 bag 很大，我们可以压缩它，默认的压缩格式是 bz2：

rosbag compress xxx.bag
1

你也可以添加 -j 手动指定压缩格式为 bz2：

rosbag compress -j xxx.bag
1

也可以使用 LZ4 来压缩数据：

rosbag compress --lz4 xxx.bag
1

rosbag解压

rosbag decompress xxx.bag解压
1

rosbag截取时间段

rosbag filter input.bag output.bag "t.to_sec() <= 1284703001.00 and t.to_sec() >= 1284703000.00"
1

rqt_bag显示与绘制

rqt_bag
1

rqt_bag是一个用于记录和管理包文件的应用程序。主要特点：

• 显示bag信息内容
• 显示图像消息（可选为时间线上的缩略图）
• 绘制消息值的可配置时间序列
• 向ROS发布/记录所选topic的消息
• 将时间范围内的消息导出到新bag

rosbag中读取图像数据

运行以下命令从input.bag的/img/cam的话题中读取图像数据

python2 export.py
1

export.py文件如下所示

# coding:utf-8
#!/usr/bin/python
#Exrtact image from a bag file.
import roslib
import rosbag
import rospy
import cv2
import os
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
image_path = './image/'
class ImageCreator():
	def __init__(self):
		self.bridge = CvBridge()
		with rosbag.Bag('./input.bag', 'r') as bag:
			for topic,msg,t in bag.read_messages():
				if topic == "/img/cam": #topic for images；
						try:
							cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
						except CvBridgeError as e:
							print e
						timestr = "%.6f" %  msg.header.stamp.to_sec() #%.6fis the precision of the timestamps you prefer；
						image_name = timestr+ ".png" #name: timestamps.png
						cv2.imwrite(image_path + image_name, cv_image)  #save；
if __name__ == '__main__':
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

rosbag中读取点云数据

确保linux系统中安装了pcl_ros包：

rospack find pcl_ros
1

若没有安装则需要运行以下命令：

sudo apt-get install ros-melodic-pcl-ros
1

启动ros的master节点

roscore
1

运行以下命令，读取点云数据

rosrun pcl_ros bag_to_pcd bag包 点云话题 保存pcd的路径
# eg: rosrun pcl_ros bag_to_pcd data.bag /velodyne_points pcd
1
2

点云可视化

pcl_viewer  xxx.pcd
1

结果如下图所示

使用Matlab读取bag信息

以下是一个matlab读取bag信息并绘图的示例（具体使用需要根据自身bag的Topic相关信息进行修改），其中
/mavros/local_position/pose 为无人机状态，包含了无人机位置、速度、姿态等信息
/prometheus/control/ref_pose_rviz为无人机期望轨迹，包含了期望位置和期望姿态（期望姿态由控制器计算得到）

filepath=fullfile('d:','Matlab/bag','subset2.bag');
bag=rosbag(filepath);

prometheus_msgs_ref_pose = select(bag,'Topic','/prometheus/control/ref_pose_rviz');
prometheus_msgs_pose = select(bag,'Topic','/mavros/local_position/pose');
drone_ref_pose = readMessages(prometheus_msgs_ref_pose,'DataFormat','struct');
drone_pose = readMessages(prometheus_msgs_pose,'DataFormat','struct');

%% 位置
drone_ref_position=zeros(length(drone_ref_pose),3);
t_ref_position = zeros(length(drone_ref_pose),1);
for i = 1:length(drone_ref_pose)
drone_ref_position(i,1) = drone_ref_pose{i,1}.Pose.Position.X;
drone_ref_position(i,2) = drone_ref_pose{i,1}.Pose.Position.Y;
drone_ref_position(i,3) = drone_ref_pose{i,1}.Pose.Position.Z;
t_ref_position(i) = drone_ref_pose{i,1}.Header.Stamp.Sec;
end

drone_position=zeros(length(drone_pose),3);
t_position = zeros(length(drone_pose),1);
for i = 1:length(drone_pose)
drone_position(i,1) = drone_pose{i,1}.Pose.Position.X;
drone_position(i,2) = drone_pose{i,1}.Pose.Position.Y;
drone_position(i,3) = drone_pose{i,1}.Pose.Position.Z;
t_position(i) = drone_pose{i,1}.Header.Stamp.Sec;
end
% x轴位置曲线
figure
plot(t_ref_position(:),drone_ref_position(:,1))
hold on
plot(t_position(:),drone_position(:,1));
hold off
xlabel('时间戳/s');
ylabel('距离/m');
% y轴位置曲线
figure
plot(t_ref_position(:),drone_ref_position(:,2))
hold on
plot(t_position(:),drone_position(:,2));
hold off
xlabel('时间戳/s');
ylabel('距离/m');
% z轴位置曲线
figure
plot(t_ref_position(:),drone_ref_position(:,3))
hold on
plot(t_position(:),drone_position(:,3));
hold off
xlabel('时间戳/s');
ylabel('距离/m');

%% 姿态角
drone_ref_orientation=zeros(length(drone_ref_pose),4);
drone_ref_angle=zeros(length(drone_ref_pose),3);
t_ref_position = zeros(length(drone_ref_pose),1);
for i = 1:length(drone_ref_pose)
drone_ref_orientation(i,1) = drone_ref_pose{i,1}.Pose.Orientation.X;
drone_ref_orientation(i,2) = drone_ref_pose{i,1}.Pose.Orientation.Y;
drone_ref_orientation(i,3) = drone_ref_pose{i,1}.Pose.Orientation.Z;
drone_ref_orientation(i,4) = drone_ref_pose{i,1}.Pose.Orientation.W;
[drone_ref_angle(i,1),drone_ref_angle(i,2),drone_ref_angle(i,3)]=quat2angle(drone_ref_orientation(i,:),'XYZ');
t_ref_position(i) = drone_ref_pose{i,1}.Header.Stamp.Sec;
end

drone_orientation=zeros(length(drone_pose),4);
drone_angle=zeros(length(drone_pose),3);
t_position = zeros(length(drone_pose),1);
for i = 1:length(drone_pose)
drone_orientation(i,1) = drone_pose{i,1}.Pose.Orientation.X;
drone_orientation(i,2) = drone_pose{i,1}.Pose.Orientation.Y;
drone_orientation(i,3) = drone_pose{i,1}.Pose.Orientation.Z;
drone_orientation(i,4) = drone_pose{i,1}.Pose.Orientation.W;
[drone_angle(i,1),drone_angle(i,2),drone_angle(i,3)]=quat2angle(drone_orientation(i,:),'XYZ');
t_position(i) = drone_pose{i,1}.Header.Stamp.Sec;
end

drone_ref_angle(:,3)=-abs(drone_ref_angle(:,3));
drone_angle(:,3)=-abs(drone_angle(:,3));

subplot(3, 1, 1);
plot(t_ref_position(:),drone_ref_angle(:,1))
hold on
plot(t_position(:),drone_angle(:,1));
hold off
xlabel('时间戳/s');
ylabel('弧度/rad');
subplot(3, 1, 2);
plot(t_ref_position(:),drone_ref_angle(:,2))
hold on
plot(t_position(:),drone_angle(:,2));
hold off
xlabel('时间戳/s');
ylabel('弧度/rad');
subplot(3, 1, 3);
plot(t_ref_position(:),drone_ref_angle(:,3))
hold on
plot(t_position(:),drone_angle(:,3));
hold off
xlabel('时间戳/s');
ylabel('弧度/rad');
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100