#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from std_msgs.msg import Float64
import math

class CameraHeightMonitor(Node):
    """节点用于监控RGB相机相对于世界坐标系的高度"""

    def __init__(self):
        super().__init__('camera_height_monitor')
        
        # 创建TF2缓冲区和监听器
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 创建发布器，发布高度信息到专门的话题
        self.height_publisher = self.create_publisher(
            Float64, 'rgb_camera_height', 10)
            
        # 创建定时器，定期查询转换
        timer_period = 0.1  # 10 Hz
        self.timer = self.create_timer(timer_period, self.timer_callback)
        
        self.get_logger().info('RGB相机高度监测节点已启动')

    def timer_callback(self):
        """定时器回调函数，查询并发布相机高度"""
        try:
            # 查询从world坐标系到RGB_camera_link的变换
            # 根据您的系统，可能需要修改坐标系名称
            transform = self.tf_buffer.lookup_transform(
                'vodom',                    # 目标坐标系（通常是世界坐标系）
                'RGB_camera_link',          # 源坐标系（相机坐标系）
                rclpy.time.Time())          # 获取最新的变换
            
            # 提取z轴值，即高度
            camera_height = transform.transform.translation.z
            
            # 创建Float64消息并发布
            height_msg = Float64()
            height_msg.data = camera_height
            self.height_publisher.publish(height_msg)
            
            # 打印高度信息
            self.get_logger().info(f'RGB相机当前高度: {camera_height:.3f} 米')
            
        except Exception as e:
            # 如果找不到变换，记录错误但不中断执行
            self.get_logger().warning(f'无法获取RGB相机的高度: {str(e)}')

def main(args=None):
    rclpy.init(args=args)
    
    camera_height_monitor = CameraHeightMonitor()
    
    try:
        rclpy.spin(camera_height_monitor)
    except KeyboardInterrupt:
        pass
    finally:
        # 清理并关闭节点
        camera_height_monitor.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()