From 190c37b07102dccc214b7487f746d98392ae4c74 Mon Sep 17 00:00:00 2001
From: TomokiMochizuki <mochizuki@space.t.u-tokyo.ac.jp>
Date: Wed, 5 Jun 2024 18:23:56 +0900
Subject: [PATCH] add python script

---
 scripts/Plot/plot_euler_angle_observer.py | 75 +++++++++++++++++++++++
 1 file changed, 75 insertions(+)
 create mode 100644 scripts/Plot/plot_euler_angle_observer.py

diff --git a/scripts/Plot/plot_euler_angle_observer.py b/scripts/Plot/plot_euler_angle_observer.py
new file mode 100644
index 000000000..554579786
--- /dev/null
+++ b/scripts/Plot/plot_euler_angle_observer.py
@@ -0,0 +1,75 @@
+#
+# Plot Angular Velocity Observer
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# local function
+from common import find_latest_log_tag
+from common import add_log_file_arguments
+from common import read_3d_vector_from_csv
+from common import read_scalar_from_csv
+# arguments
+import argparse
+
+# Arguments
+aparser = argparse.ArgumentParser()
+aparser = add_log_file_arguments(aparser)
+aparser.add_argument('--no-gui', action='store_true')
+args = aparser.parse_args()
+
+#
+# Read Arguments
+#
+# log file path
+path_to_logs = args.logs_dir
+
+read_file_tag = args.file_tag
+if read_file_tag == None:
+  print("file tag does not found. use latest.")
+  read_file_tag = find_latest_log_tag(path_to_logs)
+
+print("log: " + read_file_tag)
+
+#
+# CSV file name
+#
+read_file_name  = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv'
+
+#
+# Data read and edit
+#
+# Read S2E CSV
+time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]')
+
+euler_angle_cantilever_rad = read_3d_vector_from_csv(read_file_name, 'euler_angular_cantilever_c', 'rad')
+
+#
+# Plot
+#
+unit = ' rad'
+
+fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
+axis[0, 0].plot(time[0][0:1200], euler_angle_cantilever_rad[0][0:1200], c="orange",  label="TRUE-X")
+axis[0, 0].legend(loc = 'upper right')
+
+axis[1, 0].plot(time[0][0:1200], euler_angle_cantilever_rad[1][0:1200], c="green",  label="TRUE-Y")
+axis[1, 0].legend(loc = 'upper right')
+
+axis[2, 0].plot(time[0][0:1200], euler_angle_cantilever_rad[2][0:1200], c="blue",   label="TRUE-Z")
+axis[2, 0].legend(loc = 'upper right')
+
+fig.suptitle("Euler Angle Observer")
+fig.supylabel("Euler Angle [" + unit + "]")
+fig.supxlabel("Time [s]")
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_angular_velocity_observer.png") # save last figure only
+else:
+  plt.show()