From 73b316bef2034575e2783545cd6245e18f6fe2c6 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 12 Jun 2024 15:15:40 -0600 Subject: [PATCH] removed data_viz --- rosplane_tuning/CMakeLists.txt | 8 - rosplane_tuning/README.md | 6 - rosplane_tuning/src/data_viz/data_storage.py | 271 ------------------- rosplane_tuning/src/data_viz/data_viz.py | 235 ---------------- rosplane_tuning/src/data_viz/plot_window.py | 221 --------------- 5 files changed, 741 deletions(-) delete mode 100644 rosplane_tuning/src/data_viz/data_storage.py delete mode 100755 rosplane_tuning/src/data_viz/data_viz.py delete mode 100644 rosplane_tuning/src/data_viz/plot_window.py diff --git a/rosplane_tuning/CMakeLists.txt b/rosplane_tuning/CMakeLists.txt index 9174b8d..8d9020e 100644 --- a/rosplane_tuning/CMakeLists.txt +++ b/rosplane_tuning/CMakeLists.txt @@ -55,14 +55,6 @@ install(TARGETS signal_generator DESTINATION lib/${PROJECT_NAME}) -# Data viz -install(PROGRAMS - src/data_viz/data_storage.py - src/data_viz/data_viz.py - src/data_viz/plot_window.py - DESTINATION lib/${PROJECT_NAME} -) - #### END OF EXECUTABLES ### #### RQT #### diff --git a/rosplane_tuning/README.md b/rosplane_tuning/README.md index 0a4703e..1d854e3 100644 --- a/rosplane_tuning/README.md +++ b/rosplane_tuning/README.md @@ -57,9 +57,3 @@ To call a service from the command line use this command, replacing wi ros2 service call std_srvs/srv/Trigger ``` -## Data Viz - -Data viz is a python utility that can be used to plot the controller commands and response, as an alternative to PlotJuggler. To run data viz, -``` -ros2 run rosplane_tuning data_viz.py -``` diff --git a/rosplane_tuning/src/data_viz/data_storage.py b/rosplane_tuning/src/data_viz/data_storage.py deleted file mode 100644 index d9bf28b..0000000 --- a/rosplane_tuning/src/data_viz/data_storage.py +++ /dev/null @@ -1,271 +0,0 @@ -import bisect -from rclpy.node import Node -from rclpy.time import Time -from builtin_interfaces.msg import Time as TimeMsg -import threading -from rosplane_msgs.msg import State -from rosflight_msgs.msg import Command -from rosplane_msgs.msg import ControllerInternalsDebug -from rosplane_msgs.msg import ControllerCommands - - -def trim_data(time: list[float], data: list[float], val_thresh: float) -> None: - """Trims the time and data vectors to only have time values over the latest 'val_thresh' window""" - - ind = bisect.bisect_left(time, time[-1] - val_thresh) - if ind > 0: - del data[0:ind] - del time[0:ind] - -def time_to_seconds(time: TimeMsg) -> float: - """Converts the time to seconds - """ - seconds = time.sec + time.nanosec*1.e-9 - return seconds - -class StateStorage: - """Stores state data created from UavState""" - - def __init__(self, t_horizon) -> None: - """ Initialize the state storage - - Inputs: - t_horizon: Time horizon for storage - """ - - # Store variables - self.t_horizon = t_horizon # Time horizon - - # Create storage - self.time: list[float] = [] # Time values (seconds) - self.pn: list[float] = [] # North positions - self.pe: list[float] = [] # East positions - self.p_alt: list[float] = [] # Altitude - self.v_a: list[float] = [] # Airspeed - self.alpha: list[float] = [] # Angle of attack - self.beta: list[float] = [] # sideslip angle - self.phi: list[float] = [] # Attitude euler angles - self.theta: list[float] = [] - self.psi: list[float] = [] - self.chi: list[float] = [] # course angle - self.u: list[float] = [] # x,y,z body frame velocities - self.v: list[float] = [] - self.w: list[float] = [] - self.p: list[float] = [] # x,y,z body frame rotational velocities - self.q: list[float] = [] - self.r: list[float] = [] - self.v_g: list[float] = [] # Ground speed - self.w_n: list[float] = [] # North wind - self.w_e: list[float] = [] # East wind - - - def append(self, state: State): - """ Stores the state data and trims the vectors - """ - - if(state.header.stamp.sec == 0): - return - - # Store the data - self.time.append(time_to_seconds(state.header.stamp)) - self.pn.append(state.position.item(0)) - self.pe.append(state.position.item(1)) - self.p_alt.append(-state.position.item(2)) # -1 to get the altitude instead of down - self.phi.append(state.phi) - self.theta.append(state.theta) - self.psi.append(state.psi) - self.u.append(state.u) - self.v.append(state.v) - self.w.append(state.w) - self.p.append(state.p) - self.q.append(state.q) - self.r.append(state.r) - self.w_n.append(state.wn) - self.w_e.append(state.we) - self.v_a.append(state.va) - self.v_g.append(state.vg) - self.alpha.append(state.alpha) - self.beta.append(state.beta) - self.chi.append(state.chi) - - # Trim the data - ind = bisect.bisect_left(self.time, self.time[-1] - self.t_horizon) - if ind > 0: - del self.time[0:ind] - del self.pn[0:ind] - del self.pe[0:ind] - del self.p_alt[0:ind] - del self.phi[0:ind] - del self.theta[0:ind] - del self.psi[0:ind] - del self.u[0:ind] - del self.v[0:ind] - del self.w[0:ind] - del self.p[0:ind] - del self.q[0:ind] - del self.r[0:ind] - del self.w_n[0:ind] - del self.w_e[0:ind] - del self.v_a[0:ind] - del self.v_g[0:ind] - del self.alpha[0:ind] - del self.beta[0:ind] - del self.chi[0:ind] - -class CommandStorage: - """Stores the vehicle commands - """ - def __init__(self, t_horizon) -> None: - """ Initialize the command storage - """ - # Store the horizon variable - self.t_horizon = t_horizon - - # Create storage - self.time: list[float] = [] # Time values (seconds) - self.elevator: list[float] = [] - self.aileron: list[float] = [] - self.rudder: list[float] = [] - self.throttle: list[float] = [] - - def append(self, cmd: Command) -> None: - """ Stores the command data and trims the vectors - """ - # Append data - self.time.append(time_to_seconds(cmd.header.stamp)) - self.elevator.append(cmd.y) - self.aileron.append(cmd.x) - self.rudder.append(cmd.z) - self.throttle.append(cmd.f) - - # Trim the data - ind = bisect.bisect_left(self.time, self.time[-1] - self.t_horizon) - if ind > 0: - del self.time[0:ind] - del self.elevator[0:ind] - del self.aileron[0:ind] - del self.rudder[0:ind] - del self.throttle[0:ind] - -class ControllerInternalsDebugStorage: - - def __init__(self, t_horizon: float) -> None: - - self.t_horizon = t_horizon - - self.time: list[float] = [] # Time values (seconds) - self.pitch_c: list[float] = [] - self.roll_c: list[float] = [] - - def append(self, internals: ControllerInternalsDebug) -> None: - """ Stores the command data and trims the vectors - """ - # Append data - self.time.append(time_to_seconds(internals.header.stamp)) - self.pitch_c.append(internals.theta_c) - self.roll_c.append(internals.phi_c) - - # Trim the data - ind = bisect.bisect_left(self.time, self.time[-1] - self.t_horizon) - if ind > 0: - del self.time[0:ind] - del self.pitch_c[0:ind] - del self.roll_c[0:ind] - -class ControllerCommandsStorage: - - def __init__(self, t_horizon: float) -> None: - - self.t_horizon = t_horizon - - self.time: list[float] = [] # Time values (seconds) - self.course_c: list[float] = [] - self.alt_c: list[float] = [] - self.airspeed_c: list[float] = [] - - def append(self, con_cmd: ControllerCommands) -> None: - """ Stores the command data and trims the vectors - """ - # Append data - self.time.append(time_to_seconds(con_cmd.header.stamp)) - self.course_c.append(con_cmd.chi_c) - self.alt_c.append(con_cmd.h_c) - self.airspeed_c.append(con_cmd.va_c) - - # Trim the data - ind = bisect.bisect_left(self.time, self.time[-1] - self.t_horizon) - if ind > 0: - del self.time[0:ind] - del self.course_c[0:ind] - del self.alt_c[0:ind] - del self.airspeed_c[0:ind] - - -class RosStorageInterface: - """ Uses a given node to subscribe to state, command, and sensory information - """ - - def __init__(self, node: Node, t_horizon: float) -> None: - """ Create the ros interfaces required for storage - - Inputs: - node: Node to use for subscriptions - t_horizon: Time horizon of data to keep - """ - - # Store inputs - self.node = node - self.t_horizon = t_horizon - - # Initialize the state parameters - self.t_horizon = 100. # TODO Read in instead of hard code - self.lock = threading.Lock() # The lock is used to allow data to not be received / updated - # while being accessed - - - # Initialize the ros variables - self._sub_state = self.node.create_subscription(State, "/state", self.state_callback, 1) - self._sub_est = self.node.create_subscription(State, "/estimated_state", self.estimate_callback, 1) - self._sub_cmd = self.node.create_subscription(Command, "/command", self.command_callback, 1) - self._sub_cmd_internals = self.node.create_subscription(ControllerInternalsDebug, "/controller_inners_debug", self.cmd_internal_callback, 1) - self._sub_con_cmd = self.node.create_subscription(ControllerCommands, "/controller_commands", self.con_command_callback, 1) - - self.con_cmd = ControllerCommands() - - # Initailize the storage - self.true = StateStorage(t_horizon=self.t_horizon) - self.cmd = CommandStorage(t_horizon=self.t_horizon) - self.est = StateStorage(t_horizon=self.t_horizon) - self.con_inners = ControllerInternalsDebugStorage(t_horizon=self.t_horizon) - self.con_cmd = ControllerCommandsStorage(t_horizon=self.t_horizon) - - def state_callback(self, msg: State) -> None: - """Stores the latest state data - """ - with self.lock: - self.true.append(state=msg) - - def estimate_callback(self, msg: State) -> None: - """Stores the latest estimated state data - """ - # self.node.get_logger().info("In estimated state callback.") - with self.lock: - self.est.append(state=msg) - - - def command_callback(self, msg: Command) -> None: - """Stores the latest command data - """ - with self.lock: - self.cmd.append(cmd=msg) - - - def cmd_internal_callback(self, msg: ControllerInternalsDebug) -> None: - - with self.lock: - self.con_inners.append(internals=msg) - - def con_command_callback(self, msg: ControllerCommands) -> None: - - with self.lock: - self.con_cmd.append(con_cmd=msg) diff --git a/rosplane_tuning/src/data_viz/data_viz.py b/rosplane_tuning/src/data_viz/data_viz.py deleted file mode 100755 index d6695bb..0000000 --- a/rosplane_tuning/src/data_viz/data_viz.py +++ /dev/null @@ -1,235 +0,0 @@ -#!/usr/bin/env python3 -""" Plots data into a multi-window view -""" - -import matplotlib.pyplot as plt -from matplotlib.figure import Figure -import rclpy -from rclpy.executors import SingleThreadedExecutor -import time as pytime -import threading -from plot_window import PlotWindow, PlotNav, PlotAxis, PlotDiff -from data_storage import RosStorageInterface - -class StatePlotter: - """ Creates a plotting windows and repeatedly plots the data - """ - - def __init__(self, data: RosStorageInterface, plot_sensors: bool, plot_nav_err: bool) -> None: - """ Create the initial plots, subscriptions, and data storage - - Inputs: - data: Data storage class for obtaining the data to be plotted - plot_sensors: True => plot sensors, False => do not - """ - # Initialize general parameters - self._plot_sensors = plot_sensors # True => plot sensor data, False => do not - self._data = data - self._sensor_color = "hotpink" - - # Create the plotting window - self._pw = PlotWindow() - self._plotters: list[PlotAxis] = [] - self._figures: list[Figure] = [] - - # Create the individual figures - self._create_state_plotter() - # if plot_nav_err: - # self._create_nav_error_plotter() - # if self._plot_sensors: - # self._create_sensor_plotter() - - # print("initalized...") - - # Flush out plots and add figures to the GUI - for fig in self._figures: - fig.canvas.flush_events() - #self._pw.addPlot(name, fig) - - def _create_state_plotter(self) -> None: - """Creates the plot showing the full state of the system - """ - # Create the inidivual figures - fig, state_axis = plt.subplots(5, 4) - self._pw.addPlot("States", fig) - widget = self._figures.append(fig) - - # Create position plots - self._plotters.append(PlotNav( - true_data=self._data.true.pn, true_xdata=self._data.true.time, - nav_data=self._data.est.pn, nav_xdata=self._data.est.time, - true_label="$p_n$-True", nav_label="$p_n$-Est", show_legend=True, - ax=state_axis[0,0], title="x-axis", ylabel="$m$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.pe, true_xdata=self._data.true.time, - nav_data=self._data.est.pe, nav_xdata=self._data.est.time, - true_label="$p_e$-True", nav_label="$p_e$-Est", show_legend=True, - ax=state_axis[0,1], title="y-axis", ylabel="$m$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.p_alt, true_xdata=self._data.true.time, - nav_data=self._data.est.p_alt, nav_xdata=self._data.est.time, cmd_data=self._data.con_cmd.alt_c, - cmd_xdata=self._data.con_cmd.time, cmd_label="$alt_{cmd}$", - true_label="alt-True", nav_label="alt-Est", show_legend=True, - ax=state_axis[0,2], title="z-axis", ylabel="$m$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.est.w_n, true_xdata=self._data.est.time, - nav_data=self._data.est.w_e, nav_xdata=self._data.est.time, - ax=state_axis[0,3], title="Wind-estimated", ylabel="$m/s$", - true_label="$w_n$", nav_label="$w_e$", show_legend=True, - true_color="darkorange", nav_color="forestgreen", widget=widget - )) - - # Create linear velocity plots - self._plotters.append(PlotNav( - true_data=self._data.true.u, true_xdata=self._data.true.time, - nav_data=self._data.est.u, nav_xdata=self._data.est.time, - true_label="$u$-True", nav_label="$u$-Est", show_legend=True, - ax=state_axis[1,0], title="", ylabel="$m/s$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.v, true_xdata=self._data.true.time, - nav_data=self._data.est.v, nav_xdata=self._data.est.time, - true_label="$v$-True", nav_label="$v$-Est", show_legend=True, - ax=state_axis[1,1], title="", ylabel="$m/s$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.w, true_xdata=self._data.true.time, - nav_data=self._data.est.w, nav_xdata=self._data.est.time, - true_label="$w$-True", nav_label="$w$-Est", show_legend=True, - ax=state_axis[1,2], title="", ylabel="$m/s$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.alpha, true_xdata=self._data.true.time, - nav_data=self._data.true.beta, nav_xdata=self._data.true.time, - ax=state_axis[1,3], title="", ylabel="rad", - true_label="$\\alpha $", nav_label="$\\beta$", show_legend=False, - true_color="darkorange", nav_color="forestgreen", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.chi, true_xdata=self._data.true.time, cmd_data=self._data.con_cmd.course_c, - cmd_xdata=self._data.con_cmd.time, ax=state_axis[1,3], title="", ylabel="rad", - true_label="$\\chi$", cmd_label="$\\chi_{cmd}$", show_legend=True, - true_color="sienna", widget=widget - )) - - # Create attitude plots - self._plotters.append(PlotNav( - true_data=self._data.true.phi, true_xdata=self._data.true.time, - nav_data=self._data.est.phi, nav_xdata=self._data.est.time, cmd_data=self._data.con_inners.roll_c, - cmd_xdata=self._data.con_inners.time, true_label="$\\phi$-True", nav_label="$\\phi$-Est", - cmd_label="$\\phi_{cmd}$", show_legend=True, ax=state_axis[2,0], title="", ylabel="$rad$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.theta, true_xdata=self._data.true.time, - nav_data=self._data.est.theta, nav_xdata=self._data.est.time, cmd_data=self._data.con_inners.pitch_c, - cmd_xdata=self._data.con_inners.time, true_label="$\\theta$-True", - nav_label="$\\theta$-Est", show_legend=True, cmd_label="$\\theta_{cmd}$", - ax=state_axis[2,1], title="", ylabel="$rad$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.psi, true_xdata=self._data.true.time, - nav_data=self._data.est.psi, nav_xdata=self._data.est.time, - true_label="$\\psi$-True", nav_label="$\\psi$-Est", show_legend=True, - ax=state_axis[2,2], title="", ylabel="$rad$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.v_a, true_xdata=self._data.true.time, - nav_data=self._data.est.v_a, nav_xdata=self._data.est.time, - cmd_data=self._data.con_cmd.airspeed_c, cmd_xdata=self._data.con_cmd.time, - true_label="$V_a$-True", nav_label="$V_a$-Est", cmd_label="$V_a$-Cmd", show_legend=True, - ax=state_axis[2,3], title="", ylabel="$m/s$", widget=widget - )) - - # Create angular velocity plots - self._plotters.append(PlotNav( - true_data=self._data.true.p, true_xdata=self._data.true.time, - nav_data=self._data.est.p, nav_xdata=self._data.est.time, - true_label="$p$-True", nav_label = "$p$-Est", show_legend=True, - ax=state_axis[3,0], title="", ylabel="$rad/s$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.q, true_xdata=self._data.true.time, - nav_data=self._data.est.q, nav_xdata=self._data.est.time, - true_label="$q$-True", nav_label="$q$-Est", show_legend=True, - ax=state_axis[3,1], title="", ylabel="$rad/s$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.r, true_xdata=self._data.true.time, - nav_data=self._data.est.r, nav_xdata=self._data.est.time, - true_label="$r$-True", nav_label="$r$-Est", show_legend=True, - ax=state_axis[3,2], title="", ylabel="$rad/s$", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.true.v_g, true_xdata=self._data.true.time, - true_label="$V_g$-True", show_legend=True, - ax=state_axis[3,3], title="", ylabel="$m/s$", widget=widget - )) - - - # Create control plots - self._plotters.append(PlotNav( - true_data=self._data.cmd.elevator, true_xdata=self._data.cmd.time, - true_label="$\\delta_e$", show_legend=True, - ax=state_axis[4,0], title="", ylabel="", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.cmd.aileron, true_xdata=self._data.cmd.time, - true_label="$\\delta_a$", show_legend=True, - ax=state_axis[4,1], title="", ylabel="", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.cmd.rudder, true_xdata=self._data.cmd.time, - true_label="$\\delta_r$", show_legend=True, - ax=state_axis[4,2], title="", ylabel="", widget=widget - )) - self._plotters.append(PlotNav( - true_data=self._data.cmd.throttle, true_xdata=self._data.cmd.time, - true_label="$\\delta_t$", show_legend=True, - ax=state_axis[4,3], title="", ylabel="", widget=widget - )) - - def plot_states(self) -> None: - """Plots the state updates""" - try: - with self._data.lock: - for plotter in self._plotters: - plotter.plot() - - # Draw the figure - self._pw.draw() - - except Exception as e: - self._data.node.get_logger().error("Error in plotting: " + str(e)) - -def main(args=None): - # Initialize the node and read in parameters - rclpy.init(args=args) - node = rclpy.create_node(node_name="plotter_node") - node.declare_parameter("t_horizon", 100.) - node.declare_parameter("plot_sensors", False) - node.declare_parameter("plot_nav_error", False) - t_horizon: float = node.get_parameter("t_horizon").value - plot_sensors: bool = node.get_parameter("plot_sensors").value - plot_nav_err: bool = node.get_parameter("plot_nav_error").value - - # Create the state plotter - data = RosStorageInterface(node=node, t_horizon=t_horizon) - plotter = StatePlotter(data=data, plot_sensors=plot_sensors, plot_nav_err=plot_nav_err) - - # Create the executor - exec = SingleThreadedExecutor() - exec.add_node(node) - thread = threading.Thread(target=exec.spin, daemon=True) - thread.start() - - while rclpy.ok(): - plotter.plot_states() - pytime.sleep(1.0) # Sleep - - thread.join() - -if __name__ == '__main__': - main() diff --git a/rosplane_tuning/src/data_viz/plot_window.py b/rosplane_tuning/src/data_viz/plot_window.py deleted file mode 100644 index e7d6047..0000000 --- a/rosplane_tuning/src/data_viz/plot_window.py +++ /dev/null @@ -1,221 +0,0 @@ -import matplotlib -matplotlib.use('qt5agg') -from typing import Optional -from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas -from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar -import matplotlib.pyplot as plt -from PyQt5.QtWidgets import QMainWindow, QApplication, QWidget, QTabWidget,QVBoxLayout -from scipy.interpolate import interp1d -import sys - -class PlotAxis: - """Used to plot a single axis of data""" - def __init__(self, ax: plt.Axes, title: str = "", ylabel: str = "", xlabel: str = "time (sec)") -> None: - """ Initialize the axis - - Inputs: - ax: Axes on which to plot - title: Title for the plot - ylabel: Label for the y-axis of the plot - xlabel: Label for the x-axis of the plot - - """ - # Store the axis - self._ax = ax - - # Initialize the axes - self._ax.set_title(label=title) - self._ax.set_ylabel(ylabel=ylabel) - self._ax.set_xlabel(xlabel=xlabel) - self._ax.autoscale_view(True, True, True) - - def plot(self) -> None: - """Plots the data. Should be overloaded by children classes to actually plot the data""" - pass - - def _adjust_axes(self) -> None: - """Adjusts the axes to have the data in the axes view - """ - # Resize the axis - self._ax.relim() - self._ax.autoscale_view(True, True, True) - - -class PlotNav(PlotAxis): - """Used to plot true and navigation data on a single axis""" - def __init__(self, ax: plt.Axes, true_data: list[float], true_xdata: list[float], nav_data: Optional[list[float]] = None, - nav_xdata: Optional[list[float]]=None, cmd_data: Optional[list[float]] = None, cmd_xdata: Optional[list[float]] = None, title: str = "", ylabel: str = "", xlabel: str = "time (sec)", - true_label: str = "True", nav_label: str = "Est", cmd_label: str = "Cmd", show_legend: bool = True, - nav_color: str = 'lime', true_color: str = 'deepskyblue', cmd_color: str = 'red', widget: Optional[QWidget] = None - ) -> None: - """ Data required for plotting - - Inputs: - ax: Axes on which to plot - true_data: True data y-values - true_xdata: True data x-values (i.e., typically time) - nav_data: Navigation data y-values - nav_xdata: Navigation data x-values (i.e., typically time) - title: Title for the plot - ylabel: Label for the y-axis of the plot - xlabel: Label for the x-axis of the plot - true_label: Label of true data plot in the legend - nav_label: Label of the nav data plot in the legend - show_legend: True=> show legend, false => do not - """ - # Call constructor of super class - super().__init__(ax=ax, title=title, ylabel=ylabel, xlabel=xlabel) - - # Store the data vectors - self._true_data = true_data - self._true_xdata = true_xdata - self._nav_data = nav_data - self._nav_xdata = nav_xdata - self._cmd_data = cmd_data - self._cmd_xdata = cmd_xdata - self._widget = widget - - # Create the plots - self._nav: Optional[Line2D] = None - if (nav_data is not None) and (nav_xdata is not None): - (self._nav, ) = self._ax.plot(self._nav_xdata, self._nav_data) - self._nav.set_label(nav_label) - self._nav.set_color(nav_color) - self._nav.set_linewidth(3) - self._cmd: Optional[Line2D] = None - if (cmd_data is not None) and (cmd_xdata is not None): - (self._cmd, ) = self._ax.plot(self._cmd_xdata, self._cmd_data) - self._cmd.set_label(cmd_label) - self._cmd.set_color(cmd_color) - self._cmd.set_linewidth(3) - (self._true, ) = self._ax.plot(self._true_xdata, self._true_data) - self._true.set_label(true_label) - self._true.set_color(true_color) - - # Create the legend - if show_legend: - self._ax.legend() - - def plot(self) -> None: - """Replots the data""" - # Only plot if the window is active - # if self._widget is not None and not self._widget.isActiveWindow(): - # return - - # Replot - self._true.set_data(self._true_xdata, self._true_data) - if self._nav is not None: - self._nav.set_data(self._nav_xdata, self._nav_data) - if self._cmd is not None: - self._cmd.set_data(self._cmd_xdata, self._cmd_data) - - # Adjust the axes properties - self._adjust_axes() - -class PlotDiff(PlotAxis): - """Used to plot the difference between two data sets using interpolation of the data""" - def __init__(self, ax: plt.Axes, data_y1: list[float], data_x1: list[float], data_y2: list[float], - data_x2: list[float], title: str = "", ylabel: str = "", xlabel: str = "time (sec)", - legend_label: str = "", show_legend: bool = True, - data_color: str = 'red', widget: Optional[QWidget] = None - ) -> None: - """ Data required for plotting - - Inputs: - ax: Axes on which to plot - data_y1: Data series 1 y-values - data_x1: Data series 1 x-values - data_y2: Data series 2 y-values - data_x2: Data series 2 x-values - title: Title for the plot - ylabel: Label for the y-axis of the plot - xlabel: Label for the x-axis of the plot - legend_label: Label for the difference in the legend - show_legend: True=> show legend, false => do not - """ - # Call constructor of super class - super().__init__(ax=ax, title=title, ylabel=ylabel, xlabel=xlabel) - - # Store the data vectors and inputs - self._data_y1 = data_y1 - self._data_x1 = data_x1 - self._data_y2 = data_y2 - self._data_x2 = data_x2 - self._widget = widget - - # Create the plots - (self._diff, ) = self._ax.plot(0., 0.) - self._diff.set_label(legend_label) - self._diff.set_color(data_color) - - # Create the legend - if show_legend: - self._ax.legend() - - def plot(self) -> None: - """Replots the data""" - - # Only plot if the window is active - # if self._widget is not None and not self._widget.isActiveWindow(): - # return - - # Calculate the difference - y2_interp_fnc = interp1d(x=self._data_x2, y=self._data_y2, kind='linear', bounds_error=False, assume_sorted=True) - y2_interp = y2_interp_fnc(self._data_x1) - err = self._data_y1 - y2_interp - - # Replot - self._diff.set_data(self._data_x1, err) - - # Adjust the axes properties - self._adjust_axes() - - -class PlotWindow(): - """ Used to create a tabbed window for plotting. Copied from # Plot window class from https://github.com/superjax/plotWindow - """ - def __init__(self, parent=None) -> None: - self.app = QApplication(sys.argv) - self.MainWindow = QMainWindow() - self.MainWindow.__init__() - self.MainWindow.setWindowTitle("Small Unmanned Aerial Vehicle") - self.canvases = [] - self.figure_handles = [] - self.toolbar_handles = [] - self.tab_handles = [] - self.current_window = -1 - self.tabs = QTabWidget() - self.MainWindow.setCentralWidget(self.tabs) - self.MainWindow.resize(1280, 900) - self.MainWindow.show() - - def addPlot(self, title, figure) -> QWidget: - new_tab = QWidget() - layout = QVBoxLayout() - new_tab.setLayout(layout) - - figure.subplots_adjust(left=0.05, right=0.99, bottom=0.05, top=0.91, wspace=0.2, hspace=0.2) - new_canvas = FigureCanvas(figure) - new_toolbar = NavigationToolbar(new_canvas, new_tab) - - layout.addWidget(new_canvas) - layout.addWidget(new_toolbar) - self.tabs.addTab(new_tab, title) - - self.toolbar_handles.append(new_toolbar) - self.canvases.append(new_canvas) - self.figure_handles.append(figure) - self.tab_handles.append(new_tab) - - return new_tab - - def draw(self) -> None: - """Draws the active windows""" - - for (fig, tab) in zip(self.figure_handles, self.tab_handles): - #if True:# tab.isActiveWindow(): - fig.canvas.draw() - fig.canvas.flush_events() - - def show(self): - self.app.exec_()