#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import time
from datetime import datetime

class CameraSnapshot(Node):
    def __init__(self):
        super().__init__('camera_snapshot')
        self.bridge = CvBridge()
        self.saved = False
        self.start_time = time.time()
        
        # 订阅RGB图像话题
        self.subscription = self.create_subscription(
            Image,
            '/image_right',  # 根据实际命名空间调整，如'/stereo_camera/image_rgb'
            self.image_callback,
            10)
        self.get_logger().info('等待相机图像...')

    def image_callback(self, msg):
        current_time = time.time()
        elapsed = current_time - self.start_time
        
        # 第5秒时保存图像
        if elapsed >= 5.0 and not self.saved:
            try:
                cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
                timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
                filename = f'rgb_snapshot_{timestamp}.jpg'
                cv2.imwrite(filename, cv_image)
                self.get_logger().info(f'已保存截图: {filename}')
                self.saved = True
                
                # 显示保存的图像（可选）
                cv2.imshow('Saved Image', cv_image)
                cv2.waitKey(3000)  # 显示3秒
                cv2.destroyAllWindows()
                
            except Exception as e:
                self.get_logger().error(f'图像保存失败: {str(e)}')

def main(args=None):
    rclpy.init(args=args)
    node = CameraSnapshot()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()