36 lines
1.3 KiB
Python
36 lines
1.3 KiB
Python
|
#!/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
|