python ros 订阅robot_pose获取机器人位置

 

#!/usr/bin/env python
import rospy
import tf
from tf.transformations import *
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion


def get_pos(data):
    (roll, pitch, yaw) = euler_from_quaternion([data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w])
    rospy.loginfo("current position(x:%f,y:%f,z:%f),theta:%f", data.position.x, data.position.y, data.position.z, yaw)
    #rospy.loginfo("current position(x:%f,y:%f,z:%f)", data.position.x, data.position.y, data.position.z)


def poslistener():
    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('poslistener', anonymous=True)

    rospy.Subscriber("robot_pose", Pose, get_pos)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()


if __name__ == '__main__':
    poslistener()