空飛ぶロボットのつくりかた

ロボットをつくるために必要な技術をまとめます。ロボットの未来についても考えたりします。

座標変換のお勉強

やりたいこと

  • ロボットの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
    • 座標変換チェック

f:id:robonchu:20180304151903p:plain

tfの座標情報処理

ブロードキャスター

python

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

c++

  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の記事から抜粋

  1. ros::time::now()を使ってLookupTransformしてはいけない。

tfはtf_broadcasterによって登録されたtf間の間の時間しか、座標変換できません。

ですのでros::time::now()で時間指定すると、座標変換できません。

つまり、基本的にtfは過去の時間を指定してLookupTransformする必要があります。

そんな時は下記のリンクの通り、tfが登録されるまで時間を待つようにすると、

きちんと座標変換してくれます。

ja/tf/Tutorials/tf and Time (C++) - ROS Wiki