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
