#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')
        # 定义 QoS 配置（匹配发布者的可靠性策略）
        qos_profile = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,  # 或 BEST_EFFORT，取决于发布者
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10
        )

        self.subscription = self.create_subscription(
            Image,
            '/rgb_camera/image_raw',
            self.image_callback,
            qos_profile=qos_profile
        )
        self.subscription  # 防止未使用变量警告
        self.bridge = CvBridge()

    def image_callback(self, msg):
        try:
            # 将ROS图像消息转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
            
            # 在这里进行你的图像分析
            # 例如显示图像
            cv2.imshow("Camera Feed", cv_image)
            cv2.waitKey(1)
            
            # 可以在这里添加你的图像处理代码
            # analyze_image(cv_image)
            
        except Exception as e:
            self.get_logger().error('Failed to convert image: %s' % str(e))

def main(args=None):
    rclpy.init(args=args)
    image_subscriber = ImageSubscriber()
    
    try:
        rclpy.spin(image_subscriber)
    except KeyboardInterrupt:
        pass
    
    # 清理
    image_subscriber.destroy_node()
    rclpy.shutdown()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()