libwave/scripts/plot/quadrotor_plot.py at dad6ef7557c32a7bd2f46cb2b4e751dc12dc6120 · wavelab/libwave · 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
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
#!/usr/bin/env python3
import csv
import matplotlib.pylab as plt

QUADROTOR_ATTITUDE_CONTROLLER_OUTPUT = "/tmp/quadrotor_attitude_controller.dat"
QUADROTOR_POSITION_CONTROLLER_OUTPUT = "/tmp/quadrotor_position_controller.dat"
QUADROTOR_VELOCITY_CONTROLLER_OUTPUT = "/tmp/quadrotor_velocity_controller.dat"


def plot_quadrotor_attitude_data():
    attitude_data = {
        "t": [],
        "roll": [], "pitch": [], "yaw": []
    }

    attitude_output = open(QUADROTOR_ATTITUDE_CONTROLLER_OUTPUT, "r")
    attitude_reader = csv.reader(attitude_output, delimiter=",")
    next(attitude_reader)

    for row in attitude_reader:
        attitude_data["t"].append(float(row[0]))
        attitude_data["roll"].append(float(row[1]))
        attitude_data["pitch"].append(float(row[2]))
        attitude_data["yaw"].append(float(row[3]))

    plt.plot(attitude_data["t"], attitude_data["roll"], label="roll")
    plt.plot(attitude_data["t"], attitude_data["pitch"], label="pitch")
    plt.plot(attitude_data["t"], attitude_data["yaw"], label="yaw")

    plt.xlabel("Time (seconds)")
    plt.ylabel("Angle (degrees)")
    plt.legend(loc=0)
    plt.show()


def plot_quadrotor_position_data():
    position_data = {
        "t": [],
        "roll": [], "pitch": [], "yaw": [],
        "x": [], "y": [], "z": []
    }

    position_output = open(QUADROTOR_POSITION_CONTROLLER_OUTPUT, "r")
    position_reader = csv.reader(position_output, delimiter=",")
    next(position_reader)

    for row in position_reader:
        position_data["t"].append(float(row[0]))
        position_data["roll"].append(float(row[1]))
        position_data["pitch"].append(float(row[2]))
        position_data["yaw"].append(float(row[3]))
        position_data["x"].append(float(row[4]))
        position_data["y"].append(float(row[5]))
        position_data["z"].append(float(row[6]))

    plt.subplot(2, 1, 1)
    plt.plot(position_data["t"], position_data["roll"], label="roll")
    plt.plot(position_data["t"], position_data["pitch"], label="pitch")
    plt.plot(position_data["t"], position_data["yaw"], label="yaw")

    plt.xlabel("Time (seconds)")
    plt.ylabel("Angle (degrees)")
    plt.legend(loc=0)

    plt.subplot(2, 1, 2)
    plt.plot(position_data["t"], position_data["x"], label="x")
    plt.plot(position_data["t"], position_data["y"], label="y")
    plt.plot(position_data["t"], position_data["z"], label="z")

    plt.xlabel("Time (seconds)")
    plt.ylabel("Position (meters)")
    plt.legend(loc=0)

    plt.show()


def plot_quadrotor_velocity_data():
    data = {
        "t": [],
        "roll": [], "pitch": [], "yaw": [],
        "x": [], "y": [], "z": [],
        "vx": [], "vy": [], "vz": []
    }

    output = open(QUADROTOR_POSITION_CONTROLLER_OUTPUT, "r")
    reader = csv.reader(output, delimiter=",")
    next(reader)

    for row in reader:
        data["t"].append(float(row[0]))
        data["roll"].append(float(row[1]))
        data["pitch"].append(float(row[2]))
        data["yaw"].append(float(row[3]))
        data["x"].append(float(row[4]))
        data["y"].append(float(row[5]))
        data["z"].append(float(row[6]))
        data["vx"].append(float(row[7]))
        data["vy"].append(float(row[8]))
        data["vz"].append(float(row[9]))

    # plot attitude data
    plt.subplot(3, 1, 1)
    plt.plot(data["t"], data["roll"], label="roll")
    plt.plot(data["t"], data["pitch"], label="pitch")
    plt.plot(data["t"], data["yaw"], label="yaw")

    plt.xlabel("Time (s)")
    plt.ylabel("Angle (deg)")
    plt.legend(loc=0)

    # plot position data
    plt.subplot(3, 1, 2)
    plt.plot(data["t"], data["x"], label="x")
    plt.plot(data["t"], data["y"], label="y")
    plt.plot(data["t"], data["z"], label="z")

    plt.xlabel("Time (s)")
    plt.ylabel("Position (m)")
    plt.legend(loc=0)

    # plot velocity data
    plt.subplot(3, 1, 3)
    plt.plot(data["t"], data["vx"], label="vx")
    plt.plot(data["t"], data["vy"], label="vy")
    plt.plot(data["t"], data["vz"], label="vz")

    plt.xlabel("Time (s)")
    plt.ylabel("Velocity (ms^-1)")
    plt.legend(loc=0)

    plt.show()


if __name__ == "__main__":
    plot_quadrotor_attitude_data()
    plot_quadrotor_position_data()
    plot_quadrotor_velocity_data()