#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
测试发布者：发送图片和触发信号给 VLM 节点
用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/home/sunrise/yiliao_ws/my_model/image.png"
"""

import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32, String
from sensor_msgs.msg import CompressedImage
import cv2
import time


class TestPublisher(Node):
    def __init__(self):
        super().__init__("test_publisher")

        # 声明参数
        self.declare_parameter(
            "image_path", "/home/sunrise/yiliao_ws/my_model/image.png"
        )
        self.declare_parameter("interval", 5.0)  # 每 N 秒触发一次

        image_path = self.get_parameter("image_path").value
        interval = self.get_parameter("interval").value

        # 发布者
        self.image_pub = self.create_publisher(CompressedImage, "/image", 10)
        self.sign_pub = self.create_publisher(Int32, "/sign4return", 10)

        # 订阅结果
        self.result_sub = self.create_subscription(
            String, "/vlm_result", self.result_callback, 10
        )

        # 加载图片
        self.image_data = cv2.imread(image_path)
        if self.image_data is None:
            self.get_logger().error(f"无法读取图片: {image_path}")
            raise FileNotFoundError(f"Image not found: {image_path}")

        encode_params = [cv2.IMWRITE_JPEG_QUALITY, 90]
        _, jpeg_data = cv2.imencode(".jpg", self.image_data, encode_params)
        self.jpeg_bytes = jpeg_data.tobytes()
        self.get_logger().info(f"已加载图片: {image_path}")

        # 定时器
        self.timer = self.create_timer(interval, self.timer_callback)
        self.get_logger().info(f"每 {interval}s 发送一次图片和触发信号")

    def timer_callback(self):
        # 1. 发送压缩图像
        img_msg = CompressedImage()
        img_msg.header.stamp = self.get_clock().now().to_msg()
        img_msg.format = "jpeg"
        img_msg.data = self.jpeg_bytes
        self.image_pub.publish(img_msg)
        self.get_logger().info("已发送图片")

        # 2. 等待一小段时间让订阅者收到图片
        time.sleep(1.0)

        # 3. 发送触发信号 (sign=9)
        sign_msg = Int32()
        sign_msg.data = 9
        self.sign_pub.publish(sign_msg)
        self.get_logger().info("已发送触发信号 (sign=9)，等待 VLM 结果...")

    def result_callback(self, msg):
        self.get_logger().info(f"🔍 VLM 识别结果: {msg.data}")


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


if __name__ == "__main__":
    main()
