import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

from camera_info_manager import CameraInfoManager
from sensor_msgs.msg import CameraInfo

CAMERA_WIDTH = 1280
CAMERA_HEIGHT = 480

FRAME_WIDTH = int(CAMERA_WIDTH / 2)
FRAME_HEIGHT = int(CAMERA_HEIGHT / 2)

print(FRAME_WIDTH, FRAME_HEIGHT)
 
class CameraNode(Node):
    def __init__(self):
        super().__init__('camera_node')
        left_camera_info_url = 'file:///home/shen/openarm_test/calibrationdata/left.yaml'
        right_camera_info_url = 'file:///home/shen/openarm_test/calibrationdata/right.yaml'
        self.left_camera_info_manager = CameraInfoManager(self, cname='left', url=left_camera_info_url, namespace='/stereo_camera/left')
        self.right_camera_info_manager = CameraInfoManager(self, cname='right', url=right_camera_info_url, namespace='/stereo_camera/right')

        # 设置相机信息文件的路径（不带.yaml后缀）
        # 假设你的yaml文件放在 /home/username/camera_info/ 目录下，名为 left.yaml 和 right.yaml


        # self.left_camera_info_manager.setCameraInfoURL(left_camera_info_url)
        # self.right_camera_info_manager.setCameraInfoURL(right_camera_info_url)
        # self.left_camera_info_manager.setURL(left_camera_info_url)
        # self.right_camera_info_manager.setURL(right_camera_info_url)

        print("2ds1as23d123sa1d32as", self.left_camera_info_manager.getURL())
        self.left_camera_info_manager.loadCameraInfo()
        self.right_camera_info_manager.loadCameraInfo()
        # 然后，你可以通过CameraInfoManager获取CameraInfo消息
        self.left_camera_info = self.left_camera_info_manager.getCameraInfo()
        self.right_camera_info = self.right_camera_info_manager.getCameraInfo()

        # 注意：你需要确保yaml文件已经正确放置在指定路径，并且格式正确。
        self.left_camera_info_pub = self.create_publisher(CameraInfo, '/stereo_camera/left/camera_info', 10)
        self.right_camera_info_pub = self.create_publisher(CameraInfo, '/stereo_camera/right/camera_info', 10)

        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_1', 10)
        self.get_logger().info('正在初始化摄像头...')
    
        
        # 尝试打开物理摄像头
        self.cap = None
        for i in range(2):  # 尝试设备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, CAMERA_WIDTH)
                self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT)
                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.001, 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(0)
            time = self.get_clock().now().to_msg()
            msg_left= self.bridge.cv2_to_imgmsg(frame[:, :FRAME_WIDTH], encoding='bgr8')
            msg_right= self.bridge.cv2_to_imgmsg(frame[:, FRAME_WIDTH:], encoding='bgr8')
            msg_raw= self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
            msg_left.header.stamp = time
            msg_right.header.stamp = time
            msg_raw.header.stamp = time
            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)
            # 发布相机信息
            self.left_camera_info_pub.publish(self.left_camera_info)
            self.right_camera_info_pub.publish(self.right_camera_info)
        


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