class data_viewer: def __init__(self): time_window_length = 100 self.plotter = Plotter( plotting_frequency=20, # refresh plot every 20 time steps time_window=time_window_length ) # plot last time_window seconds of data light_theme = True if light_theme: self.plotter.use_light_theme() axis_color = 'k' min_hue = 0 #260 max_hue = 900 #500 else: axis_color = 'w' min_hue = 0 max_hue = 900 # set up the plot window # define first row pn_plots = PlotboxArgs(plots=['pn', 'pn_c'], labels={ 'left': 'pn(m)', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) pe_plots = PlotboxArgs(plots=['pe', 'pe_c'], labels={ 'left': 'pe(m)', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) pd_plots = PlotboxArgs(plots=['pd', 'pd_c'], labels={ 'left': 'pd(m)', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) first_row = [pn_plots, pe_plots, pd_plots] # define second row u_plots = PlotboxArgs(plots=['u', 'u_c'], labels={ 'left': 'u(m/s)', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) v_plots = PlotboxArgs(plots=['v', 'v_c'], labels={ 'left': 'v(m/s)', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) w_plots = PlotboxArgs(plots=['w', 'w_c'], labels={ 'left': 'w(m/s)', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) second_row = [u_plots, v_plots, w_plots] # define third row phi_plots = PlotboxArgs(plots=['phi', 'phi_c'], labels={ 'left': 'phi(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) theta_plots = PlotboxArgs(plots=['theta', 'theta_c'], labels={ 'left': 'theta(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) psi_plots = PlotboxArgs(plots=['psi', 'psi_c'], labels={ 'left': 'psi(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) third_row = [phi_plots, theta_plots, psi_plots] # define fourth row p_plots = PlotboxArgs(plots=['p', 'p_c'], labels={ 'left': 'p(deg/s)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) q_plots = PlotboxArgs(plots=['q', 'q_c'], labels={ 'left': 'q(deg/s)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) r_plots = PlotboxArgs(plots=['r', 'r_c'], labels={ 'left': 'r(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) fourth_row = [p_plots, q_plots, r_plots] plots = [first_row, second_row, third_row, fourth_row] # Add plots to the window self.plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input self.plotter.define_input_vector('true_state', [ 'pn', 'pe', 'pd', 'u', 'v', 'w', 'phi', 'theta', 'psi', 'p', 'q', 'r' ]) self.plotter.define_input_vector('estimated_state', [ 'pn_e', 'pe_e', 'pd_e', 'u_e', 'v_e', 'w_e', 'phi_e', 'theta_e', 'psi_e', 'p_e', 'q_e', 'r_e' ]) self.plotter.define_input_vector('commands', [ 'pn_c', 'pe_c', 'pd_c', 'u_c', 'v_c', 'w_c', 'phi_c', 'theta_c', 'psi_c', 'p_c', 'q_c', 'r_c' ]) # plot timer self.time = 0. def update(self, true_state, estimated_state, commanded_state, ts): commands = [ commanded_state.pn, # pn_c commanded_state.pe, # pe_c commanded_state.pd, # pd_c commanded_state.u, # u_c commanded_state.v, # v_c commanded_state.w, # w_c commanded_state.phi, # phi_c commanded_state.theta, # theta_c commanded_state.psi, # psi_c commanded_state.p, # p_c commanded_state.q, # q_c commanded_state.r ] # r_c ## Add the state data in vectors # the order has to match the order in lines 72-76 true_state_list = [ true_state.pn, true_state.pe, true_state.pd, true_state.u, true_state.v, true_state.w, true_state.phi, true_state.theta, true_state.psi, true_state.p, true_state.q, true_state.r ] estimated_state_list = [ estimated_state.pn, estimated_state.pe, estimated_state.pd, estimated_state.u, estimated_state.v, estimated_state.w, estimated_state.phi, estimated_state.theta, estimated_state.psi, estimated_state.p, estimated_state.q, estimated_state.r ] self.plotter.add_vector_measurement('true_state', true_state_list, self.time) self.plotter.add_vector_measurement('estimated_state', estimated_state_list, self.time) self.plotter.add_vector_measurement('commands', commands, self.time) # Update and display the plot self.plotter.update_plots() # increment time self.time += ts
class sensor_viewer: def __init__(self): time_window_length=100 self.plotter = Plotter(plotting_frequency=100, # refresh plot every 100 time steps time_window=time_window_length, # plot last time_window seconds of data window_title="Sensors") # set up the plot window # define first row gyro_x_plots = PlotboxArgs(plots=['gyro_x'], labels={'left': 'gyro_x(m/s)', 'bottom': 'Time (s)'}, time_window=time_window_length) gyro_y_plots = PlotboxArgs(plots=['gyro_y'], labels={'left': 'gyro_y(m/s)', 'bottom': 'Time (s)'}, time_window=time_window_length) gyro_z_plots = PlotboxArgs(plots=['gyro_z'], labels={'left': 'gyro_z(m/s)', 'bottom': 'Time (s)'}, time_window=time_window_length) static_pressure_plots = PlotboxArgs(plots=['static_pressure'], labels={'left': 'pressure(Pa)', 'bottom': 'Time (s)'}, time_window=time_window_length) first_row = [gyro_x_plots, gyro_y_plots, gyro_z_plots, static_pressure_plots] # define second row accel_x_plots = PlotboxArgs(plots=['accel_x'], labels={'left': 'accel_x(m/s^2)', 'bottom': 'Time (s)'}, time_window=time_window_length) accel_y_plots = PlotboxArgs(plots=['accel_y'], labels={'left': 'accel_y(m/s^2)', 'bottom': 'Time (s)'}, time_window=time_window_length) accel_z_plots = PlotboxArgs(plots=['accel_z'], labels={'left': 'accel_z(m/s^2)', 'bottom': 'Time (s)'}, time_window=time_window_length) diff_pressure_plots = PlotboxArgs(plots=['diff_pressure'], labels={'left': 'pressure(Pa)', 'bottom': 'Time (s)'}, time_window=time_window_length) second_row = [accel_x_plots, accel_y_plots, accel_z_plots, diff_pressure_plots] # define third row gps_n_plots = PlotboxArgs(plots=['gps_n'], labels={'left': 'distance(m)', 'bottom': 'Time (s)'}, time_window=time_window_length) gps_e_plots = PlotboxArgs(plots=['gps_e'], labels={'left': 'distance(m)', 'bottom': 'Time (s)'}, time_window=time_window_length) gps_h_plots = PlotboxArgs(plots=['gps_h'], labels={'left': 'distance(m)', 'bottom': 'Time (s)'}, time_window=time_window_length) third_row = [gps_n_plots, gps_e_plots, gps_h_plots] # define fourth row gps_Vg_plots = PlotboxArgs(plots=['gps_Vg'], labels={'left': 'gps_Vg(m/s)', 'bottom': 'Time (s)'}, time_window=time_window_length) gps_course_plots = PlotboxArgs(plots=['gps_course'], labels={'left': 'gps_course (deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) fourth_row = [gps_Vg_plots, gps_course_plots] plots = [first_row, second_row, third_row, fourth_row ] # Add plots to the window self.plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input self.plotter.define_input_vector('sensors', ['gyro_x', 'gyro_y', 'gyro_z', 'static_pressure', 'accel_x', 'accel_y', 'accel_z', 'diff_pressure', 'gps_n', 'gps_e', 'gps_h', 'gps_Vg', 'gps_course']) # plot timer self.time = 0. def update(self, sensors, ts): ## Add the sensors data in vectors # the order has to match the order in lines 72-76 sensor_list = [sensors.gyro_x, sensors.gyro_y, sensors.gyro_z, sensors.static_pressure, sensors.accel_x, sensors.accel_y, sensors.accel_z, sensors.diff_pressure, sensors.gps_n, sensors.gps_e, sensors.gps_h, sensors.gps_Vg, sensors.gps_course] self.plotter.add_vector_measurement('sensors', sensor_list, self.time) # Update and display the plot self.plotter.update_plots() # increment time self.time += ts
class input_viewer: def __init__(self): time_window_length = 100 self.plotter = Plotter( plotting_frequency=20, # refresh plot every 20 time steps time_window=time_window_length ) # plot last time_window seconds of data light_theme = True if light_theme: self.plotter.use_light_theme() axis_color = 'k' min_hue = 0 #260 max_hue = 900 #500 else: axis_color = 'w' min_hue = 0 max_hue = 900 # set up the plot window # define first row ft_plots = PlotboxArgs(plots=['ft', 'ft_z'], labels={ 'left': 'thrust', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) first_row = [ft_plots] # define second row tau_x_plots = PlotboxArgs(plots=['tau_x', 'tau_x_z'], labels={ 'left': 'tau_x', 'bottom': 'Time (s)' }, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) second_row = [tau_x_plots] # define third row tau_y_plots = PlotboxArgs(plots=['tau_y', 'tau_y_z'], labels={ 'left': 'tau_y', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) third_row = [tau_y_plots] # define fourth row tau_z_plots = PlotboxArgs(plots=['tau_z', 'tau_z_z'], labels={ 'left': 'tau_z', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length, plot_min_hue=min_hue, plot_max_hue=max_hue, axis_color=axis_color) fourth_row = [tau_z_plots] plots = [first_row, second_row, third_row, fourth_row] # Add plots to the window self.plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input self.plotter.define_input_vector('true_state', ['ft', 'tau_x', 'tau_y', 'tau_z']) self.plotter.define_input_vector( 'commands', ['ft_z', 'tau_x_z', 'tau_y_z', 'tau_z_z']) # plot timer self.time = 0. def update(self, true_state, commanded_state, ts): ## Add the state data in vectors # the order has to match the order in lines 72-76 true_state_list = [ true_state.item(0), true_state.item(1), true_state.item(2), true_state.item(3) ] commanded_state_list = [ commanded_state.item(0), commanded_state.item(1), commanded_state.item(2), commanded_state.item(3) ] self.plotter.add_vector_measurement('true_state', true_state_list, self.time) self.plotter.add_vector_measurement('commands', commanded_state_list, self.time) # Update and display the plot self.plotter.update_plots() # increment time self.time += ts
class data_viewer: def __init__(self): time_window_length = 100 self.plotter = Plotter( plotting_frequency=10, # refresh plot every 100 time steps time_window=time_window_length ) # plot last time_window seconds of data # set up the plot window # define first row pn_plots = PlotboxArgs(plots=['pn', 'pn_c'], labels={ 'left': 'pn(m)', 'bottom': 'Time (s)' }, time_window=time_window_length) pe_plots = PlotboxArgs(plots=['pe', 'pe_c'], labels={ 'left': 'pe(m)', 'bottom': 'Time (s)' }, time_window=time_window_length) h_plots = PlotboxArgs(plots=['h', 'h_c'], labels={ 'left': 'h(m)', 'bottom': 'Time (s)' }, time_window=time_window_length) first_row = [pn_plots, pe_plots, h_plots] u_plots = PlotboxArgs(plots='u', labels={ 'left': 'u(m/s)', 'bottom': 'Time (s)' }, time_window=time_window_length) v_plots = PlotboxArgs(plots='v', labels={ 'left': 'v(m/s)', 'bottom': 'Time (s)' }, time_window=time_window_length) w_plots = PlotboxArgs(plots='w', labels={ 'left': 'w(m/s)', 'bottom': 'Time (s)' }, time_window=time_window_length) second_row = [u_plots, v_plots, w_plots] # define second row phi_plots = PlotboxArgs(plots=['phi'], labels={ 'left': 'phi(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) theta_plots = PlotboxArgs(plots=['theta'], labels={ 'left': 'theta(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) psi_plots = PlotboxArgs(plots=['psi', 'psi_c'], labels={ 'left': 'psi(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) third_row = [phi_plots, theta_plots, psi_plots] # define third row p_plots = PlotboxArgs(plots=['p'], labels={ 'left': 'p(deg/s)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) q_plots = PlotboxArgs(plots=['q'], labels={ 'left': 'q(deg/s)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) r_plots = PlotboxArgs(plots=['r'], labels={ 'left': 'r(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) fourth_row = [p_plots, q_plots, r_plots] plots = [first_row, second_row, third_row, fourth_row] # Add plots to the window self.plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input self.plotter.define_input_vector('true_state', [ 'pn', 'pe', 'h', 'u', 'v', 'w', 'phi', 'theta', 'psi', 'p', 'q', 'r' ]) self.plotter.define_input_vector('commanded_state', ['pn_c', 'pe_c', 'h_c', 'psi_c']) # plot timer self.time = 0. def update(self, true_state, commanded_state, ts): ## Add the state data in vectors # the order has to match the order in lines 72-76 true_state_list = true_state.tolist() commanded_state_list = commanded_state.tolist() self.plotter.add_vector_measurement('true_state', true_state_list, self.time) self.plotter.add_vector_measurement('commanded_state', commanded_state_list, self.time) # Update and display the plot self.plotter.update_plots() # increment time self.time += ts
class dataViewer: def __init__(self): time_window_length=100 self.plotter = Plotter(plotting_frequency=100, # refresh plot every 100 time steps time_window=time_window_length) # plot last time_window seconds of data # set up the plot window # define first row pn_plots = PlotboxArgs(plots=['pn', 'pn_e'], labels={'left': 'pn(m)', 'bottom': 'Time (s)'}, time_window=time_window_length) pe_plots = PlotboxArgs(plots=['pe', 'pe_e'], labels={'left': 'pe(m)', 'bottom': 'Time (s)'}, time_window=time_window_length) h_plots = PlotboxArgs(plots=['h', 'h_e', 'h_c'], labels={'left': 'h(m)', 'bottom': 'Time (s)'}, time_window=time_window_length) wind_plots = PlotboxArgs(plots=['wn', 'wn_e', 'we', 'we_e'], labels={'left': 'wind(m/s)', 'bottom': 'Time (s)'}, time_window=time_window_length) first_row = [pn_plots, pe_plots, h_plots, wind_plots] # define second row Va_plots = PlotboxArgs(plots=['Va', 'Va_e', 'Va_c'], labels={'left': 'Va(m/s)', 'bottom': 'Time (s)'}, time_window=time_window_length) alpha_plots = PlotboxArgs(plots=['alpha', 'alpha_e'], labels={'left': 'alpha(deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) beta_plots = PlotboxArgs(plots=['beta', 'beta_e'], labels={'left': 'beta(deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) Vg_plots = PlotboxArgs(plots=['Vg', 'Vg_e'], labels={'left': 'Vg(m/s)', 'bottom': 'Time (s)'}, time_window=time_window_length) second_row = [Va_plots, alpha_plots, beta_plots, Vg_plots] # define third row phi_plots = PlotboxArgs(plots=['phi', 'phi_e', 'phi_c'], labels={'left': 'phi(deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) theta_plots = PlotboxArgs(plots=['theta', 'theta_e', 'theta_c'], labels={'left': 'theta(deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) psi_plots = PlotboxArgs(plots=['psi', 'psi_e'], labels={'left': 'psi(deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) chi_plots = PlotboxArgs(plots=['chi', 'chi_e', 'chi_c'], labels={'left': 'chi(deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) third_row = [phi_plots, theta_plots, psi_plots, chi_plots] # define fourth row p_plots = PlotboxArgs(plots=['p', 'p_e'], labels={'left': 'p(deg/s)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) q_plots = PlotboxArgs(plots=['q', 'q_e'], labels={'left': 'q(deg/s)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) r_plots = PlotboxArgs(plots=['r', 'r_e'], labels={'left': 'r(deg)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) gyro_plots = PlotboxArgs(plots=['bx', 'bx_e', 'by', 'by_e', 'bz', 'bz_e'], labels={'left': 'bias(deg/s)', 'bottom': 'Time (s)'}, rad2deg=True, time_window=time_window_length) fourth_row = [p_plots, q_plots, r_plots, gyro_plots] plots = [first_row, second_row, third_row, fourth_row ] # Add plots to the window self.plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input self.plotter.define_input_vector('true_state', ['pn', 'pe', 'h', 'Va', 'alpha', 'beta', 'phi', 'theta', 'chi', 'p', 'q', 'r', 'Vg', 'wn', 'we', 'psi', 'bx', 'by', 'bz']) self.plotter.define_input_vector('estimated_state', ['pn_e', 'pe_e', 'h_e', 'Va_e', 'alpha_e', 'beta_e', 'phi_e', 'theta_e', 'chi_e', 'p_e', 'q_e', 'r_e', 'Vg_e', 'wn_e', 'we_e', 'psi_e', 'bx_e', 'by_e', 'bz_e']) self.plotter.define_input_vector('commands', ['h_c', 'Va_c', 'phi_c', 'theta_c', 'chi_c']) # plot timer self.time = 0. def update(self, true_state, estimated_state, commanded_state, ts): commands = [commanded_state.h, # h_c commanded_state.Va, # Va_c commanded_state.phi, # phi_c commanded_state.theta, # theta_c commanded_state.chi] # chi_c ## Add the state data in vectors # the order has to match the order in lines 72-76 true_state_list = [true_state.pn, true_state.pe, true_state.h, true_state.Va, true_state.alpha, true_state.beta, true_state.phi, true_state.theta, true_state.chi, true_state.p, true_state.q, true_state.r, true_state.Vg, true_state.wn, true_state.we, true_state.psi, true_state.bx, true_state.by, true_state.bz] estimated_state_list = [estimated_state.pn, estimated_state.pe, estimated_state.h, estimated_state.Va, estimated_state.alpha, estimated_state.beta, estimated_state.phi, estimated_state.theta, estimated_state.chi, estimated_state.p, estimated_state.q, estimated_state.r, estimated_state.Vg, estimated_state.wn, estimated_state.we, estimated_state.psi, estimated_state.bx, estimated_state.by, estimated_state.bz] self.plotter.add_vector_measurement('true_state', true_state_list, self.time) self.plotter.add_vector_measurement('estimated_state', estimated_state_list, self.time) self.plotter.add_vector_measurement('commands', commands, self.time) # Update and display the plot self.plotter.update_plots() # increment time self.time += ts
], labels={ 'left': 'Y Position (m)', 'bottom': 'Z Position (m)' }, max_length=100) third_row = [xy_plot, xz_plot, yz_plot] # Use a list of lists to specify plotting window structure (3 rows, each with 3 plots) plots = [first_row, second_row, third_row] # Add plots to the window plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input plotter.define_input_vector('position', ['x', 'y', 'z']) plotter.define_input_vector('true_position', ['x_truth', 'y_truth', 'z_truth']) plotter.define_input_vector('attitude', ['phi', 'theta', 'psi']) plotter.define_input_vector('estimated_attitude', ['phi_e', 'theta_e', 'psi_e']) # setup simulation timing T = 5 Ts = 0.01 tvec = np.linspace(0, T, num=int((1 / Ts) * T)) # run the simulation for idx, t in enumerate(tvec): # Make some sinusoids and stuff x = np.sin(2 * np.pi * 1 * t) y = np.cos(2 * np.pi * 0.5 * t)
class PlotWrapper: """ PlotWrapper class for connecting ROS topics to the Plotter object """ def __init__(self, update_freq=30, use_degrees=True): self.t0 = rospy.Time.now().to_sec() # Store parameters self.use_degrees = use_degrees # Setup the plotter window self.plotter = Plotter() # Define plot names plots = [ 'n n_hat n_gps -l', 'e e_hat e_gps -l', 'h h_hat h_gps h_c -l', 'phi phi_hat -l', 'theta theta_hat -l', 'psi psi_hat -l', 'u u_hat -l', 'v v_hat -l', 'w w_hat -l', 'p p_hat p_gyro -l', 'q q_hat q_gyro -l', 'r r_hat r_gyro -l', 'ax', 'ay', 'az', 'Va Va_c -l', 'chi chi_hat chi_c chi_gps -l', 'p_diff p_abs -l' ] self.wind = np.zeros(3) self.crab = 0.0 # Add plots to the window for p in plots: self.plotter.add_plot(p) # Add legends # self.plotter.add_legend('n') # self.plotter.add_legend('e') # self.plotter.add_legend('h') # self.plotter.add_legend('p') # self.plotter.add_legend('q') # self.plotter.add_legend('r') # self.plotter.add_legend('Va') # self.plotter.add_legend('chi') self.tf_listener = tf.TransformListener() # Define input vectors for easier input self.plotter.define_input_vector('neh', ['n', 'e', 'h']) self.plotter.define_input_vector('lin_velocity', ['u', 'v', 'w']) self.plotter.define_input_vector('ang_velocity', ['p', 'q', 'r']) self.plotter.define_input_vector('euler', ['phi', 'theta', 'psi']) self.plotter.define_input_vector('neh_gps', ['n_gps', 'e_gps', 'h_gps']) self.plotter.define_input_vector('ang_gyro', ['p_gyro', 'q_gyro', 'r_gyro']) self.plotter.define_input_vector('acc', ['ax', 'ay', 'az']) self.plotter.define_input_vector('euler_est', ['phi_hat', 'theta_hat', 'psi_hat']) self.plotter.define_input_vector('neh_est', ['n_hat', 'e_hat', 'h_hat']) self.plotter.define_input_vector('vb_est', ['u_hat', 'v_hat', 'w_hat']) # Subscribe to relevant ROS topics rospy.Subscriber('twist', TwistStamped, self.velocity_cb_) rospy.Subscriber('imu_lpf', Imu, self.imu_cb_) rospy.Subscriber('Va_c', Float32, self.va_cb_) rospy.Subscriber('gps_neh', Vector3Stamped, self.gps_neh_cb_) rospy.Subscriber('gps_chi', Float32, self.gps_chi_cb_) rospy.Subscriber('gps_vg', Float32, self.gps_vg_cb_) rospy.Subscriber('h_c', Float32, self.h_cb_) rospy.Subscriber('chi_c', Float32, self.chi_cb_) rospy.Subscriber('p_diff', Float32, self.p_diff_cb_) rospy.Subscriber('p_static', Float32, self.p_static_cb_) rospy.Subscriber('euler_est', Vector3Stamped, self.euler_est_cb_) rospy.Subscriber('ned_est', Vector3Stamped, self.ned_est_cb_) rospy.Subscriber('chi_est', Float32, self.chi_est_cb_) rospy.Subscriber('vb_est', Vector3Stamped, self.vb_est_cb_) self.va_c = None self.h_c = None self.chi_c = None # Update the plots rate = rospy.Rate(update_freq) while not rospy.is_shutdown(): self.tick() self.plotter.update_plots() rate.sleep() def p_diff_cb_(self, msg): t = rospy.Time.now().to_sec() self.plotter.add_measurement('p_diff', msg.data, t - self.t0) def chi_est_cb_(self, msg): t = rospy.Time.now().to_sec() self.plotter.add_measurement('chi_hat', msg.data, t - self.t0, rad2deg=True) def p_static_cb_(self, msg): t = rospy.Time.now().to_sec() self.plotter.add_measurement('p_abs', msg.data, t - self.t0) def velocity_cb_(self, msg): # Extract time t = msg.header.stamp.to_sec() # Handle position measurements twist = msg.twist self.plotter.add_vector_measurement( 'lin_velocity', [twist.linear.x, twist.linear.y, twist.linear.z], t - self.t0) self.plotter.add_vector_measurement( 'ang_velocity', [twist.angular.x, twist.angular.y, twist.angular.z], t - self.t0, rad2deg=self.use_degrees) self.crab = np.arctan2(twist.linear.y, twist.linear.x) V_g = np.array([twist.linear.x, twist.linear.y, twist.linear.z]) Va = np.linalg.norm(V_g - self.wind) self.plotter.add_measurement('Va', Va, t - self.t0) def imu_cb_(self, msg): t = msg.header.stamp.to_sec() gyro = msg.angular_velocity acc = msg.linear_acceleration self.plotter.add_vector_measurement('acc', [acc.x, acc.y, acc.z], t - self.t0) self.plotter.add_vector_measurement('ang_gyro', [gyro.x, gyro.y, gyro.z], t - self.t0, rad2deg=self.use_degrees) def va_cb_(self, msg): # Extract time t = rospy.Time.now().to_sec() self.plotter.add_measurement('Va_c', msg.data, t - self.t0) self.va_c = msg.data def gps_neh_cb_(self, msg): t = msg.header.stamp.to_sec() self.plotter.add_vector_measurement( 'neh_gps', [msg.vector.x, msg.vector.y, msg.vector.z], t - self.t0) def vb_est_cb_(self, msg): t = msg.header.stamp.to_sec() self.plotter.add_vector_measurement( 'vb_est', [msg.vector.x, msg.vector.y, msg.vector.z], t - self.t0) def ned_est_cb_(self, msg): t = msg.header.stamp.to_sec() self.plotter.add_vector_measurement( 'neh_est', [msg.vector.x, msg.vector.y, -msg.vector.z], t - self.t0) def gps_chi_cb_(self, msg): # Extract time t = rospy.Time.now().to_sec() self.plotter.add_measurement('chi_gps', msg.data, t - self.t0, rad2deg=self.use_degrees) def gps_vg_cb_(self, msg): pass def h_cb_(self, msg): # Extract time t = rospy.Time.now().to_sec() self.plotter.add_measurement('h_c', msg.data, t - self.t0) self.h_c = msg.data def chi_cb_(self, msg): # Extract time t = rospy.Time.now().to_sec() self.plotter.add_measurement('chi_c', msg.data, t - self.t0, rad2deg=self.use_degrees) self.chi_c = msg.data def euler_est_cb_(self, msg): # Extract time t = msg.header.stamp.to_sec() self.plotter.add_vector_measurement( 'euler_est', [msg.vector.x, msg.vector.y, msg.vector.z], t - self.t0, rad2deg=self.use_degrees) def pose(self): try: # Extract time t = self.tf_listener.getLatestCommonTime("world_ned", "base_link") except: return # Handle position measurements position, quaternion = self.tf_listener.lookupTransform( "world_ned", "base_link", t) position[2] = -position[2] t = t.to_sec() self.plotter.add_vector_measurement('neh', position, t - self.t0) # Use ROS tf to convert to Euler angles from quaternion euler = tf.transformations.euler_from_quaternion(quaternion) # Add angles and angular velocities self.plotter.add_vector_measurement('euler', euler, t - self.t0, rad2deg=self.use_degrees) self.plotter.add_measurement('chi', euler[2] + self.crab, t - self.t0, rad2deg=self.use_degrees) def tick(self): self.pose() t = rospy.Time.now().to_sec() if self.va_c is not None: self.plotter.add_measurement('Va_c', self.va_c, t - self.t0) if self.h_c is not None: self.plotter.add_measurement('h_c', self.h_c, t - self.t0) if self.chi_c is not None: self.plotter.add_measurement('chi_c', self.chi_c, t - self.t0, rad2deg=self.use_degrees)
class PlotWrapper: """ PlotWrapper class for connecting ROS topics to the Plotter object """ def __init__(self, update_freq=30, use_degrees=True): # Store parameters self.use_degrees = use_degrees # Setup the plotter window self.plotter = Plotter() # Define plot names plots = [ 'n _n_g', 'e _e_g', 'h _h_g', 'Va _Va_g', 'chi _chi_g', 'e n --2d' ] #plots = [['n', 'n_g'], ['e', 'e_g'], ['h', 'h_g'], # ['Va', 'Va_g'], ['chi', 'chi_g'], ['traj']] # Add plots to the window for p in plots: self.plotter.add_plot(p) self.tf_listener = tf.TransformListener() # Define input vectors for easier input self.plotter.define_input_vector('neh', ['n', 'e', 'h']) self.plotter.define_input_vector('neh_g', ['n_g', 'e_g', 'h_g']) # Subscribe to relevant ROS topics rospy.Subscriber('ned_g', Vector3Stamped, self.ned_g_cb_) rospy.Subscriber('twist', TwistStamped, self.velocity_cb_) rospy.Subscriber('chi_g', Float32, self.chi_g_cb_) rospy.Subscriber('Va_g', Float32, self.va_g_cb_) self.crab = 0.0 self.wind = np.zeros(3) # Update the plots rate = rospy.Rate(update_freq) while not rospy.is_shutdown(): self.tick() self.plotter.update_plots() rate.sleep() def velocity_cb_(self, msg): t = msg.header.stamp.to_sec() twist = msg.twist self.crab = np.arctan2(twist.linear.y, twist.linear.x) V_g = np.array([twist.linear.x, twist.linear.y, twist.linear.z]) Va = np.linalg.norm(V_g - self.wind) self.plotter.add_measurement('Va', Va, t) def ned_g_cb_(self, msg): t = msg.header.stamp.to_sec() vector = msg.vector self.plotter.add_vector_measurement('neh_g', [vector.x, vector.y, -vector.z], t) def va_g_cb_(self, msg): # Extract time t = rospy.Time.now().to_sec() self.plotter.add_measurement('Va_g', msg.data, t) def chi_g_cb_(self, msg): # Extract time t = rospy.Time.now().to_sec() self.plotter.add_measurement('chi_g', msg.data, t, rad2deg=self.use_degrees) def pose(self): try: # Extract time t = self.tf_listener.getLatestCommonTime("world_ned", "base_link") except: return # Handle position measurements position, quaternion = self.tf_listener.lookupTransform( "world_ned", "base_link", t) position[2] = -position[2] t = t.to_sec() self.plotter.add_vector_measurement('neh', position, t) # Use ROS tf to convert to Euler angles from quaternion euler = tf.transformations.euler_from_quaternion(quaternion) # Add angles and angular velocities self.plotter.add_measurement('chi', euler[2] + self.crab, t, rad2deg=self.use_degrees) def tick(self): self.pose()
class OrbitPlotter: ''' Plotter wrapper for orbit analysis ''' def __init__(self, plotting_freq=1): self.plotter = Plotter(plotting_freq, time_window=30) self.plotter.set_plots_per_row(2) # Define plot names plots = self._define_plots() # Add plots to the window for p in plots: self.plotter.add_plotboxes(p) # Define state vectors for simpler input self._define_input_vectors() self.R_err_thresh = 0.01 self.R_thresh_reached = False self.az_err_thresh = 0.001 self.az_thresh_reached = False def _define_plots(self): plots = [['y_r x_r y_t_r x_t_r -2d', 'y x y_t x_t -2d'], ['_R R_error', '_az az_error'], ['phi_c', 'psi'], ['vx vx_e -l', 'vy vy_e']] return plots def _define_input_vectors(self): self.plotter.define_input_vector("state", ['x_r', 'y_r', 'psi', 'az', 'R']) self.plotter.define_input_vector("actual_pos", ['x', 'y']) self.plotter.define_input_vector("target_pos", ['x_t', 'y_t']) self.plotter.define_input_vector("target_rel_pos", ['x_t_r', 'y_t_r']) self.plotter.define_input_vector("target_vel", ['vx', 'vy']) self.plotter.define_input_vector("target_vel_est", ['vx_e', 'vy_e']) def update(self, state, R_c, az_err, phi_c, target_pos, target_vel, target_vel_est, t): x_r, y_r, psi, az, R = state self.plotter.add_vector_measurement("state", state, t) actual_pos = [x_r + target_pos[0], y_r + target_pos[1]] self.plotter.add_vector_measurement("actual_pos", actual_pos, t) self.plotter.add_measurement("phi_c", phi_c, t) self.plotter.add_vector_measurement("target_pos", target_pos, t) self.plotter.add_vector_measurement("target_rel_pos", [0, 0], t) R_err = R - R_c if not self.R_thresh_reached and abs(R_err) < self.R_err_thresh: print("R error threshold ({0}) reached at t={1}".format( self.R_err_thresh, t)) self.R_thresh_reached = True if not self.az_thresh_reached and abs(az_err) < self.az_err_thresh: print("az error threshold ({0}) reached at t={1}".format( self.az_err_thresh, t)) self.az_thresh_reached = True self.plotter.add_measurement("R_error", R_err, t) self.plotter.add_measurement("az_error", az_err, t) self.plotter.add_vector_measurement("target_vel", target_vel, t) self.plotter.add_vector_measurement("target_vel_est", target_vel_est, t) self.plotter.update_plots()
PlotArgs(name='XY_true', states=['x_t', 'y_t']), PlotArgs(name='XY scatter', states=['x_scat', 'y_scat'], connect=False, symbol='o', symbol_size=0.1, px_mode=False) ]) # Use a list of lists to specify plotting window structure (3 rows, each with 3 plots) plots = [first_row, [xy_plot]] # Add plots to the window plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input plotter.define_input_vector('true_position', ['x_t', 'y_t', 'z_t']) plotter.define_input_vector('est_position', ['x_e', 'y_e', 'z_e']) # setup simulation timing T = 5 Ts = 0.01 tvec = np.linspace(0, T, num=int((1 / Ts) * T)) # run the simulation for idx, t in enumerate(tvec): # Make some sinusoids and stuff x_e = np.sin(2 * np.pi * 1 * t) y_e = np.cos(2 * np.pi * 0.5 * t) z_e = t + np.cos(2 * np.pi * 2 * t) # Simulate sigma values for x_e
class data_viewer2: def __init__(self): time_window_length = 100 self.plotter = Plotter( plotting_frequency=100, # refresh plot every 100 time steps time_window=time_window_length ) # plot last time_window seconds of data # set up the plot window # define first row pn_plots = PlotboxArgs( plots=['pn', 'pn_EKF', 'pn_FSD_EKF', 'pn_FSI_EKF'], labels={'left': 'pn(m)'}, time_window=time_window_length) pe_plots = PlotboxArgs( plots=['pe', 'pe_EKF', 'pe_FSD_EKF', 'pe_FSI_EKF'], labels={'left': 'pe(m)'}, time_window=time_window_length) h_plots = PlotboxArgs(plots=['h', 'h_EKF', 'h_FSD_EKF', 'h_FSI_EKF'], labels={'left': 'h(m)'}, time_window=time_window_length) first_row = [pn_plots, pe_plots, h_plots] # define second row Va_plots = PlotboxArgs( plots=['Va', 'Va_EKF', 'Va_FSD_EKF', 'Va_FSI_EKF'], labels={'left': 'Va(m/s)'}, time_window=time_window_length) Vg_plots = PlotboxArgs( plots=['Vg', 'Vg_EKF', 'Vg_FSD_EKF', 'Vg_FSI_EKF'], labels={'left': 'Vg(m/s)'}, time_window=time_window_length) chi_plots = PlotboxArgs( plots=['chi', 'chi_EKF', 'chi_FSD_EKF', 'chi_FSI_EKF'], labels={'left': 'chi(deg)'}, rad2deg=True, time_window=time_window_length) second_row = [Va_plots, Vg_plots, chi_plots] # define third row phi_plots = PlotboxArgs( plots=['phi', 'phi_EKF', 'phi_FSD_EKF', 'phi_FSI_EKF'], labels={ 'left': 'phi(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) theta_plots = PlotboxArgs( plots=['theta', 'theta_EKF', 'theta_FSD_EKF', 'theta_FSI_EKF'], labels={ 'left': 'theta(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) psi_plots = PlotboxArgs( plots=['psi', 'psi_EKF', 'psi_FSD_EKF', 'psi_FSI_EKF'], labels={ 'left': 'psi(deg)', 'bottom': 'Time (s)' }, rad2deg=True, time_window=time_window_length) third_row = [phi_plots, theta_plots, psi_plots] plots = [first_row, second_row, third_row] # Add plots to the window self.plotter.add_plotboxes(plots) # Define and label vectors for more convenient/natural data input self.plotter.define_input_vector('true_state', [ 'pn', 'pe', 'h', 'Va', 'alpha', 'beta', 'phi', 'theta', 'chi', 'p', 'q', 'r', 'Vg', 'wn', 'we', 'psi', 'bx', 'by', 'bz' ]) self.plotter.define_input_vector('EKF', [ 'pn_EKF', 'pe_EKF', 'h_EKF', 'Va_EKF', 'alpha_EKF', 'beta_EKF', 'phi_EKF', 'theta_EKF', 'chi_EKF', 'p_EKF', 'q_EKF', 'r_EKF', 'Vg_EKF', 'wn_EKF', 'we_EKF', 'psi_EKF', 'bx_EKF', 'by_EKF', 'bz_EKF' ]) self.plotter.define_input_vector('FSD_EKF', [ 'pn_FSD_EKF', 'pe_FSD_EKF', 'h_FSD_EKF', 'Va_FSD_EKF', 'alpha_FSD_EKF', 'beta_FSD_EKF', 'phi_FSD_EKF', 'theta_FSD_EKF', 'chi_FSD_EKF', 'p_FSD_EKF', 'q_FSD_EKF', 'r_FSD_EKF', 'Vg_FSD_EKF', 'wn_FSD_EKF', 'we_FSD_EKF', 'psi_FSD_EKF2', 'bx_FSD_EKF', 'by_FSD_EKF', 'bz_FSD_EKF' ]) self.plotter.define_input_vector('FSI_EKF', [ 'pn_FSI_EKF', 'pe_FSI_EKF', 'h_FSI_EKF', 'Va_FSI_EKF', 'alpha_FSI_EKF', 'beta_FSI_EKF', 'phi_FSI_EKF', 'theta_FSI_EKF', 'chi_FSI_EKF', 'p_FSI_EKF', 'q_FSI_EKF', 'r_FSI_EKF', 'Vg_FSI_EKF', 'wn_FSI_EKF', 'we_FSI_EKF', 'psi_FSI_EKF2', 'bx_FSI_EKF', 'by_FSI_EKF', 'bz_FSI_EKF' ]) # plot timer self.time = 0. def update(self, true_state, EKF, FSD_EKF, FSI_EKF, ts): true_state_list = [ true_state.pn, true_state.pe, true_state.h, true_state.Va, true_state.alpha, true_state.beta, true_state.phi, true_state.theta, true_state.chi, true_state.p, true_state.q, true_state.r, true_state.Vg, true_state.wn, true_state.we, true_state.psi, true_state.bx, true_state.by, true_state.bz ] EKF_list = [ EKF.pn, EKF.pe, EKF.h, EKF.Va, EKF.alpha, EKF.beta, EKF.phi, EKF.theta, EKF.chi, EKF.p, EKF.q, EKF.r, EKF.Vg, EKF.wn, EKF.we, EKF.psi, EKF.bx, EKF.by, EKF.bz ] FSD_EKF_list = [ FSD_EKF.pn, FSD_EKF.pe, FSD_EKF.h, FSD_EKF.Va, FSD_EKF.alpha, FSD_EKF.beta, FSD_EKF.phi, FSD_EKF.theta, FSD_EKF.chi, FSD_EKF.p, FSD_EKF.q, FSD_EKF.r, FSD_EKF.Vg, FSD_EKF.wn, FSD_EKF.we, FSD_EKF.psi, FSD_EKF.bx, FSD_EKF.by, FSD_EKF.bz ] FSI_EKF_list = [ FSI_EKF.pn, FSI_EKF.pe, FSI_EKF.h, FSI_EKF.Va, FSI_EKF.alpha, FSI_EKF.beta, FSI_EKF.phi, FSI_EKF.theta, FSI_EKF.chi, FSI_EKF.p, FSI_EKF.q, FSI_EKF.r, FSI_EKF.Vg, FSI_EKF.wn, FSI_EKF.we, FSI_EKF.psi, FSI_EKF.bx, FSI_EKF.by, FSI_EKF.bz ] self.plotter.add_vector_measurement('true_state', true_state_list, self.time) self.plotter.add_vector_measurement('EKF', EKF_list, self.time) self.plotter.add_vector_measurement('FSD_EKF', FSD_EKF_list, self.time) self.plotter.add_vector_measurement('FSI_EKF', FSI_EKF_list, self.time) # Update and display the plot self.plotter.update_plots() # increment time self.time += ts