1'''
2AHRS - AHRS_visualize.py with synchronous mode.
3
4This example visualize AHRS by using 3D model.
5
6-------------------------------------------------------------------------------------
7Please change correct serial number or IP and port number BEFORE you run example code.
8
9For other examples please check:
10 https://github.com/WPC-Systems-Ltd/WPC_Python_driver_release/tree/main/examples
11See README.md file to get detailed usage of this example.
12
13Copyright (c) 2022-2024 WPC Systems Ltd. All rights reserved.
14'''
15
16## Python
17import numpy as np
18import stl.mesh as mesh
19import mpl_toolkits.mplot3d as mplot3d
20from PIL import Image
21import matplotlib.pyplot as plt
22import matplotlib.gridspec as gridspec
23import matplotlib.image as mpimg
24import matplotlib.font_manager as font_manager
25from matplotlib import rcParams
26from matplotlib.transforms import Affine2D
27
28## WPC
29
30from wpcsys import pywpc
31
32
33################################################################################
34## Configuration
35
36DATA_PATH = 'Material/viz_data/'
37IMG_PATH = 'Material/viz_data/avion_'
38
39for font in font_manager.findSystemFonts(DATA_PATH):
40 font_manager.fontManager.addfont(font)
41
42## Set font family globally
43rcParams['font.family'] = 'Digital-7 Mono'
44
45## Style
46plt.style.use("bmh")
47BMH_BURGUNDY = "#a70f34"
48BMH_BLUE = "#3d90be"
49BMH_PURPLE = "#7c6ca4"
50
51for font in font_manager.findSystemFonts(DATA_PATH):
52 font_manager.fontManager.addfont(font)
53
54## Set font family globally
55rcParams['font.family'] = 'Digital-7 Mono'
56
57IMG_WPC = plt.imread('Material/trademark.jpg')
58IMG_WPC_PIL = Image.open('Material/trademark.jpg')
59
60################################################################################
61## Plane Images and Constants
62
63IMG_YAW = mpimg.imread(f'{IMG_PATH}yaw.png')
64IMG_PITCH = mpimg.imread(f'{IMG_PATH}pitch.png')
65IMG_ROLL = mpimg.imread(f'{IMG_PATH}roll.png')
66
67SIZE_YAW = np.array([IMG_YAW.shape[0], IMG_YAW.shape[1]])
68CENTER_YAW = SIZE_YAW / 2
69DIAG_YAW = np.sqrt(np.sum(SIZE_YAW ** 2))
70ALPHA_YAW = (DIAG_YAW - SIZE_YAW) / 2
71
72
73SIZE_PITCH = np.array([IMG_PITCH.shape[0], IMG_PITCH.shape[1]])
74CENTER_PITCH = SIZE_PITCH / 2
75DIAG_PITCH = np.sqrt(np.sum(SIZE_PITCH ** 2))
76ALPHA_PITCH = (DIAG_PITCH - SIZE_PITCH) / 2
77
78SIZE_ROLL = np.array([IMG_ROLL.shape[0], IMG_ROLL.shape[1]])
79CENTER_ROLL = SIZE_ROLL / 2
80DIAG_ROLL = np.sqrt(np.sum(SIZE_ROLL ** 2))
81ALPHA_ROLL = (DIAG_ROLL - SIZE_ROLL) / 2
82
83## Dictionaries
84PLANE_DICT = {
85 'yaw': dict(img=IMG_YAW, size=SIZE_YAW, center=CENTER_YAW, diag=DIAG_YAW, alpha=ALPHA_YAW),
86 'pitch': dict(img=IMG_PITCH, size=SIZE_PITCH, center=CENTER_PITCH, diag=DIAG_PITCH, alpha=ALPHA_PITCH),
87 'roll': dict(img=IMG_ROLL, size=SIZE_ROLL, center=CENTER_ROLL, diag=DIAG_ROLL, alpha = ALPHA_ROLL),
88}
89
90MODEL_DICT = {
91 'cat': dict(label='Cat', view=400),
92 'rat': dict(label='Rat', view=50)
93}
94DEFAULT_MODEL = 'rat'
95
96################################################################################
97## Constants
98
99DEGREE_TO_RADIAN = np.pi / 180.0
100RADIAN_TO_DEGREE = 180.0 / np.pi
101
102################################################################################
103## Functions - utilities
104
105def WPC_initializeFigure(nb_rows=1, nb_col=1, share_x='all', share_y='all', option='none'):
106 fig = plt.figure(figsize=(10, 6))
107 fig.clf()
108 gs = gridspec.GridSpec(3, 3, width_ratios=[3, 1, 1], height_ratios=[1, 1, 1])
109
110 ## If `option` is `3D`, return a 3-D axis.
111 if option == '3D':
112 ax_main = fig.add_subplot(gs[:, 0], projection='3d')
113 ax_main.set_title('Main 3D Plot')
114 ax_plane_yaw = fig.add_subplot(gs[0, 1])
115 ax_plane_pitch = fig.add_subplot(gs[1, 1])
116 ax_plane_roll = fig.add_subplot(gs[2, 1])
117 ax_value_yaw = fig.add_subplot(gs[0, 2])
118 ax_value_pitch = fig.add_subplot(gs[1, 2])
119 ax_value_roll = fig.add_subplot(gs[2, 2])
120
121 Pil_width = IMG_WPC_PIL.width
122 Pil_height = IMG_WPC_PIL.height
123 new_pil_image = IMG_WPC_PIL.resize((int(Pil_width//3), int(Pil_height//3)))
124 fig.figimage(new_pil_image, xo=fig.bbox.xmin, yo=fig.bbox.ymin, alpha=0.8, zorder = 0)
125
126 ## fig.figimage(IMG_WPC, xo=fig.bbox.xmin, yo=fig.bbox.ymin, alpha=1)
127 ax_dict = {
128 'main': ax_main,
129 'plane_yaw': ax_plane_yaw,
130 'plane_pitch': ax_plane_pitch,
131 'plane_roll': ax_plane_roll,
132 'value_yaw': ax_value_yaw,
133 'value_pitch': ax_value_pitch,
134 'value_roll': ax_value_roll,
135 }
136
137 return fig, ax_dict
138
139 ## If `option` is `grid`, return gridspec for further usage.
140 if option == 'grid':
141 ax_grid = gridspec.GridSpec(nb_rows, nb_col, figure=fig)
142 return fig, None, None, ax_grid
143
144 ## If empty rows or columns, return `None`.
145 if nb_rows == 0 or nb_col == 0:
146 return fig, None, None, None
147
148 ## Return subplots
149 ax_mat = fig.subplots(nb_rows, nb_col, sharex=share_x, sharey=share_y, squeeze=False)
150
151 ## sharex/sharey if the subplot would share same axis, squeeze=False to ensure ax_mat is 2D array
152 ax_arr = ax_mat.flatten()
153 ax = ax_arr[0]
154
155 ## Add grid to the figure
156 fig.grid(True)
157 return fig, ax, ax_arr, ax_mat
158
159def WPC_initialize_plane(angle_type, fig, ax):
160 ## Load image
161 ax.set_axis_off()
162 d_plane = PLANE_DICT[angle_type]
163 img_plane = d_plane['img']
164 im = ax.imshow(img_plane)
165
166 size_plane = d_plane['size']
167 center_plane = d_plane['center']
168 diag_plane = d_plane['diag']
169 alpha_plane = d_plane['alpha']
170 tt = 1.15 ## Tolerance threshold of 20 percent to not have cut circle
171
172 ax.set_xlim(-alpha_plane[1]*tt, (size_plane[1] + alpha_plane[1])*tt)
173 ax.set_ylim(-alpha_plane[0]*tt, (size_plane[0] + alpha_plane[0])*tt)
174
175 ## display circle
176 cc = plt.Circle((center_plane[1], center_plane[0]), diag_plane/2, fill=False, color='black', linewidth=2, linestyle='dashdot')
177 ax.add_artist(cc)
178 return im
179
180def WPC_showFigure(fig):
181 w, h = fig.get_size_inches()
182 fig.set_size_inches(w, h, forward=True)
183 plt.ion() ## Turn on interactive mode
184 plt.show()
185 return
186
187def WPC_getRotMat(roll, pitch, yaw, use_deg=False):
188 if use_deg:
189 roll *= DEGREE_TO_RADIAN
190 pitch *= DEGREE_TO_RADIAN
191 yaw *= DEGREE_TO_RADIAN
192 rot_mat_x = np.array([[1, 0, 0], [0, np.cos(roll), np.sin(roll)], [0, -np.sin(roll), np.cos(roll)]])
193 rot_mat_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
194 rot_mat_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
195 rot_mat = rot_mat_x.dot(rot_mat_y).dot(rot_mat_z) ## Dot product from the back RxRyRz
196 return rot_mat
197
198def WPC_showEmpty(tag=DEFAULT_MODEL, save=0):
199 fig, ax_dict = WPC_initializeFigure(option='3D')
200 im_yaw = WPC_initialize_plane('yaw', fig, ax_dict.get('plane_yaw'))
201 im_pitch = WPC_initialize_plane('pitch', fig, ax_dict.get('plane_pitch'))
202 im_roll = WPC_initialize_plane('roll', fig, ax_dict.get('plane_roll'))
203
204 im_dict = {
205 'yaw': im_yaw,
206 'pitch': im_pitch,
207 'roll': im_roll,
208 }
209
210 d = MODEL_DICT[tag] #model dict dictionary
211
212 ## Load
213 model = mesh.Mesh.from_file(f'{DATA_PATH}{tag}.stl')
214 data_orig = model.vectors ## 3D array
215
216 ## Plot
217 poly = mplot3d.art3d.Poly3DCollection([], color=BMH_BLUE, edgecolor='k', lw=0.2) #or lightsteelblue
218 ax_dict.get('main').add_collection3d(poly)
219
220 ## Settings
221 scale = [-0.6*d['view'], 0.6*d['view']]
222 ax_dict.get('main').view_init(elev=0, azim=0, roll=0)
223 ax_dict.get('main').auto_scale_xyz(scale, scale, scale)
224 ax_dict.get('main').set_axis_off()
225 ax_dict.get('main').set_title(f'Sensor fusion {tag}', weight='bold')
226
227 ## Save
228 fig.set_size_inches(6, 6)
229 fig.subplots_adjust(left=0.05, right=0.95, bottom=0.05, top=0.95)
230 fig.canvas.manager.set_window_title('WPC AHRS visualization')
231
232 WPC_showFigure(fig)
233 return fig, ax_dict, im_dict, data_orig, poly
234
235def WPC_draw3DModel(fig, ax, data_orig, poly, roll, pitch, yaw):
236 ## Rotate
237 rot_mat = WPC_getRotMat(roll, pitch, yaw, use_deg=True)
238 data = data_orig.dot(rot_mat)
239
240 ## Plot
241 poly.set_verts(data)
242 return
243
244def WPC_plot_plane(fig, ax, angle_type, im, center):
245 ## Display image
246 im.remove()
247
248 ## Calculate rotation angle in radians
249 angle = np.deg2rad(angle_type) #Convert in en radians
250
251 ## Apply rotation to the image
252 rotation_matrix = Affine2D().rotate_deg_around(center[1], center[0], np.rad2deg(angle)+180)
253 im.set_transform(rotation_matrix + ax.transData)
254
255 ## Show the image again
256 ax.add_artist(im)
257 return
258
259def WPC_text_button(fig, Roll, Pitch, Yaw, ax_value_roll, ax_value_pitch, ax_value_yaw):
260 for ax in [ax_value_roll, ax_value_pitch, ax_value_yaw]:
261 ax.clear()
262 ax.axis('off')
263 ax.add_patch(plt.Rectangle((0, 0), 1, 1, facecolor='#ebebeb', transform=ax.transAxes, zorder=-1))
264 ax.grid(False)
265 roll = "{:7.2f}".format(Roll)
266 pitch = "{:7.2f}".format(Pitch)
267 yaw = "{:7.2f}".format(Yaw)
268 ax_value_roll.text(0.5, 0.5, f' Roll:\n{roll} deg', fontsize=32, weight='bold', color=BMH_BLUE,ha='center', va='center')
269 ax_value_pitch.text(0.5, 0.5, f' Pitch:\n{pitch} deg', fontsize=32, weight='bold', color=BMH_BURGUNDY, ha='center', va='center')
270 ax_value_yaw.text(0.5, 0.5, f' Yaw:\n{yaw} deg', fontsize=32, weight='bold', color=BMH_PURPLE, ha='center', va='center')
271 return
272
273def main():
274 ## Get Python driver version
275 print(f'{pywpc.PKG_FULL_NAME} - Version {pywpc.__version__}')
276
277 ## Create device handle
278 dev = pywpc.WifiDAQE3AH()
279
280 ## Show empty
281 fig, ax_dict, im_dict, data_orig, poly = WPC_showEmpty()
282
283 ## Connect to device
284 try:
285 dev.connect("192.168.5.38") ## Depend on your device
286 except Exception as err:
287 pywpc.printGenericError(err)
288 ## Release device handle
289 dev.close()
290 return
291
292 try:
293 ## Parameters setting
294 port = 0 ## Depend on your device
295 mode = 0 ## 0: Orientation, 1: Acceleration, 2: Orientation + Acceleration
296 timeout = 3 ## second
297
298 ## Get firmware model & version
299 driver_info = dev.Sys_getDriverInfo(timeout)
300 print("Model name: " + driver_info[0])
301 print("Firmware version: " + driver_info[-1])
302
303 ## Open AHRS and update rate is 333 HZ
304 err = dev.AHRS_open(port, timeout)
305 print(f"AHRS_open in port {port}, status: {err}")
306
307 ## Start AHRS
308 err = dev.AHRS_start(port, timeout)
309 print(f"AHRS_start in port {port}, status: {err}")
310
311 while plt.fignum_exists(fig.number):
312 ahrs_list = dev.AHRS_getEstimate(port, mode, timeout)
313 if len(ahrs_list) > 0:
314 WPC_draw3DModel(fig, ax_dict.get('main'), data_orig, poly, ahrs_list[0], ahrs_list[1], ahrs_list[2])
315 WPC_plot_plane(fig, ax_dict.get('plane_roll'), ahrs_list[0], im_dict.get('roll'), CENTER_ROLL)
316 WPC_plot_plane(fig, ax_dict.get('plane_pitch'), ahrs_list[1], im_dict.get('pitch'), CENTER_PITCH)
317 WPC_plot_plane(fig, ax_dict.get('plane_yaw'), ahrs_list[2], im_dict.get('yaw'), CENTER_YAW)
318 WPC_text_button(fig, ahrs_list[0], ahrs_list[1], ahrs_list[2], ax_dict.get('value_roll'), ax_dict.get('value_pitch'), ax_dict.get('value_yaw'))
319 plt.tight_layout()
320 plt.pause(2**-5)
321
322 except KeyboardInterrupt:
323 print("Press keyboard")
324
325 except Exception as err:
326 pywpc.printGenericError(err)
327
328 finally:
329 ## Stop AHRS
330 err = dev.AHRS_stop(port, timeout)
331 print(f"AHRS_stop in port {port}, status: {err}")
332
333 ## Close AHRS
334 err = dev.AHRS_close(port, timeout)
335 print(f"AHRS_close in port {port}, status: {err}")
336
337 ## Disconnect device
338 dev.disconnect()
339
340 ## Release device handle
341 dev.close()
342
343 return
344if __name__ == '__main__':
345 main()