ROS学习笔记（五）：rosbag与读取图片与点云数据-Toy模板网

这篇具有很好参考价值的文章主要介绍了ROS学习笔记（五）：rosbag与读取图片与点云数据。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

rosbag基本应用

rosbag录制数据

录制所有话题：

rosbag record -a

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

rosbag record -O xxx.bag /topic

rosbag回放数据

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

rosbag play --pause xxx.bag

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

rosbag play -r 0.5 xxx.bag

rosbag获取bag文件的信息

rosbag info xxx.bag

rosbag压缩

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

rosbag compress xxx.bag

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

rosbag compress -j xxx.bag

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

rosbag compress --lz4 xxx.bag

rosbag解压

rosbag decompress xxx.bag解压

rosbag截取时间段

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

rqt_bag显示与绘制

rqt_bag

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

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

rosbag中读取图像数据

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

python2 export.py

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

rosbag中读取点云数据

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

rospack find pcl_ros

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

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

启动ros的master节点

roscore

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

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

点云可视化

pcl_viewer  xxx.pcd

结果如下图所示
ROS学习笔记（五）：rosbag与读取图片与点云数据,ROS,自动驾驶,计算机视觉,ROS,机器人

使用Matlab读取bag信息

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

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');