get_robot_position.py 文件源码

def get_robot_current_x_y_w(self):
         t = self.tf_listener.getLatestCommonTime(self.base_frame, self.odom_frame)
         position, quaternion = self.tf_listener.lookupTransform(self.odom_frame , self.base_frame,t)

         roll,pitch,yaw = tf.transformations.euler_from_quaternion(quaternion)
#         print 'x = ' ,position[0] ,'y = ', position[1],'yaw =', yaw
         return position[0],position[1],yaw

#????????X?Y?