python ros 订阅robot_pose获取机器人位置，,#!/usr/bin

python ros 订阅robot_pose获取机器人位置，,#!/usr/bin

#!/usr/bin/env pythonimport rospyimport tffrom tf.transformations import *from std_msgs.msg import Stringfrom geometry_msgs.msg import Posefrom geometry_msgs.msg import Quaterniondef 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()

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