#!/usr/bin/env python3
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped

def get_camera_height():
    rospy.init_node('camera_height_monitor')
    
    # 创建TF2监听器
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buffer)
    
    rate = rospy.Rate(10.0)  # 10Hz
    
    while not rospy.is_shutdown():
        try:
            # 查询从world到RGB_camera_link的变换
            # 如果您的Gazebo使用其他世界坐标系名称，请相应调整
            trans = tf_buffer.lookup_transform('world', 'RGB_camera_link', rospy.Time())
            
            # 提取z轴值（高度）
            camera_height = trans.transform.translation.z
            rospy.loginfo("RGB相机当前高度: %.3f 米", camera_height)
            
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            rospy.logwarn("无法获取变换: %s", e)
            
        rate.sleep()

if __name__ == '__main__':
    try:
        get_camera_height()
    except rospy.ROSInterruptException:
        pass