发布在world坐标系下的turtle1位姿代码:
#!/usr/bin/env python |
发布者启动代码文件: <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> |
向原本有的三个坐标系当中添加坐标系,例如添加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> |