ROS之tf空间坐标广播与监听

坐标广播

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib
roslib.load_manifest('test')
import rospy
import tf
import turtlesim.msg


def turtle_pose(msg, turtlename):#回调函数
    print msg.x, msg.y
    br = tf.TransformBroadcaster()
    # 将位置信息打包广播到world坐标系
    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('tf_broadcaster')
    turtlename='turtle1'
    #订阅turtle1的位置发布话题
    rospy.Subscriber('/%s/pose' % turtlename, turtlesim.msg.Pose, turtle_pose, turtlename)
    rospy.spin()

坐标监听

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib

roslib.load_manifest('test')
import rospy
import tf
import math
import geometry_msgs.msg
import turtlesim.srv
import turtlesim.msg


def 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('tf_listener')
    listener = tf.TransformListener()
    rospy.wait_for_service('spawn')
    print 'GET'
    spawner = rospy.ServiceProxy('/spawn', turtlesim.srv.Spawn)
    response = spawner(2, 0, 1, 'turtle6')
    # turtle6产生后也必须监听位置，回调向world广播，才能加入tf体系。
    rospy.Subscriber('/turtle6/pose', turtlesim.msg.Pose, turtle_pose, 'turtle6')
    print response
    turtle_vel = rospy.Publisher('turtle6/cmd_vel', geometry_msgs.msg.Twist, queue_size=10)
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            listener.waitForTransform('/turtle1', '/turtle6', rospy.Time(0), rospy.Duration(5), None)
            # 查询以turtle6为原点的turtle1的位置坐标，龟头方向为+x方向
            (trans, rot) = listener.lookupTransform('/turtle6', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
            print e
            continue
        print trans[0], trans[1]

        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()