侧边栏壁纸
  • 累计撰写 129 篇文章
  • 累计创建 16 个标签
  • 累计收到 4 条评论

目 录CONTENT

文章目录
ROS   

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

广播类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

0

评论区