dbgprint('received ' + cmd) words = cmd.split(':') dbgprint(words) # motor:fsx:0.5 if words[0] == 'motor': resp = cc.setMotorThrottle(words[1], float(words[2])) conn.sendall(bytes(resp + ' - ' + cmd, 'utf8')) elif words[0] == 'allmotors': resp = cc.setAllMotorThrottle(float(words[2])) # rhotheta:0.8:3.14214 elif words[0] == 'rhotheta': resp = cc.setSpeedRhoTheta(float(words[1]), float(words[2])) conn.sendall(bytes(resp + ' - ' + cmd, 'utf8')) elif words[0] == 'move': resp = cc.move(float(words[1])) conn.sendall(bytes(resp + ' - ' + cmd, 'utf8')) elif words[0] == 'turn': resp = cc.turn(float(words[1])) conn.sendall(bytes(resp + ' - ' + cmd, 'utf8')) elif words[0] == 'stop': resp = cc.stop() conn.sendall(bytes(resp + ' - ' + cmd, 'utf8')) else: conn.sendall(bytes('NO ACTION' + ' - ' + cmd, 'utf8')) except: print('closing socket') sock.close() time.sleep(5)
class MainWindow(QMainWindow, Ui_MainWindow): # define signals signal_gesture_read = pyqtSignal(name="signal_gesture_read") signal_prediction_show = pyqtSignal(name="signal_prediction_show") signal_command_send = pyqtSignal(str, name='signal_command_send') def __init__(self, parent=None): super(MainWindow, self).__init__(parent) self.setupUi(self) # CNN self.model = load_model() if torch.cuda.is_available(): self.gpu = True self.model.cuda() self.prediction_frequency = 10 # each 10 images arise a prediction self.prediction_count = 0 # camera self.camera = cv2.VideoCapture() self.CAM_NUM = 0 self.camera_x0 = 400 self.camera_y0 = 200 self.camera_height = 200 self.camera_width = 200 # car video stream self.stream = CarStream() # car controller self.controller = None self.host_detector = HostDetector() # control self.timer_camera = QtCore.QTimer() self.timer_stream = QtCore.QTimer() self.horizontalSlider.setValue(70) self.threshold = self.horizontalSlider.value() # SLOT self.slot_init() @log_decorator def slot_init(self): self.timer_camera.timeout.connect(self.camera_show) self.pushButton_open_cam.clicked.connect(self.camera_open) self.pushButton_close_cam.clicked.connect(self.camera_close) self.pushButton_left.clicked.connect(self.box_left) self.pushButton_right.clicked.connect(self.box_right) self.pushButton_down.clicked.connect(self.box_down) self.pushButton_up.clicked.connect(self.box_up) self.pushButton_connect_car.clicked.connect(self.car_control_open) self.pushButton_disconnect.clicked.connect(self.car_control_close) self.horizontalSlider.valueChanged.connect(self.threshold_update) self.signal_prediction_show.connect(self.prediction_show) self.signal_gesture_read.connect(self.gesture_show) self.signal_command_send.connect(self.command_send) @log_decorator def car_control_open(self, *args): ''' open car camera and connect the car controller. :return: ''' print(">>> loading.") self.host_detector.detect_hosts() self.stream.pull() # call pull to init car camera. # check ESP 8266 connection if self.controller is None: controller_ip = None print("Found hosts:", self.host_detector.hosts) for host in self.host_detector.hosts: if 'esp' in host or 'ESP' in host: controller_ip = self.host_detector.hosts[host] break # TODO: seems ESP8266 can't be detect # used static IP for debug. # IP is pre-found from router configuration. if controller_ip is None: controller_ip = '192.168.43.213' # debug end. if controller_ip is not None: self.controller = CarController(controller_ip) self.controller.status = 'online' # TODO: check connection # self.controller.test() else: print(">>> ERROR: can't find ESP8266. Current hosts: ", self.host_detector.hosts) msg = QtWidgets.QMessageBox.warning( self, u"Warning", u"请检测汽车芯片ESP8266是否正确连接路由", buttons=QtWidgets.QMessageBox.Ok, defaultButton=QtWidgets.QMessageBox.Ok) else: # had connected before. self.controller.status = 'online' # open car video stream frame = self.stream.read_buffer() if frame is not None: if not self.timer_stream.isActive(): self.timer_stream.start(30) self.timer_stream.timeout.connect(self.stream_show) @log_decorator def car_control_close(self, *args): ''' close car camera stream and the connection of car controller. :return: ''' # disconnect with stream self.timer_stream.timeout.disconnect(self.stream_show) self.label_car.setText("Not recieve car video stream yet.") self.controller.status = 'idle' # @log_decorator def stream_show(self): ''' show camera image, emit signal_gesture_read signal :return: ''' frame = self.stream.read_buffer() frame = cv2.resize(frame, (640, 480)) show = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) showImage = QtGui.QImage(show.data, show.shape[1], show.shape[0], QtGui.QImage.Format_RGB888) self.label_car.setPixmap(QtGui.QPixmap.fromImage(showImage)) @log_decorator def camera_open(self, *args): ''' open laptop camera :param args: :return: ''' if self.timer_camera.isActive() == False: flag = self.camera.open(self.CAM_NUM) if flag == False: msg = QtWidgets.QMessageBox.warning( self, u"Warning", u"请检测相机与电脑是否连接正确", buttons=QtWidgets.QMessageBox.Ok, defaultButton=QtWidgets.QMessageBox.Ok) else: if not self.timer_camera.isActive(): self.timer_camera.start(30) @log_decorator def camera_close(self, *args): ''' close laptop camera :param args: :return: ''' self.timer_camera.stop() self.camera.release() self.label_camera.clear() self.label_camera.setText("Not recieve car video stream yet.") def camera_show(self): ''' show camera image, emit signal_gesture_read signal :return: ''' flag, frame = self.camera.read() frame = cv2.flip(frame, 3) frame = cv2.resize(frame, (640, 480)) self.image = frame # notations threshold_str = "Current threshlold: %d %%" % self.threshold cv2.putText(frame, 'Driver Romm <( ^-^ )>', (FX, FY), FONT, SIZE, (0, 255, 0), 1, 1) cv2.putText(frame, threshold_str, (FX, FY + 2 * FH), FONT, SIZE, (0, 255, 0), 1, 1) cv2.rectangle(frame, (self.camera_x0, self.camera_y0), (self.camera_x0 + self.camera_width, self.camera_y0 + self.camera_height), (0, 255, 0), 1) show = cv2.cvtColor(self.image, cv2.COLOR_BGR2RGB) showImage = QtGui.QImage(show.data, show.shape[1], show.shape[0], QtGui.QImage.Format_RGB888) self.label_camera.setPixmap(QtGui.QPixmap.fromImage(showImage)) # call gesture filter self.signal_gesture_read.emit() # @log_decorator def gesture_show(self): ''' show gesture image after binary mask, emit signal_prediction_show signal :return: ''' roi = skinMask(self.image, self.camera_x0, self.camera_y0, self.camera_width, self.camera_height) showImage = QtGui.QImage(roi.data, roi.shape[1], roi.shape[0], QtGui.QImage.Format_Grayscale8) self.label_gesture.setPixmap(QtGui.QPixmap.fromImage(showImage)) # predict image if self.prediction_count == self.prediction_frequency: t = threading.Thread(target=models.predict_gesture, args=[self.model, roi], kwargs={"verbose": False}) t.start() self.prediction_count = 0 else: self.prediction_count += 1 self.signal_prediction_show.emit() # @log_decorator def prediction_show(self): ''' show prediction, and emit signal with prediction to controller :return: ''' # draw probability plot plot = models.update() cmd = None # write result if len(global_vars.jsonarray) > 0: prediction = max(global_vars.jsonarray, key=lambda x: global_vars.jsonarray[x]) if global_vars.jsonarray[prediction] * 100 > self.threshold: self.label_predict.setText(GESTURE_CLASSES[int(prediction)]) cmd = GESTURE_CLASSES[int(prediction)] else: self.label_predict.setText('nothing') showImage = QtGui.QImage(plot.data, plot.shape[1], plot.shape[0], QtGui.QImage.Format_RGB888) self.label_probability.setPixmap(QtGui.QPixmap.fromImage(showImage)) # emit sigal if cmd is not None and self.controller.status == 'online': self.signal_command_send.emit(cmd) @log_decorator def keyPressEvent(self, event): ''' rewrite keyboard event for car controller debug. :param event: :return: ''' if self.controller is not None and self.controller.status == 'online': if event.key() == Qt.Key_W: # send move forward signal self.signal_command_send.emit("go_ahead") elif event.key() == Qt.Key_A: # send turn left signal self.signal_command_send.emit("turn_left") elif event.key() == Qt.Key_D: # send turn right signal self.signal_command_send.emit("turn_right") elif event.key() == Qt.Key_S: # send back up signal: self.signal_command_send.emit("back_up") else: print("Not connect to ESP8266 yet.") if event.key() == Qt.Key_I: # try to find ESP 8266 's IP self.host_detector.detect_hosts() elif event.key() == Qt.Key_P: # print current detected IPs print(self.host_detector.hosts) # SLOT: update value def box_left(self): self.camera_x0 -= 5 def box_right(self): self.camera_x0 += 5 def box_up(self): self.camera_y0 -= 5 def box_down(self): self.camera_y0 += 5 def threshold_update(self): self.threshold = self.horizontalSlider.value() def command_send(self, prediction): self.controller.move(prediction)