#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class CameraNode(Node):
    def __init__(self):
        super().__init__('camera_node')
        self.declare_parameter('camera_device', '/dev/video0')
        self.declare_parameter('frame_width', 320)
        self.declare_parameter('frame_height', 240)
        self.declare_parameter('fps', 15)

        self.camera_device = self.get_parameter('camera_device').value
        self.frame_width = self.get_parameter('frame_width').value
        self.frame_height = self.get_parameter('frame_height').value
        self.fps = self.get_parameter('fps').value

        self.bridge = CvBridge()
        self.publisher = self.create_publisher(Image, '/phone/camera/image_raw', 10)
        self.timer_period = 1.0 / self.fps
        self.timer = self.create_timer(self.timer_period, self.timer_callback)
        self.cap = None
        self.open_camera()

    def open_camera(self):
        try:
            self.cap = cv2.VideoCapture(self.camera_device)
            if self.cap.isOpened():
                self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.frame_width)
                self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frame_height)
                self.cap.set(cv2.CAP_PROP_FPS, self.fps)
                self.get_logger().info(f"Camera opened: {self.camera_device}")
        except Exception as e:
            self.get_logger().error(f"Error opening camera: {e}")

    def timer_callback(self):
        if self.cap is None or not self.cap.isOpened():
            return
        try:
            ret, frame = self.cap.read()
            if ret and frame is not None:
                msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
                msg.header.stamp = self.get_clock().now().to_msg()
                msg.header.frame_id = 'phone_camera'
                self.publisher.publish(msg)
        except Exception as e:
            self.get_logger().error(f"Error: {e}")

    def destroy_node(self):
        if self.cap is not None:
            self.cap.release()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = CameraNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
