IMUTools/visualizer_options.py at master · AsiiaPine/IMUTools · GitHub

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
from typing import Callable
import numpy as np
from RedisPostman.models import IMUMessage, MadgwickMessage, Message, IMU9250Message
from config import imu_calibrated_message_channel, imu_raw_message_channel, madgwick_message_channel, imu_1_name, imu_2_name

def read_imu_1_data(message: IMU9250Message) -> dict[str, np.ndarray]:
    return {"acc": message.imu_1.acc, "gyr": message.imu_1.gyr, "mag": message.imu_1.mag, "quaternion": np.zeros(4)}

def read_imu_2_data(message: IMU9250Message) -> dict[str, np.ndarray]:
    return {"acc": message.imu_2.acc, "gyr": message.imu_2.gyr, "mag": message.imu_2.mag,"quaternion": np.zeros(4)}

def read_quaternion_1_data(message: MadgwickMessage) -> dict[str, np.ndarray]:
    return {"acc": np.zeros(3), "gyr": np.zeros(3), "mag": np.zeros(3),"quaternion": message.imu_1.value}

def read_quaternion_2_data(message: MadgwickMessage) -> dict[str, np.ndarray]:
    return {"acc": np.zeros(3), "gyr": np.zeros(3), "mag": np.zeros(3), "quaternion": message.imu_2.value}

options: dict[str, dict[str, bool| Callable | str | type[Message]]] = {
    "plot imu 1 raw data":
        {
            "to_draw_imu_data": True,
            "to_draw_3d": False,
            "channel": imu_raw_message_channel,
            "reader" : read_imu_1_data,
            "dataClass": IMU9250Message,
            "imu_name": imu_1_name
        },
    "plot imu 2 raw data" :
        {
            "to_draw_imu_data": True,
            "to_draw_3d": False,
            "channel": imu_raw_message_channel,
            "reader" : read_imu_2_data,
            "dataClass": IMU9250Message,
            "imu_name": imu_2_name


        },
    "plot imu 1 calibrated data" :
        {
            "to_draw_imu_data": True,
            "to_draw_3d": False,
            "channel": imu_calibrated_message_channel,
            "reader" : read_imu_1_data,
            "dataClass": IMU9250Message,
            "imu_name": imu_1_name

        },

    "plot imu 2 calibrated data" :
        {
            "to_draw_imu_data": True,
            "to_draw_3d": False,
            "channel": imu_calibrated_message_channel,
            "reader" : read_imu_2_data,
            "dataClass": IMU9250Message,
            "imu_name": imu_2_name

        },

    "draw imu 1 madgwick results":
        {
            "to_draw_imu_data": False,
            "to_draw_3d": True,
            "channel": madgwick_message_channel,
            "reader" : read_quaternion_1_data,
            "dataClass": MadgwickMessage,
            "imu_name": imu_1_name
        },
    "draw imu 2 madgwick results":
        {
            "to_draw_imu_data": False,
            "to_draw_3d": True,
            "channel": madgwick_message_channel,
            "reader" : read_quaternion_2_data,
            "dataClass": MadgwickMessage,
            "imu_name": imu_2_name
        },
}

options_names = options.keys()