ros tf2使用示例

关于ROS中使用tf/tf2获取到transformation之后的怎样处理，写一个例子作为参考：

已知两组固定坐标系，B->A，D->C，其关系定义如代码 static_publisher.py所示

#!/usr/bin/env python
# static_publisher.py

import rospy
import sys
import tf2_ros as tf2
import tf_conversions as tfc
import geometry_msgs.msg
from tf2_msgs.msg import TFMessage
import math
import numpy

def __genTf(frameT, frameS, posTs, q):
    ts = geometry_msgs.msg.TransformStamped()
    ts.header.stamp = rospy.Time.now()
    ts.header.frame_id = frameT
    ts.child_frame_id = frameS
    
    ts.transform.translation.x = posTs[0]
    ts.transform.translation.y = posTs[1]
    ts.transform.translation.z = posTs[2]

    ts.transform.rotation.x = q[0]
    ts.transform.rotation.y = q[1]
    ts.transform.rotation.z = q[2]
    ts.transform.rotation.w = q[3]
    return ts

def __main():
    rospy.init_node("AB&CD")
    rate = rospy.Rate(100)
    tbf = tf2.Buffer()
    tl = tf2.TransformListener(tbf)
    
    t = TFMessage()
    sbc = tf2.StaticTransformBroadcaster()
    qA2B = tfc.transformations.quaternion_from_euler(math.pi, 0, math.pi / 2)
    ts1 = __genTf("A", "B", (-1, 0, 0.2), qA2B)
    t.transforms.append(ts1)

    qC2D = tfc.transformations.quaternion_from_euler(math.pi, 0, 0)
    ts2 = __genTf("C", "D", (0, 0, 1), qC2D)
    t.transforms.append(ts2)

    sbc.pub_tf.publish(t)
    rospy.spin()


if __name__ == "__main__":
    __main()

先假定D传感器的观测数据会发布在B坐标系下，而A是实际地图坐标系，求C物体坐标系(source frame, child frame)到A坐标系(destination frame, parent frame)的转换，代码如下：

#!/usr/bin/env python
# dynamic_publisher.py

import rospy
import sys
import tf2_ros as tf2
import tf_conversions as tfc
import geometry_msgs.msg
from tf2_msgs.msg import TFMessage
import math
import numpy

def qRot(p, q):
    qInvert = tfc.transformations.quaternion_inverse
    qMultiply = tfc.transformations.quaternion_multiply
    qInv = qInvert(q)
    pNew = qMultiply(qMultiply(q, p), qInv)
    return pNew

def qRotInv(p, q):
    qInvert = tfc.transformations.quaternion_inverse
    qMultiply = tfc.transformations.quaternion_multiply
    qInv = qInvert(q)
    pNew = qMultiply(qMultiply(qInv, p), q)
    return pNew


def __genTf(frameT, frameS, posTs, q):
    ts = geometry_msgs.msg.TransformStamped()
    ts.header.stamp = rospy.Time.now()
    ts.header.frame_id = frameT
    ts.child_frame_id = frameS
    
    ts.transform.translation.x = posTs[0]
    ts.transform.translation.y = posTs[1]
    ts.transform.translation.z = posTs[2]

    ts.transform.rotation.x = q[0]
    ts.transform.rotation.y = q[1]
    ts.transform.rotation.z = q[2]
    ts.transform.rotation.w = q[3]
    return ts

def __main():
    rospy.init_node("A2C")
    rate = rospy.Rate(1)
    tbf = tf2.Buffer()
    tl = tf2.TransformListener(tbf)
    
    a = 0
    pp = 0
    while not tbf.can_transform("C", "D", rospy.Time()) and not rospy.is_shutdown():
        print "waiting for static tf ready.{C-D}"
        rate.sleep()
        
    while not tbf.can_transform("D", "C", rospy.Time()) and not rospy.is_shutdown():
        print "waiting for static tf ready.{D-C}"
        rate.sleep()
        
    while not tbf.can_transform("A", "B", rospy.Time()) and not rospy.is_shutdown():
        print "waiting for static tf ready.{A-B}"
        rate.sleep()
        
    while not tbf.can_transform("B", "A", rospy.Time()) and not rospy.is_shutdown():
        print "waiting for static tf ready.{B-A}"
        rate.sleep()
        
    print ("all static tf ready")
            
    roundCount = 0
    rateFast = rospy.Rate(10)
    
    qInvert = tfc.transformations.quaternion_inverse
    qMultiply = tfc.transformations.quaternion_multiply
    while not rospy.is_shutdown():
        print("begin: " + str(roundCount))
        roundCount += 1
        # lookup_transform(desFrame, srcFrame, time)
        tfD2C = tbf.lookup_transform("C", "D", rospy.Time())
        tfC2D = tbf.lookup_transform("D", "C", rospy.Time())
        tfB2A = tbf.lookup_transform("A", "B", rospy.Time())
        tfA2B = tbf.lookup_transform("B", "A", rospy.Time())
        
        bc = tf2.TransformBroadcaster()

        tD2B = (2 * math.cos(a), 1 + 2 * math.sin(a), -1, 0)
        qD2B = tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)
        
        ts = __genTf("B", "DB", tD2B, qD2B)
        bc.sendTransform(ts)

        # db -> da
        tfm = tfB2A
        r = tfm.transform.rotation
        ts = tfm.transform.translation
        qB2A = numpy.array([r.x, r.y, r.z, r.w])
        tB2A = numpy.array([ts.x, ts.y, ts.z, 0])
        qD2A = qMultiply(qB2A, qD2B)
        tD2A = qRot(tD2B, qB2A)  + tB2A

        ts = __genTf("A", "DA", tD2A, qD2A)
        bc.sendTransform(ts)
        
        # da -> ca
        tfm = tfC2D
        r = tfm.transform.rotation
        ts = tfm.transform.translation
        qC2D = numpy.array([r.x, r.y, r.z, r.w])
        tC2D = numpy.array([ts.x, ts.y, ts.z, 0])
        qC2A = qMultiply(qD2A, qC2D)
        tC2A = qRot(tC2D, qD2A) + tD2A
        ts = __genTf("A", "C", tC2A, qC2A)
        bc.sendTransform(ts)

        if a < math.pi * 2:
            a += 0.01
        else:
            a = 0

        rateFast.sleep()


if __name__ == "__main__":
    __main() 

上述代码首先模拟了传感器D发布自身在B上的绝对位置

tD2B = (2 * math.cos(a), 1 + 2 * math.sin(a), -1, 0)
qD2B = tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)

之后通过D->B，B->A得到D->A的坐标系转换，再通过C->D得到C->A的坐标系转换

这里需要注意的是四元数乘法不遵守交换率，因此不要将转换次序弄错

运行roscore, rviz,再运行python static_publisher.py, python dynamic_publisher.py, 在rviz选择frame A为主坐标系，之后可以看到D，DA，DB重合在一起围绕A做圆周运动