广播类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
评论区