需求：

在txt文档里写了一串机械臂运动的点，里面包含了xyz位置和四元数组，有n行，每行n个数据，需要将这些数据用rostopic发布出来，让机械臂依次运行这些点

代码：

#!/usr/bin/env python  
import roslib
#roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
import sys	
import time
import json
import string
from geometry_msgs.msg import PoseStamped
i = 0

def txt_read ():
    test_pose = PoseStamped()
    global i
    f = open("SamplePoses.txt","r") 
    lines = f.readlines() 
    pose = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]    
    for line in lines:
        pose = line.split()
        
        print(pose[0])
        
        test_pose.pose.position.x = float(pose[0])
        test_pose.pose.position.y = float(pose[1])
        test_pose.pose.position.z = float(pose[2])
        test_pose.pose.orientation.x = float(pose[3])
        test_pose.pose.orientation.y = float(pose[4])
        test_pose.pose.orientation.z = float(pose[5])
        test_pose.pose.orientation.w = float(pose[6])
        rate = rospy.Rate(1) 
        
    
        rospy.loginfo(test_pose)
        pose_pub.publish(test_pose)
        rate.sleep()
            

        
 
    f.close()

if __name__ == '__main__':
    rospy.init_node('read_txt_pose', anonymous=True)
    pose_pub = rospy.Publisher('txt_pose', PoseStamped, queue_size=10)
    txt_read()