Z's learning note

Search

SearchSearch

tf2

Jun 27, 2022, 4 min read

TF2

图形化当前TF树状图

  • 首先安装tf2-tools： sudo apt install ros-$ROS_DISTRO-tf2-tools  然后运行rosrun tf2_tools view_frames.py 

发布静态变换Static Transformation

  • 用法示例： 在节点初始化时发布一些固定坐标系的变换。参考

    Publishing static transforms is useful to define the relationship between a robot base and its sensors or non-moving parts.

  • 引用TF2库
import tf2_ros
import geometry_msgs.msg
broadcaster = tf2_ros.StaticTransformBroadcaster()
  • 初始化所要发布的变换（TF2不包含四元数转欧拉角等数学变换工具）
static_transformStamped = geometry_msgs.msg.TransformStamped()
static_transformStamped.header.stamp = rospy.Time.now()
static_transformStamped.header.frame_id = "world"
static_transformStamped.child_frame_id = sys.argv[1]
static_transformStamped.transform.translation.x = float(sys.argv[2])
static_transformStamped.transform.translation.y = float(sys.argv[3])
static_transformStamped.transform.translation.z = float(sys.argv[4])
quat = tf.transformations.quaternion_from_euler(
           float(sys.argv[5]),float(sys.argv[6]),float(sys.argv[7]))
static_transformStamped.transform.rotation.x = quat[0]
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]
  • 发布变换
broadcaster.sendTransform(static_transformStamped)
  • 可以通过命令行或者launch声明变换

发布多个静态变换

  • 将多个变换拼接成为列表再由broadcaster发布，参考
broadcaster.sendTransform([t1, t2])

发布一个变换

  • 用法示例：监听一个位姿topic，再将其发布至TF中。参考
  • 初始化broadcaster
br = tf2_ros.TransformBroadcaster()
  • 初始化变换与静态变换一样
  • 发布变换
br.sendTransform(t)

添加一个坐标系（add a frame，在ROS 2中似乎不再提及这种方法）

  • 用法示例：临时添加一个坐标系方便计算？

    For many tasks related to transformations, it is easier to think inside a local frame. For example, it is easiest to reason about laser scan measurements in a frame at the center of the laser scanner. tf2 allows you to define a local frame for each sensor, link, or joint in your system.

  • 添加坐标系将TF信息直接发送至ROS topic /tf，不需要实例化broadcaster
import rospy
import tf2_ros
import tf2_msgs.msg
import geometry_msgs.msg
class FixedTFBroadcaster:
    def __init__(self):
        self.pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1)
        while not rospy.is_shutdown():
            # Run this loop at about 10Hz
            rospy.sleep(0.1)
            t = geometry_msgs.msg.TransformStamped()
            t.header.frame_id = "turtle1"
            t.header.stamp = rospy.Time.now()
            t.child_frame_id = "carrot1"
            t.transform.translation.x = 0.0
            t.transform.translation.y = 2.0
            t.transform.translation.z = 0.0
            t.transform.rotation.x = 0.0
            t.transform.rotation.y = 0.0
            t.transform.rotation.z = 0.0
            t.transform.rotation.w = 1.0
            tfm = tf2_msgs.msg.TFMessage([t])
            self.pub_tf.publish(tfm)
if __name__ == '__main__':
    rospy.init_node('fixed_tf2_broadcaster')
    tfb = FixedTFBroadcaster()
    rospy.spin()

Static transformation，Transformation和Add a frame的区别

  • 参考1
  • Static Transformation假定相关TF一经发布后不需要修改，或很少需要修改，在发布时使用topic latching，被发布到/tf_static
  • Transformation假定相关的TF是处在随时变化中的，被发布到/tf
  • Frame

TF2不再提供数学变换函数

def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk
 
    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss
 
    return q

Backlinks