Created
March 25, 2024 15:06
-
-
Save dj1711572002/38d776e39a83163ee2c8855a5dc4f328 to your computer and use it in GitHub Desktop.
Python SkiTurn Vector Graph RTK MovingBase Ski Turn Measuremnt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| # This is a sample Python script. | |
| # 機能：STA24 SystemのログデータをCSV形式で読み込んで、データの検査と修復をおこなって各デバイス間の同期を整頓する | |
| # iTOW抜け：ループで回して、発見したら補間した行を作って、DFの再下端に追加していく、全部ループが終了したらiTOWをSORTして整頓 | |
| # graph00では、1個のグラフだったので、01では、全体を網羅するグラフ散布図ベクトル矢印を作る | |
| # BootsOnのデータも読み込む | |
| import math | |
| import numpy | |
| import pandas as pd | |
| import matplotlib.pyplot as plt | |
| import matplotlib.cm as cm | |
| import tkinter | |
| import tkinter.filedialog as FileDialog | |
| import os | |
| import sys | |
| import numpy as np | |
| import pickle | |
| import keyboard | |
| def is_num(x): # 数値か文字かを判別する関数 | |
| try: | |
| float(x) | |
| except ValueError: | |
| return False | |
| else: | |
| return True | |
| def additions(df): | |
| # ---headMot Heading 角度補正 | |
| df["headMot360"] = df.headMot_B.multiply(0.00001) | |
| df["headMot360"] = np.where(df["headMot360"] > 360, df["headMot360"] - 360, df["headMot360"]) | |
| print(df.headMot360) | |
| # ---Heading角 補正 | |
| df["Heading360"] = df.relPosHeading_R.multiply(0.00001) + 180 | |
| df["Heading360"] = np.where(df["Heading360"] > 360, df["Heading360"] - 360, df["Heading360"]) | |
| print(df.Heading360) | |
| # ---headMot移動平均３個MA3 | |
| df["headMotMA3"] = df["headMot360"].rolling(3).mean() | |
| print(df.headMot360, df.headMotMA3) | |
| # ---差分 diff() | |
| df["headMotMA3diff"] = df["headMotMA3"].diff() | |
| # ---差分ゼロ点を１してマーク | |
| zerocnt = 0 | |
| zeroflag = 0 | |
| zeroflag_1 = 0 | |
| for i in range(2, len(df.index)): | |
| if df.loc[i, "headMotMA3diff"] * df.loc[i - 1, "headMotMA3diff"] < 0 and df.loc[ | |
| i - 1, "diffzero"] == 0 and i - zeroflag > 4: | |
| zeroflag = i | |
| df.loc[i, "diffzero"] = zerocnt | |
| zerocnt += 1 | |
| else: | |
| df.loc[i, "diffzero"] = 0 | |
| # 差分ゼロ点の座標 | |
| # ||||||||||||||||||||||||||||def readMBFile|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||| | |
| def readMBFile(filename, basename, head0): | |
| # print(result.gSpeed_B) | |
| df = pd.read_csv(filename, names=head0, low_memory=True) # CSV Read dfに読み込まれる head0の列名がつく | |
| print("PATH:", filename, "READ:", basename) # basenameは、パスをのぞいたファイル名 ファイル名ubx とbnoで処理分ける | |
| pd.set_option('display.max_columns', 120) # 120列まで表示 | |
| df_Base = df[df.iloc[:, 0] == 'B'] # id列[:,0]が'B'ならばdf_Baseとして抽出 | |
| df_Base = df_Base.fillna(0) # NANをゼロに変換 | |
| df_Base = df_Base.reset_index() # index番号リセット | |
| df_Base = df_Base.add_suffix('_B', axis=1) # head0の名称末尾に'_B'をつけてBaseのデータ列命名 | |
| df_Base = df_Base.drop(columns='index_B') # index_Bは不要なので消去 | |
| df_Base = df_Base.drop(columns='id_B') # id_Bも不要なので消去 数値以外のデータ除去 | |
| headB = df_Base.columns # 完成したヘッダ文字LISTを保持 | |
| print("headB=", headB) | |
| k = [] # itow エラー list k[]宣言 | |
| # ------------df_Baseをlistへ読み出し----------- | |
| list_data = [] # df_Baseの値をlistに読み込む DFでの処理よりlistで処理したほうが速い | |
| list_data = df_Base.to_numpy().tolist() # list_dataに値を代入 | |
| # print("list_data=",list_data) | |
| print("list_data size=", [len(v) for v in list_data]) # | |
| # ----------------------------------------- | |
| lbase = len(df_Base) # 全行数 | |
| # lastrow = len(df_Base) | |
| for i in range(1, lbase): # 行数ループ 整数iでカウント | |
| # print("range i=",i) | |
| if i > 1: # i-1対策 | |
| itow0 = (int)(df_Base.iloc[i, 0]) # 整数で番地指定しているのでiloc使用 | |
| itow_1 = (int)(df_Base.iloc[i - 1, 0]) # 1個手前のitow_1 | |
| # print(itow0-itow_1) | |
| c = [] # 補間計算データ行リスト | |
| k = 0 | |
| if itow0 - itow_1 > 120: # itow差が120以上あったらエラー | |
| itowadd = itow_1 + 120 | |
| print("itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1) | |
| for j in range(0, 55): # 列をループして平均値計算 | |
| ss = df_Base.iloc[i, j] | |
| # if is_num(ss): | |
| a = float(df_Base.iloc[i, j]) | |
| b = float(df_Base.iloc[i - 1, j]) | |
| # print(j,a,b) | |
| c.append(int((a + b) / 2)) # 補間行リストに平均値代入 | |
| # add c to list_data | |
| list_data.append(c) # 行代入が完了したcをlist_dataに追加 元のlistデータの最高単に追加される | |
| # print("c len=",len(c)) | |
| # print("Err made c=",c) | |
| # print("list_data size=", [len(v) for v in list_data]) | |
| # ================All list_data をdf_Base2にしてitow SORTして修正完成===================================== | |
| df_Base2 = pd.DataFrame(list_data, columns=headB) # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成 | |
| # 数値以外のデータが含まれている行の抽出 | |
| non_numeric_rows = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1) | |
| # print("nonnumericRow=",non_numeric_rows) | |
| # 数値以外のデータが含まれている列の抽出 | |
| non_numeric_cols = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all()) | |
| # print("nonnumericCol=",non_numeric_cols) | |
| # df_Rover2 = df_Base2.fillna(0) | |
| # df_Base2['iTOWB']=df_Base2['iTOW_B'].astype('int') | |
| # -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する-------------------------------------- | |
| df_Base2['iTOW_B'] = df_Base2['iTOW_B'].astype('str') | |
| df_Base2['iTOW_B'] = df_Base2['iTOW_B'].str.replace('-', '').str.replace(':', '').str.replace(' ', '') | |
| df_Base2['iTOW_B'] = df_Base2['iTOW_B'].astype(int) | |
| # ---------------------------------------------------------------------------------------------------- | |
| print(df_Base2) | |
| df_Base2.info() | |
| # pd.set_option('display.max_rows', None) | |
| df_Base2 = df_Base2.sort_values("iTOW_B", ascending=True) # SORT iTOWBでSORT | |
| print("Sorted df_Base2=", df_Base2) | |
| # ----------CHECK SORT-------------- | |
| itowerr = 0 | |
| for i in range(1, len(df_Base2) - 1): | |
| itow0 = df_Base2.iloc[i + 1, 0] | |
| itow_1 = df_Base2.iloc[i, 0] | |
| if ((itow0 - itow_1) != 120): | |
| itowerr += 1 | |
| print("***********i=", i, itow0, "itowErr=", itowerr) | |
| # ============================BASE ITOW 補修終了=============================== | |
| # ========================ROVER iTOW処理====================================== | |
| # df_Base2.info() | |
| # df_Rover = df[df.iloc[:, 0] == 'R'] | |
| # df_Rover.reset_index(drop=True, inplace=True) | |
| # df_Rover = df_Rover.add_suffix('_R', axis=1) | |
| df_Rover = df[df.iloc[:, 0] == 'R'] # id列[:,0]が'B'ならばdf_Baseとして抽出 | |
| df_Rover = df_Rover.fillna(0) # NANをゼロに変換 | |
| df_Rover = df_Rover.reset_index() # index番号リセット | |
| df_Rover = df_Rover.add_suffix('_R', axis=1) # head0の名称末尾に'_B'をつけてBaseのデータ列命名 | |
| df_Rover = df_Rover.drop(columns='index_R') # index_Bは不要なので消去 | |
| df_Rover = df_Rover.drop(columns='id_R') # id_Bも不要なので消去 数値以外のデータ除去 | |
| headR = df_Rover.columns # 完成したヘッダ文字LISTを保持 | |
| print("headR=", headR) | |
| print(df_Rover) | |
| kr = [] # itow エラー list kr[]宣言 | |
| # ------------df_Roverをlistへ読み出し----------- | |
| list_datar = [] # df_Baseの値をlistに読み込む DFでの処理よりlistで処理したほうが速い | |
| list_datar = df_Rover.to_numpy().tolist() # list_datarに値を代入 | |
| # print("list_datar=",list_datar) | |
| print("list_datar size=", [len(v) for v in list_datar]) # | |
| # ----------------------------------------- | |
| lbaser = len(df_Rover) # 全行 | |
| for i in range(1, lbaser): # 行数ループ 整数iでカウント | |
| # print("range i=",i) | |
| if i > 1: # i-1対策 | |
| itow0 = (int)(df_Rover.iloc[i, 0]) # 整数で番地指定しているのでiloc使用 | |
| itow_1 = (int)(df_Rover.iloc[i - 1, 0]) # 1個手前のitow_1 | |
| # print(itow0-itow_1) | |
| cr = [] # 補間計算データ行リスト | |
| kr = 0 | |
| if itow0 - itow_1 > 120: # itow差が120以上あったらエラー | |
| itowadd = itow_1 + 120 | |
| print("itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1) | |
| for j in range(0, 55): # 列をループして平均値計算 | |
| ss = df_Base.iloc[i, j] | |
| # if is_num(ss): | |
| a = float(df_Base.iloc[i, j]) | |
| b = float(df_Base.iloc[i - 1, j]) | |
| # print(j,a,b) | |
| cr.append(int((a + b) / 2)) # 補間行リストに平均値代入 | |
| # add c to list_data | |
| list_datar.append(cr) # 行代入が完了したcをlist_dataに追加 元のlistデータの最高単に追加される | |
| # print("cr len=",len(cr)) | |
| # print("Err made cr=",cr) | |
| # print("list_datar size=", [len(v) for v in list_datar]) | |
| # ================All list_data をdf_Base2にしてitow SORTして修正完成===================================== | |
| df_Rover2 = pd.DataFrame(list_datar, columns=headR) # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成 | |
| # 数値以外のデータが含まれている行の抽出 | |
| non_numeric_rows = df_Rover2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1) | |
| print("nonnumericRow=", non_numeric_rows) | |
| # 数値以外のデータが含まれている列の抽出 | |
| non_numeric_cols = df_Rover2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all()) | |
| print("nonnumericCol=", non_numeric_cols) | |
| df_Rover2 = df_Rover2.fillna(0) | |
| # df_Rover2['iTOW_R']=df_Rover2['iTOW_R'].astype('int') | |
| # -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する-------------------------------------- | |
| df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].astype('str') | |
| df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].str.replace('-', '').str.replace(':', '').str.replace(' ', '') | |
| df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].astype(int) | |
| # ---------------------------------------------------------------------------------------------------- | |
| print(df_Rover2) | |
| df_Rover2.info() | |
| # pd.set_option('display.max_rows', None) | |
| df_Rover2 = df_Rover2.sort_values("iTOW_R", ascending=True) # SORT iTOW_RでSORT | |
| print("Sorted df_Rover2=", df_Rover2) | |
| # ----------CHECK SORT-------------- | |
| itowerr = 0 | |
| for i in range(1, len(df_Rover2) - 1): | |
| itow0 = df_Rover2.iloc[i + 1, 0] | |
| itow_1 = df_Rover2.iloc[i, 0] | |
| if ((itow0 - itow_1) != 120): | |
| itowerr += 1 | |
| print("***********i=", i, itow0, "itowErr=", itowerr, "sa=", itow0 - itow_1) | |
| # ============================Rover ITOW 補修終了=============================== | |
| # df_Base2とdf_Rover2の長さを短いほうにあわせる | |
| # drop(range(削除したい最初の行, 削除したい最後の行+1), axis=”rows“) | |
| print("df_Base2 tail=", df_Base2.tail()) | |
| print("df_Rover2 tail=", df_Rover2.tail()) | |
| Base2len = len(df_Base2) | |
| Rover2len = len(df_Rover2) | |
| if Base2len > Rover2len: | |
| last_row = df_Base2.shape[0] - 1 | |
| upper_row = last_row - (len(df_Base2) - len(df_Rover2)) | |
| df_Base2 = df_Base2.drop(range(upper_row, last_row), axis="rows") | |
| elif len(df_Base2) < len(df_Rover2): | |
| last_row = df_Rover2.shape[0] - 1 | |
| upper_row = last_row - (len(df_Rover2) - len(df_Base2)) | |
| df_Rover2 = df_Rover2.drop(range(upper_row, last_row), axis="rows") | |
| df_Base2.reset_index() | |
| df_Rover2.reset_index() | |
| print("df_Base2 len=", len(df_Base2), "df_Rover2 len=", len(df_Rover2)) | |
| print("--------------------CONCATでfd_Base2とfd_Rover2を横に合体BR_ファイルCSV保存---------------------") | |
| result = pd.concat([df_Base2, df_Rover2], axis=1) | |
| result = result.fillna(0) # NANをゼロに変換 | |
| result = result.reset_index(drop=True) | |
| result = result.drop(result.index[-1]) | |
| print("relposHeading Max=", result["relPosHeading_B"].max(), "min=", result["relPosHeading_B"].min()) | |
| additions(result) | |
| result.to_csv(r'C:/RTK_Log/BR_' + basename) | |
| print(result) | |
| return result | |
| def plotheadmot(result, basename): | |
| # =========================================================================================== | |
| # ===================PLOT headMot Heading diff NutralPoint=================================== | |
| # ========================================================================================= | |
| # １つのfigにたくさんグラフを作る | |
| # fig, ax = plt.subplots(2, 2, figsize=(12, 10)) | |
| fig, ax = plt.subplots(figsize=(12, 10)) | |
| fig.suptitle(basename) | |
| # graph1 | |
| # ax[0,0].plot(result.index,result.gSpeed_B,"green","gSpeed_B")# Graph1 gSpeed | |
| # graph2 | |
| # ax2= ax.twinx() | |
| # ax[0, 0].plot(result.index, result.headMot360, "red", "headMot360")#Graph2 headMot | |
| # ax[0, 0].plot(result.index, result.Heading360, "blue", "Heading360")#Graph2 Heading | |
| ax.plot(result.index, result.headMot360, "red", label="headMot360") # Graph2 headMot | |
| ax.plot(result.index, result.Heading360, "blue", label="Heading360") # Graph2 Heading | |
| # ax[0,1].plot(result.index,result.headMotMA3,"black","headMotMA3")#Graph3 headMotMA3 | |
| # ax2 = ax[0,0].twinx()#2軸作成 | |
| ax2 = ax.twinx() # 2軸作成 | |
| ax2.plot(result.index, result.headMotMA3diff, "black", label="headMotMA3diff") # Graph3 headMotMA3diff | |
| ax2.grid() | |
| # 縦線 | |
| for i in range(2, len(result.index)): | |
| if result.loc[i, "headMotMA3diff"] * result.loc[i - 1, "headMotMA3diff"] < 0: | |
| ax2.axvline(i, color="aqua") | |
| result.diffzero = 1 | |
| else: | |
| result.diffzero = 0 | |
| # graph3 | |
| result['relPosN_B'] = result['relPosN_B'].astype('int') | |
| result['relPosE_B'] = result['relPosE_B'].astype('int') | |
| # print(result.relPosE_B) | |
| # print(result.relPosN_B) | |
| # ax[1, 0].scatter(result.relPosE_B, result.relPosN_B)#Graph4 BaseXY | |
| ax.legend(loc="upper left") | |
| ax2.legend(loc="center left") | |
| # ax2.legend() | |
| plt.show() | |
| # ----------------PLOT VECTOR----------------------------------------------------- | |
| def resultrange(result): | |
| # headmotMA3のMaxMin | |
| hmax = result["headMotMA3"].max() | |
| hmin = result["headMotMA3"].min() | |
| return hmax, hmin | |
| def arrowmake(df, index): | |
| x0 = df.loc[index, "relPosE_B"] | |
| y0 = df.loc[index, "relPosN_B"] | |
| x1 = df.loc[index + 1, "relPosE_B"] | |
| y1 = df.loc[index + 1, "relPosN_B"] | |
| dist = numpy.sqrt((x1 - x0) ** 2 + (y1 - y0) ** 2) * 0.8 | |
| # headMot | |
| hmotdeg = df.loc[index, "headMotMA3"] | |
| hmotrad = math.radians(df.loc[index, "headMotMA3"]) | |
| vxm = dist * math.sin(hmotrad) | |
| vym = dist * math.cos(hmotrad) | |
| print("vxm=", vxm, "vym=", vym) | |
| # Heading | |
| headdeg = df.loc[index, "Heading360"] | |
| headrad = math.radians(df.loc[index, "Heading360"]) | |
| hxm = dist * math.sin(headrad) | |
| hym = dist * math.cos(headrad) | |
| gsp = df.loc[index, "gSpeed_B"] * 0.001 * 3600 * 0.001 | |
| skid = hmotdeg - headdeg | |
| high = df.loc[index, "height_B"] * 0.001 | |
| edge=df.loc[index,"roll"] | |
| return vxm, vym, hxm, hym, gsp, skid, high,edge | |
| def plotvector2(df): | |
| # diifzeroが１から番号がついてる行を抽出してdfを作成 | |
| # dfzero=[df["diffzero"]>0] | |
| dfzero = df.query('diffzero >0') | |
| zeropoint = dfzero.index | |
| print("dfzero", dfzero) | |
| print("zeropoint=", zeropoint) | |
| for i in range(1, len(dfzero) - 1): | |
| stindex = zeropoint[i] # ターン開始result.index番号 | |
| stpindex = zeropoint[i + 2] # ターン終了result.index番号 | |
| #Turn Slope計算 | |
| xd=df.loc[stpindex,"relPosE_B"]-df.loc[stindex,"relPosE_B"] | |
| yd=df.loc[stpindex,"relPosN_B"]-df.loc[stindex,"relPosN_B"] | |
| hd=df.loc[stpindex,"height_B"]-df.loc[stindex,"height_B"] | |
| hd=hd*0.1 | |
| turndist=math.sqrt(xd**2+yd**2) | |
| turnslope=math.degrees(math.atan(hd/turndist)) | |
| turnslopestr="{:.1f}".format(turnslope) | |
| rng = stpindex - stindex | |
| print(stindex, "-", stpindex) | |
| x0 = df.loc[stindex, "relPosE_B"] | |
| y0 = df.loc[stindex, "relPosN_B"] | |
| x = [] | |
| y = [] | |
| vmx = [] | |
| vmy = [] | |
| hmx = [] | |
| hmy = [] | |
| gsp = [] | |
| skid = [] | |
| high = [] | |
| edge=[] | |
| cntj = 1 | |
| for j in range(stindex, stpindex): | |
| print(cntj, j, ":", df.loc[j, "relPosN_B"], df.loc[j, "relPosE_B"]) | |
| x.append(df.loc[j, "relPosE_B"] - x0) | |
| y.append(df.loc[j, "relPosN_B"] - y0) | |
| # print("x[",cntj,"]=",x[cntj],"y[",cntj,"]=",y[cntj]) | |
| print(x) | |
| print(y) | |
| # headMot | |
| vm = arrowmake(df, j) | |
| vmx.append(vm[0]) | |
| vmy.append(vm[1]) | |
| hmx.append(vm[2]) | |
| hmy.append(vm[3]) | |
| gsp.append(vm[4]) | |
| skid.append(vm[5]) | |
| high.append(vm[6]) | |
| edge.append(vm[7]) | |
| print("vmx=", vmx, "vmy=", vmy) | |
| print("hmx=", hmx, "hmy=", hmy) | |
| print("gsp=", gsp) | |
| print("skid=", skid) | |
| print("high=", high) | |
| print("edge=", edge) | |
| cntj += 1 | |
| fig, ax = plt.subplots(figsize=(12, 10)) | |
| fig.suptitle("dataNo" + str(stindex) + "-" + str(stpindex) + "<" + basename + ">"+"\n FigNo="+str(i)+"TurnSlope="+turnslopestr+"deg") | |
| ax.set_aspect('equal') | |
| # v=[-200,4000,-4000,200] | |
| # ax.axis(v) | |
| ax.scatter(x, y, s=40, c="black") | |
| ax.quiver(x, y, vmx, vmy, color="red", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3, | |
| headaxislength=1.3) | |
| ax.quiver(x, y, hmx, hmy, color="blue", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3, | |
| headaxislength=1.3) | |
| # 指定した位置に注釈と矢印を入れる | |
| for k in range(1, rng, 4): | |
| if skid[k] > 0: | |
| xt = x[k] + 40 | |
| yt = y[k] + 60 | |
| elif skid[k] <= 0: | |
| xt = x[k] + 100 | |
| yt = y[k] + 100 | |
| # gspstr=str(gsp[k]) | |
| gspstr = "{:.1f}".format(gsp[k]) | |
| # gspstr=gspstr+"kmh" | |
| skidstr = "{:.1f}".format(skid[k]) | |
| highstr = "{:.2f}".format(high[k] - high[0]) | |
| edgestr="{:.1f}".format(edge[k]) | |
| txtstr = "[spd=" + gspstr + "kmh" + "\nskid=" + skidstr + "deg" + "\nDrop=" + highstr + "m"+"\nEdge"+edgestr | |
| # ax.annotate("point A", xy=(xt, yt), size=15,color="red") | |
| ax.text(xt, yt, txtstr, fontsize="small") | |
| # matplotlib.pyplot.text(x, y, s, fontdict=None, withdash= < deprecatedparameter >, ** kwargs) | |
| plt.show() | |
| input("ANY key return") | |
| """ | |
| def plotvector(df, dfbno): | |
| # diifzeroが１から番号がついてる行を抽出してdfを作成 | |
| # dfzero=[df["diffzero"]>0] | |
| dfzero = df.query('diffzero >0') | |
| zeropoint = dfzero.index | |
| print("dfzero", dfzero) | |
| print("zeropoint=", zeropoint) | |
| for i in range(1, len(dfzero) - 1): | |
| stindex = zeropoint[i] # ターン開始result.index番号 | |
| stpindex = zeropoint[i + 2] # ターン終了result.index番号 | |
| rng = stpindex - stindex | |
| print(stindex, "-", stpindex) | |
| x0 = df.loc[stindex, "relPosE_B"] | |
| y0 = df.loc[stindex, "relPosN_B"] | |
| x = [] | |
| y = [] | |
| vmx = [] | |
| vmy = [] | |
| hmx = [] | |
| hmy = [] | |
| gsp = [] | |
| skid = [] | |
| high = [] | |
| cntj = 1 | |
| for j in range(stindex, stpindex): | |
| print(cntj, j, ":", df.loc[j, "relPosN_B"], df.loc[j, "relPosE_B"]) | |
| x.append(df.loc[j, "relPosE_B"] - x0) | |
| y.append(df.loc[j, "relPosN_B"] - y0) | |
| # print("x[",cntj,"]=",x[cntj],"y[",cntj,"]=",y[cntj]) | |
| print(x) | |
| print(y) | |
| # headMot | |
| vm = arrowmake(df, j) | |
| vmx.append(vm[0]) | |
| vmy.append(vm[1]) | |
| hmx.append(vm[2]) | |
| hmy.append(vm[3]) | |
| gsp.append(vm[4]) | |
| skid.append(vm[5]) | |
| high.append(vm[6]) | |
| print("vmx=", vmx, "vmy=", vmy) | |
| print("hmx=", hmx, "hmy=", hmy) | |
| print("gsp=", gsp) | |
| print("skid=", skid) | |
| print("high=", high) | |
| cntj += 1 | |
| fig, ax = plt.subplots(figsize=(12, 10)) | |
| fig.suptitle("dataNo" + str(stindex) + "-" + str(stpindex) + "<" + basename + ">") | |
| ax.set_aspect('equal') | |
| # v=[-200,4000,-4000,200] | |
| # ax.axis(v) | |
| ax.scatter(x, y, s=40, c="black") | |
| ax.quiver(x, y, vmx, vmy, color="red", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3, | |
| headaxislength=1.3) | |
| ax.quiver(x, y, hmx, hmy, color="blue", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3, | |
| headaxislength=1.3) | |
| # 指定した位置に注釈と矢印を入れる | |
| for k in range(1, rng, 4): | |
| if skid[k] > 0: | |
| xt = x[k] + 40 | |
| yt = y[k] + 60 | |
| elif skid[k] <= 0: | |
| xt = x[k] + 100 | |
| yt = y[k] + 100 | |
| # gspstr=str(gsp[k]) | |
| gspstr = "{:.1f}".format(gsp[k]) | |
| # gspstr=gspstr+"kmh" | |
| skidstr = "{:.1f}".format(skid[k]) | |
| highstr = "{:.2f}".format(high[k] - high[0]) | |
| txtstr = "[spd=" + gspstr + "kmh" + "\nskid=" + skidstr + "deg" + "\nDrop=" + highstr + "m]" | |
| # ax.annotate("point A", xy=(xt, yt), size=15,color="red") | |
| ax.text(xt, yt, txtstr, fontsize="small") | |
| # matplotlib.pyplot.text(x, y, s, fontdict=None, withdash= < deprecatedparameter >, ** kwargs) | |
| plt.show() | |
| input("ANY key return") | |
| """ | |
| # |||||||||||||||||||||||||||||||||readMBFIle END||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||| | |
| # [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[def main]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
| def main(): | |
| # id+PVT33+RELPOSNED 20 | |
| # --------------------------------------------MBUX FILES----------------------------------------------------------------- | |
| # UBXデータは、BaseとRoverから同形式で出力されたバイナリーをSTA24システムでUBX値に変換してCSVファイル | |
| # 形式は、Baseは、id列に’B'行で Roverはid列に’R'行がついている。同じitowが２行となる。 | |
| # PVT３３個 RELPOSNED 20個 計55個ｘ２=110個整数 | |
| global result, basename, df_BNO | |
| ubxflag = 0 | |
| bnoflag = 0 | |
| df_BNO = pd.DataFrame | |
| list_PVT = ["id", "iTOW", "year", "month", "day", "hour", "min", "sec", "valid", "tAcc", "nano", "fixtye", "flags", | |
| "flags2", "numSV", "lon", "lat", "height", "hMSL", | |
| "hAcc", "vAcc", "velN", "velE", "velD", "gSpeed", "headMot", "sAcc", "headAcc", "pDOP", "flags3", | |
| "reserved1", "headVeh", "magDec", "magAcc", ""] # 33 | |
| list_BasePVT = [s + '_B' for s in list_PVT] | |
| list_RoverPVT = [s + '_R' for s in list_PVT] | |
| list_RELP = ["version", "reserved2", "refStationId", "iTOWR", "relPosN", "relPosE", "relPosD", "relPosLength", | |
| "relPosHeading", "relPosHPN", | |
| "relPosHPE", "relPosHPD", "relPosHPLength", "accN", "accE", "accD", "accLength", "accHeading", | |
| "reserverd4", "flagsR", "a"] | |
| list_BaseRELP = [s + '_B' for s in list_RELP] | |
| list_RoverRELP = [s + '_R' for s in list_RELP] | |
| head0 = list_PVT + list_RELP | |
| # print(list_BasePVT) | |
| # print(list_RoverPVT) | |
| # @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
| # @@@@@@@@@@@@@@@@@@@@@@@@@@ CONTROL KEY @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
| # @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
| v = " " | |
| cmnt = "-->Read_to_DF Finished" | |
| while v != "e": | |
| print("STAT::" + cmnt) | |
| print("<<<<PUSH KEY::r:read file,h:plot headmot,x:plot xy,v:plot vector, e=EXIT>>>> ") | |
| while True: | |
| if keyboard.read_key(): | |
| v = keyboard.read_key() | |
| print("キーが押されました: " + v) | |
| if v == "e": | |
| sys.exit() | |
| break | |
| # @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
| if v == "v": | |
| #plotvector(result, df_BNO) | |
| plotvector2( resultBRBN) | |
| # --------------------------------------------MovingBase BNO FILES----------------- | |
| if v == "r": | |
| list_BNO = ["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az", "com1", "com2", "com3"] | |
| list_BNO0=["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az"] | |
| root = tkinter.Tk() | |
| root.withdraw() | |
| iDir = r'C:/RTK_Log/' | |
| fTyp = [("データファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")] | |
| filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp) | |
| # filenameからbasenameをとりだしてファイル種類区別 | |
| basename = os.path.basename(filename) | |
| # ************************************************************************************************************ | |
| # *************************************MovingBase File ******************************************************* | |
| # ************************************************************************************************************ | |
| if "ubx" in basename and v == "r": | |
| ubxflag = 1 | |
| result = readMBFile(filename, basename, head0) | |
| print("result=", result) | |
| print(f"result memory usage: {result.memory_usage(deep=True).sum() / 1024 ** 2} MB") | |
| cmnt = "-[<" + basename + "> to result]-" + cmnt | |
| elif "bno" in basename and v == "r": # MB BNO File | |
| head1 = list_BNO | |
| bnoflag = 1 | |
| df_BNO = pd.read_csv(filename, names=head1, low_memory=True) | |
| print("PATH:", filename, "READ:", basename) | |
| pd.set_option('display.max_columns', 40) | |
| # ----bnitowの異常値修正 異常値は、前の正常値でインクリメントして埋める | |
| df_BNO = df_BNO.drop(df_BNO.index[-1]) # last コメント除去 | |
| df_BNO=df_BNO.drop(["com1","com2","com3"],axis=1) | |
| print(df_BNO) | |
| #df_BNO=bnitowRe(df_BNO)# bnitow=0行をitowインクリメントで埋める関数 | |
| df_BNO=bnitowsort(df_BNO) | |
| print("Prepaired df_BNO:",df_BNO) | |
| print(df_BNO.dtypes) | |
| df_BNO.to_csv(r'C:/RTK_Log/BNO_' + basename)#CSV save | |
| cmnt = "-[<" + basename + "> to df_BNO]-" + cmnt | |
| print(f"df_BNO memory usage: {df_BNO.memory_usage(deep=True).sum() / 1024 ** 2} MB") | |
| #-----------result."iTow_B"とdf_BNO120の同期-------------- | |
| df_BNO = df_BNO.drop(df_BNO.index[-1]) | |
| df_BNO120=itow120df(df_BNO)#120周期のDF作成 | |
| df_BNO120.to_csv(r'C:/RTK_Log/BNO120_' + basename) # CSV save | |
| print("df_BNO120 made",df_BNO120) | |
| list_bnoit = [] # listは、数値を入れれば1次元 listを入れれば２次元になる | |
| bnc = [] | |
| if ubxflag == 1:#resultが準備されていること | |
| #start行番号検索 | |
| itb0=result.loc[0, "iTOW_B"] | |
| itbn0=int(df_BNO120.iloc[0, 0]) | |
| isa=int(itb0-itbn0) | |
| if isa<0:#baseが先行ある | |
| basestart=abs(int(isa/120)) | |
| bnostart=0 | |
| else:#bnoが先行 | |
| basestart = 0 | |
| bnostart = abs(int(isa/10)) | |
| print("isa=",isa,"basestart=",basestart,"bnostart=",bnostart) | |
| print("basestart itow=",result.iloc[basestart,0],"bnostart itow=",df_BNO120.iloc[0,0]) | |
| #result左端にdf_bno120を追加するために一旦listにして再構築 | |
| #0からbasestart | |
| list_bno = [] | |
| r=[] | |
| if basestart>bnostart:#bnostartの上にbasestart分のゼロDFを作成して合体 | |
| list_bno=[[0] * 7] * (basestart)#上のデータゼロ埋め | |
| #print(list_bno) | |
| blen=len(df_BNO120) | |
| for i in range(0,blen-1):#中間にdf_BNO120をlistに追加 | |
| r=df_BNO120.iloc[i,:] | |
| list_bno.append(r) | |
| sn=basestart+len(df_BNO120) | |
| rn=len(result) | |
| print("Upper-listbno=",len(list_bno)) | |
| print("for sn=",sn,"rn=",rn,"result len=",len(result)) | |
| for i in range(sn,rn+1):#下のデータ埋め | |
| l0=[0,0,0,0,0,0,0] | |
| list_bno.append(l0) | |
| print("listbno=",len(list_bno)) | |
| df_BNOL=pd.DataFrame(list_bno,columns=list_BNO0) | |
| # 現在の最大表示行数の出力 | |
| pd.set_option('display.max_rows', None) | |
| pd.set_option('display.max_columns', None) | |
| print(df_BNOL) | |
| print(result["iTOW_B"]) | |
| print("len df_BNOL=",len(df_BNOL)) | |
| print("len result=",len(result)) | |
| print("Check basestart=",basestart,"@iTOW_B=",result.iloc[220,0],"df_BNOL bnitow=",df_BNOL.loc[220,"bnitow"]) | |
| #concat | |
| resultBRBN=pd.concat([result,df_BNOL],axis=1) | |
| pd.set_option('display.max_rows', 50) | |
| pd.set_option('display.max_columns',100) | |
| print(resultBRBN) | |
| resultBRBN.to_csv(r'C:/RTK_Log/BRBN_' + basename) # CSV save | |
| #elif basestart<bnostart:#bnostartをカットしてresultと合体 | |
| # ***************************************************************************************************** | |
| # *********************RTK File BootsOn**************************************************************** | |
| # ***************************************************************************************************** | |
| elif "RTK" in basename and v == "r": # BootsOn RTK_ ファイル | |
| df_RTKBNO = pd.read_csv(filename, names=head0, low_memory=False) | |
| df_RTKBNO = df_RTKBNO.drop(df_RTKBNO.index[-1]) | |
| print("df_RTKBNO=", df_RTKBNO) | |
| print(f"df_RTKBNO memory usage: {df_RTKBNO.memory_usage(deep=True).sum() / 1024 ** 2} MB") | |
| print(df_RTKBNO.dtypes) | |
| df_SRTK = df_RTKBNO[df_RTKBNO.iloc[:, 0] == 'U'] # id列[:,0]が'B'ならばdf_RTKとして抽出 | |
| print("df_SRTK::", df_SRTK) | |
| df_SRTK = df_SRTK.add_suffix('_S', axis=1) # head0の名称末尾に'_S'をつけてRTK Sringleのデータ列命名 | |
| print("df_SRTK::", df_SRTK) | |
| df_SRTK.to_csv(r'C:/RTK_Log/SRTK_' + basename) | |
| df_SBNO = df_RTKBNO[df_RTKBNO.iloc[:, 0] == 'B'] # id列[:,0]が'B'ならばdf_RTKとして抽出 | |
| df_SBNO.columns = range(df_SBNO.shape[1]) # ヘッダーを削除 | |
| print("Before drop::", df_SBNO) | |
| df_SBNO = df_SBNO.drop([x for x in range(45, 56)], axis=1) # rtk用の列を削除 | |
| df_SBNO.to_csv(r'C:/RTK_Log/SBNO_' + basename) | |
| print(df_SBNO) | |
| cmnt = "-[<" + basename + "> to df_SRTK,df_SBNO]-" + cmnt | |
| # =============Plot Vector===================== | |
| elif v == "h" and ubxflag == 1: | |
| plotheadmot(result, basename) | |
| def itow120df(df):#dfは、先頭がiTOW | |
| itow120list=[] | |
| for i in range(1,len(df)-1): | |
| mod=int(df.iloc[i,0]) % 120 | |
| if mod<10 : | |
| itow120list.append(df.iloc[i,:]) | |
| print("itow12oCheck:",i,">bnitow=",df.iloc[i,0]) | |
| #print(itow120list) | |
| df_itow120=pd.DataFrame(itow120list) | |
| #print("df_itwo120=",df_itow120) | |
| return df_itow120 | |
| def bnitowsort(df): | |
| initzeroflag=0 | |
| i=0 | |
| while initzeroflag==0: | |
| if int(df.loc[i,"bnitow"])==0 and initzeroflag==0: | |
| df.drop(index=i) | |
| else: | |
| initzeroflag=1 | |
| firstbnitow=int(df.loc[i,"bnitow"]) | |
| i+=1 | |
| for i in range(0,len(df)): | |
| df.loc[i, "bnitow"] = firstbnitow+ 10*i | |
| print(df) | |
| df.to_csv(r'C:/RTK_Log/bnitow10_' + basename) # CSV save | |
| return df | |
| def bnitowRe(df):#bnitowの異常値を修正 | |
| #0,1,2行ゼロ処理 | |
| czeroflag=0 | |
| for i in range(0,len(df)-1): | |
| if int(df.loc[i,"bnitow"])==0: | |
| j=i | |
| while int(df.loc[j,"bnitow"])==0: | |
| print("==0:j=",j,int(df.loc[j,"bnitow"])) | |
| j+=1 | |
| for k in range(j,i,-1):#j+1のbnitow値から-10decriment | |
| df.loc[k-1,"bnitow"]=int(df.loc[k,"bnitow"])-10 | |
| print("==0",df.loc[k-1,"bnitow"]) | |
| elif int(df.loc[i,"bnitow"])>10: | |
| j = i | |
| while int(df.loc[j, "bnitow"]) > 10: | |
| print(">10:j=", j, int(df.loc[j, "bnitow"])) | |
| j += 1 | |
| for k in range(j, i, -1): # j+1のbnitow値から-10decriment | |
| df.loc[k - 1, "bnitow"] = int(df.loc[k, "bnitow"]) - 10 | |
| print(">10:",df.loc[k - 1, "bnitow"]) | |
| print("def Repaired=",df) | |
| return df | |
| # [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[def main END ]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
| # 最後に呼び出す | |
| main() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment