ros订阅图片python_ros python 订阅robot_pose

#!/usr/bin/env python

import rospy

import tf

import timefrom tf.transformations import *

fromstd_msgs.msg import Stringfromgeometry_msgs.msg import Posefromgeometry_msgs.msg import Quaternion

filenm="/opt/bp/tmp"tms=25tp_x=0.0tp_y=0.0tp_a=0.0i=tms

def get_pos(data):globaltp_xglobaltp_yglobaltp_aglobaliglobaltms

(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+') asf:

f.write(str(data.position.x)+","+str(data.position.y)+",&#