| Note: This tutorial assumes you have completed the writing a tf broadcaster tutorial (Python) (C++). |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
tf listenerを書く (Python)
Description: このチュートリアルでは、どのようにtfを使って座標系フレームの変化を取得するのかを学びますTutorial Level: BEGINNER
Next Tutorial: Adding a frame (Python)
Show EOL distros:
Contents
前回のチュートリアルでは、tf broadcasterをtfにturtleのポーズを配信するために作りました。このチュートリアルでは、tfを使い始めるためにtf listenerを作っていきます。
どのようにtf listenerをつくるか
まず、ソースコードを作りましょう。まず、前回のチュートリアルで作ったパッケージに移動します:
$ roscd learning_tf
コード
エディタを立ち上げ、以下のソースコードをnodes/turtle_tf_listener.pyとして、コピー＆ペーストして保存してください。
1 #!/usr/bin/env python
2 import roslib
3 roslib.load_manifest('learning_tf')
4 import rospy
5 import math
6 import tf
7 import turtlesim.msg
8 import turtlesim.srv
9
10 if __name__ == '__main__':
11 rospy.init_node('tf_turtle')
12
13 listener = tf.TransformListener()
14
15 rospy.wait_for_service('spawn')
16 spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
17 spawner(4, 2, 0, 'turtle2')
18
19 turtle_vel = rospy.Publisher('turtle2/command_velocity', turtlesim.msg.Velocity)
20
21 rate = rospy.Rate(10.0)
22 while not rospy.is_shutdown():
23 try:
24 (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
25 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
26 continue
27
28 angular = 4 * math.atan2(trans[1], trans[0])
29 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
30 turtle_vel.publish(turtlesim.msg.Velocity(linear, angular))
31
32 rate.sleep()
1 #!/usr/bin/env python
2 import roslib
3 roslib.load_manifest('learning_tf')
4 import rospy
5 import math
6 import tf
7 import geometry_msgs.msg
8 import turtlesim.srv
9
10 if __name__ == '__main__':
11 rospy.init_node('tf_turtle')
12
13 listener = tf.TransformListener()
14
15 rospy.wait_for_service('spawn')
16 spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
17 spawner(4, 2, 0, 'turtle2')
18
19 turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist)
20
21 rate = rospy.Rate(10.0)
22 while not rospy.is_shutdown():
23 try:
24 (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
25 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
26 continue
27
28 angular = 4 * math.atan2(trans[1], trans[0])
29 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
30 cmd = geometry_msgs.msg.Twist()
31 cmd.linear.x = linear
32 cmd.angular.z = angular
33 turtle_vel.publish(cmd)
34
35 rate.sleep()
nodeを実行可能にするのを忘れないようにしてください:
chmod +x nodes/turtle_tf_listener.py
コード解説
さて、tfにturtleのポーズを配信している部分に関係しているコードを見てみましょう。
6 import tf
tfパッケージは、transforms（座標変換）を簡単に受け取れるようにtf.TransformListener の実行を提供してくれます.
13 listener = tf.TransformListener()
ここで、TransformListenerのオブジェクトを作っています。一旦listenerが作成されると、ケーブルを通してtfのtransformationsを受け取り始め、１０秒間それらをバッファにためます。
23 try:
24 (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
25 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
26 continue
27
28 angular = 4 * math.atan2(trans[1], trans[0])
29 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
30 turtle_vel.publish(turtlesim.msg.Velocity(linear, angular))
ここで、実際の作業を行っています。特定のtransformationについてlistenerに尋ねます。 ４つの引数を見てみましょう:
- まず、取得したいtransform(変換)のfromにあたるフレームを指定します。
- 取得したいtransform(変換)のtoにあたるフレームを指定します。
transformがしたい時間を指定します。ros::Time(0)を指定すると最新のtransformを取得できます。
例外を取得するためにtry-catchに書かれているのがこれですべてです。
listenerを実行する
テキストエディタで start_demo.launchのlaunchファイルを開き、以下の行を足してください:
<launch>
...
<node pkg="learning_tf" type="turtle_tf_listener.py"
name="listener" />
</launch>まず、前回のチュートリアルのlaunchファイルは止めていることを確認してください(ctrl-cを使ってください)今、完全なturtleのデモを始める準備ができました。:
$ roslaunch learning_tf start_demo.launch
2匹の亀が動いているturtle simが見えていますね。
結果を確認する
tfのbroadcasterがうまく動作しているかを確かめるには、矢印キーを用いて１匹目の亀を操作してみればよいだけです。（シミュレータではなくターミナルのウィンドウがアクティブになるようにしてください）すると、２匹目の亀が１匹目の亀についてくるのを確認できます。
turtlesimが立ち上がったときに以下のようなものが表示されているかもしれません:
[ERROR] [1412840409.742716292]: "turtle2" passed to lookupTransform argument target_frame does not exist.
これは、turtlesimを立ち上げ、tfのフレームをブロードキャストし始めるのに少し時間がかかっていることで生じていて、この時間はlistenerがturtle2のメッセージが届く前に、transform(変換)を計算しようとしているからです。
さて、これでさらに次のチュートリアルへ進むことができます。次は、どのようにフレームを追加するかを学習します。(Python) (C++)
