ros python 订阅robot_pose

 

#!/usr/bin/env python
import rospy
import tf
import time
from tf.transformations import *
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion
filenm="/opt/bp/tmp"
tms=25
tp_x=0.0
tp_y=0.0
tp_a=0.0
i=tms
def get_pos(data):
    global tp_x
    global tp_y
    global tp_a
    global i
    global tms
    (roll, pitch, yaw) = euler_from_quaternion([data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w])
    if(i==0):
        rospy.loginfo("current position(x:%f,y:%f),theta:%f", data.position.x, data.position.y, yaw)
        if(tp_x==round(data.position.x,6)):
            if(tp_y==round(data.position.y,6)):
                if(tp_a==round(yaw,6)):
                    rospy.loginfo("still!")
                else:
                    with open(filenm, 'w+') as f:
                        f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))
                    rospy.loginfo("write!")
            else:
                with open(filenm, 'w+') as f:
                    f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))
                rospy.loginfo("write!")
        else:
            with open(filenm, 'w+') as f:
                f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))
            rospy.loginfo("write!")
        i=tms
    tp_x=round(data.position.x,6)
    tp_y=round(data.position.y,6)
    tp_a=round(yaw,6)
    i=i-1
    #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()