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


1.1 载入

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

1
2
3
4
import rosbag

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

1.2 读取信息

1
2
info = bag.get_type_and_topic_info()
print(info)

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
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，以我上面的数据为例，如果我要提取图片，则可以：

1
2
3
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 格式的，具体来说就是：

1
2
3
4
5
6
7
8
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属性来查看其编码：

1
print(msg.format)

所有的编码格式为：

1
2
3
4
5
6
7
8
9
10
11
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 完整代码

以上的代码连起来就是：

1
2
3
4
5
6
7
8
9
10
11
12
13
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 提取点云

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

1
2
3
4
5
6
7
8
9
10
11
12
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 对象，然后会返回一个迭代器，每次迭代返回点云中的一个点。