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 對象，然後會返回一個迭代器，每次迭代返回點雲中的一個點。