#!/usr/bin/env python3
import rospy
from gazebo_msgs.srv import GetModelState

def get_camera_height_from_gazebo():
    rospy.init_node('camera_height_monitor_gazebo')
    
    # 等待Gazebo的GetModelState服务可用
    rospy.wait_for_service('/gazebo/get_model_state')
    get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
    
    rate = rospy.Rate(10.0)  # 10Hz
    
    while not rospy.is_shutdown():
        try:
            # 获取机器人模型状态
            # 注意：需要替换为您的机器人模型名称，通常是"robot"或类似名称
            model_state = get_model_state("robot", "world")
            
            # 计算相机高度 (机器人z坐标 + 相机相对于机器人的z偏移)
            # 根据您之前查找的数据：机器人高度一半(54.5mm) + 相机高度(125.794mm)
            robot_z = model_state.pose.position.z
            camera_height = robot_z + 0.180294  # 根据之前计算的相机高度
            
            rospy.loginfo("RGB相机当前高度: %.3f 米", camera_height)
            
        except rospy.ServiceException as e:
            rospy.logerr("服务调用失败: %s", e)
            
        rate.sleep()

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