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.launchWhat 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.pdfUsing rqt_tf_tree
rosrun rqt_tf_tree rqt_tf_treeUsing tf_echo
rosrun tf tf_echo [reference_frame] [target_frame]rviz and tf
rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rvizWriting 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.bashHow 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.pyRunning 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.launchChecking the results
rosrun tf tf_echo /world /turtle1Writing 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.pyRunning 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.launchChecking 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.pyRunning 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.launchBroadcasting 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.pyRunning 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.launchDebugging tf problems
- Finding the tf request
- Checking the Frames
- Checking the Timestamp
Thankyou for this terrific post, I am glad I found this site on yahoo.