Vicon轨迹topic转tum格式

rostopic echo /xx >> xx.txt

import numpy as np
import os, sys,re
name="room_dark_06"
basedir="/media/yinjie/2498AF1F98AEEF0C/adataset/gtmid/"+name+'.txt'
txtdir="/media/yinjie/2498AF1F98AEEF0C/adataset/aftgt/"+name+'.txt'
f1=open(basedir,'r')
f2=open(txtdir,"w")
#f2.write('#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]\n')
line=f1.readline()
pflag=0
oflag=0
while line:
    if 'secs' in line and 'nsecs' not in line:
        sec=int(re.findall(r"\-?\d+\.?\d*" ,line)[0])
    if 'nsecs' in line:
        nsec=int(re.findall(r"\-?\d+\.?\d*" ,line)[0])
        #f2.write(str(sec+nsec*0.000000001)+' ')
        f2.write(str('%.9f' %(sec+nsec*0.000000001))+' ')
    if 'position:' in line:
        pflag=1
        oflag=0
    if 'orientation:' in line:
        pflag=0
        oflag=1

    if 'x:' in line and pflag:
        px=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
        #f2.write(str(px)+' ')
    if 'y:' in line and pflag:
        py=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
        #f2.write(str(py)+' ')
    if 'z:' in line and pflag:
        pz=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
        #f2.write(str(pz)+' ')
    if 'w:' in line and oflag:
        ww=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
        f2.write(str(px)+' '+str(py)+' '+str(pz)+' '+str(ww)+' '+str(wx)+' '+str(wy)+' '+str(wz)+'\n')
    if 'x:' in line and oflag:
        wx=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
       # f2.write(str(wx)+' ')
    if 'y:' in line and oflag:
        wy=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
       # f2.write(str(wy)+' ')
    if 'z:' in line and oflag:
        wz=float(re.findall(r"\-?\d+\.?\d*" ,line)[0])
        #f2.write(str(wz)+'\n')
    
    line=f1.readline()


#print(BLH2xyz(L, B, H, rad=False))
#L 经度 B纬度
# (-2144900.7573362007, 4397698.262572753, 4078136.627140711)
# 可以简单判断一下，90°E~179°E区域x为负，北半球区域y为正