38 import geometry_msgs.msg
41 if __name__ ==
'__main__':
42 rospy.init_node(
'turtle_tf2_listener')
47 rospy.wait_for_service(
'spawn')
48 spawner = rospy.ServiceProxy(
'spawn', turtlesim.srv.Spawn)
49 turtle_name = rospy.get_param(
'turtle',
'turtle2')
52 turtle_vel = rospy.Publisher(
'%s/cmd_vel' % turtle_name, geometry_msgs.msg.Twist, queue_size=1)
54 rate = rospy.Rate(10.0)
55 while not rospy.is_shutdown():
57 trans = tfBuffer.lookup_transform(turtle_name,
'turtle1', rospy.Time())
58 except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
62 msg = geometry_msgs.msg.Twist()
64 msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
65 msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2)
67 turtle_vel.publish(msg)