import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
 
class CameraNode(Node):
    def __init__(self):
        super().__init__('camera_node')
        
        self.bridge = CvBridge()
        self.publisher_left = self.create_publisher(Image, '/camera/left/image_raw', 10)
        self.publisher_right = self.create_publisher(Image, '/camera/right/image_raw', 10)
        self.publisher_raw = self.create_publisher(Image, '/image', 10)
        self.get_logger().info('正在初始化摄像头...')
    
        
        # 尝试打开物理摄像头
        self.cap = None
        for i in range(1):  # 尝试设备0,1,2
            self.get_logger().info(f'尝试打开摄像头设备 /dev/video{i}...')
            self.cap = cv2.VideoCapture(i)
            if self.cap.isOpened():
                # 设置摄像头参数
                self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1600)
                self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 600)
                self.cap.set(cv2.CAP_PROP_FPS, 30)
                
                # 让摄像头预热
                for _ in range(5):
                    self.cap.read()
                
                # 验证摄像头能正常读取
                ret, frame = self.cap.read()
                if ret and frame is not None:
                    self.get_logger().info(f'成功打开物理摄像头 /dev/video{i}')
                    self.get_logger().info(f'图像尺寸: {frame.shape}')
                    break

        
        self.timer = self.create_timer(0.033, self.timer_callback)  # 30Hz
        self.get_logger().info('摄像头节点已启动')

    def timer_callback(self):
        ret, frame = self.cap.read()
        if ret and frame is not None:
            # frameDist = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE)
            # cv2.imshow("camera_left", frame[:, :800])
            # cv2.imshow("camera_right", frame[:, 800:])
            cv2.waitKey(10)
            msg_left= self.bridge.cv2_to_imgmsg(frame[:, :800], 'bgr8')
            msg_right= self.bridge.cv2_to_imgmsg(frame[:, 800:], 'bgr8')
            msg_raw= self.bridge.cv2_to_imgmsg(frame, 'bgr8')
            msg_left.header.stamp = self.get_clock().now().to_msg()
            msg_right.header.stamp = self.get_clock().now().to_msg()
            msg_raw.header.stamp = self.get_clock().now().to_msg()
            msg_left.header.frame_id = 'camera_frame_left'
            msg_right.header.frame_id = 'camera_frame_right'
            msg_raw.header.frame_id = 'camera_frame_raw'
            self.publisher_left.publish(msg_left)
            self.publisher_right.publish(msg_right)
            self.publisher_raw.publish(msg_raw)
        


def main():
    rclpy.init()
    node = CameraNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
 
if __name__ == '__main__':
    main()
