ROSBAG使用(二)：使用python提取bag中的图像和点云

1.1 载入

ROS提供了解析 bag 的python API，载入一个 bag 文件可以：

import rosbag

bag_file = 'test.bag'
bag = rosbag.Bag(bag_file, "r")

1.2 读取信息

info = bag.get_type_and_topic_info()
print(info)

可以得到类似如下的信息：

TypesAndTopicsTuple(
	msg_types={
		'sensor_msgs/PointCloud2': '1158d486dd51d683ce2f1be655c3c181', 
		'sensor_msgs/CompressedImage': '8f7a12909da2c9d3332d540a0977563f'
		}, 
    topics={
        '/pandora/sensor/pandora/hesai40/PointCloud2': 		
        TopicTuple(
            msg_type='sensor_msgs/PointCloud2', 
            message_count=1183, 
            connections=1, 
            frequency=10.071482170278314), 	
        '/pandora/sensor/pandora/camera/front_color/compressed': 
        TopicTuple(
            msg_type='sensor_msgs/CompressedImage', 
            message_count=1183, 
            connections=1, 
            frequency=10.062505847777844)
            }
		)

1.3 读取topic

可以看到，我的 bag 包里面的图片所在的 topic 是 /pandora/sensor/pandora/camera/front_color/compressed，其包含的文件类型是 sensor_msgs/CompressedImage。

可以用 read_messages() 方法来读取 bag 里面的内容，该方法返回一个迭代器，每次迭代时返回三个值： topic，msg，t。msg 是具体的数据，t 是时间戳。

该方法可以筛选出想要的 topic，以我上面的数据为例，如果我要提取图片，则可以：

bag_data = bag.read_messages('/pandora/sensor/pandora/camera/front_color/compressed'):
for topic, msg, t in bag_data:
    pass

如果不给定 topic ，则会遍历所有的 topic。

1.4 转换图像数据

但是这里的图片类型是 sensor_msgs/CompressedImage，并不能直接使用。

因此需要使用到 cv_bridge，顾名思义，这是ROS和opencv之间的一道桥，其中提供的 compressed_imgmsg_to_cv2() 方法可以把 sensor_msgs/CompressedImage 格式转换成 opencv 格式的，具体来说就是：

import cv2
from cv_bridge import CvBridge

bridge = CvBridge()
for topic, msg, t in bag_data:
    cv_image = bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
    cv2.imshow("Image window", cv_image)
    cv2.waitKey(3)

代码中的 bgr8 表示图像编码，你可以通过format属性来查看其编码：

print(msg.format)

所有的编码格式为：

mono8: CV_8UC1, grayscale image

mono16: CV_16UC1, 16-bit grayscale image

bgr8: CV_8UC3, color image with blue-green-red color order

rgb8: CV_8UC3, color image with red-green-blue color order

bgra8: CV_8UC4, BGR color image with an alpha channel

rgba8: CV_8UC4, RGB color image with an alpha channel

1.5 完整代码

以上的代码连起来就是：

import rosbag
import cv2
from cv_bridge import CvBridge

bag_file = 'test.bag'
bag = rosbag.Bag(bag_file, "r")

bridge = CvBridge()
bag_data = bag.read_messages('/pandora/sensor/pandora/camera/front_color/compressed')
for topic, msg, t in bag_data:
    cv_image = bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
    cv2.imshow("Image window", cv_image)
    cv2.waitKey(3)

这样就能显示 bag 里面的图片了，你也可以直接保存或者进行其他的操作。

1.6 图片格式问题

注意，我的 bag 文件里面的图片的格式是 sensor_msgs/CompressedImage，但是大部分的 bag 文件的图片格式可能是 sensor_msgs/Image，这时候只需要把 compressed_imgmsg_to_cv2 换成 imgmsg_to_cv2 就可以了。

我一开始就是没有注意到有两种图片的格式，因此用 imgmsg_to_cv2 怎么试都是报错，浪费了很多时间。

1.7 提取点云

知道了图像的提取方法，提取点云的就简单了，直接上代码：

import rosbag
import numpy as np
import sensor_msgs.point_cloud2 as pc2

bag_file = 'test.bag'
bag = rosbag.Bag(bag_file, "r")
bag_data = bag.read_messages('/pandora/sensor/pandora/hesai40/PointCloud2')

for topic, msg, t in bag_data:
    lidar = pc2.read_points(msg)
    points = np.array(list(lidar))
    # 看你想如何处理这些点云

这里用到了 pc2.read_points() ，其接受一个 sensor_msgs.PointCloud2 对象，然后会返回一个迭代器，每次迭代返回点云中的一个点。