ROS_TF API详解_基础监听与广播

来源：https://zhengyu.tech

广播类API

仅有一个类：TransformBroadcaster:用于定义初始化，queue_size为最大队列数，默认为100：

import tf
import rospy
rospy.init_node('tf_broadcaster')
br = tf.TransformBroadcaster(queue_size=100)

并具有以下功能函数：

sendTransform(self, translation, rotation, time, child, parent):

用于广播信息到 /tf 中，需要对以下信息逐一填写不可遗漏，并且没有初始值。

• translation：位置信息，(x, y, z)，以元组的形式输入
• rotation：方向信息，这是采用的是四元数，(x, y, z, w)，需要注意顺序，有些库的w在第一个
• time：时间戳，一般用：rospy.Time.now()
• child：子框架
• parent：父框架

def sendTransform(self, translation, rotation, time, child, parent):
    t = geometry_msgs.msg.TransformStamped()
    t.header.frame_id = parent
    t.header.stamp = time
    t.child_frame_id = child
    t.transform.translation.x = translation[0]
    t.transform.translation.y = translation[1]
    t.transform.translation.z = translation[2]

    t.transform.rotation.x = rotation[0]
    t.transform.rotation.y = rotation[1]
    t.transform.rotation.z = rotation[2]
    t.transform.rotation.w = rotation[3]

    self.sendTransformMessage(t)

例子：

br.sendTransform((0, 0, 0),(0,0,0,1),rospy.Time.now(),"Camera","world")

sendTransformMessage(self, transform) ！！

也可以对tf信息geometry_msgs.msg.TransformStamped先全部填好再广播

def sendTransformMessage(self, transform):
    self.tf2_broadcaster.sendTransform([transform])

例子：

import geometry_msgs.msg
import tf

t = geometry_msgs.msg.TransformStamped()

t.header.frame_id = 'world'
t.header.stamp = rospy.Time.now()
t.child_frame_id = 'camera'
t.transform.translation.x = 0
t.transform.translation.y = 0
t.transform.translation.z = 0
t.transform.rotation.w = 1
t.transform.rotation.x = 0
t.transform.rotation.y = 0
t.transform.rotation.z = 0

m.sendTransformMessage(t)

监听类API

监听类为TransformListener，可以看在它还继承了TransformerROS类，而TransformerROS又继承了Transformer(object)类（我全都要？~）。

class TransformListener(TransformerROS):
class TransformerROS(Transformer):
class Transformer(object):

实例化例子，没有初始化参数：

import tf
listener = tf.TransformListener()

Transformer(object)类

该类具体实现了监听器的各类功能，例如阻塞等待frame连接，确定frame是否存在等。

lookupTransform ！！

返回当时从 source_frame 到 target_frame 的变换。 time 检测时间点。 变换以位置 (x, y, z) 和四元数 (x, y, z, w) 的形式返回。

def lookupTransform(self, target_frame, source_frame, time):
    msg = self._buffer.lookup_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time)
    t = msg.transform.translation
    r = msg.transform.rotation
    return [t.x, t.y, t.z], [r.x, r.y, r.z, r.w]

例子：

 #rospy.Time(0)不表示0时刻的tf，而是指最近一帧tf 
 (trans,rot) = listener.lookupTransform('/camera', '/world', rospy.Time(0))

canTransform ！！

通常在其他操作前，我们会检查两个frame是否联通，time指获取转换的时间（0 会得到最新的）。返回是Ture or false (bool)

def canTransform(self, target_frame, source_frame, time):

例子：

listener.canTransform('camera_base','/H_t1',rospy.Time(0))

canTransformFull

canTransform的升级版，通过设置fixed_frame(通常为世界固定参考系)来对两个都进行检测，因为如果没有与fixed_frame连通，则二者肯定没有连通。

def canTransformFull(self, target_frame, target_time, source_frame, source_time, fixed_frame):

例子：

listener.canTransformFull('camera_base',rospy.Time(0),'/H_t1',rospy.Time(0),'/world')

waitForTransform ！！

阻塞程序进程直到两个frame连通，time可以设置为当前时间开始，timeout为最大的等待时间。

def waitForTransform(self, target_frame, source_frame, time, timeout, polling_sleep_duration=None):

例子，从现在开始检测连通性，最大等待时间为4秒：

listener.waitForTransform("/base_link", "/link1", rospy.Time.now(), rospy.Duration(4.0))

深入底层，我们可以发现实际上还是调用的的canTransform功能，只不过增加了延时循环检测时间timeout

def waitForTransform(self, target_frame, source_frame, time, timeout, polling_sleep_duration=None):
    if not self._using_dedicated_thread:
        raise tf2_ros.TransformException("cannot wait for transform without a dedicated thread that listens to incoming TF messages")
    # 调用can_transform
    can_transform, error_msg = self._buffer.can_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time, timeout, return_debug_tuple=True)
    if not can_transform:
        raise tf2_ros.TransformException(error_msg or "no such transformation: \"{}\" -> \"{}\"".format(source_frame, target_frame))


def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0), return_debug_tuple=False):
    if timeout != rospy.Duration(0.0):
        start_time = rospy.Time.now()
        r= rospy.Rate(20)
        while (rospy.Time.now() < start_time + timeout and 
               not self.can_transform_core(target_frame, source_frame, time)[0] and
               (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them
            r.sleep()
    core_result = self.can_transform_core(target_frame, source_frame, time)
    if return_debug_tuple:
        return core_result
    return core_result[0]

chain

用于返回 source_frame 连接到 target_frame 之间的frame链。

def chain(self, target_frame, target_time, source_frame, source_time, fixed_frame):

例子

chain("/base_link", rospy.Time.now(), "/link1", rospy.Time.now(), "/world"):

frameExists

确认该frame是否存在，通过在所有的frame id中检索，返回是Ture or false (bool)

    def frameExists(self, frame_id):
        return frame_id in self.getFrameStrings()

例子：

frameExists("/base_link"):

getFrameStrings

将 tf 中的所有frame id作为列表返回

    def getFrameStrings(self):
        data = yaml.load(self._buffer.all_frames_as_yaml()) or {}
        return [p for p, _ in data.items()]

getLatestCommonTime

两个 frame 之间的变换的最近时间，即 source_frame 和 target_frame 之间的变换。 返回一个 rospy.Time时间点

def getLatestCommonTime(self, source_frame, target_frame):

clear

清除所有tf，无返回。

def clear(self):
    self._buffer.clear()

TransformerROS(Transformer):类

这是针对Transformer类的拓展，添加了处理 ROS 消息的方法。

asMatrix

用于lookupTransform()查找 ROS 消息头 hdr 的frame到 target_frame 的转换，并将转换作为numpy 4x4 变换矩阵返回。

def asMatrix(self, target_frame, hdr):
    translation,rotation = self.lookupTransform(target_frame, hdr.frame_id, hdr.stamp)
    return self.fromTranslationRotation(translation, rotation)

fromTranslationRotation ！！

用于将translation(x,y,z), rotation(x,y,z,w)合并转换为 4x4 变换矩阵的表示形式。

def fromTranslationRotation(self, translation, rotation):
    return numpy.dot(transformations.translation_matrix(translation), transformations.quaternion_matrix(rotation))

transformPoint

将原来PointStamped中的其他frame转为target_frame，再返回PointStamped。源代码中是先读取原frame与target_frame的相对位置关系，再将PointStamped中的point数据修改。

def transformPoint(self, target_frame, ps):
    mat44 = self.asMatrix(target_frame, ps.header)
    xyz = tuple(numpy.dot(mat44, numpy.array([ps.point.x, ps.point.y, ps.point.z, 1.0])))[:3]
    r = geometry_msgs.msg.PointStamped()
    r.header.stamp = ps.header.stamp
    r.header.frame_id = target_frame
    r.point = geometry_msgs.msg.Point(*xyz)
    return r

transformVector3

将原Vector3Stamped中的其他frame转为target_frame，返回一个新的Vector3Stamped 消息，实现方式上同。

def transformVector3(self, target_frame, v3s):
    mat44 = self.asMatrix(target_frame, v3s.header)
    mat44[0,3] = 0.0
    mat44[1,3] = 0.0
    mat44[2,3] = 0.0
    xyz = tuple(numpy.dot(mat44, numpy.array([v3s.vector.x, v3s.vector.y, v3s.vector.z, 1.0])))[:3]
    r = geometry_msgs.msg.Vector3Stamped()
    r.header.stamp = v3s.header.stamp
    r.header.frame_id = target_frame
    r.vector = geometry_msgs.msg.Vector3(*xyz)
    return r

transformQuaternion

将原QuaternionStamped中的其他frame转为target_frame，返回一个新的QuaternionStamped 消息，上同。

    def transformQuaternion(self, target_frame, ps):
        mat44 = self.asMatrix(target_frame, ps.header)
        pose44 = xyzw_to_mat44(ps.quaternion)
        txpose = numpy.dot(mat44, pose44)
        quat = tuple(transformations.quaternion_from_matrix(txpose))
        r = geometry_msgs.msg.QuaternionStamped()
        r.header.stamp = ps.header.stamp
        r.header.frame_id = target_frame
        r.quaternion = geometry_msgs.msg.Quaternion(*quat)
        return r

transformPose

将原PoseStamped 中与其他frame的位姿关系转为与target_frame的位姿关系，返回一个新的PoseStamped 消息，上同。

    def transformPose(self, target_frame, ps):
        mat44 = self.asMatrix(target_frame, ps.header)
        pose44 = numpy.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation))
        txpose = numpy.dot(mat44, pose44)
        xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
        quat = tuple(transformations.quaternion_from_matrix(txpose))
        r = geometry_msgs.msg.PoseStamped()
        r.header.stamp = ps.header.stamp
        r.header.frame_id = target_frame
        r.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat))
        return r

transformPointCloud

将原sensor_msgs.msg.PointCloud()中与其他frame的点坐标关系转为与target_frame的点坐标关系，返回一个新的sensor_msgs.msg.PointCloud() 消息。可以在源代码中看到利用def xf(p):函数将所有点的坐标关系转换到了target_frame参考系下。

def transformPointCloud(self, target_frame, point_cloud):
    r = sensor_msgs.msg.PointCloud()
    r.header.stamp = point_cloud.header.stamp
    r.header.frame_id = target_frame
    r.channels = point_cloud.channels

    mat44 = self.asMatrix(target_frame, point_cloud.header)
    def xf(p):
        xyz = tuple(numpy.dot(mat44, numpy.array([p.x, p.y, p.z, 1.0])))[:3]
        return geometry_msgs.msg.Point(*xyz)
    r.points = [xf(p) for p in point_cloud.points]
    return r

参考：
1、tf文档
2、sensor_msgs
3、geometry_msgs