ROS中的tf发布读取转换

ROS中tf是核心基础功能之一，负责将世界中的所有link联系在一起，记录带有时间戳的相对三维坐标关系，让我们可以随时得到不同link之间的相对位置关系。

基础使用

查看tf信息

1. 创建link关系图

此命令将会创建一个pdf类型的tf关系图，可以查看发布者、频率等信息

$ rosrun tf view_frames

2.在rqt中查看link关系图

$ rosrun rqt_tf_tree rqt_tf_tree

3. 终端中输出tf变换关系

$ rosrun tf tf_echo [reference_frame] [target_frame]

4. rviz中查看

打开rivz，点击Add添加tf查看

程序中使用

ros wiki 中给出了比较详细的介绍与教程，包括命令行工具、教程、API接口等，可以进行初步的了解与学习。

API链接 tf，支持python与C++两种语言。

以下使用C++语言举例：
最常用的是tf::Transformer 、 tf::TransformBroadcaster 和 tf::TransformListener，分别负责转换、发布与监听的任务。

Transformer

1. frameExists()
   查看frame是否存在
2. lookupTransform()
   查看两个frame之间的转换关系
3. transformPose()
   转换获的得一个posestamped值基于某坐标系的位姿
4. waitForTransform()
   等待两个坐标系的转换发出

示例1：
获取两个坐标系的转换关系
说明： 获取tf时经常会抛出异常，所以使用循环不断读取直到获取，其中waitForTransform()也可以提高获取概率

    rate = rospy.Rate(10.0)
    listener = tf.TransformListener()
    get_pose = False
    while not rospy.is_shutdown() and not get_pose:
      try:
        now = rospy.Time.now()
        listener.waitForTransform(base_link, target_link, now, rospy.Duration(0.3))
        (trans, rot) = listener.lookupTransform(base_link, target_link, now)
        # print(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3])
        get_pose = True
      except (tf.Exception, tf.LookupException, tf.ConnectivityException):
        print("tf echo error")
        continue
      rate.sleep()

示例2：
转换获的得一个posestamped值基于某坐标系的位姿

    pose_stamped = PoseStamped()
    pose_stamped.pose = pose
    pose_stamped.header.frame_id = "/camera_color_frame"
    rate = rospy.Rate(10.0)
    listener = tf.TransformListener()
    get_pose = False
    while not rospy.is_shutdown() and not get_pose:
      try:
        # now = rospy.Time.now() - rospy.Duration(5.0)
        now = rospy.Time.now()
        listener.waitForTransform("/base_link", "camera_color_frame", now, rospy.Duration(0.3))
        pose_stamped_return = listener.transformPose("/base_link", pose_stamped)
        # print(pose_stamped_return.pose.position.x, pose_stamped_return.pose.position.y, pose_stamped_return.pose.position.z, pose_stamped_return.pose.orientation.x, pose_stamped_return.pose.orientation.y, pose_stamped_return.pose.orientation.z, pose_stamped_return.pose.orientation.w)      
        # print(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3])
        get_pose = True
      except (tf.Exception, tf.LookupException, tf.ConnectivityException):
        print("tf echo error")
        continue
      rate.sleep()

TransformBroadcaster

1. sendTransform()
   发布坐标系转换

示例：
发布base_link与camera_color_frame的坐标系tf转换

    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
      pose = Pose()
      br.sendTransform((pose.position.x, pose.position.y, pose.position.z),
                       (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
                       rospy.Time.now(),
                       "base_link",
                       "camera_color_frame")    
      rate.sleep()

TransformListener

TransformListener类继承了Transformer，所以可以直接使用Transformer中函数

1. transformPose()
   转换获的得一个posestamped值基于某坐标系的位姿