import pandas as pd
from readpos import readpos
import math
from geopy.distance import geodesic


LG69T_path = "./llh.csv"
TC_Nav_path = "/Users/wakanda_shaw/Desktop/TC_Nav_Zjut/TC_NAV_Zjut_post/data/630_3/llh.pos"

D2R = math.pi/180

# 从.pos文件得到python lsit 格式数据
llh = readpos(TC_Nav_path)

LG69T_data = pd.read_csv(LG69T_path,names=['tow','lat','lon'],dtype=float)
# list 转换到 dataframe
TC_Nav_data = pd.DataFrame(llh,columns=['tow','lat','lon'],dtype=float)
for i in range(len(TC_Nav_data)):
    tow = TC_Nav_data.iloc[i,0]
    TC_lat = TC_Nav_data.iloc[i,1]
    TC_lon = TC_Nav_data.iloc[i,2]
    LG_lat = LG69T_data.loc[LG69T_data['tow']==tow,['lat']]
    LG_lat = LG_lat.iloc[0,0]
    LG_lon = LG69T_data.loc[LG69T_data['tow']==tow,['lon']]
    LG_lon = LG_lon.iloc[0,0]

    distance = geodesic((TC_lat,TC_lon),(LG_lat,LG_lon)).m
    print("距离：{:.3f}m".format(distance))