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
|