34 lines
1.1 KiB
Python
34 lines
1.1 KiB
Python
#!/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 |