静态TF广播
我们说TF的主要作用是对坐标系进行管理,那就管理一个试试呗?
坐标变换中最为简单的应该是相对位置不发生变化的情况,比如你家的房子在哪个位置,只要房子不拆,这个坐标应该就不会变化。
在机器人系统中也很常见,比如激光雷达和机器人底盘之间的位置关系,安装好之后基本不会变化。
在TF中,这种情况也称之为静态TF变换,我们来看看在程序中该如何实现?
运行效果
启动终端,运行如下命令:
$ ros2 run learning_tf static_tf_broadcaster$ ros2 run tf2_tools view_frames
可以看到当前系统中存在两个坐标系,一个是world,一个是house,两者之间的相对位置不会发生改变,通过一个静态的TF对象进行维护。
代码解析
来看下在代码中是如何创建坐标系并且发布静态变换的。
learning_tf/static_tf_broadcaster.py
#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@说明: ROS2 TF示例-广播静态的坐标变换"""import rclpy # ROS2 Python接口库from rclpy.node import Node # ROS2 节点类from geometry_msgs.msg import TransformStamped # 坐标变换消息import tf_transformations # TF坐标变换库from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster # TF静态坐标系广播器类class StaticTFBroadcaster(Node): def __init__(self, name): super().__init__(name) # ROS2节点父类初始化 self.tf_broadcaster = StaticTransformBroadcaster(self) # 创建一个TF广播器对象 static_transformStamped = TransformStamped() # 创建一个坐标变换的消息对象 static_transformStamped.header.stamp = self.get_clock().now().to_msg() # 设置坐标变换消息的时间戳 static_transformStamped.header.frame_id = 'world' # 设置一个坐标变换的源坐标系 static_transformStamped.child_frame_id = 'house' # 设置一个坐标变换的目标坐标系 static_transformStamped.transform.translation.x = 10.0 # 设置坐标变换中的X、Y、Z向的平移 static_transformStamped.transform.translation.y = 5.0 static_transformStamped.transform.translation.z = 0.0 quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0) # 将欧拉角转换为四元数(roll, pitch, yaw) static_transformStamped.transform.rotation.x = quat[0] # 设置坐标变换中的X、Y、Z向的旋转(四元数) static_transformStamped.transform.rotation.y = quat[1] static_transformStamped.transform.rotation.z = quat[2] static_transformStamped.transform.rotation.w = quat[3] self.tf_broadcaster.sendTransform(static_transformStamped) # 广播静态坐标变换,广播后两个坐标系的位置关系保持不变def main(args=None): rclpy.init(args=args) # ROS2 Python接口初始化 node = StaticTFBroadcaster("static_tf_broadcaster") # 创建ROS2节点对象并进行初始化 rclpy.spin(node) # 循环等待ROS2退出 node.destroy_node() # 销毁节点对象 rclpy.shutdown()
完成代码的编写后需要设置功能包的编译选项,让系统知道Python程序的入口,打开功能包的setup.py文件,加入如下入口点的配置:
entry_points={ 'console_scripts': [ 'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main', ], },
经过这段代码,两个坐标系的变化是描述清楚了,到了使用的时候,我们又该如何查询呢?
TF监听
我们再来学习下如何查询两个坐标系之间的位置关系。
运行效果
启动一个终端,运行如下节点,就可以在终端中看到周期显示的坐标关系了。
$ ros2 run learning_tf tf_listener
代码解析
这个节点中是如何查询坐标关系的,我们来看下代码
learning_tf/tf_listener.py
#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@说明: ROS2 TF示例-监听某两个坐标系之间的变换"""import rclpy # ROS2 Python接口库from rclpy.node import Node # ROS2 节点类import tf_transformations # TF坐标变换库from tf2_ros import TransformException # TF左边变换的异常类from tf2_ros.buffer import Buffer # 存储坐标变换信息的缓冲类from tf2_ros.transform_listener import TransformListener # 监听坐标变换的监听器类class TFListener(Node): def __init__(self, name): super().__init__(name) # ROS2节点父类初始化 self.declare_parameter('source_frame', 'world') # 创建一个源坐标系名的参数 self.source_frame = self.get_parameter( # 优先使用外部设置的参数值,否则用默认值 'source_frame').get_parameter_value().string_value self.declare_parameter('target_frame', 'house') # 创建一个目标坐标系名的参数 self.target_frame = self.get_parameter( # 优先使用外部设置的参数值,否则用默认值 'target_frame').get_parameter_value().string_value self.tf_buffer = Buffer() # 创建保存坐标变换信息的缓冲区 self.tf_listener = TransformListener(self.tf_buffer, self) # 创建坐标变换的监听器 self.timer = self.create_timer(1.0, self.on_timer) # 创建一个固定周期的定时器,处理坐标信息 def on_timer(self): try: now = rclpy.time.Time() # 获取ROS系统的当前时间 trans = self.tf_buffer.lookup_transform( # 监听当前时刻源坐标系到目标坐标系的坐标变换 self.target_frame, self.source_frame, now) except TransformException as ex: # 如果坐标变换获取失败,进入异常报告 self.get_logger().info( f'Could not transform {self.target_frame} to {self.source_frame}: {ex}') return pos = trans.transform.translation # 获取位置信息 quat = trans.transform.rotation # 获取姿态信息(四元数) euler = tf_transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w]) self.get_logger().info('Get %s -- > %s transform: [%f, %f, %f] [%f, %f, %f]' % (self.source_frame, self.target_frame, pos.x, pos.y, pos.z, euler[0], euler[1], euler[2]))def main(args=None): rclpy.init(args=args) # ROS2 Python接口初始化 node = TFListener("tf_listener") # 创建ROS2节点对象并进行初始化 rclpy.spin(node) # 循环等待ROS2退出 node.destroy_node() # 销毁节点对象 rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项,让系统知道Python程序的入口,打开功能包的setup.py文件,加入如下入口点的配置:
entry_points={ 'console_scripts': [ 'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main', 'tf_listener = learning_tf.tf_listener:main', ], },
好啦,大家现在对TF的基本使用有所了解了。我们继续挑战两只海龟跟随的案例。
-
机器人
+关注
关注
210文章
28187浏览量
206418 -
坐标系
+关注
关注
0文章
29浏览量
7280 -
程序
+关注
关注
116文章
3773浏览量
80833 -
ROS
+关注
关注
1文章
276浏览量
16963
发布评论请先 登录
相关推荐
评论