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

class OpenCVCam(Node):
    def __init__(self):
        super().__init__('opencv_cam_node')
        # 发布话题，名字和 params.yaml 里的一致
        self.publisher_ = self.create_publisher(Image, '/camera/image_raw', 10)
        self.timer = self.create_timer(0.033, self.timer_callback) # 30 FPS
        self.bridge = CvBridge()
        
        # 打开 0 号摄像头
        self.cap = cv2.VideoCapture(6)
        
        # ====================================================
        # 核心：强制使用 MJPEG，解决卡顿的关键！
        # ====================================================
        self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        self.cap.set(cv2.CAP_PROP_FPS, 60)

        if not self.cap.isOpened():
            print("❌ 无法打开摄像头！")
            exit()

        print(f"✅ 摄像头已打开 (MJPEG模式): 640x480 @ 60FPS")

    def timer_callback(self):
        ret, frame = self.cap.read()
        if ret:
            # 转成 ROS 消息并发布
            msg = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
            self.publisher_.publish(msg)
            # print("已发布一帧...") # 可以在这里看有没有在跑

def main(args=None):
    rclpy.init(args=args)
    node = OpenCVCam()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()