ROStfのお勉強 - 空飛ぶロボットのつくりかた

TFコマンド

フレームツリーグラフ

$ rosrun tf view_frames
$ evince frames.pdf

フレーム関係

$ roslaunch turtle_tf turtle_tf_demo.launch
$ rosrun tf tf_echo world turtle1
At time 1496036356.439
- Translation: [5.544, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

TFの構成

ブロードキャスター：座標情報の登録
リスナー：登録した座標を変換して利用

TFの実装例

ブロードキャスター

#!/usr/bin/env python  
import rospy

# Because of transformations
import tf

import tf2_ros
import geometry_msgs.msg
import turtlesim.msg


def handle_turtle_pose(msg, turtlename):
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "world"
    t.child_frame_id = turtlename
    t.transform.translation.x = msg.x
    t.transform.translation.y = msg.y
    t.transform.translation.z = 0.0
    q = tf.transformations.quaternion_from_euler(0, 0, msg.theta)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]

    br.sendTransform(t)

参考：tf2/Tutorials/Writing a tf2 broadcaster (Python) - ROS Wiki

リスナー

TFで'odom'と'base'フレームの相対位置、回転の取得

import tf.transformations as T
import tf2_ros
import tf2_geometry_msgs                                                                                                                                                                          

tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)

rospy.sleep(2)
rate = rospy.Rate(10.0)

while not rospy.is_shutdown():
    try:
        trans_odom_base = tfBuffer.lookup_transform('odom','base', rospy.Time())
        break
    except (tf2_ros.LookupException,tf2_ros.ConnectivityException,tf2_ros.ExtrapolationException):
        print "tf odom to base not found"
        continue


translation_odom_base = trans_odom_base.transform.translation
rotation_odom_base = trans_odom_base.transform.rotation

position_x = translation_odom_base.x
position_y = translation_odom_base.y
position_z = translation_odom_base.z

quaternion_odom_base = [rotation_odom_base.x, rotation_odom_base.y, rotation_odom_base.z, rotation_odom_base.w]
euler_odom_base = T.euler_from_quaternion(quaternion_odom_base)
yaw_odom_base = euler_odom_base[2]

座標変換

TFで'base'フレームからあるオフセット分だけ移動した'odom'フレーム視点での座標

import tf.transformations as T
import tf2_ros
import tf2_geometry_msgs 

pose_st = PoseStamped()
pose_st.pose = geometry.tuples_to_pose(geometry.pose(x=0.3,y=0.2,z=0.6,ei=0,ej=3.14/2,ek=3.14))
pose_st.header.frame_id = 'base'

trans = tfBuffer.transform(pose_st, 'odom')                                                                                                                                                  

position = [trans.pose.position.x,trans.pose.position.y, trans.pose.position.z]
quat = [trans.pose.orientation.x, trans.pose.orientation.y, trans.pose.orientation.z, trans.pose.orientation.w]
euler = T.euler_from_quaternion(quat)