ros tf坐标
参考：
- 讲解：https://www.bilibili.com/video/BV1zt411G7Vn/?p=18&vd_source=3a1ad336af3eaae4fcced56c75d309d1
- ROS程序：https://gitee.com/guyuehome/ros_21_tutorials/tree/master/learning_tf
- ROS2程序：https://gitee.com/guyuehome/ros2_21_tutorials/tree/master/learning_tf
- 将点云从相机坐标系转到世界坐标系：https://blog.csdn.net/a17381562089/article/details/115127318
- 机械臂抓取坐标转换：https://blog.csdn.net/qq_33328642/article/details/122667192
- 具体的程序讲解：https://blog.csdn.net/qq_52785580/article/details/124673098
- wiki官方文档的链接：https://blog.csdn.net/ZhangRelay/article/details/62052505
01 可视化
Rviz实时显示：
终端打开rviz
点击add 添加tf
可以看到实时的变化
TF 树：
rosrun tf view_frames
会生成pdf文件，显示坐标关系，pdf文件保存在当前终端的文件夹下
rosrun rqt_tf_tree rqt_tf_tree
在rqt中显示
命令行监听
列出tf命令行工具 rosrun tf tf_
监听 tf tf_echo
02 坐标变换
03 程序
3.1 坐标发布与接收
3.1.1 tf发布
① ros c++
/**
* 发布tf
*/
#include ② ros python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务，服务数据类型learning_service::Person
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import turtlesim.msg
def handle_turtle_pose(msg, turtlename):
br = tf.TransformBroadcaster()
br.sendTransform((msg.x, msg.y, 0),
tf.transformations.quaternion_from_euler(0, 0, msg.theta),
rospy.Time.now(),
turtlename,
"world")
if __name__ == '__main__':
rospy.init_node('turtle_tf_broadcaster')
turtlename = rospy.get_param('~turtle')
rospy.Subscriber('/%s/pose' % turtlename,
turtlesim.msg.Pose,
handle_turtle_pose,
turtlename)
rospy.spin()
③ ros2 python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@文件名: static_tf_broadcaster.py
@说明: 广播静态的坐标变换
"""
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()
2.1.2 tf监听
① ros c++
/**
* 监听tf数据，并计算、发布turtle2的速度指令
*/
#include ② ros python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务，服务数据类型learning_service::Person
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('turtle_tf_listener')
listener = tf.TransformListener()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
angular = 4 * math.atan2(trans[1], trans[0])
linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
turtle_vel.publish(cmd)
rate.sleep()
③ ros2 python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@文件名: tf_listener.py
@说明: 监听某两个坐标系之间的变换
"""
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接口
04 动态与静态坐标变换程序注意
- 过程和讲解参考这篇（这个写的很好，需要多参考）——ROS中动态坐标变换（动态参数调节+动态坐标变换）：https://blog.csdn.net/weixin_45590473/article/details/122912545
- 总结参考这篇——二十三、TF坐标变换（三）：动态坐标变换：https://blog.csdn.net/qq_43280851/article/details/125733199
总结：
主要是时间戳
-
在静态坐标变换中，
坐标系的相对关系不变，所以时间戳时间既可以是当前的，也可以忽略时间戳，格式如下：C++： 下列三种都可以
- tfs.header.stamp = ros::Time::now();
- tfs.header.stamp = ros::Time(0.0);
- tfs.header.stamp = ros::Time();
Python： 下列两种都可以
- point_source.header.stamp = rospy.Time.now()
- point_source.header.stamp = rospy.Time()
-
在动态坐标变换中，
ROS系统会根据时间戳进行坐标系和坐标点的匹配，来保证坐标变换的准确性，当时间戳相差较大时，就会报错。
由于存放坐标关系的buffer时间和坐标点的时间戳一个在循环外，一个在循环内，时间相差较大，所以系统会报错。
虽然buffer的时间戳一直不变，但是其内容是一直变得，在一直更新，所以，如果忽略时间戳，并不会影响转换精度，因此，在动态坐标变换中（有循环），每次循环必须更新订阅端的无效时间戳（注意不是没有时间戳啦！），格式如下：C++: 下列两种都可以
- tfs.header.stamp = ros::Time(0.0);
- tfs.header.stamp = ros::Time();
Python:
- point_source.header.stamp = rospy.Time()