70 lines
2.4 KiB
Python
70 lines
2.4 KiB
Python
#!/usr/bin/env python3
|
||
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from tf2_ros.buffer import Buffer
|
||
from tf2_ros.transform_listener import TransformListener
|
||
from std_msgs.msg import Float64
|
||
import math
|
||
|
||
class CameraHeightMonitor(Node):
|
||
"""节点用于监控RGB相机相对于世界坐标系的高度"""
|
||
|
||
def __init__(self):
|
||
super().__init__('camera_height_monitor')
|
||
|
||
# 创建TF2缓冲区和监听器
|
||
self.tf_buffer = Buffer()
|
||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||
|
||
# 创建发布器，发布高度信息到专门的话题
|
||
self.height_publisher = self.create_publisher(
|
||
Float64, 'rgb_camera_height', 10)
|
||
|
||
# 创建定时器，定期查询转换
|
||
timer_period = 0.1 # 10 Hz
|
||
self.timer = self.create_timer(timer_period, self.timer_callback)
|
||
|
||
self.get_logger().info('RGB相机高度监测节点已启动')
|
||
|
||
def timer_callback(self):
|
||
"""定时器回调函数，查询并发布相机高度"""
|
||
try:
|
||
# 查询从world坐标系到RGB_camera_link的变换
|
||
# 根据您的系统，可能需要修改坐标系名称
|
||
transform = self.tf_buffer.lookup_transform(
|
||
'vodom', # 目标坐标系（通常是世界坐标系）
|
||
'RGB_camera_link', # 源坐标系（相机坐标系）
|
||
rclpy.time.Time()) # 获取最新的变换
|
||
|
||
# 提取z轴值，即高度
|
||
camera_height = transform.transform.translation.z
|
||
|
||
# 创建Float64消息并发布
|
||
height_msg = Float64()
|
||
height_msg.data = camera_height
|
||
self.height_publisher.publish(height_msg)
|
||
|
||
# 打印高度信息
|
||
self.get_logger().info(f'RGB相机当前高度: {camera_height:.3f} 米')
|
||
|
||
except Exception as e:
|
||
# 如果找不到变换，记录错误但不中断执行
|
||
self.get_logger().warning(f'无法获取RGB相机的高度: {str(e)}')
|
||
|
||
def main(args=None):
|
||
rclpy.init(args=args)
|
||
|
||
camera_height_monitor = CameraHeightMonitor()
|
||
|
||
try:
|
||
rclpy.spin(camera_height_monitor)
|
||
except KeyboardInterrupt:
|
||
pass
|
||
finally:
|
||
# 清理并关闭节点
|
||
camera_height_monitor.destroy_node()
|
||
rclpy.shutdown()
|
||
|
||
if __name__ == '__main__':
|
||
main() |