tf Tutorial Memo

Platform: Ubuntu 14.04 & ROS Indigo

NOTICE! This is only a rough summary of ROS tf tutorials. For further details, please turn to:

http://wiki.ros.org/tf/Tutorials


Introduction to tf

Running the Demo

roslaunch turtle_tf turtle_tf_demo.launch

What is Happening

This demo is using the tf library to create three coordinate frames: a world frame, a turtle1 frame, and a turtle2 frame. This tutorial uses a tf broadcaster to publish the turtle coordinate frames and a tf listener to compute the difference in the turtle frames and move one turtle to follow the other.

tf Tools

Using view_frames

rosrun tf view_frames
evince frames.pdf

Using rqt_tf_tree

rosrun rqt_tf_tree rqt_tf_tree

Using tf_echo

rosrun tf tf_echo [reference_frame] [target_frame]

rviz and tf

rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

Writing a tf broadcaster (Python)

Create package

mkdir %YOUR_CATKIN_WORKSPACE_HOME%/src
cd %YOUR_CATKIN_WORKSPACE_HOME%/src
catkin_create_pkg learning_tf tf roscpp rospy turtlesim
cd %YOUR_CATKIN_WORKSPACE_HOME%/
catkin_make
source ./devel/setup.bash

How to broadcast transforms

roscd learning_tf
mkdir nodes
gedit nodes/turtle_tf_broadcaster.py
#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

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

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()
chmod +x nodes/turtle_tf_broadcaster.py

Running the broadcaster

<launch>
  <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

  <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
  </node>
  <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" /> 
  </node>

</launch>
roslaunch learning_tf start_demo.launch

Checking the results

rosrun tf tf_echo /world /turtle1

Writing a tf listener (Python)

How to create a tf listener

roscd learning_tf
gedit nodes/turtle_tf_listener.py
#!/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()
chmod +x nodes/turtle_tf_listener.py

Running the listener

rosed learning_tf start_demo.launch
<launch>
  ...
  <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />
</launch>
roslaunch learning_tf start_demo.launch

Checking the results

To see if things work, simply drive around the first turtle using the arrow keys (make sure your terminal window is active, not your simulator window), and you’ll see the second turtle following the first one!


Adding a frame (Python)

How to add a frame

roscd learning_tf
gedit nodes/fixed_tf_broadcaster.py
#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')

import rospy
import tf

if __name__ == '__main__':
    rospy.init_node('fixed_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()
chmod +x nodes/fixed_tf_broadcaster.py

Running the frame broadcaster

rosed learning_tf start_demo.launch
<launch>
  ...
  <node pkg="learning_tf" type="fixed_tf_broadcaster.py" name="broadcaster_fixed" />
</launch>
roslaunch learning_tf start_demo.launch

Broadcasting a moving frame

roscd learning_tf
gedit nodes/_tf_broadcaster.py
#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')

import rospy
import tf
import math

if __name__ == '__main__':
    rospy.init_node('dynamic_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(),
                         "carrot2",
                         "carrot1")
        rate.sleep()
chmod +x nodes/dynamic_tf_broadcaster.py

Running the frame broadcaster

rosed learning_tf start_demo.launch
<launch>
  ...
  <node pkg="learning_tf" type="dynamic_tf_broadcaster.py" name="broadcaster_dynamic" />
</launch>

Learning about tf and time (Python)

rospy.Time(0) or rospy.Time.now()

rosed learning_tf turtle_tf_listener.py
...
#    while not rospy.is_shutdown():
#        try:
#            (trans,rot) = listener.lookupTransform('/turtle2', '/carrot2', rospy.Time(0))
#        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
#            continue

    listener.waitForTransform("/turtle2", "/carrot2", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now()
            listener.waitForTransform("/turtle2", "/carrot2", now, rospy.Duration(4.0))
            (trans,rot) = listener.lookupTransform("/turtle2", "/carrot2", now)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
...

Time travel with tf (Python)

Time travel

rosed learning_tf turtle_tf_listener.py
...
        try:
            now = rospy.Time.now() - rospy.Duration(5.0)
            listener.waitForTransform("/turtle2", "/turtle1", now, rospy.Duration(1.0))
            (trans, rot) = listener.lookupTransform("/turtle2", "/turtle1", now)
        except (tf.Exception, tf.LookupException, tf.ConnectivityException):
...

Is your turtle driving around uncontrollably like in this screenshot? So what is happening?

  • We asked tf, “What was the pose of /turtle1 5 seconds ago, relative to /turtle2 5 seconds ago?”. This means we are controlling the second turtle based on where it was 5 seconds ago as well as where the first turtle was 5 seconds ago.
  • What we really want to ask is, “What was the pose of /turtle1 5 seconds ago, relative to the current position of the /turtle2?”.

Advanced API for lookupTransform

...
        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")
...

Checking the results

roslaunch learning_tf start_demo.launch

Debugging tf problems

  • Finding the tf request
  • Checking the Frames
  • Checking the Timestamp

1人评论了“tf Tutorial Memo”

发表评论

您的电子邮箱地址不会被公开。

此站点使用Akismet来减少垃圾评论。了解我们如何处理您的评论数据