def camera_connect(self):
self.statusBar.showMessage('Camera connecting...')
self.camera_thread = CameraThread(
self,
name='CameraThread',
width=load_value('camera', 'width', 800),
height=load_value('camera', 'height', 600),
fps=load_value('camera', 'fps', 15),
delay=load_value('camera', 'delay', 0.05))
def init_load_data(self):
mkdir(self.figure_dir)
mkdir(self.logs_dir)
mkdir(self.detail_dir)
self.measurements = load_value(
os.path.join(self.data_dir, 'measures.npy'))
self.states = load_value(os.path.join(self.data_dir, 'states.npy'))
self.states_clean = load_value(
os.path.join(self.data_dir, 'states_clean.npy'))
def serial_connect(self):
self.statusBar.showMessage('Serial connecting...')
self.serial_thread = SerialThread(
parent=self,
name='SerialThread',
port=self.serial_port,
baudrate=load_value('serial', 'baudrate', 9600),
timeout=load_value('serial', 'timeout', 5),
delay=load_value('serial', 'delay', 1),
)
def get_train_data(self, k):
self.measurements = list()
self.states = list()
self.next_measurement = list()
self.next_states = list()
for index in range(self.train_num):
data_dir = os.path.join('../Lorenz_data/' + str(index), 'data')
measurements = load_value(os.path.join(data_dir, 'measures.npy'))
states = load_value(os.path.join(data_dir, 'states.npy'))
# sample = random.sample(np.shape(measurements)[0]-self.delay, self.sample_num[index]) #从训练集文件中采样
self.measurements.append(measurements[k:k - self.delay:-1])
self.states.append(states[k:k - self.delay:-1, 0])
self.next_measurement.append(measurements[k + 1].tolist()[0])
self.next_states.append(states[k + 1, :])
def update_window(self):
if self.camera_thread:
img = self.camera_thread.get_image()
if img is not None:
width = load_value('camera', 'width', 300)
height = load_value('camera', 'height', 240)
main_img = resize_image(img, width, height)
main_img = cv2.cvtColor(main_img, cv2.COLOR_BGR2RGB)
height, width, bpc = main_img.shape
bpl = bpc * width
main_image = QImage(main_img.data, width, height, bpl,
QImage.Format_RGB888)
self.cameraWidget.set_image(main_image)
if self.serial_thread:
rpy = self.serial_thread.get_rpy()
self.rpyLabel.setText('RPY: ({}, {}, {})'.format(
rpy[0], rpy[1], rpy[2]))
def update_kinematic(self):
self.button_motions = load_value('kinematic', 'button_motions', {})
self.parent.macroCommandComboBox.clear()
# Add command list into combobox
for key in self.button_motions.keys():
if sublist(['name', 'data'], self.button_motions[key].keys()):
if self.button_motions[key]['name'] != '-':
self.parent.macroCommandComboBox.addItem(
self.button_motions[key]['name'])
def load(self):
name = self.parent.macroCommandLoadComboBox.currentText()
new_data = load_value('macro', name, None)
if new_data is None:
return
self.macroData = []
self.macroModel.clear()
self.macroModel.setColumnCount(3)
self.macroModel.setHorizontalHeaderLabels(['', 'Name', 'Data'])
for item in new_data:
self.append_row_to_model(item['command'], item['name'],
item['data'], item['checked'])
def initialize(self, x, P):
"""
Initializes the filter with the specified mean and
covariance. Only need to call this if you are using the filter
to filter more than one set of data; this is called by __init__
Parameters
----------
x : np.array(dim_z)
state mean
P : np.array((dim_x, dim_x))
covariance of the state
"""
if self.x.ndim != 1:
raise ValueError('x must be a 1D array')
# self.sigmas = multivariate_normal(mean=self.x, cov=self.P, size=self.N) #sample
self.sigmas = np.zeros((self.N, 3))
self.xk = np.zeros((self.N, self.d))
self.train_measurement = list()
for index in range(self.N):
train_states = load_value(
os.path.join('../Lorenz_data/{}/data'.format(index),
'states.npy'))
self.sigmas[index] = train_states[self.d, :]
self.xk[index] = train_states[:self.d, 0][-1::-1]
# measurement = np.reshape(load_value(os.path.join('../Lorenz_data/{}/data'.format(index), 'measures.npy')), -1)
# self.train_measurement.append(measurement)
# print(self.train_measurement[index][:self.d][-1::-1])
# self.xk[index] = measurement[:self.d][-1::-1]
self.x = x
self.P = P
# these will always be a copy of x,P after predict() is called
self.x_prior = self.x.copy()
self.P_prior = self.P.copy()
# these will always be a copy of x,P after update() is called
self.x_post = self.x.copy()
self.P_post = self.P.copy()
class DisplayWindowClass(QMainWindow, main_form_class):
camera_thread = None
serial_thread = None
serial_port = load_value('serial', 'port', 'Test')
serial_port_action_list = []
logger = pyqtSignal(str)
camera_connected = pyqtSignal()
camera_disconnected = pyqtSignal()
serial_connected = pyqtSignal()
serial_disconnected = pyqtSignal()
def __init__(self, parent=None):
super(DisplayWindowClass, self).__init__(parent)
self.setupUi(self)
# logging
self.logger.connect(self.append_log)
log_text_edit = QPlainTextEditLogger(self)
log_text_edit.setFormatter(
logging.Formatter(logging_config['widget']['format']))
log_text_edit.setLevel(logging_config['widget']['level'])
logging.getLogger().addHandler(log_text_edit)
# setup menus and shortcut
self.setup_file_menu()
self.setup_serial_menu()
self.setup_shortcut()
self.cameraWidget = ImageWidget(self.cameraWidget)
self.rpyLabel.setText('RPY: Disconnected')
# signal and slot
self.serial_connected.connect(self.update_window_serial_connected)
self.serial_disconnected.connect(
self.update_window_serial_disconnected)
self.camera_connected.connect(self.update_window_camera_connected)
self.camera_disconnected.connect(
self.update_window_camera_disconnected)
# update widget every 100ms
self.timer = QTimer(self)
self.timer.timeout.connect(self.update_window)
self.timer.start(100) # 100ms
# macro
self.macro = widget.MacroWidget(self)
self.kinematic = widget.KinematicWidget(self)
def setup_file_menu(self):
self.actionQuit.triggered.connect(self.close)
self.actionCameraConnect.triggered.connect(self.camera_connect)
self.actionCameraDisconnect.triggered.connect(self.camera_disconnect)
self.actionSerialConnect.triggered.connect(self.serial_connect)
self.actionSerialDisconnect.triggered.connect(self.serial_disconnect)
def setup_serial_menu(self):
port_list = get_serial_port_list()
if len(port_list):
for port in port_list:
action = QAction(port, self)
action.setEnabled(True)
action.setCheckable(True)
self.menuSerialPort.addAction(action)
self.serial_port_action_list.append(action)
if self.serial_port == port:
action.setChecked(True)
else:
action = QAction('Cannot detect usable port ...', self)
action.setEnabled(False)
action.setCheckable(False)
self.menuSerialPort.addAction(action)
self.serial_port_action_list.append(action)
action = QAction('Test', self)
action.setEnabled(True)
action.setCheckable(True)
self.menuSerialPort.addSeparator()
self.menuSerialPort.addAction(action)
self.serial_port_action_list.append(action)
if self.serial_port == 'Test':
action.setChecked(True)
self.menuSerialPort.triggered[QAction].connect(self.select_serial_port)
def setup_shortcut(self):
# game start & stop menu action
# self.actionGameStart.shortcut = QShortcut(QKeySequence(Qt.Key_4, Qt.Key_5, Qt.Key_6), self)
# self.actionGameStart.shortcut.activated.connect(self.game_start)
# self.actionGameStart.triggered.connect(self.game_start)
# self.actionGameStop.shortcut = QShortcut(QKeySequence(Qt.Key_3, Qt.Key_2, Qt.Key_1), self)
# self.actionGameStop.shortcut.activated.connect(self.game_stop)
# self.actionGameStop.triggered.connect(self.game_stop)
pass
def select_serial_port(self, action):
# un-check all menu actions
for item in self.serial_port_action_list:
item.setChecked(False)
# check selected serial port action
action.setChecked(True)
self.serial_port = action.text()
save_value('serial', 'port', self.serial_port)
def serial_connect(self):
self.statusBar.showMessage('Serial connecting...')
self.serial_thread = SerialThread(
parent=self,
name='SerialThread',
port=self.serial_port,
baudrate=load_value('serial', 'baudrate', 9600),
timeout=load_value('serial', 'timeout', 5),
delay=load_value('serial', 'delay', 1),
)
def serial_disconnect(self):
self.statusBar.showMessage('Serial disconnecting...')
if self.serial_thread is not None:
self.serial_thread.do_stop()
self.serial_thread.join()
self.serial_thread = None
def camera_connect(self):
self.statusBar.showMessage('Camera connecting...')
self.camera_thread = CameraThread(
self,
name='CameraThread',
width=load_value('camera', 'width', 800),
height=load_value('camera', 'height', 600),
fps=load_value('camera', 'fps', 15),
delay=load_value('camera', 'delay', 0.05))
def camera_disconnect(self):
self.statusBar.showMessage('Camera disconnecting...')
if self.camera_thread is not None:
self.camera_thread.do_stop()
self.camera_thread.join()
self.camera_thread = None
def update_window(self):
if self.camera_thread:
img = self.camera_thread.get_image()
if img is not None:
width = load_value('camera', 'width', 300)
height = load_value('camera', 'height', 240)
main_img = resize_image(img, width, height)
main_img = cv2.cvtColor(main_img, cv2.COLOR_BGR2RGB)
height, width, bpc = main_img.shape
bpl = bpc * width
main_image = QImage(main_img.data, width, height, bpl,
QImage.Format_RGB888)
self.cameraWidget.set_image(main_image)
if self.serial_thread:
rpy = self.serial_thread.get_rpy()
self.rpyLabel.setText('RPY: ({}, {}, {})'.format(
rpy[0], rpy[1], rpy[2]))
def update_window_serial_connected(self):
self.actionSerialConnect.setEnabled(False)
self.actionSerialDisconnect.setEnabled(True)
self.macroStartButton.setEnabled(True)
self.macroStopButton.setEnabled(False)
# kinematic
self.kinematicOptionSend.setEnabled(True)
self.kinematicOptionLoadAndSend.setEnabled(True)
self.kinematicSendButton.setEnabled(True)
self.rpyLabel.setText('RPY: Connected')
self.statusBar.showMessage('Serial Connected')
def update_window_serial_disconnected(self):
self.actionSerialConnect.setEnabled(True)
self.actionSerialDisconnect.setEnabled(False)
self.macroStartButton.setEnabled(False)
self.macroStopButton.setEnabled(False)
# kinematic
self.kinematicOptionSend.setEnabled(False)
self.kinematicOptionLoadAndSend.setEnabled(False)
self.kinematicSendButton.setEnabled(False)
self.rpyLabel.setText('RPY: Disconnected')
self.statusBar.showMessage('Serial Disconnected')
def update_window_camera_connected(self):
self.actionCameraConnect.setEnabled(False)
self.actionCameraDisconnect.setEnabled(True)
self.statusBar.showMessage('Camera Connected')
def update_window_camera_disconnected(self):
self.actionCameraConnect.setEnabled(True)
self.actionCameraDisconnect.setEnabled(False)
self.statusBar.showMessage('Camera Disconnected')
def append_log(self, msg):
self.logPlainTextEdit.appendPlainText(msg)
def closeEvent(self, event):
self.camera_disconnect()
self.serial_disconnect()