座標変換のお勉強
やりたいこと
ロボットの3次元座標の考え方を理解して、制御に役立てる
ROSのtfの理解
座標変換教科書
tf教科書
tfを簡単に体験する
- roslaunch turtle_tf turtle_tf_demo.launch
- rosnode list
- node チェック
- rosrun tf view_frames
- tf チェック
- rosrun tf tf_echo world turtle1
- rosrun tf tf_echo turtle1 turtle2
- 座標変換チェック
tfの座標情報処理
- ブロードキャスター: 座標情報の登録
- リスナー: 座標変換&利用
ブロードキャスター
br = tf.TransformBroadcaster() br.sendTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world")
static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
リスナー
listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
/turtle2を/turtle1座標系に変換する
#!/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('turtle_tf_listener') 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,queue_size=1) 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()
フレームの追加
#!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import tf import math if __name__ == '__main__': rospy.init_node('my_tf_broadcaster') br = tf.TransformBroadcaster() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): t = rospy.Time.now().to_sec() * math.pi br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "carrot1", "turtle1") rate.sleep()
過去のtfの時間を利用
try: now = rospy.Time.now() past = now - rospy.Duration(5.0) listener.waitForTransformFull("/turtle2", now, "/turtle1", past, "/world", rospy.Duration(1.0)) (trans, rot) = listener.lookupTransformFull("/turtle2", now, "/turtle1", past, "/world")
所感
tfまぁ便利。時間の考え方がわかったようなわからないような。。。
世界で一番簡単なtfの使い方 - MyEnigmaの記事から抜粋
- ros::time::now()を使ってLookupTransformしてはいけない。
tfはtf_broadcasterによって登録されたtf間の間の時間しか、座標変換できません。
ですのでros::time::now()で時間指定すると、座標変換できません。
つまり、基本的にtfは過去の時間を指定してLookupTransformする必要があります。
そんな時は下記のリンクの通り、tfが登録されるまで時間を待つようにすると、
きちんと座標変換してくれます。