-
Notifications
You must be signed in to change notification settings - Fork 0
/
get_map.py
103 lines (80 loc) · 3.88 KB
/
get_map.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
import matplotlib.pyplot as plt
import matplotlib.ticker as tickers
import csv
import math
def get_map(file_path, lat_goal, lon_goal):
with open(file_path) as f:
#スペースで区切る
reader = csv.reader(f, delimiter=',')
log_data = [row for row in reader]
#転地行列の取得
log_data_T = [list(x) for x in zip(*log_data)]
#-----緯度・経度・方位角の取得-----#
gps_lat = [float(v) for v in log_data_T[0]]
gps_lon = [float(v) for v in log_data_T[1]]
rover_azimuth = [float(v) for v in log_data_T[2]]
cos_azimuth_array = []
sin_azimuth_array = []
for i in range(len(rover_azimuth)-1):
cos_azimuth = math.cos(rover_azimuth[i])
sin_azimuth = math.sin(rover_azimuth[i])
cos_azimuth_array.append(cos_azimuth)
sin_azimuth_array.append(sin_azimuth)
cos_azimuth_array[0] = 0
sin_azimuth_array[0] = 0
cos_azimuth_array.append(0)
sin_azimuth_array.append(0)
#-----描画処理-----#
#グラフのタイトル
plt.title("A visualized control record")
#軸の範囲(少し大き目に設定)
lim_x1 = min(gps_lat) - 0.000150
lim_x2 = max(gps_lat) + 0.000200
lim_y1 = min(gps_lon) - 0.000150
lim_y2 = max(gps_lon) + 0.000200
plt.xlim(lim_x1, lim_x2)
plt.ylim(lim_y1, lim_y2)
#目盛りの設定
plt.gca().xaxis.get_major_formatter().set_useOffset(False)
plt.gca().yaxis.get_major_formatter().set_useOffset(False)
#軸ラベル
plt.xlabel("Latitude")
plt.ylabel("Longitude")
#座標のプロット
plt.scatter(gps_lat[0], gps_lon[0], color="blue")
# やじるしの描画
# plt.quiver(gps_lat, gps_lon, cos_azimuth_array, sin_azimuth_array, color="red", width=0.003, edgecolor="black", scale=20)
plt.plot(gps_lat, gps_lon, label="Trajectory", linestyle="dashed", color="black")
#スタート地点、ゴール地点、制御終了地点の座標の表示
plt.scatter(gps_lat[0], gps_lon[0], color="blue", edgecolors="black")
plt.scatter(lat_goal, lon_goal, color="red", edgecolors="black")
plt.scatter(gps_lat[-1], gps_lon[-1], color="red", edgecolors="black")
#地図の情報を表示
arrow_dict = dict(arrowstyle="wedge", color="silver", connectionstyle="arc3")
text_dict = text_dict = dict(boxstyle="round",fc="silver", ec="mediumblue")
plt.annotate("Target point" + "\n" + str(lat_goal) + ", "+ str(lon_goal), xy=(lat_goal, lon_goal), xytext=(lim_x2 - 0.000100, lim_y2 - 0.000900), arrowprops=arrow_dict, bbox=text_dict)
plt.annotate("Control start point" + "\n" + str(gps_lat[0]) + ", "+ str(gps_lon[0]), xy=(gps_lat[0], gps_lon[0]), xytext=(lim_x2 - 0.000100, lim_y2 - 0.000300), arrowprops=arrow_dict, bbox=text_dict)
plt.annotate("Control finish point" + "\n" + str(gps_lat[-1]) + ", "+ str(gps_lon[-1]), xy=(gps_lat[-1], gps_lon[-1]), xytext=(lim_x2 - 0.000100, lim_y2 - 0.000600), arrowprops=arrow_dict, bbox=text_dict)
#グリッドの表示
plt.grid()
#軸凡例
plt.legend()
#地図の保存
plt.savefig('A visualized control record.png', bbox_inches='tight')
plt.show()
# print(gps_lat)
# print(gps_lon)
# print(rover_azimuth)
#表の作成
plt.rcParams['font.family'] = 'Times New Roman'
fig = plt.figure()
ax1 = fig.add_subplot(111)
ax1.axis('off')
ax1.table(cellText=log_data, colLabels=['Latitude', 'Longtitude', 'theta'], loc='center')
plt.table(cellText=log_data, loc='center')
plt.show()
if __name__ == '__main__':
file_path = 'map_test.csv'
lat_goal = 35.9242411
lon_goal = 139.9120618
get_map(file_path, lat_goal, lon_goal)