TF §

图形化当前TF树状图 §

• 参见使用tf2_tools图形化当前TF树状图

声明一个静态坐标系 §

rosrun tf static_transform_publisher 0 0 -0.03 1.57 0 1.57 yumi_base_link ar_marker_90 50

获取坐标系变换 §

使用TF的准备工作 §

• 必须在一个node下操作，否则会报错 AttributeError: 'TransformListener' object has no attribute 'tf_sub' 如果出现这个错误，在使用TF前添加rospy.init_node('YOUR_NODE_NAME', anonymous=True)

TF获取与设置类 §

• 参考代码

import tf
from tf.transformations import quaternion_from_matrix, quaternion_matrix
 
class workspace_tf:
    def __init__(self):
        self.listener = tf.TransformListener()
        self.caster = tf.TransformBroadcaster()
 
    def get_tf(self, ref_frame, obj):
        ## return a homogeneous transformation matrix
        updated = False
        while updated==False:
            try:
                (trans,rot) = self.listener.lookupTransform(ref_frame, obj, rospy.Time(0))
                h = quaternion_matrix(rot)
                h[:3,3] = trans
                return h
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.sleep(0.1)
 
    def set_tf(self, ref_frame, obj, h, delay=1):
        ## h is the homogeneous transformation matrix
        updated = False
        q = quaternion_from_matrix(h)
        o = h[:3,3]
        self.caster.sendTransform(o, q, rospy.Time.now(), obj, ref_frame)

• 使用说明
  • 将新的坐标系添加到世界环境中：ws_tf.set_tf("world", "new_object", t_obj2world)，其中t_obj2world为4x4的齐次变换矩阵

获取TF坐标系 §

• 函数的使用

  import tf
  listener = tf.TransformListener()
  try:
      (trans,rot) = listener.lookupTransform('/ref_frame', '/ojb_frame', rospy.Time(0))
  except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
      continue

• 函数传入参数的解释
  • 第一个参数为作为坐标系基准的参照系，第二个为待转换的坐标系 lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform)
  • https://www.hepeng.me/ros-tf-whoever-wrote-the-python-tf-api-f-ked-up-the-concept/
  • https://answers.ros.org/question/194046/the-problem-of-transformerlookuptransform/
  • 输出的四元数格式为

B.lookupTransform()函数参数说明
a.官网的例程说的是实现从“/turtle2”到“/turtle1”的转换，但实际中使用时，我发现转换得出的坐标是在“/turtle2”坐标系下的
b.不可以把ros::Time(0)改成ros::time::now()，因为监听做不到实时，会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据，ros::time::now()则指当下，如果非要使用ros::time::now,则需要结合waitForTransform()使用，具体见:http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C++%29
实际中ros::Time(0)大多数情况下可以满足要求。

添加TF坐标系 §

添加TF坐标系的方法 §

• sendTransform函数的使用

  import tf
  caster = tf.TransformBroadcaster()
  caster.sendTransform((0, 1, 2), (0, 0, 0, 1), rospy.Time.now(), '/obj_frame', '/ref_frame')

• 函数传入参数的解释 - 第一个参数为位移，第二个参数为旋转的四元数表示
• 如果选取的参照系不是根坐标系，则将在下一次调用sendTransform并且参照系为跟坐标系时才更新目标坐标系的信息
• 其它参考 http://wiki.ros.org/tf/Tutorials/Adding%20a%20frame%20%28Python%29 http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29 https://answers.ros.org/question/358188/converting-cvmat-rotation-matrix-to-quaternion/

添加TF坐标系时要注意的问题 §

添加的依赖关系（两颗树的合并） §

• 一个子节点只能有一个父节点，一个父节点可以有多个子节点。例如，

ws_tf.set_tf("cam", "marker", ht1)
ws_tf.set_tf("world", "marker", ht2)

会导致生成的TF不正常。因为试图让marker同时以cam和world作为参考坐标系（cam<-marker->world）。一种改进方法是

ws_tf.set_tf("marker", "cam", np.lingla.inv(ht1))
ws_tf.set_tf("world", "marker", ht2)

此时cam以marker作为参考坐标系，marker以world作为参考坐标系（world<-marker<-cam）

添加坐标系的顺序问题（有待更多测试？） §

• 假设有world<-link1<-link2，那么添加顺序应为： 先添加link1<-link2，再添加world<-link1

Source与target §

• 使用rosrun tf tf_echo /world /child，获取以/world为参考坐标系时/child的姿态
• The tf_echo give you the inverse, which is the transform to move the coordinate frames as a parallel to the published transforms for easier publishing debugging.
• The lookup API is named with respect to transforming data from the source frame into the target frame.
  • lookup API：以target frame为基准，获取source frame相对于其的姿态 When referring to transform between coordinate frames (transforming the frames, it is the inverse of the transform of data between the two frames) Depending on the documentation if you’re referring to the transform value for the data, or the transform value of the coordinate frames you get the inverse value.

在Rviz中显示TF §

• 如果TF不是持续更新的，那么在RViz中会在一段时间后消失（默认是15秒）
• 如果想持续显示一个坐标系，则添加一个axes，在Reference Frame中选择相应的TF

连接两个TF树 §

• 假设有两个TF树，world和cam
• 通过tf.TransformBroadcaster，sendTransform直接在两个树根（‘/world’和’/cam’）间建立变换关系。整合后的代码参考上述TF获取与设置类中的set_tf()