import os
import cv2
import rosbag2_py
from cv_bridge import CvBridge
from rclpy.serialization import deserialize_message
from rosidl_runtime_py.utilities import get_message
def save_image(image_data, image_count, output_dir):
"""将图像数据保存为文件"""
filename = os.path.join(output_dir, f"image_{image_count:05d}.jpg")
cv2.imwrite(filename, image_data)
print(f"保存图片：{filename}")
def parse_ros2_bag(bag_path, topic_name, output_dir):
"""解析ROS 2 bag文件中的图像数据"""
if not os.path.exists(output_dir):
os.makedirs(output_dir)
创建rosbag2读取器
reader = rosbag2_py.SequentialReader()
storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id='sqlite3')
converter_options = rosbag2_py.ConverterOptions('', '')
reader.open(storage_options, converter_options)
获取话题列表
topic_types = reader.get_all_topics_and_types()
type_map = {topic.name: topic.type for topic in topic_types}
print(f"Bag文件中的话题：{list(type_map.keys())}")
检查话题是否存在
if topic_name not in type_map:
print(f"错误：话题 {topic_name} 不存在于 bag 文件中")
return
获取消息类型
msg_type = type_maptopic_name
msg_class = get_message(msg_type)
初始化CvBridge
bridge = CvBridge()
image_count = 0
while reader.has_next():
(topic, data, timestamp) = reader.read_next()
if topic == topic_name:
try:
反序列化字节流为Image消息
image_msg = deserialize_message(data, msg_class)
使用cv_bridge转换消息为OpenCV格式图像
image = bridge.imgmsg_to_cv2(image_msg, "bgr8")
save_image(image, image_count, output_dir)
image_count += 1
except Exception as e:
print(f"跳过无法解析的消息：{e}")
print(f"图片提取完成，共提取 {image_count} 张图片。")
if name == "main":
bag_path = "/home/yao/workspace/data/yao_bag/" # ROS 2 bag 文件路径
topic_name = "/pic_hsp004" # 图像话题名称
output_dir = "output_images" # 保存图片的文件夹
parse_ros2_bag(bag_path, topic_name, output_dir)