import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from sensor_msgs.msg import CompressedImage
import cv2
import numpy as np


class CameraSubscriber(Node):
    def __init__(self):
        super().__init__('camera_subscriber')

        # QoS completo equivalente al publisher
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
            durability=DurabilityPolicy.VOLATILE
        )

        self.subscription = self.create_subscription(
            CompressedImage,
            'camera/image/compressed',
            self.image_callback,
            qos_profile
        )

        self.frame_count = 0
        self.get_logger().info('Subscriber Python iniciado - Esperando imágenes...')

    def image_callback(self, msg):
        self.frame_count += 1

        # Decodificar imagen
        np_arr = np.frombuffer(msg.data, np.uint8)
        frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

        if frame is not None:
            # Mostrar info
            self.get_logger().info(f'Frame {self.frame_count} recibido - Tamaño: {frame.shape}')

            # Mostrar imagen
            cv2.imshow('Imagen Recibida', frame)
            cv2.waitKey(1)


def main(args=None):
    rclpy.init(args=args)
    node = CameraSubscriber()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('Cerrando subscriber...')
    finally:
        cv2.destroyAllWindows()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()