座標変換パッケージ "tf"の使い方 - うごくものづくりのために

ROSの座標変換ライブラリ、tfについて勉強しています。ひとまずtutorial(Python)をひと通り終えました。

今までのところでわかったことを記述しておきます。

tfでは、様々な座標軸を定義することで、定義された座標軸から見た別の座標軸の位置を簡単に求めることができます。どのくらい簡単かというと、ベースとなる座標系と、その座標系上で表したい別の座標軸、この２つの座標系の名前を指定するだけです。三角関数とか同次変換行列とか使う必要ありません。ｼﾝｼﾞﾗﾚﾅｰｲ

利用の流れとしては、座標系の登録(Broadcaster) -> 座標系の変換(Listener) という順になります。

座標系の登録(Broadcaster)

座標変換を行うために、まずは座標系の登録を行います。参考のソースコードをチュートリアルページから引っ張ってきました。

ja/tf/Tutorials/Writing a tf broadcaster (Python) - ROS Wiki

  #!/usr/bin/env python  
  import roslib
  roslib.load_manifest('learning_tf')
  import rospy
  
  import tf
  import turtlesim.msg
  
  def handle_turtle_pose(msg, turtlename):
      br = tf.TransformBroadcaster()
      br.sendTransform((msg.x, msg.y, 0),
                       tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                       rospy.Time.now(),
                       turtlename,
                       "world")
  
  if __name__ == '__main__':
      rospy.init_node('turtle_tf_broadcaster')
      turtlename = rospy.get_param('~turtle')
      rospy.Subscriber('/%s/pose' % turtlename,
                       turtlesim.msg.Pose,
                       handle_turtle_pose,
                       turtlename)
      rospy.spin()

この例は、以下のような流れで座標登録を行っています。

turtle*/poseトピックをSubscribeし、"world"座標系から見た"turtle*"の座標(Pose型)を取得
座標系をtfへ登録
turtle*/poseトピックが更新される度、座標系を更新登録

実際に2の座標系登録を行っているのは、以下の部分になります。

      br.sendTransform((msg.x, msg.y, 0),
                       tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                       rospy.Time.now(),
                       turtlename,
                       "world")

TransformBroadcaster.sendTransform()を用いて座標の登録を行います。

tf (Python) — tf 0.1.0 documentation #TransformBroadcaster.sendTransform

sendTransform()メソッドは、5つの引数を取ります。

第1引数は、ベースとなる座標系から見た、登録対象の直交座標です。x,y,zをまとめたタプルとして与えます。
第2引数は、ベースとなる座標系から見た、登録対象の回転です。これは、クオータニオンで与えます。クオータニオンについてはよく理解していませんが、quaterninon_from_euler()で、オイラー角からクオータニオンへの変換が行えるようです。こちらも、タプルとして与えます。
第3引数は、タイムスタンプです。基本的には現在時刻を与えます。(tfは、内部で座標の時間推移を保存しているようです。)
第4引数は、登録したい座標系の、任意の名前を指定します。
第5引数は、ベースとなる座標系の名前を指定します。

座標系の変換(Listener)

登録した座標系を用いて、２つの座標系間での変換を行います。同じように、参考のソースコードをチュートリアルページから引っ張ってきました。

ja/tf/Tutorials/Writing a tf listener (Python) - ROS Wiki

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
 
if __name__ == '__main__':
    rospy.init_node('tf_turtle')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')
 
    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
 
        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)
  
        rate.sleep()

座標変換を行っているのは、以下の部分です。

        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

例外構文がありますが、これは座標変換が失敗したときにその後の処理をしないようにするためのものです。座標変換自体は、

            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))

この一行です。lookupTransform()メソッドを用います。

tf (Python) — tf 0.1.0 documentation #Transformer.lookuptransform

lookupTransform()メソッドは、3つの引数を取ります。

第1引数は、変換のベースとなる座標系の名前を指定します。
第2引数は、変換したい対象の座標系の名前を指定します。
第3引数は、変換したい時間を指定します。基本的には、rospy.Time(0)で良いようですが、過去のデータを元に座標変換したいときはきちんと指定しないとまずいみたいです。

また、戻り値として、変換した座標データが戻ってきます。直交座標をまとめたタプル(x, y, z)と、回転をまとめたタプル(x, y, z, w)（クオータニオン）が戻ってきます。

例外は、例にあげた３つが返ってくるようです。詳しくはドキュメントを見てください。

気になったこと

座標系が登録されてから、実際にそれが座標変換に使えるようになるまで、数msの時間がかかるようです。座標変換に使えるようになるまで待機するメソッドもあるのですが、そもそも数msの遅延があるのが若干不安です…

とりあえず、今わかっている情報はこんなところです。

チュートリアルをひと通りやった感じだと、かなーり便利そう。今後わかった情報があれば随時更新していこうと思います。