python逐行读取txt文件里的数据并且赋值给变量

需求:

在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()

猜你喜欢

转载自blog.csdn.net/SAPmatinal/article/details/131741516