【ROS】tfを用いた座標変換

画像元

tfとは

• 座標系管理システム
• ROSでは順運動学の問題にtfを使用している

tfの特徴

1. 時間動機ができる
2. 分散システムが使える
3. rvizとの連携が強力

以下のようなものを作る場合には必須

• 自立移動ロボット
• 複数の関節を持つロボットアーム
• 3次元物体の認識プログラム

$ roslaunch pr2_gazebo pr2_empty_world.launch 

$ cd catkin_ws/src/
$ git clone https://github.com/OTL/ez_utils.git
$ cd ../
$ catkin_make
$ rosrun rviz rviz

表示させるものをいい感じに選択する（下の画像の左メニューを参考に）

そして、TFの「head_plate_frame」と「I_gripper_led_frame」にチェックを入れる

するとこのように、必要なものだけを表示させることができる

相対関係を取得する

• /head_plate_frameが頭
• /I_gripper_led_frameが左手先

tf_echoでこれらのフレーム間の相対位置を調べることができる

$ rosrun tf tf_echo /head_plate_frame /l_gripper_led_frame
At time 817.691
- Translation: [0.756, 0.188, -0.271]
- Rotation: in Quaternion [-0.002, -0.391, 0.000, 0.920]
            in RPY (radian) [-0.005, -0.803, 0.003]
            in RPY (degree) [-0.286, -46.031, 0.167]

そして、look_hand.pyを作成する

#!/usr/bin/env python
import rospy
import tf2_ros
from ez_utils.ez_joints import EzJoints

if __name__ == "__main__":
    rospy.init_node( 'pr2_look_left_hand' )
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener( tf_buffer )
    
    # make EzJoints instance and this topic to Publish
    head = EzJoints( '/head_traj_controller' )
    left_arm = EzJoints( '/l_arm_controller' )
    
    yaw_angle = 0.0 # init
    pitch_angle = 0.0 # init
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            trans = tf_buffer.lookup_transform( 'head_plate_frame', 'l_gripper_led_frame', rospy.Time() )
            
            # decide angle
            yaw_angle = trans.transform.translation.y / 1
            pitch_angle = -trans.transform.translation.z / 1
            
            print trans.transform.translation
            head.set_positions( [yaw_angle, pitch_angle] ) # Send angle
        except ( tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException ):
            rospy.logwarn( 'tf not found' )
        rate.sleep()

• 4行目：EzJointsを使うとPR2の関節を簡単に動かすことができる
• 8、9行目：TFのデータを蓄えるバッファを作成し、引数にしてTransformListenerを作り、tf_listenerに代入する。このtf_listenerを使用してTFの機能にアクセスする
• 20行目：指定したフレーム間の相対関係を取得する。rospy.Time()は取得可能な最新の時間。また、この関数lookup_transformは失敗すると例外を投げるため、try, exceptで例外処理をする

$ chmod 755 look_hand.py 
$ rosrun ros_start look_hand.py

$ rosrun rqt_ez_publisher rqt_ez_publisher 

1. topicを選択
2. スライダを増やすために＋をクリックする

これで上の画像のようになる。

この時、PR2このように顔が手の方向を向いていることが分かる。