tf编程学习（python）

发布在world坐标系下的turtle1位姿代码：

#!/usr/bin/env python   import roslib roslib.load_manifest('tf_demo') import rospy   import tf import turtlesim.msg   def handle_turtle_pose(msg, turtlename):      br = tf.TransformBroadcaster()     br.sendTransform((msg.x, msg.y, 0), #the translation of the transformtion as a tuple (x, y, z)                      tf.transformations.quaternion_from_euler(0, 0, msg.theta),                                                  #the rotation of the transformation as a tuple (x, y, z, w)                      rospy.Time.now(), #the time of the transformation, as a rospy.Time()                      turtlename, #child frame in tf, string                      "world") #parent frame in tf, string   if __name__ == '__main__':     rospy.init_node('turtle_tf_broadcaster')     turtlename = rospy.get_param('~turtle')                        #takes parameter "turtle", which specifies a turtle name, e.g. "turtle1" or "turtle2"      rospy.Subscriber('/%s/pose' % turtlename, # subscribe to turtlename's /pose topic                      turtlesim.msg.Pose,      # Pose message data structure                      handle_turtle_pose,      # callback function,                      turtlename)              # argument for callback function     rospy.spin()

发布者启动代码文件：   <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="tf_demo" type="broadcaster.py" respawn="false" output="screen" >       <param name="turtle" type="string" value="turtle1" />     </node>     <node name="turtle2_tf_broadcaster" pkg="tf_demo" type="broadcaster.py" respawn="false" output="screen" >       <param name="turtle" type="string" value="turtle2" />      </node>     </launch>

turtle2监听turtle1在世界坐标系下的位姿，并计算在本坐标系下距离turtle1的距离，并执行运行距离命令：

#!/usr/bin/env python   import roslib roslib.load_manifest('tf_demo') import rospy import math import tf import geometry_msgs.msg import turtlesim.srv   if __name__ == '__main__':     rospy.init_node('tf_turtle')       #next three lines: Create another turtle onto the stage     rospy.wait_for_service('spawn')     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)      #turtlesim has a service named: Spawn     spawner(4, 4, 0, 'turtle2')  #(x,y,theta,name)       #Publisher for the turtle2's movement     turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)       listener = tf.TransformListener() #tf transforListener          rate = rospy.Rate(10.0)     while not rospy.is_shutdown():         #All this is wrapped in a try-except block to catch possible exceptions:         try:             (trans,rot) = listener.lookupTransform('/turtle2', '/carrot1', 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() #declare an Twist object         cmd.linear.x = linear         cmd.angular.z = angular         turtle_vel.publish(cmd)  # publish the cmd out           rate.sleep()

启动监听、计算距离，并执行文件：   <launch>     ...     <node pkg="tf_demo" type="listener.py" name="listener" />   </launch>

向原本有的三个坐标系当中添加坐标系，例如添加cattor到turtle1下，作为子坐标系：

添加的静态坐标系	添加的动态坐标系
#!/usr/bin/env python   import roslib roslib.load_manifest('tf_demo')   import rospy import tf   if __name__ == '__main__':     rospy.init_node('my_tf_broadcaster')     br = tf.TransformBroadcaster()     rate = rospy.Rate(10.0)     while not rospy.is_shutdown():         br.sendTransform((0.0, 2.0, 0.0), #The carrot1 frame is 2 meters offset from the turtle1 frame                          (0.0, 0.0, 0.0, 1.0), #no rotation                          rospy.Time.now(),                          "carrot1",                          "turtle1")         rate.sleep()	#!/usr/bin/env python   import roslib roslib.load_manifest('tf_demo')   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(),                           "carrot1",                           "turtle1")              rate.sleep()
启动相对turtle1增添的固定坐标系文件： <launch>     ...     <node pkg="tf_demo" type="add_fixed_frame.py"           name="broadcaster_fixed_frame" />   </launch>	启动相对turtle1增添的动态坐标系文件： <launch>     ...     <node pkg="tf_demo" type="add_dynamic_frame.py"           name="broadcaster_dynamic" />   </launch>