nextwheel 0.2.1a1__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
nextwheel/__init__.py ADDED
@@ -0,0 +1,1169 @@
1
+ # Copyright 2023 NextWheel Developers
2
+
3
+ # Licensed under the Apache License, Version 2.0 (the "License");
4
+ # you may not use this file except in compliance with the License.
5
+ # You may obtain a copy of the License at
6
+
7
+ # http://www.apache.org/licenses/LICENSE-2.0
8
+
9
+ # Unless required by applicable law or agreed to in writing, software
10
+ # distributed under the License is distributed on an "AS IS" BASIS,
11
+ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12
+ # See the License for the specific language governing permissions and
13
+ # limitations under the License.
14
+
15
+ """Provides the NextWheel class that fetches data from an intrumented wheel."""
16
+
17
+ import json
18
+ import os
19
+ import socket
20
+ import struct
21
+ import threading
22
+ from enum import IntEnum
23
+ from time import time as get_current_time
24
+ from typing import Any
25
+
26
+ import numpy as np
27
+ import requests
28
+ from requests.exceptions import ConnectTimeout
29
+
30
+ try:
31
+ from kineticstoolkit import TimeSeries # type: ignore
32
+ except ModuleNotFoundError:
33
+
34
+ class TimeSeries: # type: ignore
35
+ """Object similar to the Kinetics Toolkit TimeSeries class."""
36
+
37
+ def __init__(
38
+ self,
39
+ time: list[float] | np.ndarray | None = None,
40
+ data: dict[str, np.ndarray] | None = None,
41
+ info: dict[str, dict[str, Any]] | None = None,
42
+ ):
43
+ self.time = time if time is not None else []
44
+ self.data = data if data is not None else {}
45
+ self.info = info if info is not None else {}
46
+
47
+ def __str__(self):
48
+ return "Object with attributes time and data"
49
+
50
+ def __repr__(self):
51
+ return "Object with attributes time and data"
52
+
53
+
54
+ # Constants
55
+
56
+ GRAVITY = 9.80665
57
+ STANDARD_MAG_RANGE = 2500
58
+ HTTP_RESPONSE_CODE_OK = 200
59
+ HTTP_RESPONSE_CODE_BAD_REQUEST = 400
60
+ DEFAULT_UDP_PORT = 12345
61
+ UDP_RECEIVE_BUFFER_SIZE = 65535
62
+ UDP_SOCKET_TIMEOUT = 0.2 # s, delay to notice that streaming was stopped
63
+ ENCODER_RESOLUTION = 0.087890625 / 180 * np.pi # rad per tick
64
+
65
+
66
+ class FrameType(IntEnum):
67
+ """Identify frame type from a UDP datagram or a dat file."""
68
+
69
+ CONFIG = 1
70
+ ADC = 2
71
+ IMU = 3
72
+ POWER = 4
73
+ ENCODER = 7
74
+ SUPERFRAME = 255
75
+
76
+
77
+ class GlobalConfig:
78
+ """
79
+ Global configuration of the instrumented wheel.
80
+
81
+ This is be used to store the current configuration and calculate
82
+ conversions from raw data.
83
+ """
84
+
85
+ def __init__(self):
86
+ # IMU CONFIG
87
+ self.accel_range = 16
88
+ self.gyro_range = 2000
89
+ self.mag_range = STANDARD_MAG_RANGE
90
+ self.imu_sampling_rate = 240
91
+ self.adc_sampling_rate = 240
92
+ self.encoder_sampling_rate = 240
93
+
94
+ # ADC CONFIG
95
+ self.adc_rate = 240
96
+
97
+ # ENCODER CONFIG
98
+ self.encoder_rate = 240
99
+
100
+ # ACCORDING TO ADS8688 DATASHEET
101
+ self._adc_v_ref = 4.096
102
+ # ADC RANGING from [-5,5] V according to PCB design
103
+ self._adc_in_max = 1.25 * self._adc_v_ref
104
+ self._adc_in_min = -1.25 * self._adc_v_ref
105
+
106
+ # ACCORDING TO BMX160 DATASHEET
107
+ self._accel_mg_lsb_2g = 0.000061035
108
+ self._accel_mg_lsb_4g = 0.000122070
109
+ self._accel_mg_lsb_8g = 0.000244141
110
+ self._accel_mg_lsb_16g = 0.000488281
111
+ self._gyro_sensitivity_125dps = 0.0038110
112
+ self._gyro_sensitivity_250dps = 0.0076220
113
+ self._gyro_sensitivity_500dps = 0.0152439
114
+ self._gyro_sensitivity_1000dps = 0.0304878
115
+ self._gyro_sensitivity_2000dps = 0.0609756
116
+ self._mag_ut_lsb = 0.3
117
+
118
+ def convert_adc_int_to_volts(self, values: np.ndarray) -> np.ndarray:
119
+ """
120
+ Convert ADC values from ADC integers to volts.
121
+
122
+ Parameters
123
+ ----------
124
+ values
125
+ ADC outputs (integer).
126
+
127
+ Returns
128
+ -------
129
+ np.ndarray
130
+ ADC inputs in volts.
131
+
132
+ """
133
+ # This is according to the 86888 datasheet
134
+ return (
135
+ values * (self._adc_in_max - self._adc_in_min) / 65535
136
+ + self._adc_in_min
137
+ )
138
+
139
+ def convert_accel_int_to_m_s2(self, values: np.ndarray) -> np.ndarray:
140
+ """
141
+ Convert acceleration values from integers to m/s2.
142
+
143
+ Parameters
144
+ ----------
145
+ values
146
+ Integers as reported by the sensor.
147
+
148
+ Returns
149
+ -------
150
+ np.ndarray
151
+ Acceleration in m/s2.
152
+
153
+ """
154
+ factor = {
155
+ 2: self._accel_mg_lsb_2g * GRAVITY,
156
+ 4: self._accel_mg_lsb_4g * GRAVITY,
157
+ 8: self._accel_mg_lsb_8g * GRAVITY,
158
+ 16: self._accel_mg_lsb_16g * GRAVITY,
159
+ }
160
+
161
+ try:
162
+ return values * factor[self.accel_range]
163
+ except KeyError as exc:
164
+ raise ValueError("Invalid accel range") from exc
165
+
166
+ def convert_gyro_int_to_deg_s(self, values: np.ndarray) -> np.ndarray:
167
+ """
168
+ Convert gyro values from integers to deg/s.
169
+
170
+ Parameters
171
+ ----------
172
+ values
173
+ Integers as reported by the sensor.
174
+
175
+ Returns
176
+ -------
177
+ np.ndarray
178
+ Angular velocity in m/s2.
179
+
180
+ """
181
+ factor = {
182
+ 125: self._gyro_sensitivity_125dps,
183
+ 250: self._gyro_sensitivity_250dps,
184
+ 500: self._gyro_sensitivity_500dps,
185
+ 1000: self._gyro_sensitivity_1000dps,
186
+ 2000: self._gyro_sensitivity_2000dps,
187
+ }
188
+
189
+ try:
190
+ return values * factor[self.gyro_range]
191
+ except KeyError as exc:
192
+ raise ValueError("Invalid gyro range") from exc
193
+
194
+ def convert_mag_values(self, values: np.ndarray) -> np.ndarray:
195
+ """
196
+ Convert magnetometer integers to uT.
197
+
198
+ Parameters
199
+ ----------
200
+ values
201
+ Integers as reported by the sensor.
202
+
203
+ Returns
204
+ -------
205
+ np.ndarray
206
+ Magnetic field in uT.
207
+
208
+ """
209
+ if self.mag_range == STANDARD_MAG_RANGE:
210
+ return values * self._mag_ut_lsb
211
+
212
+ raise ValueError("Invalid mag range")
213
+
214
+ def convert_encoder_values(self, values: np.ndarray) -> np.ndarray:
215
+ """
216
+ Convert encoder ticks to radians.
217
+
218
+ Parameters
219
+ ----------
220
+ values
221
+ Integers as reported by the sensor.
222
+
223
+ Returns
224
+ -------
225
+ np.ndarray
226
+ Angle in radians.
227
+
228
+ """
229
+ return values * ENCODER_RESOLUTION
230
+
231
+
232
+ class NextWheel:
233
+ """
234
+ Communicate with the instrumented wheel.
235
+
236
+ The class permit to communicate with the instrumented wheel with 3 methods:
237
+ - NextWheel.start_streaming() asks the wheel to stream its data over
238
+ UDP and starts receiving it in a background thread.
239
+ - NextWheel.fetch() return the streamed data in a nested dictionary
240
+ with all the usefull informations.
241
+ - NextWheel.stop_streaming() stops the stream.
242
+ It needs an IP address like in the example below.
243
+
244
+ The wheel sends its data to the UDP address and port given to
245
+ NextWheel.start_streaming() (by default, broadcast 255.255.255.255 on
246
+ port 12345). Use a different port for each wheel to reduce network load
247
+ on each receiver. The local firewall must accept incoming UDP datagrams
248
+ on this port.
249
+
250
+ Raises
251
+ ------
252
+ TimeoutError
253
+ When ip.setter cannot connect to the wheel.
254
+
255
+
256
+ Exemple
257
+ -------
258
+ >>> from nextwheel import NextWheel
259
+ >>> nw = NextWheel("196.168.1.254")
260
+ >>> nw.start_streaming()
261
+
262
+ >>> print(nw.fetch())
263
+
264
+ >>> nw.stop_streaming()
265
+
266
+ """
267
+
268
+ def __init__(
269
+ self,
270
+ ip: str = "",
271
+ *,
272
+ port: int = 80,
273
+ debug: bool = False,
274
+ ):
275
+ # General configuration
276
+ self.max_imu_samples = 0
277
+ self.max_analog_samples = 0
278
+ self.max_encoder_samples = 0
279
+ self.max_power_samples = 0
280
+ self._config = GlobalConfig()
281
+ self._debug = debug
282
+ self.has_calibration_matrix = False
283
+ self.calibration_matrix: np.ndarray = np.eye(6)
284
+ self.calibration_offset: np.ndarray = np.zeros(6)
285
+
286
+ # Communication stuff (port must be set before ip, which connects)
287
+ self.port = port
288
+ self.ip = ip
289
+ self._udp_socket: None | socket.socket = None
290
+ self._udp_thread: None | threading.Thread = None
291
+ self._stop_event = threading.Event()
292
+ self._mutex = threading.Lock()
293
+ self._thread_is_running = False
294
+
295
+ # Data buffers. In these arrays, column 0 is the time
296
+ self._adc_values = [] # type: list[np.ndarray]
297
+ self._imu_values = [] # type: list[np.ndarray]
298
+ self._power_values = [] # type: list[np.ndarray]
299
+ self._encoder_values = [] # type: list[np.ndarray]
300
+
301
+ @property
302
+ def ip(self):
303
+ """Getter for ip property."""
304
+ return self._ip
305
+
306
+ @ip.setter
307
+ def ip(self, value):
308
+ """Setter for ip property."""
309
+ self._ip = value
310
+ if value == "":
311
+ return
312
+
313
+ try:
314
+ self.set_time(get_current_time())
315
+ except ConnectTimeout as e:
316
+ raise TimeoutError from e
317
+
318
+ # Calibration constants
319
+ self.file_download("calibration.json")
320
+ self._load_calibration("calibration.json")
321
+
322
+ def _load_calibration(self, filename="calibration.json"):
323
+ """Load calibration file if the file is found."""
324
+ try:
325
+ with open(filename, encoding="utf8") as json_file:
326
+ calibration = json.load(json_file)
327
+ self.calibration_matrix = np.array(calibration["Matrix"])
328
+ self.calibration_offset = np.array(calibration["Offset"])
329
+ self.has_calibration_matrix = True
330
+ except FileNotFoundError:
331
+ self.calibration_matrix = np.identity(6)
332
+ self.calibration_offset = np.zeros((6,))
333
+ self.has_calibration_matrix = False
334
+ print("No Calibration File Detected")
335
+
336
+ def _parse_message(self, stream: bytes, offset: int = 0) -> int: # noqa PLR0911 Too many return statements (7 > 6)
337
+ """
338
+ Parse a series of bytes corresponding to a messages.
339
+
340
+ It only parses the first message from the stream, then returns the
341
+ position of the next message in the stream. If the received message
342
+ is a superframe, all messages from the superframe are parsed and
343
+ processed.
344
+
345
+ Parameters
346
+ ----------
347
+ stream
348
+ The received message or the data stored in the dat file.
349
+ offset
350
+ Start processing at this position in the stream.
351
+
352
+ Returns
353
+ -------
354
+ offset
355
+ The position of the next message in the stream.
356
+
357
+ """
358
+ if not isinstance(stream, bytes):
359
+ raise ValueError("stream must be bytes.")
360
+
361
+ if offset >= len(stream):
362
+ return offset
363
+
364
+ # Extract the type of message
365
+ frame_type, timestamp, data_size = struct.unpack(
366
+ "<BQB", stream[offset : offset + 10]
367
+ )
368
+ offset += 10
369
+ time = timestamp / 1e6
370
+
371
+ # Process the frame
372
+ if frame_type == FrameType.CONFIG:
373
+ # Config frame (should always be first)
374
+ data = stream[offset : offset + data_size]
375
+ offset += data_size
376
+
377
+ (
378
+ accel_range,
379
+ gyro_range,
380
+ mag_range,
381
+ imu_sampling_rate,
382
+ adc_sampling_rate,
383
+ encoder_sampling_rate,
384
+ ) = struct.unpack("<6I", data)
385
+
386
+ # Update configuration
387
+ self._config.accel_range = accel_range
388
+ self._config.gyro_range = gyro_range
389
+ self._config.mag_range = mag_range
390
+ self._config.imu_sampling_rate = imu_sampling_rate
391
+ self._config.adc_sampling_rate = adc_sampling_rate
392
+ self._config.encoder_sampling_rate = encoder_sampling_rate
393
+ return offset
394
+
395
+ if frame_type == FrameType.ADC: # frame type of the ADC values
396
+ data = stream[offset : offset + data_size]
397
+ offset += data_size
398
+
399
+ self._adc_values.append(
400
+ np.hstack([[time], struct.unpack("<8H", data)])
401
+ )
402
+
403
+ if len(self._adc_values) > self.max_analog_samples:
404
+ self._adc_values.pop(0)
405
+ return offset
406
+
407
+ if frame_type == FrameType.IMU: # frame type of the IMU
408
+ data = stream[offset : offset + data_size]
409
+ offset += data_size
410
+
411
+ self._imu_values.append(
412
+ np.hstack(
413
+ [
414
+ [time],
415
+ self._config.convert_accel_int_to_m_s2(
416
+ np.array(struct.unpack_from("<3h", data[:6]))
417
+ ),
418
+ self._config.convert_gyro_int_to_deg_s(
419
+ np.array(struct.unpack_from("<3h", data[6:12]))
420
+ ),
421
+ self._config.convert_mag_values(
422
+ np.array(struct.unpack_from("<3h", data[12:18]))
423
+ ),
424
+ ]
425
+ )
426
+ )
427
+
428
+ if len(self._imu_values) > self.max_imu_samples:
429
+ self._imu_values.pop(0)
430
+ return offset
431
+
432
+ if frame_type == FrameType.POWER: # frame type of the POWER
433
+ data = stream[offset : offset + data_size]
434
+ offset += data_size
435
+
436
+ self._power_values.append(
437
+ np.hstack([[time], struct.unpack_from("<3fB", data)])
438
+ )
439
+
440
+ if len(self._power_values) > self.max_power_samples:
441
+ self._power_values.pop(0)
442
+ return offset
443
+
444
+ if frame_type == FrameType.ENCODER: # frame type of the ENCODER
445
+ data = stream[offset : offset + data_size]
446
+ offset += data_size
447
+
448
+ self._encoder_values.append(
449
+ np.hstack(
450
+ [
451
+ [time],
452
+ self._config.convert_encoder_values(
453
+ np.array(struct.unpack_from("<q", data))
454
+ ),
455
+ ]
456
+ )
457
+ )
458
+
459
+ if len(self._encoder_values) > self.max_encoder_samples:
460
+ self._encoder_values.pop(0)
461
+ return offset
462
+
463
+ if frame_type == FrameType.SUPERFRAME:
464
+ for _ in range(data_size): # data_size = number of frames
465
+ offset = self._parse_message(stream, offset)
466
+ return offset
467
+
468
+ raise ValueError(f"Received an unknown frame type: {frame_type}")
469
+
470
+ def _parse_datagram(self, datagram: bytes) -> None:
471
+ """
472
+ Parse all messages contained in a received UDP datagram.
473
+
474
+ Parameters
475
+ ----------
476
+ datagram
477
+ Information containing data in bytes.
478
+
479
+ """
480
+ offset = 0
481
+ with self._mutex:
482
+ while (
483
+ new_offset := self._parse_message(datagram, offset)
484
+ ) > offset:
485
+ offset = new_offset
486
+
487
+ def _receive_loop(self, udp_socket: socket.socket, wheel_ip: str) -> None:
488
+ """
489
+ Receive UDP datagrams from the wheel until streaming is stopped.
490
+
491
+ Parameters
492
+ ----------
493
+ udp_socket
494
+ Bound UDP socket with a timeout.
495
+ wheel_ip
496
+ Datagrams not coming from this address (e.g., from another wheel
497
+ broadcasting on the same port) are ignored.
498
+
499
+ """
500
+ while not self._stop_event.is_set():
501
+ try:
502
+ datagram, (sender_ip, _) = udp_socket.recvfrom(
503
+ UDP_RECEIVE_BUFFER_SIZE
504
+ )
505
+ except TimeoutError:
506
+ continue
507
+ except OSError:
508
+ # Socket closed by stop_streaming
509
+ break
510
+
511
+ if sender_ip != wheel_ip:
512
+ continue
513
+
514
+ if self._debug:
515
+ print(f"Received {len(datagram)} bytes from {sender_ip}")
516
+
517
+ try:
518
+ self._parse_datagram(datagram)
519
+ except (ValueError, struct.error) as e:
520
+ # A corrupted datagram should not stop the stream
521
+ print("Invalid datagram received: ", e)
522
+
523
+ def _update_config_from_params(self, params: dict) -> None:
524
+ """Update the conversion configuration from get_sensors_params()."""
525
+ with self._mutex:
526
+ self._config.accel_range = params["accelerometer_precision"]
527
+ self._config.gyro_range = params["gyrometer_precision"]
528
+ self._config.imu_sampling_rate = params["imu_sampling_rate"]
529
+ self._config.adc_sampling_rate = params["adc_sampling_rate"]
530
+ self._config.encoder_sampling_rate = params[
531
+ "encoder_sampling_rate"
532
+ ]
533
+
534
+ def start_streaming( # noqa: PLR0913
535
+ self,
536
+ address: str = "255.255.255.255",
537
+ port: int = DEFAULT_UDP_PORT,
538
+ *,
539
+ max_imu_samples: int = 1000,
540
+ max_analog_samples: int = 1000,
541
+ max_encoder_samples: int = 100,
542
+ max_power_samples: int = 10,
543
+ ):
544
+ """
545
+ Start streaming.
546
+
547
+ Parameters
548
+ ----------
549
+ address : str, optional
550
+ IP address (x.x.x.x) where the wheel sends the streamed data. The
551
+ default 255.255.255.255 broadcasts to the whole local network.
552
+ Using the IP address of this computer avoids broadcasting.
553
+ port : int, optional
554
+ UDP port where the wheel sends the streamed data, and where this
555
+ computer listens. The default is 12345.
556
+ max_imu_samples : int, optional
557
+ Maximum IMU data to keep in memory. The default is 1000.
558
+ max_analog_samples : int, optional
559
+ Maximum ADC data to keep in memory. The default is 1000.
560
+ max_encoder_samples : int, optional
561
+ Maximum encoder data to keep in memory. The default is 100.
562
+ max_power_samples : int, optional
563
+ Maximum Power data to keep in memory. The default is 10.
564
+
565
+ Returns
566
+ -------
567
+ None.
568
+
569
+ """
570
+ if self._thread_is_running:
571
+ return
572
+
573
+ self.max_imu_samples = max_imu_samples
574
+ self.max_analog_samples = max_analog_samples
575
+ self.max_encoder_samples = max_encoder_samples
576
+ self.max_power_samples = max_power_samples
577
+
578
+ # The config frame is only sent when streaming starts, which may have
579
+ # been done by another client. Get the current config from the REST
580
+ # API so that the data can be converted in any case.
581
+ params = self.get_sensors_params()
582
+ self._update_config_from_params(params)
583
+
584
+ udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
585
+ udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
586
+ if hasattr(socket, "SO_REUSEPORT"):
587
+ # Allows several NextWheel instances to receive broadcasts on the
588
+ # same port.
589
+ udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
590
+ udp_socket.bind(("", port))
591
+ udp_socket.settimeout(UDP_SOCKET_TIMEOUT)
592
+ self._udp_socket = udp_socket
593
+
594
+ self._stop_event.clear()
595
+ self._udp_thread = threading.Thread(
596
+ target=self._receive_loop,
597
+ args=(udp_socket, socket.gethostbyname(self.ip)),
598
+ daemon=True,
599
+ )
600
+ self._udp_thread.start()
601
+ self._thread_is_running = True
602
+
603
+ if self._debug:
604
+ print(
605
+ "Listening on UDP port",
606
+ port,
607
+ "for data sent to",
608
+ address,
609
+ )
610
+
611
+ # Ask the wheel to start streaming. "Already streaming" means that
612
+ # another client started it (possibly to another destination), which
613
+ # is fine.
614
+ response = requests.get(
615
+ f"http://{self.ip}:{self.port}/start_streaming",
616
+ params={"address": address, "port": str(port)},
617
+ )
618
+ already_streaming = (
619
+ response.status_code == HTTP_RESPONSE_CODE_BAD_REQUEST
620
+ and "Already streaming" in response.text
621
+ )
622
+ if (
623
+ response.status_code != HTTP_RESPONSE_CODE_OK
624
+ and not already_streaming
625
+ ):
626
+ self.stop_streaming()
627
+ raise RuntimeError(
628
+ f"Could not start streaming: {response.status_code} "
629
+ f"{response.text}"
630
+ )
631
+
632
+ def stop_streaming(self):
633
+ """
634
+ Stop streaming.
635
+
636
+ Returns
637
+ -------
638
+ None.
639
+
640
+ """
641
+ if not self._thread_is_running:
642
+ return
643
+
644
+ try:
645
+ requests.get(f"http://{self.ip}:{self.port}/stop_streaming")
646
+ finally:
647
+ self._stop_event.set()
648
+ if self._udp_thread is not None:
649
+ self._udp_thread.join()
650
+ self._udp_thread = None
651
+ if self._udp_socket is not None:
652
+ self._udp_socket.close()
653
+ self._udp_socket = None
654
+ self._thread_is_running = False
655
+
656
+ def monitor(
657
+ self, address: str = "255.255.255.255", port: int = DEFAULT_UDP_PORT
658
+ ) -> None:
659
+ """
660
+ Blocking function that monitors the sensors measurements.
661
+
662
+ This function shows the current sensor states in a simple GUI
663
+ for testing and monitoring. To log data, use NextWheel.fetch()
664
+ instead. See start_streaming() for address and port.
665
+ """
666
+ # Don't import until needed.
667
+ import nextwheel.monitor as nwm # noqa
668
+
669
+ self.start_streaming(address, port)
670
+ nwm.monitor(self)
671
+ self.stop_streaming()
672
+
673
+ def graphical_monitor(self) -> None: # noqa because under development
674
+ """
675
+ Blocking function that monitors using Matplotlib.
676
+
677
+ Under development.
678
+ """
679
+ import matplotlib as mpl # noqa
680
+ from cycler import cycler # noqa
681
+ import matplotlib.pyplot as plt # noqa
682
+ from matplotlib import animation # noqa
683
+
684
+ is_running = [True]
685
+
686
+ def on_close(_):
687
+ is_running[0] = False
688
+
689
+ self.start_streaming()
690
+ plt.pause(0.5)
691
+
692
+ # Matplotlib defaults
693
+ mpl.rcParams["axes.prop_cycle"] = cycler(
694
+ color=["r", "g", "b", "c", "m", "y", "k", "tab:orange"]
695
+ )
696
+ mpl.rcParams["figure.figsize"] = [10, 5]
697
+ mpl.rcParams["figure.dpi"] = 75
698
+ mpl.rcParams["lines.linewidth"] = 1
699
+
700
+ # Create figure and plots
701
+ fig = plt.figure(figsize=(12, 8))
702
+ fig.canvas.mpl_connect("close_event", on_close)
703
+
704
+ adc_plot = plt.subplot(4, 1, 1)
705
+ adc_plot.set_title("Analogs")
706
+ adc_lines = []
707
+ for i in range(6):
708
+ adc_lines.append(adc_plot.plot([])[0])
709
+ adc_plot.set_xlim(0, self.max_analog_samples)
710
+ adc_plot.set_ylim(0, 65536)
711
+ adc_plot.set_ylabel("Raw channels")
712
+
713
+ accel_plot = plt.subplot(4, 1, 2)
714
+ accel_plot.set_title("Accelerometers")
715
+ accel_lines = []
716
+ for i in range(3):
717
+ accel_lines.append(accel_plot.plot([])[0])
718
+ accel_plot.set_xlim(0, self.max_imu_samples)
719
+ accel_plot.set_ylim(-20, 20)
720
+ accel_plot.set_ylabel("m/s2")
721
+
722
+ gyro_plot = plt.subplot(4, 1, 3)
723
+ gyro_plot.set_title("Gyrometers")
724
+ gyro_lines = []
725
+ for i in range(3):
726
+ gyro_lines.append(gyro_plot.plot([])[0])
727
+ gyro_plot.set_xlim(0, self.max_imu_samples)
728
+ gyro_plot.set_ylim(-1000, 1000)
729
+ gyro_plot.set_ylabel("deg/s")
730
+
731
+ encoder_plot = plt.subplot(4, 1, 4)
732
+ encoder_plot.set_title("Encoder")
733
+ encoder_line = encoder_plot.plot([])[0]
734
+ encoder_plot.set_xlim(0, self.max_encoder_samples)
735
+ encoder_plot.set_ylim(0, 4000)
736
+ encoder_plot.set_ylabel("radians")
737
+
738
+ plt.tight_layout()
739
+
740
+ def on_timer(_):
741
+ self._mutex.acquire()
742
+ adc_values = np.zeros((self.max_analog_samples, 9))
743
+ adc_values[-len(self._adc_values) :] = np.array(self._adc_values)
744
+
745
+ imu_values = np.zeros((self.max_imu_samples, 10))
746
+ imu_values[-len(self._imu_values) :] = np.array(self._imu_values)
747
+
748
+ encoder_values = np.zeros((self.max_encoder_samples, 2))
749
+ encoder_values[-len(self._encoder_values) :] = (
750
+ np.array(self._encoder_values) % 4000
751
+ )
752
+ self._mutex.release()
753
+
754
+ try:
755
+ for i in range(6):
756
+ adc_lines[i].set_data(
757
+ range(self.max_analog_samples), adc_values[:, i + 1]
758
+ )
759
+ except ValueError:
760
+ pass
761
+
762
+ try:
763
+ for i in range(3):
764
+ accel_lines[i].set_data(
765
+ range(self.max_imu_samples), imu_values[:, i + 1]
766
+ )
767
+
768
+ for i in range(3):
769
+ gyro_lines[i].set_data(
770
+ range(self.max_imu_samples), imu_values[:, i + 4]
771
+ )
772
+ except ValueError:
773
+ pass
774
+
775
+ try:
776
+ encoder_line.set_data(
777
+ range(self.max_encoder_samples), encoder_values[:, 1]
778
+ )
779
+ except ValueError:
780
+ pass
781
+
782
+ animation.FuncAnimation(
783
+ fig,
784
+ on_timer, # type: ignore
785
+ interval=33,
786
+ cache_frame_data=False,
787
+ ) # 30 ips
788
+
789
+ while is_running[0]:
790
+ plt.pause(0.5)
791
+
792
+ self.stop_streaming()
793
+
794
+ def fetch(self, clear=False) -> dict[str, TimeSeries]:
795
+ """
796
+ Fetch data and return a nested dictionary. Clear the buffer.
797
+
798
+ Returns a dictionary of multiple TimeSeries, the dictionary having
799
+ these keys:
800
+ - Analog:
801
+ TimeSeries that corresponds to the ADC values, and that contains
802
+ these data:
803
+ "Channels": Nx6 raw uncalibrated voltage values as integers.
804
+ "Forces": Nx4 forces in newtons in the form [Fx, Fy, Fz, 0.0].
805
+ "Moments": Nx4 moments in Nm in the form [Mx, My, Mz, 0.0].
806
+ Forces and Moments are present only if a calibration matrix was
807
+ found.
808
+ - IMU:
809
+ TimeSeries that corresponds to the Inertial Measurement Unit, and
810
+ that contains these data:
811
+ "Acc": Nx4 acceleration in m/s2, in the form
812
+ [acc_x, acc_y, acc_z, 0.0]
813
+ "Gyro": Nx4 angular speed in rad/s, in the form
814
+ [w_x, w_y, w_z, 0.0]
815
+ "Mag": Nx4 magnetometer values, uncalibrated, in the form
816
+ [mag_x, mag_y, mag_z, 0.0]
817
+ - Encoder:
818
+ TimeSeries that corresponds to the angular encoder, and that
819
+ contains this data:
820
+ "Angle": series of N angles in radians.
821
+ - Power:
822
+ TimeSeries that corresponds to the electric power meter, and that
823
+ contains these data:
824
+ "Voltage": series of N battery voltages in Volts.
825
+ "Current": series of N battery currents in Amperes.
826
+ "Power": series of N battery powers in Watts.
827
+
828
+ Parameters
829
+ ----------
830
+ clear
831
+ True to clear the buffer so that every call to fetch returns new
832
+ data. False to return the whole buffer, which size is set by the
833
+ arguments of NextWheel.start_streaming().
834
+
835
+ Returns
836
+ -------
837
+ data : dict[str, TimeSeries]
838
+ A dictionary of multiple TimeSeries described above.
839
+
840
+ """
841
+ self._mutex.acquire()
842
+
843
+ # Concatenate all samples in big numpy arrays
844
+ adc_values = np.array(self._adc_values)
845
+ imu_values = np.array(self._imu_values)
846
+ encoder_values = np.array(self._encoder_values)
847
+ power_values = np.array(self._power_values)
848
+
849
+ if len(adc_values) == 0:
850
+ adc_values = np.zeros((0, 7))
851
+ if len(imu_values) == 0:
852
+ imu_values = np.zeros((0, 10))
853
+ if len(encoder_values) == 0:
854
+ encoder_values = np.zeros((0, 2))
855
+ if len(power_values) == 0:
856
+ power_values = np.zeros((0, 4))
857
+
858
+ if clear:
859
+ # Reset the buffers
860
+ self._adc_values = []
861
+ self._imu_values = []
862
+ self._encoder_values = []
863
+ self._power_values = []
864
+
865
+ self._mutex.release()
866
+
867
+ if len(adc_values) > 0 and self.has_calibration_matrix:
868
+ calibrated_adc_values = (
869
+ np.dot(self.calibration_matrix, adc_values[:, 1:7].T).T
870
+ - self.calibration_offset
871
+ )
872
+ else:
873
+ calibrated_adc_values = adc_values
874
+
875
+ data = {}
876
+ data["Analog"] = TimeSeries(time=adc_values[:, 0])
877
+ data["Analog"].data["Channels"] = adc_values[:, 1:8]
878
+ data["Analog"].info["Channels"] = {"Unit": "raw"}
879
+ if self.has_calibration_matrix:
880
+ data["Analog"].data["Forces"] = np.zeros(
881
+ (calibrated_adc_values.shape[0], 4)
882
+ )
883
+ data["Analog"].data["Forces"][:, 0:3] = calibrated_adc_values[
884
+ :, 0:3
885
+ ]
886
+ data["Analog"].info["Forces"] = {"Unit": "N"}
887
+ data["Analog"].data["Moments"] = np.zeros(
888
+ (calibrated_adc_values.shape[0], 4)
889
+ )
890
+ data["Analog"].data["Moments"][:, 0:3] = calibrated_adc_values[
891
+ :, 3:6
892
+ ]
893
+ data["Analog"].info["Moments"] = {"Unit": "Nm"}
894
+
895
+ data["IMU"] = TimeSeries(
896
+ time=imu_values[:, 0],
897
+ data={
898
+ "Acc": imu_values[:, 1:4],
899
+ "Gyro": imu_values[:, 4:7],
900
+ "Mag": imu_values[:, 7:],
901
+ },
902
+ info={
903
+ "Acc": {"Unit": "m/s2"},
904
+ "Gyro": {"Unit": "rad/s"},
905
+ "Mag": {"Unit": "uncalibrated"},
906
+ },
907
+ )
908
+ data["Encoder"] = TimeSeries(
909
+ time=encoder_values[:, 0],
910
+ data={"Angle": encoder_values[:, 1]},
911
+ info={"Angle": {"Unit": "radians"}},
912
+ )
913
+ data["Power"] = TimeSeries(
914
+ time=power_values[:, 0],
915
+ data={
916
+ "Voltage": power_values[:, 1],
917
+ "Current": power_values[:, 2],
918
+ "Power": power_values[:, 3],
919
+ },
920
+ info={
921
+ "Voltage": {"Unit": "V"},
922
+ "Current": {"Unit": "A"},
923
+ "Power": {"Unit": "W"},
924
+ },
925
+ )
926
+ return data
927
+
928
+ def set_time(self, unix_time: int) -> None:
929
+ """
930
+ Set the time of the instrumented wheel.
931
+
932
+ Parameters
933
+ ----------
934
+ unix_time : int
935
+ Unix time in seconds.
936
+
937
+ Returns
938
+ -------
939
+ requests.Response
940
+
941
+ """
942
+ requests.post(
943
+ f"http://{self.ip}:{self.port}/config_set_time",
944
+ params={"time": unix_time},
945
+ )
946
+
947
+ def set_sensors_params( # noqa: PLR0913
948
+ self,
949
+ adc_sampling_rate: int,
950
+ imu_sampling_rate: int,
951
+ encoder_sampling_rate: int,
952
+ accelerometer_precision: int,
953
+ gyrometer_precision: int,
954
+ ) -> requests.Response:
955
+ """
956
+ Set the parameters of the instrumented wheel.
957
+
958
+ Parameters
959
+ ----------
960
+ adc_sampling_rate
961
+ Sampling rate for the forces in Hz. Valid values are 120, 240,
962
+ 480, 960, 1000, 2000
963
+ imu_sampling_rate
964
+ Sampling rate for the IMU, in Hz. Valid values are 60, 120, 240.
965
+ encoder_sampling_rate
966
+ Sampling rate for the encoder, in Hz. Valid values are 60, 120,
967
+ 240.
968
+ accelerometer_precision
969
+ Accelerometer range, in g. Value values are 2, 4, 8, 16.
970
+ gyrometer_precision
971
+ Gyrometer range, in degrees per second. Valid values are 250, 500,
972
+ 1000, 2000.
973
+
974
+ Returns
975
+ -------
976
+ requests.Response
977
+
978
+ """
979
+ params = {
980
+ "adc_sampling_rate": adc_sampling_rate,
981
+ "imu_sampling_rate": imu_sampling_rate,
982
+ "encoder_sampling_rate": encoder_sampling_rate,
983
+ "accelerometer_precision": accelerometer_precision,
984
+ "gyrometer_precision": gyrometer_precision,
985
+ }
986
+
987
+ response = requests.post(
988
+ f"http://{self.ip}:{self.port}/config_update", params=params
989
+ )
990
+ return response
991
+
992
+ def get_sensors_params(self) -> dict:
993
+ """
994
+ Get the parameters of the instrumented wheel sensors.
995
+
996
+ Returns
997
+ -------
998
+ dict
999
+ A dictionary in the form parameter:value.
1000
+
1001
+ """
1002
+ response = requests.get(f"http://{self.ip}:{self.port}/config")
1003
+ return json.loads(response.content)
1004
+
1005
+ def get_system_state(self) -> dict:
1006
+ """
1007
+ Get the system state of the instrumented wheel.
1008
+
1009
+ Returns
1010
+ -------
1011
+ dict
1012
+ A dictionary in the form parameter:value
1013
+
1014
+ """
1015
+ response = requests.get(f"http://{self.ip}:{self.port}/system_state")
1016
+ return json.loads(response.content)
1017
+
1018
+ def get_ntp_time(self) -> dict:
1019
+ """
1020
+ Get the NTP time status of the instrumented wheel.
1021
+
1022
+ Returns
1023
+ -------
1024
+ dict
1025
+ A dictionary with keys synced, server, time (epoch seconds),
1026
+ local_time (ISO 8601), last_sync (epoch seconds) and
1027
+ adjusting (remaining gradual correction in seconds).
1028
+
1029
+ """
1030
+ response = requests.get(f"http://{self.ip}:{self.port}/ntp_time")
1031
+ return json.loads(response.content)
1032
+
1033
+ def resync_ntp(self) -> None:
1034
+ """
1035
+ Request a new NTP synchronization on the instrumented wheel.
1036
+
1037
+ NTP is stopped after the next synchronization. Use get_ntp_time()
1038
+ to check when last_sync is updated.
1039
+
1040
+ Returns
1041
+ -------
1042
+ None
1043
+
1044
+ """
1045
+ requests.get(f"http://{self.ip}:{self.port}/ntp_resync")
1046
+
1047
+ def start_recording(self) -> None:
1048
+ """
1049
+ Start recording data on the instrumented wheel.
1050
+
1051
+ Returns
1052
+ -------
1053
+ None
1054
+
1055
+ """
1056
+ requests.get(f"http://{self.ip}:{self.port}/start_recording")
1057
+
1058
+ def stop_recording(self) -> None:
1059
+ """
1060
+ Stop recording data on the instrumented wheel.
1061
+
1062
+ Returns
1063
+ -------
1064
+ None
1065
+
1066
+ """
1067
+ requests.get(f"http://{self.ip}:{self.port}/stop_recording")
1068
+
1069
+ def file_list(self) -> dict:
1070
+ """
1071
+ Get the list of files on the instrumented wheel.
1072
+
1073
+ Returns
1074
+ -------
1075
+ dict
1076
+
1077
+ """
1078
+ response = requests.get(f"http://{self.ip}:{self.port}/file_list")
1079
+ return json.loads(response.content)
1080
+
1081
+ def file_download(self, filename: str, save_path: str = ".") -> int:
1082
+ """
1083
+ Download a file from the instrumented wheel.
1084
+
1085
+ Parameters
1086
+ ----------
1087
+ filename
1088
+ The name of the file to download
1089
+ save_path
1090
+ Optional. Where to save the file. The default is the current
1091
+ folder.
1092
+
1093
+ Returns
1094
+ -------
1095
+ int
1096
+ The size of the file
1097
+
1098
+ """
1099
+ param = {"file": filename}
1100
+ response = requests.get(
1101
+ f"http://{self.ip}:{self.port}/file_download",
1102
+ params=param,
1103
+ stream=True,
1104
+ )
1105
+ if response.status_code == HTTP_RESPONSE_CODE_OK:
1106
+ with open(os.path.join(save_path, filename), "wb") as f:
1107
+ f.write(response.content)
1108
+ # Return size...
1109
+ return f.tell()
1110
+
1111
+ # filed download
1112
+ return 0
1113
+
1114
+ def file_delete(self, filename: str) -> None:
1115
+ """
1116
+ Delete a file from the instrumented wheel.
1117
+
1118
+ Parameters
1119
+ ----------
1120
+ filename
1121
+ The name of the file to delete
1122
+
1123
+ Returns
1124
+ -------
1125
+ None
1126
+
1127
+ """
1128
+ requests.get(
1129
+ f"http://{self.ip}:{self.port}/file_delete",
1130
+ params={"file": filename},
1131
+ )
1132
+
1133
+
1134
+ def read_dat(filename: str, calibration_file: str | None = None) -> dict:
1135
+ """
1136
+ Read a dat file downloaded from the instrumented wheel.
1137
+
1138
+ Parameters
1139
+ ----------
1140
+ filename
1141
+ The name of the local file to read.
1142
+ calibration_file
1143
+ Optional. The calibration.json file, as found on the wheel and
1144
+ downloaded using NextWheel.file_download("calibration.json"). If not
1145
+ provided, forces and moments are not reconstructed from channel values.
1146
+
1147
+ Returns
1148
+ -------
1149
+ dict
1150
+ A dictionary with the same structure as given by NextWheel.fetch.
1151
+
1152
+ """
1153
+ # Create a dummy wheel to parse the data
1154
+ nw = NextWheel()
1155
+ if calibration_file is not None:
1156
+ nw._load_calibration(calibration_file)
1157
+ nw.max_analog_samples = int(1e15)
1158
+ nw.max_encoder_samples = int(1e15)
1159
+ nw.max_imu_samples = int(1e15)
1160
+ nw.max_power_samples = int(1e15)
1161
+
1162
+ offset = 0
1163
+ with open(filename, "rb") as fid:
1164
+ stream = fid.read()
1165
+
1166
+ while (new_offset := nw._parse_message(stream, offset)) > offset:
1167
+ offset = new_offset
1168
+
1169
+ return nw.fetch()