需求：
在txt文档里写了一串机械臂运动的点，里面包含了xyz位置和四元数组，有n行，每行n个数据，需要将这些数据用rostopic发布出来，让机械臂依次运行这些点
代码：
代码语言：javascript复制#!/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()
