import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
import cv2
import numpy as np
class StereoReceiver(Node):
    def __init__(self):
        super().__init__('stereo_receiver')
        self.subscription = self.create_subscription(
            CompressedImage,
            '/camera/kfc_compressed',
            self.callback,
            1
        )
    def callback(self, msg: CompressedImage):
        # ====== 解码 JPEG ======
        np_arr = np.frombuffer(msg.data, np.uint8)
        combined_img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        if combined_img is None:
            self.get_logger().warn("Decode jpeg failed!")
            return
        # ====== 分割左右图 ======
        height, width_total = combined_img.shape[:2]
        width = width_total // 2
        if width <= 0:
            self.get_logger().warn("Invalid image width!")
            return
        left_img  = combined_img[:, :width]
        right_img = combined_img[:, width:width + width]
        # ====== 调试输出 ======
        self.get_logger().info(f"Received: {width_total}x{height}, left/right={width}x{height}")
        # （可选）显示图像
        # cv2.imshow("left", left_img)
        # cv2.imshow("right", right_img)
        # cv2.waitKey(1)
def main(args=None):
    rclpy.init(args=args)
    node = StereoReceiver()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
if __name__ == '__main__':
    main()