ROS之TF入门

目录

1.坐标msg信息

2.静态坐标变换（Python实现）

Publisher的实现

Subscriber的实现

---

1.坐标msg信息

传输坐标系(transform)相关位置信息：

geometry_msgs/TransformStamped

传输某个坐标系内坐标点(point)的信息：

geometry_msgs/PointStamped

获取TransformStamped的内容：

rosmsg info geometry_msgs/TransformStamped

#获取的内容

std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

获取PointStamped的内容：

rosmsg info geometry_msgs/PointStamped

#获取到的内容

std_msgs/Header header                    #头
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

2.静态坐标变换（Python实现）

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

案例：假设一个人（people）站在相对地球地心(world)坐标位置(0.2,0,0.5)上，人所看到的树(tree)在人的坐标位置(2,3,5)上，此时树在地心上的坐标是多少？

Publisher的实现

首先我们要用rospy.init_node()函数初始化ROS节点,则需导入ros.py包。

用tf2_ros.StaticTransformBroadcaster()函数创建广播器来实现数据的发布，则需导入tf2_ros包。既然已经创建了广播器，我们下一步就用TransformStamped()函数对被广播消息进行创建及编写(根据我们前面部分所查询到的详细内容填写)，最后用broadcaster.sendTransform()函数发布广播。

根据以上我们可知主坐标轴是world，子坐标轴是people

#! /usr/bin/env python

# 1.导包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("publisher")
    # 3.创建 静态坐标广播器
    broadcaster = tf2_ros.StaticTransformBroadcaster()
    # 4.创建并组织被广播的消息
    tfs = TransformStamped()
    # --- 头信息
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.header.seq = 101
    # --- 子坐标系
    tfs.child_frame_id = "people"
    # --- 坐标系相对信息
    # ------ 偏移量
    tfs.transform.translation.x = 0.2
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.5
    # ------ 四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]


    # 5.广播器发送消息
    broadcaster.sendTransform(tfs)
    # 6.spin
    rospy.spin()

Subscriber的实现

创建了发布者(publisher)后，我们现在来创建订阅者(subscriber)

我们通过tf2_ros.TransformListener()函数来获取publisher所发布的人坐标与地坐标位置信息，发现tf2_ros.TransformListener()函数需要填入一个buffer参数，因此我们用tf2_ros.Buffer()函数来创建buffer参数。然后用PointStamped()函数对被树相对人坐标位置消息进行创建及编写(根据我们前面部分所查询到的详细内容填写)，最后用Transform()函数进行坐标的变换。

#! /usr/bin/env python

# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("subscriber")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建一个 radar 坐标系中的坐标点
        tree = PointStamped()
        tree.header.frame_id = "people"
        tree.header.stamp = rospy.Time.now()
        tree.point.x = 10
        tree.point.y = 2
        tree.point.z = 3

        try:
    #     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            tree_world = buffer.transform(tree,"world")
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            tree_world.point.x,
                            tree_world.point.y,
                            tree_world.point.z)
        except Exception as e:
            rospy.logerr("异常:%s",e)

    #     6.spin
        rate.sleep()

以上代码内容参考autolabor赵左虚