录bag包和播放bag包，将bag中的图片提出出来

前景：
录制bag包数据（这个bag包含彩色图片，点云数据等等），将录制中的彩色图片数据用训练，那么就需要将bag中的图片提出出来。

一、录包

ros2 bag record -o "包名" --topics 话题名称

示例：

ros2 bag record -o "01_rosbag" \
  --topics \
   /tf \
  /tf_static \
  /pose_with_cov \
  /camera/color/camera_info \
  /camera/color/image_raw \
 /camera/depth/camera_info \
  /camera/depth/image_raw \
  /camera/depth/points \
  /camera_dabai1/color/camera_info \
  /camera_dabai1/color/image_raw \
 /camera_dabai1/depth/camera_info \
  /camera_dabai1/depth/image_raw \
  /camera_dabai1/depth/points 

话题根据自己所需填写
二、播放bag

ros2 bag play bag/20260204_zhongwu/  # 启动数据包  bag/20260204_zhongwu/为你的录制bag的文件路径

三、运行python代码抓取
播放bag，提取彩色图片

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
# 导入QoS相关模块
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
import cv2
from cv_bridge import CvBridge, CvBridgeError
import os
from datetime import datetime

class ImageSaver(Node):
    def __init__(self):
        super().__init__('gemini335_image_saver')
        
        # 配置参数
        self.save_dir = "/workspace/images/gemini/"  # 你的保存目录
        self.frame_interval = 3  # 每15帧保存一次
        self.frame_count = 0  # 帧计数器
        
        # 创建CV桥接器
        self.bridge = CvBridge()
        
        # ========== 关键修正：QoS改为BEST_EFFORT（和相机发布端一致） ==========
        qos_profile = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,  # 匹配相机的BEST_EFFORT
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10  # 队列深度
        )
        
        # 正确的话题名称
        self.subscription = self.create_subscription(
            Image,
            '/camera_head/color/image_raw',
            self.image_callback,
            qos_profile
        )
        
        # 创建保存目录
        os.makedirs(self.save_dir, exist_ok=True)
        self.get_logger().info(f"图像保存器已启动，保存目录：{self.save_dir}")
        self.get_logger().info(f"每 {self.frame_interval} 帧保存一次图像")
        self.get_logger().info("QoS策略已设置为BEST_EFFORT，匹配相机发布端")

    def image_callback(self, msg):
        """图像话题回调函数"""
        try:
            self.frame_count += 1
            
            if self.frame_count % self.frame_interval == 0:
                # 转换ROS图像到OpenCV格式（BGR8）
                cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                
                # 生成带时间戳的文件名
                timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3]
                filename = f"gemini_color_{timestamp}.jpg"
                save_path = os.path.join(self.save_dir, filename)
                
                # 保存图像
                cv2.imwrite(save_path, cv_image)
                self.get_logger().info(f"已保存第 {self.frame_count} 帧图像：{filename}")
                
        except CvBridgeError as e:
            self.get_logger().error(f"图像转换失败：{str(e)}")
        except Exception as e:
            self.get_logger().error(f"保存图像失败：{str(e)}")

def main(args=None):
    rclpy.init(args=args)
    image_saver = ImageSaver()
    
    try:
        rclpy.spin(image_saver)
    except KeyboardInterrupt:
        image_saver.get_logger().info("用户中断，程序退出")
    finally:
        image_saver.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()