class TestDataPlotter(unittest.TestCase): def setUp(self): self.model = ChemotaxisModel() self.model.initialize() self.model.run(start=0, end=10) self.plotter = DataPlotter(self.model) def testLines(self): self.plotter.lines(["fYp", "Bp", "fTT__"])
def test_live_plot(): plotter = DataPlotter('step', 'v1', 'v2') step = 0 print() while step < 10: v1 = np.round(np.random.random() * 10, 1) v2 = np.round(np.random.random() * 12, 1) print(f'new points ({step:5d}: {v1} ++ {v2})') plotter.add_data(step, v1, v2) step += 1 time.sleep(1.2)
def pressure(): # Extract the token from a request to the backend. auth_token = request.headers.get('Authorization') # Fetch the time series data from the IoT Time Series Service. data = DataFetcher(auth_token, ASSET_ID, ASPECT).fetch_data('pressure') # Create the line chart. chart = DataPlotter().plot_line_chart(data[0], data[1], 'Air Pressure') # Render the web page template. return render_template('pressure.html', line_chart=Markup(chart))
def load_data(self): directory_path = filedialog.askdirectory( initialdir=os.getcwd(), mustexist=True, title="Please select the data directory...") extractor = DataExtractor(directory_path, self.best_nest_var.get(), self.max_sim_time_var.get()) invalid_files, unfinished_sims = extractor.extract_data() self.data_set = extractor.data_set self.data_plot = DataPlotter(self.data_set) msg_string = "%s simulations had missing or blank files.\n" % invalid_files msg_string += "%s simulations exceeded than the maximum time and so were removed." % unfinished_sims messagebox.showinfo('Data Loaded', msg_string) self.list_box.delete(0, tk.END) grid_row = 0 for data in self.data_set: raw_data_string = "" for key, value in data.items(): raw_data_string += "%s=%s, " % (key, value) grid_row += 1 self.list_box.insert(tk.END, raw_data_string[:-2]) if grid_row % 2 == 0: self.list_box.itemconfig(tk.END, bg='#e0e0e0') else: self.list_box.itemconfig(tk.END, bg='#f4f4f4') # Updating the list of options to split the data by options = self.data_set[0].keys() menu = self.split_options["menu"] menu.delete(0, "end") menu.add_command(label='none', command=lambda: self.split_on_var.set('none')) for string in options: menu.add_command( label=string, command=lambda option=string: self.split_on_var.set(option)) self.add_button.config(state=tk.ACTIVE)
def test_traj(traj, traj_type='circ', show_anim=True, save_plot=False, plot_type="xy"): init_pos = np.array([traj[0][0], traj[0][3], traj[0][6]]) cf = CrazyflieDynamics(init_pos=init_pos) if show_anim: plot = DataPlotter(is_type="traj") anim = CrazyflieAnimation(traj) # Create class objects rate_ctrl = RateController() attitude_ctrl = AttitudeController() ctrl_mixer = ControlMixer() altitiude_ctrl = AltitudeController() xy_traj_ctrl = XYTrajController() yaw_ctrl = YawController() # off-borad controller input values u_ob = np.array([ [0.0], # pitch (phi -> x) - 0 [0.0], # roll (theta -> y) - 1 [0.0], # yaw rate - 2 [0.0], # thrust - 3 ]) z_c = 0.0 psi_c = 0.0 t = P.t_start if save_plot: fig, ax = plt.subplots() if plot_type == 'xy': ax.set_title("Circular Trajectory XY Data Sim") ax.set_xlabel("x [m]") ax.set_ylabel("y [m]") ax.grid() x_list = [] y_list = [] xref_list = [] yref_list = [] # Use for loop to ensure at correct point in trajectory for i in range(traj.shape[0] - 1): # t_next_phys = t + P.t_phys t_next_ob = t + P.t_ob # reference values if traj_type == 'circ': ref = np.array([ [traj[i, 0]], # x [traj[i, 1]], # xd [traj[i, 3]], # y [traj[i, 4]], # yd ]) elif traj_type == 'sw': ref = np.array([ [traj[i, 0]], # x [traj[i, 1]], # xd [0.0], # y [0.0], # yd ]) # Altitude off-board controller update u_ob[3, 0] = altitiude_ctrl.update(ref.item(2), cf.state.item(2)) # Yaw rate off-board controller update u_ob[3, 0] = yaw_ctrl.update(ref.item(3), cf.state.item(3)) if traj_type == 'circ': r_t = np.array([traj[i, 0], traj[i, 3]]) # traj pos values rd_t = np.array([traj[i, 1], traj[i, 4]]) # traj vel values elif traj_type == 'sw': r_t = np.array([traj[i, 0], 0.0]) rd_t = np.array([traj[i, 1], 0.0]) r_t_vect = np.array([ traj[i + 1, 0], traj[i + 1, 3] ]) - r_t # vector from current pos to next pos in traj rdd_t = np.array([traj[i, 2], traj[i, 5]]) # X-Y off-board controller update u_ob[0, 0], u_ob[1, 0] = xy_traj_ctrl.update(r_t, rd_t, r_t_vect, cf.state, psi_c, rdd_t) # while t < t_next_phys: # attitude controller runs at 250 hz while t < t_next_ob: t_next_att = t + P.t_att # Conduct attitude control # phi controls x, theta controls y p_c, q_c = attitude_ctrl.update(u_ob.item(0), u_ob.item(1), cf.state) while t < t_next_att: # rate controller is the fastest running at 500 hz t = t + P.t_rate # Conduct rate control del_phi, del_theta, del_psi = rate_ctrl.update( p_c, q_c, u_ob.item(2), cf.state) # Update state of model u = ctrl_mixer.update(u_ob.item(3), del_phi, del_theta, del_psi) y = cf.update(u) if save_plot: if plot_type == "xy": x_list.append(cf.state.item(0)) # x y_list.append(cf.state.item(1)) # y xref_list.append(ref.item(0)) # xref yref_list.append(ref.item(2)) # yref if show_anim: plot.update((100.0 / 30.0) * t, ref, cf.state, u, is_type="traj") anim.update(cf.state) plt.pause(0.00000001) if save_plot: if plot_type == "xy": ax.plot(x_list, y_list, c='r') ax.plot(xref_list, yref_list, c='b') fig.savefig("../plots/traj_circ_sim_2omega_20200408") # elif plot_type == "components": plt.show(block=False)
def test_all(x_c, y_c, z_c, psi_c, show_anim=True, save_plot=False, plot_type='x'): cf = CrazyflieDynamics(init_pos=np.array([0.0, 0.0, 0.0])) if show_anim: plot = DataPlotter() traj = np.array([x_c, y_c, z_c]) anim = CrazyflieAnimation(traj) # Create class objects rate_ctrl = RateController() attitude_ctrl = AttitudeController() ctrl_mixer = ControlMixer() altitiude_ctrl = AltitudeController() xy_ctrl = XYController() yaw_ctrl = YawController() # off-borad controller input values u_ob = np.array([ [0.0], # pitch (theta -> x) - 0 [deg] [0.0], # roll (phi -> y) - 1 [deg] [0.0], # thrust - 2 [deg] [0.0], # yaw rate - 3 [deg] ]) # reference values r = np.array([ [x_c], # x - 0 [m] [y_c], # y - 1 [m] [z_c], # z - 2 [m] [psi_c], # psi - 3 [deg] ]) t = P.t_start if save_plot: fig, ax = plt.subplots() ax.grid() x_list = [] y_list = [] ref_list = [] ax.set_xlabel("time [s]") if plot_type == 'x': ax.set_title("CF X Simulation") ax.set_ylabel("x [m]") elif plot_type == 'y': ax.set_title("CF Y Simulation") ax.set_ylabel("y [m]") elif plot_type == 'z': ax.set_title("CF Z Simulation") ax.set_ylabel("z [m]") elif plot_type == 'psi': ax.set_title("CF Yaw Rate Simulation") ax.set_ylabel("psi [rad]") while t < P.t_end: # plotter can run the slowest t_next_plot = t + P.t_plot while t < t_next_plot: # offboard controller is slowest at 100 hz # t_next_ob = t + P.t_ob t_next_phys = t + P.t_phys # Altitude off-board controller update u_ob[2, 0] = altitiude_ctrl.update(r.item(2), cf.state.item(2)) # XY off-borad controller update # theta_c, phi_c x_c , y_c u_ob[0, 0], u_ob[1, 0] = xy_ctrl.update(r.item(0), r.item(1), cf.state) # print("theta_c {} phi_c {}".format(u_ob[0,0], u_ob[1,0])) # Yaw rate off-board controller update u_ob[3, 0] = yaw_ctrl.update(r.item(3), cf.state.item(3)) # For plotter ctrl = np.array([ [u_ob[0, 0]], # [deg] [u_ob[1, 0]], # [deg] [u_ob[2, 0]], # [PWM] [u_ob[3, 0]], # [deg/s] ]) # while t < t_next_ob: # attitude controller runs at 250 hz while t < t_next_phys: t_next_att = t + P.t_att # Conduct attitude control # phi controls x, theta controls y q_c, p_c = attitude_ctrl.update(u_ob.item(0), u_ob.item(1), cf.state) # print("p_c {} q_c {}".format(p_c, q_c)) while t < t_next_att: # rate controller is the fastest running at 500 hz t = t + P.t_rate # Conduct rate control del_phi, del_theta, del_psi = rate_ctrl.update( q_c, p_c, u_ob.item(3), cf.state) # print("del phi {} del theta {} del psi {}".format(del_phi, del_theta, del_psi)) # Update state of model u_pwm = ctrl_mixer.update(u_ob.item(2), del_phi, del_theta, del_psi) u = cf.pwm_to_rpm(u_pwm) y = cf.update(u) if save_plot: x_list.append((100.0 / 30.0) * t) if plot_type == 'x': y_list.append(cf.state.item(0)) ref_list.append(r.item(0)) elif plot_type == 'y': y_list.append(cf.state.item(1)) ref_list.append(r.item(1)) elif plot_type == 'z': y_list.append(cf.state.item(2)) ref_list.append(r.item(2)) elif plot_type == 'psi': y_list.append(cf.state.item(3)) ref_list.append(0.0174533 * r.item(3)) # if show_anim: # plot.update(t, r, cf.state, ctrl) # anim.update(cf.state) # plt.pause(0.0000001) # if show_anim: # plot.update(t, r, cf.state, ctrl) # anim.update(cf.state) # plt.pause(0.0000001) if show_anim: plot.update(t, r, cf.state, ctrl) anim.update(cf.state) plt.pause(0.0000001) # # Worst animation granularity, very fast # if show_anim: # plot.update(t, r, cf.state, ctrl) # anim.update(cf.state) # plt.pause(0.1) if save_plot: ax.plot(x_list, y_list, c='r') ax.plot(x_list, ref_list, c='b') if plot_type == 'x': fig.savefig("../plots/hover_x_1m_sim") elif plot_type == 'y': fig.savefig("../plots/hover_y_1m_sim") elif plot_type == 'z': fig.savefig("../plots/hover_z_1m_sim_2") elif plot_type == 'psi': fig.savefig("../plots/hover_yaw_1rad_sim") if show_anim: print('Press key to close') plt.waitforbuttonpress() plt.close()
def setUp(self): self.model = ChemotaxisModel() self.model.initialize() self.model.run(start=0, end=10) self.plotter = DataPlotter(self.model)
def hover(self, xr, yr, zr, goal_r): print('Start hover controller') # Initialize function specfic values ze_hist = 0. ze_prev = 0. x_b_prev = 0. xe_b_hist = 0. y_b_prev = 0. ye_b_hist = 0. origin = self.getPose('crazyflie4') pose = origin yawr = 0. # Plot with same class as in model plot = DataPlotter() state = np.array([ [P.x0], # 0 [P.y0], # 1 [P.z0], # 2 [P.psi0], # 3 [P.theta0], # 4 [P.phi0], # 5 [P.u0], # 6 [P.v0], # 7 [P.w0], # 8 [P.r0], # 9 [P.q0], # 10 [P.p0], # 11 ]) r = np.array([ [0.0], # x [0.0], # y [zr], # z [0.0], # psi [0.0], # theta [0.0], # phi ]) u = np.array([ [0.0], [0.0], [0.0], [0.0], ]) t = P.t_start while t < P.t_end: t_next_plot = t + P.t_plot while t < t_next_plot: # for _ in range(100): # Get current drone pose pose_prev = pose pose = self.getPose('crazyflie4') if math.isnan( pose.orientation.x ): # If nan is thrown, set to last known position pose = pose_prev ### Altitude controller ### z = pose.position.z # Get true z value ze = zr - z # Get error if ze_hist <= self.ze_cap: # prevent wind-up ze_hist += (ze * self.time_step) # Find integral component ze_der = ( ze - ze_prev) / self.time_step # Find derivative component ze_prev = ze ze_tot = self.z_feed_forward + (ze * self.z_kp) + (ze_hist * self.z_ki) \ + (ze_der * self.z_kd) # Eq. 3.1.7 Sum PID errors and multiply by gains self.msg.linear.z = ze_tot # print("Total thrust commanded:", ze_tot) ### xy position controller ### x = pose.position.x y = pose.position.y # Get true x and y values state[0, 0] = x state[1, 0] = y state[2, 0] = z xe = xr - x ye = yr - y # Get position error # Obtain yaw angle from quaternion quat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] R = Rotation.from_quat(quat) x_global = R.apply([1, 0, 0]) # project to world x-axis yaw = np.arctan2( np.cross([1, 0, 0], x_global)[2], np.dot(x_global, [1, 0, 0])) x_b = x * np.cos(yaw) + y * np.sin(yaw) # Get x in body frame u = (x_b - x_b_prev) / self.time_step # u is x-vel in body frame x_b_prev = x_b # Reset previous val y_b = -(x * np.sin(yaw)) + y * np.cos( yaw) # Get y in body frame v = (y_b - y_b_prev) / self.time_step # v is y-vel in body frame y_b_prev = y_b # Reset previous val xe_b = xe * np.cos(yaw) + ye * np.sin( yaw) # Get errors in body frame ye_b = -(xe * np.sin(yaw)) + ye * np.cos(yaw) xe_b_hist += ((xe_b - u) * self.time_step ) # Accumulate and store histroical error ye_b_hist += ((ye_b - v) * self.time_step) xe_b_tot = ((xe_b - u) * self.x_kp) + ( xe_b_hist * self.x_ki) # Eq. 3.1.11 and Eq. 3.1.12 ye_b_tot = ((ye_b - v) * self.y_kp) + (ye_b_hist * self.y_ki) # Cap roll (y) and pitch (x) to prevent unstable maneuvers if xe_b_tot >= self.x_cap: xe_b_tot = self.x_cap elif xe_b_tot <= -self.x_cap: xe_b_tot = -self.x_cap elif ye_b_tot >= self.y_cap: ye_b_tot = self.y_cap elif ye_b_tot <= -self.y_cap: ye_b_tot = -self.y_cap self.msg.linear.x = xe_b_tot self.msg.linear.y = ye_b_tot ### yaw-rate controller Eq. 3.1.13 ### yawe = yawr - yaw yawe_tot = self.yaw_kp * yawe self.msg.angular.z = yawe_tot ### Goal behavior ### offset = 0.05 # additional z boundary to speed tests if (x > (xr - goal_r) and x < (xr + goal_r)) and \ (y > (yr - goal_r) and y < (yr + goal_r)) and \ (z > (zr - goal_r - offset) and z < (zr + goal_r + offset)): print('Found the hover setpoint!') # break # # Add to plotter # to_plot_add = np.array([x, y, z]) # self.to_plot = np.vstack((self.to_plot, to_plot_add)) t += self.time_step self.pub.publish(self.msg) self.rate.sleep() plot.update(t, r, state, u) plt.pause(0.000001) # Keeps the program from closing until the user presses a button. print('Press key to close') plt.waitforbuttonpress() plt.close()
def __init__(self, application): self.app = application self.app.print_comment("Starting GUI interface:") self.app.print_comment("please wait while the application loads...") self._mode = "standby" self._update_counter = 0 self.data = None #build the GUI interface as a seperate window win = Tk() Pmw.initialise(win) #initialize Python MegaWidgets win.withdraw() win.wm_title(WINDOW_TITLE) win.focus_set() #put focus on this new window self.win = win #handle the user hitting the 'X' button self.win.protocol("WM_DELETE_WINDOW", self._close) #build the left panel left_frame = Frame(win) button_bar = Frame(left_frame) self.start_loop_button = Button(button_bar,text="Start Loop" ,command = self.start_loop) self.stop_loop_button = Button(button_bar,text="Stop Loop" ,command = self.stop_loop, state='disabled') self.chanA_pid_mode_button = Button(button_bar,text="Chan A PID = ON",command = self.toggle_chanA_pid_mode, relief = "sunken") self.chanB_pid_mode_button = Button(button_bar,text="Chan B PID = ON",command = self.toggle_chanB_pid_mode, relief = "sunken") self.send_command1_button = Button(button_bar,text="Send Command to TEIS" ,command = self.send_command_to_peltier_pid) self.run_script_button = Button(button_bar,text="Run Script" ,command = self.run_script) self.abort_script_button = Button(button_bar,text="Abort Script",command = self.abort_script, state='disabled') self.setpointGRAD_button = Button(button_bar,text="Set Gradient",command = lambda: self.change_setpoint(mode='GRAD')) self.setpointA_button = Button(button_bar,text="Set Temp. A" ,command = lambda: self.change_setpoint(mode='TEMP_A')) self.setpointB_button = Button(button_bar,text="Set Temp. B" ,command = lambda: self.change_setpoint(mode='TEMP_B')) self.clear_button = Button(button_bar,text="Clear Data" ,command = self.clear_data) self.export_button = Button(button_bar,text="Export Data" ,command = self.export_data) button_pack_opts = {'side':'top','fill':'x', 'expand':'yes', 'anchor':'nw'} self.start_loop_button.pack(**button_pack_opts) self.stop_loop_button.pack(**button_pack_opts) self.chanA_pid_mode_button.pack(**button_pack_opts) self.chanB_pid_mode_button.pack(**button_pack_opts) self.send_command1_button.pack(**button_pack_opts) self.run_script_button.pack(**button_pack_opts) self.abort_script_button.pack(**button_pack_opts) self.setpointGRAD_button.pack(**button_pack_opts) self.setpointA_button.pack(**button_pack_opts) self.setpointB_button.pack(**button_pack_opts) self.clear_button.pack(**button_pack_opts) self.export_button.pack(**button_pack_opts) button_bar.pack(side='top', fill='x', expand='no', anchor='nw') left_frame.pack(side='left', fill='both', padx=10) #build the middle panel mid_panel = Frame(win) self.data_plotter = DataPlotter(mid_panel) self.text_display = TextDisplayBox(mid_panel,text_height=TEXT_DISPLAY_HEIGHT, buffer_size = TEXT_BUFFER_SIZE) self.data_plotter.pack(fill='both',expand='yes') self.text_display.pack(fill='both',expand='yes') mid_panel.pack(fill='both', expand='yes',side='left') #build the right panel right_panel = Frame(win) self.sample_name_entry = Pmw.EntryField(right_panel, labelpos = 'w', label_text = 'Sample Name', command = self.change_sample_name) self.sample_name_entry.pack() right_panel.pack(side='right', fill='both', padx=10) #make a dialog window for sending a command self.command_dialog = Pmw.Dialog(parent = win, buttons = ('OK', 'Cancel'), defaultbutton = 'OK') self.command_dialog.withdraw() self.command_entry = Pmw.EntryField(self.command_dialog.interior(), labelpos = 'w', label_text = 'Command:', ) self.command_entry.pack() #make a dialog window for changing setpoint self.change_setpoint_dialog = Pmw.Dialog(parent = win, buttons = ('OK', 'Cancel'), defaultbutton = 'OK') self.change_setpoint_dialog.withdraw() self.change_setpoint_entry = Pmw.EntryField(self.change_setpoint_dialog.interior(), labelpos = 'w', label_text = 'Setpoint Value:', validate = "real") self.change_setpoint_entry.pack()
class GUI(object): def __init__(self, application): self.app = application self.app.print_comment("Starting GUI interface:") self.app.print_comment("please wait while the application loads...") self._mode = "standby" self._update_counter = 0 self.data = None #build the GUI interface as a seperate window win = Tk() Pmw.initialise(win) #initialize Python MegaWidgets win.withdraw() win.wm_title(WINDOW_TITLE) win.focus_set() #put focus on this new window self.win = win #handle the user hitting the 'X' button self.win.protocol("WM_DELETE_WINDOW", self._close) #build the left panel left_frame = Frame(win) button_bar = Frame(left_frame) self.start_loop_button = Button(button_bar,text="Start Loop" ,command = self.start_loop) self.stop_loop_button = Button(button_bar,text="Stop Loop" ,command = self.stop_loop, state='disabled') self.chanA_pid_mode_button = Button(button_bar,text="Chan A PID = ON",command = self.toggle_chanA_pid_mode, relief = "sunken") self.chanB_pid_mode_button = Button(button_bar,text="Chan B PID = ON",command = self.toggle_chanB_pid_mode, relief = "sunken") self.send_command1_button = Button(button_bar,text="Send Command to TEIS" ,command = self.send_command_to_peltier_pid) self.run_script_button = Button(button_bar,text="Run Script" ,command = self.run_script) self.abort_script_button = Button(button_bar,text="Abort Script",command = self.abort_script, state='disabled') self.setpointGRAD_button = Button(button_bar,text="Set Gradient",command = lambda: self.change_setpoint(mode='GRAD')) self.setpointA_button = Button(button_bar,text="Set Temp. A" ,command = lambda: self.change_setpoint(mode='TEMP_A')) self.setpointB_button = Button(button_bar,text="Set Temp. B" ,command = lambda: self.change_setpoint(mode='TEMP_B')) self.clear_button = Button(button_bar,text="Clear Data" ,command = self.clear_data) self.export_button = Button(button_bar,text="Export Data" ,command = self.export_data) button_pack_opts = {'side':'top','fill':'x', 'expand':'yes', 'anchor':'nw'} self.start_loop_button.pack(**button_pack_opts) self.stop_loop_button.pack(**button_pack_opts) self.chanA_pid_mode_button.pack(**button_pack_opts) self.chanB_pid_mode_button.pack(**button_pack_opts) self.send_command1_button.pack(**button_pack_opts) self.run_script_button.pack(**button_pack_opts) self.abort_script_button.pack(**button_pack_opts) self.setpointGRAD_button.pack(**button_pack_opts) self.setpointA_button.pack(**button_pack_opts) self.setpointB_button.pack(**button_pack_opts) self.clear_button.pack(**button_pack_opts) self.export_button.pack(**button_pack_opts) button_bar.pack(side='top', fill='x', expand='no', anchor='nw') left_frame.pack(side='left', fill='both', padx=10) #build the middle panel mid_panel = Frame(win) self.data_plotter = DataPlotter(mid_panel) self.text_display = TextDisplayBox(mid_panel,text_height=TEXT_DISPLAY_HEIGHT, buffer_size = TEXT_BUFFER_SIZE) self.data_plotter.pack(fill='both',expand='yes') self.text_display.pack(fill='both',expand='yes') mid_panel.pack(fill='both', expand='yes',side='left') #build the right panel right_panel = Frame(win) self.sample_name_entry = Pmw.EntryField(right_panel, labelpos = 'w', label_text = 'Sample Name', command = self.change_sample_name) self.sample_name_entry.pack() right_panel.pack(side='right', fill='both', padx=10) #make a dialog window for sending a command self.command_dialog = Pmw.Dialog(parent = win, buttons = ('OK', 'Cancel'), defaultbutton = 'OK') self.command_dialog.withdraw() self.command_entry = Pmw.EntryField(self.command_dialog.interior(), labelpos = 'w', label_text = 'Command:', ) self.command_entry.pack() #make a dialog window for changing setpoint self.change_setpoint_dialog = Pmw.Dialog(parent = win, buttons = ('OK', 'Cancel'), defaultbutton = 'OK') self.change_setpoint_dialog.withdraw() self.change_setpoint_entry = Pmw.EntryField(self.change_setpoint_dialog.interior(), labelpos = 'w', label_text = 'Setpoint Value:', validate = "real") self.change_setpoint_entry.pack() def launch(self): #run the GUI handling loop IgnoreKeyboardInterrupt() self.win.deiconify() #loop until killed self.win.mainloop() NoticeKeyboardInterrupt() def update_data(self): self.data = self.app.update_data() def update_plot(self): self.data_plotter.update(self.data) def start_loop(self): self.start_loop_button.config(state='disabled') self.stop_loop_button.config(state='normal') self._mode = "loop" self._loop_data_update() self._loop_plot_update() def _loop_data_update(self): if self._mode == "loop": self.update_data() self.win.after(DATA_UPDATE_PERIOD, self._loop_data_update) def _loop_plot_update(self): if self._mode == "loop": self.update_plot() self.win.after(PLOT_UPDATE_PERIOD, self._loop_plot_update) def stop_loop(self): self.stop_loop_button.config(state='disabled') self.start_loop_button.config(state='normal') self._mode = "standby" def toggle_chanA_pid_mode(self): relief = self.chanA_pid_mode_button.cget('relief') if relief == 'raised': self.chanA_pid_mode_button.config(text="Chan A PID = ON", relief='sunken') self.app.print_comment("Toggling Channel A PID state -> 'on'") self.app.peltier_pid.set_pid_control_mode(chan='A', mode = 'on') elif relief == 'sunken': self.chanA_pid_mode_button.config(text="Chan A PID = OFF", relief='raised') self.app.print_comment("Toggling Channel A PID state -> 'off'") self.app.peltier_pid.set_pid_control_mode(chan='A', mode = 'off') def toggle_chanB_pid_mode(self): relief = self.chanB_pid_mode_button.cget('relief') if relief == 'raised': self.chanB_pid_mode_button.config(text="Chan B PID = ON", relief='sunken') self.app.print_comment("Toggling Channel B PID state -> 'on'") self.app.peltier_pid.set_pid_control_mode(chan='B', mode = 'on') elif relief == 'sunken': self.chanB_pid_mode_button.config(text="Chan B PID = OFF", relief='raised') self.app.print_comment("Toggling Channel B PID state -> 'off'") self.app.peltier_pid.set_pid_control_mode(chan='B', mode = 'off') def send_command_to_peltier_pid(self): #self.command_entry.focus_set() result = self.command_dialog.activate() if result == "OK": cmd = self.command_entry.getvalue() resp = self.app.send_command_to_peltier_pid(cmd) def run_script(self): self.stop_loop() fdlg = FileDialog(self.win,title="Run Script") input_dir = "/home/cversek/gitwork/umass-physics/teis/python/src/pyTEIS/scripts/" #FIXME #os.getcwd() filepath = fdlg.go(dir_or_file = input_dir, pattern="*.py", ) if filepath: self.disable_controls() self.abort_script_button.config(state='normal') self._mode = "scripting" self.app.run_script(filepath) self._script_loop() def _script_loop(self): if self._mode == "scripting": while not self.app._script_event_queue.empty(): event_type, obj = self.app._script_event_queue.get() if event_type == "PRINT": self.app.print_comment(obj) elif event_type == "UPDATE_DATA": self.update_data() elif event_type == "UPDATE_PLOT": self.update_plot() elif event_type == "UPDATE": self.update_data() self.update_plot() elif event_type == "CLEAR_DATA": self.clear_data() elif event_type == "ERROR": exc_type, exc, tb = obj msg = traceback.format_exception(exc_type, exc, tb) msg = "".join(msg) self.app.print_comment("Caught Error in Script: %s" % msg) msg = "%s\nCheck the console for the traceback" % (exc,) Pmw.MessageDialog(parent = self.win, title = 'Script Error', message_text = msg, iconpos = 'w', icon_bitmap = 'error',buttons = ('OK',) ) elif event_type == "ABORTED": self.app.print_comment("Script aborted.") else: self.app.print_comment("Got unknown event '%s' with obj=%r" % (event_type,obj)) if self.app._script_thread.is_alive(): self.win.after(SCRIPT_LOOP_UPDATE_PERIOD, self._script_loop) else: self.app.print_comment("Script finished.") self.enable_controls() self._mode = "standby" def abort_script(self): self.app._script_thread.terminate() self.app._script_thread.join() self.abort_script_button.config(state='disabled') self.enable_controls() def disable_controls(self): #disable all controls self.start_loop_button.config(state='disabled') self.chanA_pid_mode_button.config(state='disabled') self.chanB_pid_mode_button.config(state='disabled') self.send_command1_button.config(state='disabled') self.run_script_button.config(state='disabled') self.setpointGRAD_button.config(state='disabled') self.setpointA_button.config(state='disabled') self.setpointB_button.config(state='disabled') self.clear_button.config(state='disabled') self.export_button.config(state='disabled') def enable_controls(self): #enable most controls self.start_loop_button.config(state='normal') self.chanA_pid_mode_button.config(state='normal') self.chanB_pid_mode_button.config(state='normal') self.send_command1_button.config(state='normal') self.run_script_button.config(state='normal') self.setpointGRAD_button.config(state='normal') self.setpointA_button.config(state='normal') self.setpointB_button.config(state='normal') self.clear_button.config(state='normal') self.export_button.config(state='normal') def print_to_text_display(self, text, eol='\n'): self.text_display.print_text(text, eol=eol) def warn(self, msg): warnings.warn(msg) self.app.print_comment("Warning: %s" % msg) def change_setpoint(self, mode): result = self.change_setpoint_dialog.activate() if result == "OK": temp = self.change_setpoint_entry.getvalue() temp = float(temp) if mode == 'GRAD': self.app.change_gradient_setpoint(temp) elif mode == 'TEMP_A': self.app.change_setpointA(temp) elif mode == 'TEMP_B': self.app.change_setpointB(temp) def clear_data(self): self.app.metadata['start_timestamp'] = time.time() self.app._init_data() self.data_plotter.setup() def change_sample_name(self): sample_name = self.sample_name_entry.getvalue() self.app.metadata['sample_name'] = sample_name self.data_plotter.change_title(new_title = sample_name) def export_data(self): self.app.print_comment("exporting data...") data = self.app.data metadata = self.app.metadata d = datetime.datetime.fromtimestamp(metadata['start_timestamp']) d = d.strftime("%Y-%m-%d") default_filename = "%s_%s_peltier_test.csv" % (d,metadata['sample_name']) fdlg = SaveFileDialog(self.win,title="Export Data") output_dir = os.getcwd() filename = fdlg.go(dir_or_file = output_dir, pattern="*.csv", default=default_filename, key = None ) if filename: self.app.export_data(filename) def _close(self): self.win.destroy()
class MainApp: def __init__(self, master): self.master = master self.canvas = None menu_bar = tk.Menu(master) file_menu = tk.Menu(menu_bar, tearoff=0) file_menu.add_command(label="Load Emigration Data", command=self.load_data) file_menu.add_command(label="Save Last Plot") file_menu.add_separator() file_menu.add_command(label="Exit", command=master.quit) menu_bar.add_cascade(label="File", menu=file_menu) master.config(menu=menu_bar) control_f = tk.Frame(master, width=200, background='grey', relief=tk.RAISED, bd=2) control_f.pack_propagate(0) self.pack_controls_header(control_f, "Data Options:") data_options_f = tk.Frame(control_f) self.grid_row = 0 self.split_on_var = tk.StringVar(value='none') self.split_options = self.label_and_field(data_options_f, 'Split Data On', 'OptionMenu', self.split_on_var, ('None', )) self.best_nest_var = tk.IntVar() self.best_nest_var.set(2) self.label_and_field(data_options_f, "Best Nest ID", 'Entry', self.best_nest_var) self.max_sim_time_var = tk.IntVar() self.max_sim_time_var.set(120) self.label_and_field(data_options_f, "Max Sim Duration", 'Entry', self.max_sim_time_var) data_options_f.pack(ipady=3) self.pack_controls_header(control_f, "Display Plots:") tk.Button(control_f, text="Display All Basic Plots", command=lambda: self.display_plot('all')).pack(fill='x') tk.Button(control_f, text="Display Custom Plot").pack(fill='x') self.pack_controls_header(control_f, "Individual Plots:") self.plot_option = tk.StringVar(value=next(iter(plot_options.keys()))) tk.OptionMenu(control_f, self.plot_option, *plot_options).pack(fill='x') display_individual_plot = lambda: self.display_plot(plot_options[ self.plot_option.get()]) tk.Button(control_f, text="Show Plot", command=display_individual_plot).pack(fill='x') self.pack_controls_header(control_f, "Plot Options:") plot_options_f = tk.Frame(control_f) self.trim_data = tk.BooleanVar() self.label_and_field(plot_options_f, 'Trim min/max Data', 'Checkbox', self.trim_data) self.trim_data_percent = tk.DoubleVar(value=5.0) self.label_and_field(plot_options_f, 'Trim Data %', 'Entry', self.trim_data_percent) plot_styles = ['line', 'basic scatter', 'grouped scatter', 'box plot'] self.plot_style = tk.StringVar(value=plot_styles[0]) self.label_and_field(plot_options_f, "Plot As", 'OptionMenu', self.plot_style, plot_styles) error_bar_opts = ['1 SD', '2 SD', 'none'] self.error_bars = tk.StringVar(value=error_bar_opts[0]) self.label_and_field(plot_options_f, 'Error Bars', 'OptionMenu', self.error_bars, error_bar_opts) plot_options_f.pack(fill='x') self.constraint_list = [] constraint_buttons = tk.Frame(control_f) self.add_button = tk.Button(constraint_buttons, text="Add New Constraint", command=self.add_constraint, state=tk.DISABLED) self.add_button.pack(expand=1, fill='x', side='left') tk.Button(constraint_buttons, text="Remove", command=self.remove_constraint).pack(fill='x', side='left') constraint_buttons.pack(fill='x') constraints_frame = tk.Frame(control_f) scrollbar = tk.Scrollbar(constraints_frame, orient=tk.VERTICAL) self.constraints_box = tk.Listbox(constraints_frame, yscrollcommand=scrollbar.set, height=3) scrollbar.config(command=self.constraints_box.yview) scrollbar.pack(side=tk.RIGHT, fill=tk.Y) self.constraints_box.pack(fill='x') constraints_frame.pack(fill='x') self.pack_controls_header(control_f, "Pairwise T-Tests:") t_test_f = tk.Frame(control_f) self.two_tolerance = tk.BooleanVar() self.label_and_field(t_test_f, 'Show Both 0.05 & 0.01', 'CheckBox', self.two_tolerance) t_test_f.pack(fill='x') tk.Button( control_f, text='Speed-Quorum Tests', command=lambda: self.calculate_t_tests('speed')).pack(fill='x') tk.Button( control_f, text='Cohesion-Quorum Tests', command=lambda: self.calculate_t_tests('cohesion')).pack(fill='x') control_f.pack(side='left', padx=(10, 0), pady=10, ipadx=5, fill='y') self.data_frame = tk.Frame(master, width=700, bg='green') scrollbar_y = tk.Scrollbar(self.data_frame) scrollbar_y.pack(side=tk.RIGHT, fill=tk.Y) scrollbar_x = tk.Scrollbar(self.data_frame, orient=tk.HORIZONTAL) scrollbar_x.pack(side=tk.BOTTOM, fill=tk.X) self.list_box = tk.Listbox(self.data_frame, background='grey', yscrollcommand=scrollbar_y.set, xscrollcommand=scrollbar_x.set) self.list_box.insert('end', "No data is loaded, load via the File menu.") self.list_box.pack(fill='both', expand=True) scrollbar_y.config(command=self.list_box.yview) scrollbar_x.config(command=self.list_box.xview) self.data_frame.pack(side='left', fill='both', padx=10, pady=10, expand=True) self.data_set = None self.data_plot = None @staticmethod def pack_controls_header(parent, text): tk.Label(parent, text=text, fg='white', bg='#003489', relief=tk.RAISED, font=("", 12, "bold")).pack(fill='x', pady=(5, 0), ipady=5) def label_and_field(self, parent, text, field_type, answer_var, options=None): row = self.grid_row self.grid_row += 1 parent.grid_columnconfigure(1, weight=1) tk.Label(parent, text=text).grid(row=row, column=0) if field_type == 'Entry': input_widget = tk.Entry(parent, textvariable=answer_var) elif field_type == 'OptionMenu': input_widget = tk.OptionMenu(parent, answer_var, *options) else: # field_type == 'CheckButton': input_widget = tk.Checkbutton(parent, text="", variable=answer_var) input_widget.grid(row=row, column=1, sticky='W E') return input_widget def load_data(self): directory_path = filedialog.askdirectory( initialdir=os.getcwd(), mustexist=True, title="Please select the data directory...") extractor = DataExtractor(directory_path, self.best_nest_var.get(), self.max_sim_time_var.get()) invalid_files, unfinished_sims = extractor.extract_data() self.data_set = extractor.data_set self.data_plot = DataPlotter(self.data_set) msg_string = "%s simulations had missing or blank files.\n" % invalid_files msg_string += "%s simulations exceeded than the maximum time and so were removed." % unfinished_sims messagebox.showinfo('Data Loaded', msg_string) self.list_box.delete(0, tk.END) grid_row = 0 for data in self.data_set: raw_data_string = "" for key, value in data.items(): raw_data_string += "%s=%s, " % (key, value) grid_row += 1 self.list_box.insert(tk.END, raw_data_string[:-2]) if grid_row % 2 == 0: self.list_box.itemconfig(tk.END, bg='#e0e0e0') else: self.list_box.itemconfig(tk.END, bg='#f4f4f4') # Updating the list of options to split the data by options = self.data_set[0].keys() menu = self.split_options["menu"] menu.delete(0, "end") menu.add_command(label='none', command=lambda: self.split_on_var.set('none')) for string in options: menu.add_command( label=string, command=lambda option=string: self.split_on_var.set(option)) self.add_button.config(state=tk.ACTIVE) def display_plot(self, plot_type): if self.data_plot is None: messagebox.showwarning( 'No Data Set Loaded', 'A data set must be loaded before plots can be displayed.') split_on = self.split_on_var.get() trim = self.trim_data.get() trim_pc = self.trim_data_percent.get() style = self.plot_style.get() err_bars = self.error_bars.get() self.data_plot.basic_plots(split_on, plot_type, self.constraint_list, trim, trim_pc, style, err_bars) def display_custom_plot(self): pass def add_constraint(self): pop_window = tk.Toplevel(self.master) add_constraint_f = tk.Frame(pop_window) keys = self.data_set[0].keys() constraint = tk.StringVar(value=next(iter(keys))) tk.OptionMenu(add_constraint_f, constraint, *keys).grid(row=0, column=0) operator = tk.StringVar(value='=') tk.OptionMenu(add_constraint_f, operator, '=', '!=', '<', '>', '<=', '>=').grid(row=0, column=1) value = tk.StringVar() tk.Entry(add_constraint_f, textvariable=value).grid(row=0, column=2) confirm_add = lambda: self.confirm_add_constraint( constraint.get(), operator.get(), value.get(), pop_window) tk.Button(add_constraint_f, text="Add", command=confirm_add).grid(row=1, column=0, columnspan=2) tk.Button(add_constraint_f, text="Cancel", command=lambda: self.close_window(pop_window)).grid(row=1, column=2) add_constraint_f.pack() def confirm_add_constraint(self, constraint, operator, value, window): self.constraint_list.append(Constraint(constraint, operator, value)) self.constraints_box.insert(tk.END, "%s %s %s" % (constraint, operator, value)) window.destroy() def remove_constraint(self): if len(self.constraints_box.curselection()) < 1: return index = self.constraints_box.curselection()[0] del (self.constraint_list[index]) self.constraints_box.delete(index) @staticmethod def close_window(window): window.destroy() def calculate_t_tests(self, data_type): p_values, quorums = self.data_plot.t_tests( data_type, self.constraint_list, self.trim_data.get(), self.trim_data_percent.get()) pop_window = tk.Toplevel(self.master, bg='black') for i in range(0, len(quorums)): tk.Label(pop_window, text=int(quorums[i]), bg='grey').grid(row=i + 1, column=0, sticky='N S E W', pady=1, padx=1) tk.Label(pop_window, text=int(quorums[i]), bg='grey').grid(row=0, column=i + 1, sticky='N S E W', pady=1, padx=1) upper_threshold = 0.10 # 0.05 lower_threshold = 0.05 # 0.01 for x in range(0, len(quorums)): for y in range(0, len(quorums)): if p_values[x][y] == 'NaN': value = 'NaN' colour = 'grey' else: value = round(p_values[x][y], 3) if self.two_tolerance.get( ) == True and value > lower_threshold and value <= upper_threshold: colour = 'orange' elif value > upper_threshold: colour = 'red' else: colour = 'green' tk.Label(pop_window, text=value, bg=colour).grid(row=x + 1, column=y + 1, sticky='N S E W', pady=1, padx=1) tk.Grid.rowconfigure(pop_window, x + 1, weight=1) tk.Grid.columnconfigure(pop_window, y + 1, weight=1)
def visual_odometry_core(self): """ Runs pose estimation using a visual odometry pipeline assuming that images are obtained from a monocular camera set on a duckiebot wondering around duckietown :return: The estimated transformation between the current image and the one from last call. :rtype: geometry_msgs.TransformStamped """ parameters = self.parameters train_image = self.images[1] query_image = self.images[0] # Initialize transformation between camera frames t = Transform() ############################################ # MAIN BODY # ############################################ processed_data_plotter = DataPlotter(train_image, query_image, parameters) # Instantiate histogram logic filter histogram_filter = HistogramLogicFilter(train_image.width, train_image.height) start = time.time() if parameters.matcher == 'KNN': # K-Nearest Neighbors matcher bf = cv2.BFMatcher() matches = bf.knnMatch(train_image.descriptors, query_image.descriptors, k=parameters.knn_neighbors) else: # Brute force matcher bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) matches = bf.match(train_image.descriptors, query_image.descriptors) end = time.time() print("TIME: Matching done. Elapsed time: %s", end - start) # Initialize fitness trackers fitness = float('-inf') max_fit = fitness # Explore all the weight values for weight in parameters.knn_weight: # Filter knn matches by best to second best match ratio if parameters.matcher == 'KNN': start = time.time() matches = knn_match_filter(matches, weight) end = time.time() print("TIME: Distance filtering of matches done. Elapsed time: %s", end - start) # Filter histograms by gaussian function fitting if parameters.filter_by_histogram: start = time.time() histogram_filter.filter_matches_by_histogram_fitting( matches, train_image.keypoints, query_image.keypoints, parameters.threshold_angle, parameters.threshold_length) end = time.time() print("TIME: Histogram filtering done. Elapsed time: %s", end - start) fitness = histogram_filter.angle_fitness + histogram_filter.length_fitness # Store current configuration as best configuration if fitness is new maximum if fitness > max_fit: histogram_filter.save_configuration() max_fit = fitness # Recover best configuration from histogram filtering (for best weight) if parameters.filter_by_histogram and histogram_filter.saved_configuration is not None: unfiltered_matches = matches matches = histogram_filter.saved_configuration.filter_data_by_histogram() # Publish the results of histogram filtering if parameters.plot_histogram_filtering: self.histogram_img = processed_data_plotter.plot_histogram_filtering(unfiltered_matches, matches, histogram_filter) n_final_matches = len(matches) # Lists of final filtered matches matched_train_points = [None] * n_final_matches matched_query_points = [None] * n_final_matches for match_index, match_object in enumerate(matches): matched_train_points[match_index] = train_image.keypoints[match_object.queryIdx].pt matched_query_points[match_index] = query_image.keypoints[match_object.trainIdx].pt try: [h, w] = [query_image.height, query_image.width] start = time.time() # Split between far-region and close region matches match_distance_filter = \ DistanceFilter(matched_query_points, matched_train_points, self.camera_K, self.camera_D, (h, w), (parameters.shrink_x_ratio, parameters.shrink_y_ratio)) match_distance_filter.split_by_distance_mask(self.stingray_mask) end = time.time() print("TIME: Mask filtering done. Elapsed time: %s", end - start) if parameters.plot_masking: self.mask_img = processed_data_plotter.plot_displacements_from_distance_mask(match_distance_filter) start = time.time() n_distant_matches = len(match_distance_filter.rectified_distant_query_points) if n_distant_matches > 0: rot_hypothesis = [] # Average sign of rotation rot_sign = np.sign(np.average(np.array(matched_query_points) - np.array(matched_train_points), axis=0)[0]) # Calculate two rotation hypothesis for all far matches assuming that they lay distant to camera for distant_q_p, distant_t_p in \ zip(match_distance_filter.rectified_distant_query_points, match_distance_filter.rectified_distant_train_points): a = (distant_t_p[0] - distant_q_p[0]) \ / np.sqrt((distant_t_p[0]*distant_q_p[0])**2 + distant_t_p[0]**2 + distant_q_p[0]**2 + 1) rot = np.arcsin(a) # Use hypothesis whose sign is consistent with the average sign for rot_h in np.unique(rot): if np.sign(rot_h) == rot_sign: rot_hypothesis.append(-rot_h) else: rot_hypothesis.append(rot_h) rot_hypothesis = np.unique(rot_hypothesis) rot_hypothesis_rmse = np.zeros((len(rot_hypothesis), 1)) # Select the best hypothesis using 1 point RANSAC for hypothesis_index in range(0, len(rot_hypothesis)): hypothesis = rot_hypothesis[hypothesis_index] rot_mat = np.array([[np.cos(hypothesis), 0, np.sin(hypothesis)], [0, 1, 0], [-np.sin(hypothesis), 0, np.cos(hypothesis)]]) rotated_train_points = np.matmul( np.append(np.reshape(match_distance_filter.rectified_distant_train_points, (n_distant_matches, 2)), np.ones((n_distant_matches, 1)), axis=1), rot_mat) # Calculate rmse of hypothesis with all peripheral points error = rotated_train_points[:, 0:2] - np.reshape( match_distance_filter.rectified_distant_query_points, (n_distant_matches, 2)) rot_hypothesis_rmse[hypothesis_index] = np.sum(np.sqrt(np.sum(np.power(error, 2), axis=1))) theta = rot_hypothesis[np.argmin(rot_hypothesis_rmse)] self.last_theta = theta else: # If there were not enough matches to estimate a new theta, use the one from previous iteration theta = self.last_theta z_rot_mat = np.array([[np.cos(theta), np.sin(theta), 0], [-np.sin(theta), np.cos(theta), 0], [0, 0, 1]]) t_vec = [self.joy_command.v / 30.0, 0, 0] # Calculate quaternion of z-mapped rotation [roll, pitch, yaw] = rotation_matrix_to_euler_angles(z_rot_mat) z_quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) end = time.time() print("TIME: Pose estimation done. Elapsed time: %s", end - start) t.translation = Vector3(t_vec[0], t_vec[1], t_vec[2]) t.rotation = Quaternion(z_quaternion[0], z_quaternion[1], z_quaternion[2], z_quaternion[3]) except Exception: raise return t, self.histogram_img, self.mask_img
import sys import numpy as np import crazyflie_param as P # from signal_generator import signal_generator # from crazyflie_animation import crazyflie_animation from data_plotter import DataPlotter from crazyflie_dynamics import CrazyflieDynamics # from crazyflie_controller import RateController, AttitudeController, ControlMixer, OffBoardController # instatiate crazyflie, controller, and reference classes cf = CrazyflieDynamics() # TODO create controller, crazyflie_animation, signal_generator # # instatiate the simulation plots and animation data_plot = DataPlotter() # animation = crazyflie_animation if __name__ == "__main__": # PWM is sent from controller to motors 0 - 65535 # RPM is sent from motor to cf dynamics 4070.3 - 21666.4 by Eq. 2.6.1 u = np.array([ [0.0], [0.0], [0.0], [0.0], ]) # r = 0.5 # zref value [m] # Reference is x,y,z global position and cf yaw angle r = np.array([ [0.0], # x
def temperature(): auth_token = request.headers.get('Authorization') data = DataFetcher(auth_token, ASSET_ID, ASPECT).fetch_data('temperature') chart = DataPlotter().plot_line_chart(data[0], data[1], 'Temperature') return render_template('temperature.html', line_chart=Markup(chart))