예제 #1
0
class main(QtGui.QMainWindow):
    def __init__(self,parent=None):
        QtGui.QWidget.__init__(self,parent)
        self.file_dialog=QtGui.QFileDialog()
        self.ui=Ui_MainWindow()
        self.ui.setupUi(self)
        self.fn=None
        QtCore.QObject.connect(self.ui.action_Save,QtCore.SIGNAL("triggered()"),self.save_file)
        QtCore.QObject.connect(self.ui.actionNew,QtCore.SIGNAL("triggered()"),self.new_file)
        QtCore.QObject.connect(self.ui.actionOpen,QtCore.SIGNAL("triggered()"),self.open_file)
        QtCore.QObject.connect(self.ui.action_Saveas,QtCore.SIGNAL("triggered()"),self.saveas_file)
        QtCore.QObject.connect(self.ui.actionPrint,QtCore.SIGNAL("triggered()"),self.save_file)
        QtCore.QObject.connect(self.ui.actionPrint_Preview,QtCore.SIGNAL("triggered()"),self.save_file)
        QtCore.QObject.connect(self.ui.action_quit,QtCore.SIGNAL("triggered()"),self.quit)
    def save_file(self):
        if self.fn!=None:
            fileobj=open(self.fn,'w')
            fileobj.write(self.ui.textEdit.toPlainText())
            fileobj.close()
        else:
            filename=self.file_dialog.getSaveFileName(self,u"保存文件")
            try:
                fileobj=open(filename,'w')
                fileobj.write(self.ui.textEdit.toPlainText())
                self.fn=filename
                fileobj.close()
            except Exception ,e:
                pass
예제 #2
0
파일: gui.py 프로젝트: DrWrong/SnifferLite
class StartQT4(QtGui.QMainWindow):

    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.clidict = Param_To_Cli()
        self.ui.interface_choice.addItems(netifaces.interfaces())
        QtCore.QObject.connect(
            self.ui.start, QtCore.SIGNAL("clicked()"), self.start_sniffer)
#        QtCore.QObject.connect(
#            self.ui.interface_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_interface)
#
#        QtCore.QObject.connect(
#            self.ui.mode_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_mode)
#        QtCore.QObject.connect(
# self.ui.lineEdit, QtCore.SIGNAL("editingFinished()"), self.get_filter)

    def start_sniffer(self):
        self.clidict.interface = str(self.ui.interface_choice.currentText())
        self.clidict.mode = str(self.ui.mode_choice.currentText())
        self.clidict.filter = str(self.ui.lineEdit.text())
        log.debug(self.clidict.interface + '' +
                  self.clidict.mode + '' + self.clidict.filter)
        sniffer_prepare(self.clidict)
        self.sniff_queue = Queue()
        sniffer = SnifferProcess(
            self.sniff_queue, iface=self.clidict.interface)
        sniffer.start()
        while True:
            item = self.sniff_queue.get()

            self.ui.packet_list.addItem(item.summary())
            QtGui.QApplication.processEvents()
예제 #3
0
파일: gui.py 프로젝트: qristin/qface
class Gui(QtGui.QMainWindow):
    def __init__(self,capture, getFrame, parent = None):
        QtGui.QWidget.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.video = Video(capture, getFrame)
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.play)
        self._timer.start(27)
        self.update()

    def play(self):
        try:
            self.video.captureNextFrame()
            self.ui.videoFrame.setPixmap(self.video.convertFrame())
            self.ui.videoFrame.setScaledContents(True)
            gethistogram(self.ui.axes,tuple(self.video.histogram.values()),tuple(self.video.histogram.keys()))
            # l = [random.randint(0, 10) for i in range(4)]
            # self.ui.axes.plot(self.video.histogram.values(), 'r')
            self.ui.canvas.draw()

            if(len(self.video.labels)!=0):
                self.ui.label1.setText(self.video.name)
                # self.ui.label2.setText(self.video.labels[1])
                # self.ui.label3.setText(self.video.labels[2])
                # self.ui.label4.setText(self.video.labels[3])
                # self.ui.label5.setText(self.video.labels[4])
        except TypeError as e:
            print("No Frame", e)
예제 #4
0
class MyForm(QtGui.QMainWindow):
    def showMessage(self,string):
        self.ui.messageTextbox.setText(string)
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)     
        #here start
        downloader.fileLabel = self.ui.fileLabel
        downloader.nowSizeLabel = self.ui.nowSizeLabel
        downloader.totalSizeLabel = self.ui.totalSizeLabel
        downloader.messageTextbox = self.ui.messageTextbox
        #pass para
        self.connect(self.ui.aButton,QtCore.SIGNAL('clicked()'),self.aButton_clicked)
        self.connect(self.ui.bButton,QtCore.SIGNAL('clicked()'),self.bButton_clicked)
        self.connect(self.ui.aAfternoonButton,QtCore.SIGNAL('clicked()'),self.aAfternoonButton_clicked)
        self.connect(self.ui.bAfternoonButton,QtCore.SIGNAL('clicked()'),self.bAfternoonButton_clicked)
        self.connect(self.ui.downloadPercentButton,QtCore.SIGNAL('clicked()'),self.downloadPercentButton_clicked)
        self.connect(self.ui.nextButton,QtCore.SIGNAL('clicked()'),self.nextButton_clicked)
        #event
    def aButton_clicked(self):
        thread.start_new_thread(downloader.main,(1,True))
    def bButton_clicked(self):
        thread.start_new_thread(downloader.main,(0,True))
    def aAfternoonButton_clicked(self):
        thread.start_new_thread(downloader.main,(1,False))
    def bAfternoonButton_clicked(self):
        thread.start_new_thread(downloader.main,(0,False))
    def nextButton_clicked(self):
        downloader.goNext = True
    def downloadPercentButton_clicked(self):
        downloader.refreshPercent()
예제 #5
0
파일: main.py 프로젝트: lwwhsh/DataPort
class mainForm(QtGui.QMainWindow):
    def __init__(self, parent = None):
        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        self.pannel = []

        # 기본 패널

        self.pannel.append(tab_widget(addTab, self))
        self.func_insert_tab(u'새로운 탭')
        self.pannel.append(tab_widget(communicationPannel, self))
        self.func_insert_tab(u'DataPort')

        QtCore.QObject.connect(self.pannel[0].pannel.ui.clb, QtCore.SIGNAL("clicked()"), self.AddTab)

    def AddTab(self):
        current_index = len(self.ui.tabWidget)

        self.pannel.append(tab_widget(communicationPannel, self))
        self.func_insert_tab(u'DataPort')

  # 프로그램을 닫으려 할때
    def closeEvent(self, event):
        reply = QtGui.QMessageBox.warning(self,
            u'프로그램 종료', u"진행중인 모든 데이터 송·수신을 중지합니다.\n프로그램을 종료하시겠습니까?",
            QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
            QtGui.QMessageBox.No)
        if reply == QtGui.QMessageBox.Yes:
            event.accept()
        elif reply == QtGui.QMessageBox.No:
            event.ignore()

    def func_insert_tab(self, tab_name):
        """
        * 탭이 추가되면 항상 '새탭추가탭' 앞 쪽에 위치
        * 추가된 탭이 활성화
        """
        current_index = len(self.ui.tabWidget)
        self.ui.tabWidget.insertTab(current_index - 1, self.pannel[current_index], unicode(tab_name))
        self.ui.tabWidget.setCurrentIndex(current_index - 1)



    def slot_close_tab(self, tab_index):
        print tab_index

        if 1 == self.ui.tabWidget.count():
            # '새탭 추가탭'을 닫으려 할 경우
            return
        reply = QtGui.QMessageBox.warning(
            self,u'탭 닫기', u"저장하지 않은 작업 내용은 잃게됩니다.\n탭을 닫으시겠습니까?",
            QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
            QtGui.QMessageBox.No)

        if reply == QtGui.QMessageBox.Yes:
            self.pannel.pop(tab_index + 1)
            self.ui.tabWidget.removeTab(tab_index)
        elif reply == QtGui.QMessageBox.No: pass
예제 #6
0
파일: main.py 프로젝트: wavicles/PyPhysEd
class Gui(QtGui.QMainWindow):
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.video = Video(cv2.VideoCapture(0))
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.play)
        self._timer.start(27)
        self.update()
 
    def play(self):
        '''
        capture frame and display
        '''
        try:
            self.video.capture_next_frame()
            self.ui.videoFrame.setPixmap(
                self.video.convert_frame())
            self.ui.videoFrame.setScaledContents(True)
        except TypeError:
            print "No frame"


    def mousePressEvent(self, event):
        '''
        click mouse and put point on opencv window
        '''
        self.video.add_point(QtCore.QPoint(event.pos()), 
                            self.ui.videoFrame.geometry())
예제 #7
0
class MyForm(QtGui.QMainWindow):
    def __init__(self, logAnalyser, parent=None):
        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self, len(logAnalyser.files))
        self.logAnalyser = logAnalyser
        self.threadLogAnalyser =  ThreadedLogAnalyser(self, self.logAnalyser)
        self.connect(self.threadLogAnalyser, QtCore.SIGNAL("appendLogs"), self.ui.appendLogs)

    
        
    def main(self):
        self.threadLogAnalyser.start()
예제 #8
0
    def __init__(self):
        """
        Initializes the DecrypterWindow
        """
        super(DecrypterWindow, self).__init__()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        # connect event handlers
        self.ui.calibrateButton.clicked.connect(self.calibrateButtonHandler)
        self.ui.importButton.clicked.connect(self.importButtonHandler)
        self.ui.exportButton.clicked.connect(self.exportButtonHandler)
        for c in string.lowercase:
            self.ui.edit[c].textEdited.connect(self.editModifiedHandler)

        # disable unavailable functionality
        self.ui.importButton.setDisabled(True)
        self.ui.exportButton.setDisabled(True)
        for c in string.lowercase:
            self.ui.edit[c].setDisabled(True)

        self.decrypter = None

        self.show()

        self.setFixedSize(self.size())
예제 #9
0
 def __init__(self, settings, application=None):
     '''
     UI Object that draws the main window.
     
     Listens to the following settings:
     - delay: 
     '''
     QtGui.QMainWindow.__init__(self)
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)
     
     self._renderwidgets = [
                            RenderWidget(self),
                            RenderWidget(self),
                            RenderWidget(self),
                            RenderWidget(self),
                            ]
     
     self.update()
     self.application = application
     
     self.settings = settings
     self.settings.settingChanged.connect(self.settingChanged)
     self.ui.delay.setText(str(settings.getSetting("delay")))
     self._loadScreens(self.settings.getSetting("layouts")[self.settings.getSetting("selectedlayout")]["screen"])
     self._bindings = {}
     self._windowstate = None
     self._loadBindings(settings.getSetting('keybinding'))
예제 #10
0
 def __init__(self, pod):
     super().__init__()
     MainWindow.console = ConsoleWidget(
         namespace = {
             'pod': pod,
             'win': self,
         },
         text='You can use this window to enter Python commands.')
     MainWindow.console.setWindowTitle('Python interaction')
     self._ui = Ui_MainWindow()
     self._ui.setupUi(self)
     self._ui.startButton.clicked.connect(pod.begin)
     self._ui.submitButton.clicked.connect(self.submitCommand)
     pod.connected.connect(self.enable)
     self._ui.actionSettings.triggered.connect(self.networkDialog)
     self._ui.actionReconnect.triggered.connect(pod.begin)
     self._ui.actionConsoleOpen.triggered.connect(self.openConsole)
     pod.add_listener('*', self.appendNetworkLog)
     pod.add_listener('v', self.updateVelocityLCD)
     pod.add_listener('v', self._ui.velocityPlot.datum)
     pod.add_listener('h', self.updateHeightLCD)
     pod.add_listener('h', self._ui.heightPlot.datum)
     pod.add_listener('d', self.updateDistanceLCD)
     pod.add_listener('d', self._ui.distancePlot.datum)
     self.command.connect(pod)
     self.networkUpdate.connect(pod.try_connect)
     self.timer = QTimer()
     self.timer.timeout.connect(self._ui.velocityPlot.update)
     self.timer.timeout.connect(self._ui.heightPlot.update)
     self.timer.timeout.connect(self._ui.distancePlot.update)
     self.timer.start(_1s)
예제 #11
0
파일: gui.py 프로젝트: DrWrong/SnifferLite
 def __init__(self, parent=None):
     QtGui.QWidget.__init__(self, parent)
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)
     self.clidict = Param_To_Cli()
     self.ui.interface_choice.addItems(netifaces.interfaces())
     QtCore.QObject.connect(
         self.ui.start, QtCore.SIGNAL("clicked()"), self.start_sniffer)
    def __init__(self):
        super(Main, self).__init__()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        self.load_data()
        self.connect_signals()
        self.show()
예제 #13
0
	def __init__(self,parent=None):
		QtGui.QWidget.__init__(self,parent)
		self.ui = Ui_MainWindow()
		self.ui.setupUi(self)
		self.ui.start_button.clicked.connect(self.start_master)
		self.ui.connect_button.clicked.connect(self.start_client)
		self.ui.close_button.clicked.connect(self.close)
		self.CONNECTION_PORT = 1111
		self.update()
예제 #14
0
파일: gui.py 프로젝트: qristin/qface
 def __init__(self,capture, getFrame, parent = None):
     QtGui.QWidget.__init__(self,parent)
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)
     self.video = Video(capture, getFrame)
     self._timer = QtCore.QTimer(self)
     self._timer.timeout.connect(self.play)
     self._timer.start(27)
     self.update()
예제 #15
0
파일: gui.py 프로젝트: DrWrong/SnifferLite
class StartQT4(QtGui.QMainWindow):

    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.clidict = Param_To_Cli()
        self.ui.interface_choice.addItems(netifaces.interfaces())
        QtCore.QObject.connect(
            self.ui.start, QtCore.SIGNAL("clicked()"), self.start_sniffer)
        QtCore.QObject.connect(
            self.ui.action_stop, QtCore.SIGNAL("triggered()"), self.stop_sniffer)
        #self.ui.packet_list.setHorizontalHeaderLabels(['时间','源地址','目的地址','内容'])
#        QtCore.QObject.connect(
#            self.ui.interface_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_interface)
#
#        QtCore.QObject.connect(
#            self.ui.mode_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_mode)
#        QtCore.QObject.connect(
#            self.ui.lineEdit, QtCore.SIGNAL("editingFinished()"), self.get_filter)
        

    def start_sniffer(self):
        self.clidict.interface = str(self.ui.interface_choice.currentText())
        self.clidict.mode = str(self.ui.mode_choice.currentText())
        self.clidict.filter = str(self.ui.lineEdit.text())
        log.debug(self.clidict.interface + '' + self.clidict.mode + '' + self.clidict.filter)
        sniffer_prepare(self.clidict)
        self.sniff_queue = multiprocessing.JoinableQueue()
        self.sniffer = SnifferProcess(self.sniff_queue, iface=str(self.clidict.interface))
        self.sniffer.start()
        self.displaythreading = DisplayPacket(self.sniff_queue, self.ui.packet_list)
        self.displaythreading.start()
        self.ui.action_stop.setEnabled(True)

    def stop_sniffer(self):
        self.sniffer.terminate()

        self.sniff_queue.close()
        while True:
            if self.sniff_queue.empty():
                break
        self.displaythreading.terminate()
예제 #16
0
class Gui(QtGui.QMainWindow):
	def __init__(self,parent=None):
		QtGui.QWidget.__init__(self,parent)
		self.ui = Ui_MainWindow()
		self.ui.setupUi(self)
		self.ui.start_button.clicked.connect(self.start_master)
		self.ui.connect_button.clicked.connect(self.start_client)
		self.ui.close_button.clicked.connect(self.close)
		self.CONNECTION_PORT = 1111
		self.update()

	def start_master(self):
		hablar_master(self.CONNECTION_PORT, self.ui.label)
		
	def start_client(self):
		ip = self.ui.ip.getText()
		hablar_client(ip, self.CONNECTION_PORT, self.ui.label)
		
	def close(self):
		sys.exit()
예제 #17
0
 def __init__(self,parent=None):
     QtGui.QWidget.__init__(self,parent)
     self.file_dialog=QtGui.QFileDialog()
     self.ui=Ui_MainWindow()
     self.ui.setupUi(self)
     self.fn=None
     QtCore.QObject.connect(self.ui.action_Save,QtCore.SIGNAL("triggered()"),self.save_file)
     QtCore.QObject.connect(self.ui.actionNew,QtCore.SIGNAL("triggered()"),self.new_file)
     QtCore.QObject.connect(self.ui.actionOpen,QtCore.SIGNAL("triggered()"),self.open_file)
     QtCore.QObject.connect(self.ui.action_Saveas,QtCore.SIGNAL("triggered()"),self.saveas_file)
     QtCore.QObject.connect(self.ui.actionPrint,QtCore.SIGNAL("triggered()"),self.save_file)
     QtCore.QObject.connect(self.ui.actionPrint_Preview,QtCore.SIGNAL("triggered()"),self.save_file)
     QtCore.QObject.connect(self.ui.action_quit,QtCore.SIGNAL("triggered()"),self.quit)
예제 #18
0
파일: reader.py 프로젝트: karoon/freereader
class FreereaderUi(QtGui.QMainWindow):
    def __init__(self, parent=None):
        QtGui.QWidget.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        QtCore.QObject.connect(self.ui.readButton, QtCore.SIGNAL("clicked()"), self.readButton)
        QtCore.QMetaObject.connectSlotsByName(self)
    
    def readButton(self):
        self.notify('please wait')
        
        try:
            obj = feedparser.parse(str(self.ui.urlText.text()))
            self.notify('feed readed')
            if obj.entries != []:
                for entry in obj.entries:
                    self.ui.listWidget.addItem(entry.title)
        except:
            self.notify('error in parsing url')
    
    def notify(self, string = ''):
        self.ui.notification.setText(string)
예제 #19
0
class MainWindow(QMainWindow):
    '''First application window the user sees'''

    def __init__(self, pod):
        super().__init__()
        self._pod = pod
        self._ui = Ui_MainWindow()
        self._ui.setupUi(self)
        self._ui.actionNetwork.triggered.connect(self.networkDialog)

    def networkDialog(self):
        '''Open a window for changing network settings'''
        la = self._pod.get_local_addr()
        ra = self._pod.get_remote_addr()
        info = NetworkInfo(local=la, remote=ra)
        dialog = NetworkDialog(info)
        if dialog.exec_() == QDialog.Accepted:
            la, old = info.local, la
            if la != old:
                self._pod.set_local_addr(info.local)
            ra, old = info.remote, ra
            if ra != old:
                self._pod.set_remote_addr(info.remote)
예제 #20
0
파일: main.py 프로젝트: lwwhsh/DataPort
    def __init__(self, parent = None):
        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        self.pannel = []

        # 기본 패널

        self.pannel.append(tab_widget(addTab, self))
        self.func_insert_tab(u'새로운 탭')
        self.pannel.append(tab_widget(communicationPannel, self))
        self.func_insert_tab(u'DataPort')

        QtCore.QObject.connect(self.pannel[0].pannel.ui.clb, QtCore.SIGNAL("clicked()"), self.AddTab)
예제 #21
0
 def __init__(self, parent=None):
     QtGui.QWidget.__init__(self, parent)
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)     
     #here start
     downloader.fileLabel = self.ui.fileLabel
     downloader.nowSizeLabel = self.ui.nowSizeLabel
     downloader.totalSizeLabel = self.ui.totalSizeLabel
     downloader.messageTextbox = self.ui.messageTextbox
     #pass para
     self.connect(self.ui.aButton,QtCore.SIGNAL('clicked()'),self.aButton_clicked)
     self.connect(self.ui.bButton,QtCore.SIGNAL('clicked()'),self.bButton_clicked)
     self.connect(self.ui.aAfternoonButton,QtCore.SIGNAL('clicked()'),self.aAfternoonButton_clicked)
     self.connect(self.ui.bAfternoonButton,QtCore.SIGNAL('clicked()'),self.bAfternoonButton_clicked)
     self.connect(self.ui.downloadPercentButton,QtCore.SIGNAL('clicked()'),self.downloadPercentButton_clicked)
     self.connect(self.ui.nextButton,QtCore.SIGNAL('clicked()'),self.nextButton_clicked)
예제 #22
0
파일: m2todo.py 프로젝트: toyg/m2todo
    def __init__(self, **args):
        super(QApplication, self).__init__([])

        self.setApplicationName(NAME)
        self.setApplicationVersion(VERSION)
        self.setOrganizationName(COMPANY)
        self.setOrganizationDomain(DOMAIN)

        self.settings = QSettings()

        self._mw = QMainWindow()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self._mw)

        self.ui.actionOpen_MM.triggered.connect(self.openMM)
        self.ui.actionOpen_M2T.triggered.connect(self.openM2T)
        self.ui.actionMerge.triggered.connect(self.merge)
        self.ui.actionSave.triggered.connect(self.save)
        self.ui.actionSave_As.triggered.connect(self.saveAs)
        self.ui.actionAbout_M2Todo.triggered.connect(self.about)
        self.ui.actionHelp.triggered.connect(self.help)
        self.ui.actionFreeMind.triggered.connect(self.launchFreemind)

        self.ui.treeWidget.itemChanged.connect(self.itemStriker)

        self._openPath = None
        self._savePath = None

        self.ui.actionMerge.setDisabled(True)

        self._mw.show()

        lastFile = str(self.settings.value("session/lastOpenFile", None).toString())
        if lastFile:
            fType = self.settings.value("session/lastOpenFileType")
            self._openPath = {"type": fType, "path": lastFile}
            if fType == "m2t":
                self._parseM2T(lastFile)
            elif fType == "mm":
                self._parseMM(lastFile)
            self._mw.statusBar().showMessage(lastFile)
            self._mw.setWindowTitle(basename(str(lastFile)) + " - M2Todo")
            self.ui.actionMerge.setDisabled(False)
        # not debug!! here because it's always the last thing to do
        # even if/when we have a "last open file" option
        self.ui.actionSave.setDisabled(True)
예제 #23
0
 def __init__(self):
     super(MainWindow, self).__init__()
     
     # UI Setup
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)
     # Set the default icons for UI actions
     self.ui.actionNew.setIcon(QtGui.QIcon.fromTheme("document-new"))
     self.ui.actionOpen.setIcon(QtGui.QIcon.fromTheme("document-open"))
     self.ui.actionSave.setIcon(QtGui.QIcon.fromTheme("document-save"))
     self.ui.actionSave_As.setIcon(QtGui.QIcon.fromTheme("document-save-as"))
     self.ui.actionPrint.setIcon(QtGui.QIcon.fromTheme("document-print"))
     self.ui.actionQuit.setIcon(QtGui.QIcon.fromTheme("system-log-out"))
     self.ui.actionUndo.setIcon(QtGui.QIcon.fromTheme("edit-undo"))
     self.ui.actionRedo.setIcon(QtGui.QIcon.fromTheme("edit-redo"))
     self.ui.actionCut.setIcon(QtGui.QIcon.fromTheme("edit-cut"))
     self.ui.actionCopy.setIcon(QtGui.QIcon.fromTheme("edit-copy"))
     self.ui.actionPaste.setIcon(QtGui.QIcon.fromTheme("edit-paste"))
     self.ui.actionDelete.setIcon(QtGui.QIcon.fromTheme("edit-delete"))
     self.ui.actionPreferences.setIcon(QtGui.QIcon.fromTheme("preferences"))
     self.ui.actionAbout.setIcon(QtGui.QIcon.fromTheme("help-browser"))
     self.ui.actionRun.setIcon(QtGui.QIcon.fromTheme("go-next"))
     # UI ToolBox
     self.buttonGroup = QtGui.QButtonGroup()
     self.buttonGroup.setExclusive(False)
     self.buttonGroup.buttonClicked[int].connect(self.toolBoxButtonClicked)
     self.toolBox = self.ui.componentBrowser
     # Populate the toolbox
     self.createToolBox()  
     # Create a scene for the GraphicsView
     self.scene=DiagramScene()
     self.ui.graphicsView.setScene(self.scene)
     self.scene.setSceneRect(0,0,600,400)
     # Make it bigger
     self.setWindowState(QtCore.Qt.WindowMaximized)
     # Create an UNDO stack and view
     self.undoStack = QtGui.QUndoStack(self)
     self.undoView = QtGui.QUndoView(self.undoStack)
     self.undoView.setWindowTitle("Undo View")
     self.undoView.show()
     self.undoView.setAttribute(QtCore.Qt.WA_QuitOnClose, False)
     self.createActions()
     self.createMenus()
     # Set the window title      
     self.setWindowTitle("Schematix")
예제 #24
0
    def __init__(self, N):
        super(MainWindow, self).__init__()

        # This is always the same
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        # no resizing
        self.setFixedSize(self.width(), self.height())

        
        from sph_demo import SPHDemo
        enable_advanced_rendering = not '--disable-advanced-rendering' in sys.argv
        self.sph_demo = SPHDemo(N, size=(self.ui.fluid.width(), self.ui.fluid.height()), enable_advanced_rendering=enable_advanced_rendering)

        # ui.fluid is the FluidWidget QGLWidget-widget.
        self.ui.fluid.init(self.sph_demo)
        self.ui.fluid.gl_initialized.connect(self.update_gui_from_params)

        self.setup_signals()
        statusBarInfo = QtGui.QLabel()
        statusBarInfo.setText("%s particles, initial density: %s, mass: %.02f, gas constant k: %s, timestep: %.05f" % (
                self.sph_demo.fluid_simulator.N,
                self.sph_demo.fluid_simulator.density0, 
                self.sph_demo.fluid_simulator.mass,
                self.sph_demo.fluid_simulator.k,
                self.sph_demo.fluid_simulator.dt
                ))

        self.statusBarFps = QtGui.QLabel()
        self.statusBar().addPermanentWidget(self.statusBarFps)
        self.statusBar().addWidget(statusBarInfo)

        if not self.sph_demo.enable_advanced_rendering:
            # disable ui elements dealing with advanced rendering
            self.ui.rm_points.setChecked(True)
            for el in (self.ui.rm_balls, 
                       self.ui.rm_advanced, 
                       self.ui.advanced,):
                el.setEnabled(False)
예제 #25
0
class Gui(QMainWindow):
    """!
    Main GUI Class

    Contains the main function and interfaces between the GUI and functions.
    """
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Groups of ui commonents """
        self.error_lcds = [
            self.ui.rdoutBaseErrors, self.ui.rdoutShoulderErrors,
            self.ui.rdoutElbowErrors, self.ui.rdoutWristErrors,
            self.ui.rdoutWrist2Errors, self.ui.rdoutWrist3Errors
        ]
        self.joint_readouts = [
            self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC,
            self.ui.rdoutWristJC, self.ui.rdoutWrist2JC, self.ui.rdoutWrist3JC
        ]
        self.joint_slider_rdouts = [
            self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow,
            self.ui.rdoutWrist, self.ui.rdoutWrist2, self.ui.rdoutWrist3
        ]
        self.joint_sliders = [
            self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow,
            self.ui.sldrWrist, self.ui.sldrWrist2, self.ui.sldrWrist3
        ]
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm()
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        # Video
        self.ui.videoDisplay.setMouseTracking(True)
        self.ui.videoDisplay.mouseMoveEvent = self.trackMouse
        self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress
        # Buttons
        # Handy lambda function that can be used with Partial to only set the new state if the rexarm is initialized
        nxt_if_arm_init = lambda next_state: self.sm.set_next_state(
            next_state if self.rexarm.initialized else None)
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_init_arm.clicked.connect(self.initRexarm)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate'))
        self.ui.btn_exec.clicked.connect(self.execute)
        self.ui.btnUser2.setText("Reset Waypoints")
        self.ui.btnUser2.clicked.connect(self.reset)
        self.ui.btnUser3.setText("Add Waypoint")
        self.ui.btnUser3.clicked.connect(self.teach)
        self.ui.btnUser4.setText("Gripper Toggle")
        self.ui.btnUser4.clicked.connect(self.toggle_gripper)
        self.ui.btnUser5.setText("Add Color Points")
        self.ui.btnUser5.clicked.connect(self.colPoints)
        self.ui.btnUser6.setText("Click and Grab")
        self.ui.btnUser6.clicked.connect(self.click_grab)
        # Sliders
        for sldr in self.joint_sliders:
            sldr.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        # Direct Control
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        # Status
        self.ui.rdoutStatus.setText("Waiting for input")
        # Auto exposure
        self.ui.chkAutoExposure.stateChanged.connect(self.autoExposureChk)
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """Setup Threads"""
        # Rexarm runs its own thread

        # Video
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        # State machine
        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        # Display
        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.updateJointErrors.connect(self.updateJointErrors)
        self.displayThread.start()

    """ Slots attach callback functions to signals emitted from threads"""

    @pyqtSlot(QImage, QImage, QImage)
    def setImage(self, rgb_image, depth_image, depth_filter_image):
        """!
        @brief      Display the images from the kinect.

        @param      rgb_image    The rgb image
        @param      depth_image  The depth image
        """
        if (self.ui.radioVideo.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
        if (self.ui.radioDepth.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))
        if (self.ui.radioUsr1.isChecked()):
            self.ui.videoDisplay.setPixmap(
                QPixmap.fromImage(depth_filter_image))

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        for rdout, joint in zip(self.joint_readouts, joints):
            rdout.setText(str('%+.2f' % (joint * R2D)))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
        # self.ui.rdoutG.setText(str("%+.2f" % (pos[4])))
        # self.ui.rdoutP.setText(str("%+.2f" % (pos[5])))

    @pyqtSlot(list)
    def updateJointErrors(self, errors):
        for lcd, error in zip(self.error_lcds, errors):
            lcd.display(error)

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)

    """ Other callback functions attached to GUI elements"""

    def estop(self):
        self.sm.set_next_state("estop")

    def execute(self):
        # self.sm.set_next_state("execute")
        self.sm.set_next_state("execute_tp")

    def reset(self):
        self.sm.set_next_state("reset")

    def teach(self):
        self.sm.set_next_state("teach")

    def colPoints(self):
        if self.sm.current_state == "color":  #if we're already doing colors, now save it
            self.sm.set_next_state("save_color")
        else:
            self.sm.set_next_state("color")

    def toggle_gripper(self):
        self.rexarm.toggle_gripper()

    def click_grab(self):
        # TODO: WRITE FUNCTION HERE!!!
        pass

    def sliderChange(self):
        """!
        @brief Slider changed

        Function to change the slider labels when sliders are moved and to command the arm to the given position
        """
        for rdout, sldr in zip(self.joint_slider_rdouts, self.joint_sliders):
            rdout.setText(str(sldr.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")

        # Do nothing if the rexarm is not initialized
        if self.rexarm.initialized:
            self.rexarm.set_torque_limits(
                [self.ui.sldrMaxTorque.value() / 100.0] *
                self.rexarm.num_joints)
            self.rexarm.set_speeds_normalized_all(self.ui.sldrSpeed.value() /
                                                  100.0)
            joint_positions = np.array(
                [sldr.value() * D2R for sldr in self.joint_sliders])
            # Only send the joints that the rexarm has
            self.rexarm.set_positions(
                joint_positions[0:self.rexarm.num_joints])

    def directControlChk(self, state):
        """!
        @brief      Changes to direct control mode

                    Will only work if the rexarm is initialized.

        @param      state  State of the checkbox
        """
        if state == Qt.Checked and self.rexarm.initialized:
            # Go to manual and enable sliders
            self.sm.set_next_state("manual")
            self.ui.SliderFrame.setEnabled(True)
        else:
            # Lock sliders and go to idle
            self.sm.set_next_state("idle")
            self.ui.SliderFrame.setEnabled(False)
            self.ui.chk_directcontrol.setChecked(False)

    def autoExposureChk(self, state):
        """!
        @brief      Sets the Kinect auto exposer

        @param      state  State of the checkbox
        """
        if state == Qt.Checked and self.kinect.kinectConnected == True:
            self.kinect.toggleExposure(True)
        else:
            self.kinect.toggleExposure(False)

    def trackMouse(self, mouse_event):
        """!
        @brief      Show the mouse position in GUI

                    TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB
                    video image.

        @param      mouse_event  QtMouseEvent containing the pose of the mouse at the time of the event not current time
        """
        if self.kinect.DepthFrameRaw.any() != 0:
            u = mouse_event.pos().x()
            v = mouse_event.pos().y()
            d = self.kinect.DepthFrameRaw[v, u]
            self.ui.rdoutMousePixels.setText("(" + str(u) + "," + str(v) +
                                             "," + str(d) + ")")
            worldCoords = self.kinect.pix2Glob(np.array(
                [u, v, 1])) * 1000  #1000 for mm
            self.ui.rdoutMouseWorld.setText(
                f"({np.round(worldCoords[0])},{np.round(worldCoords[1])},{np.round(worldCoords[2])})"
            )

    def calibrateMousePress(self, mouse_event):
        """!
        @brief Record mouse click positions for calibration

        @param      mouse_event  QtMouseEvent containing the pose of the mouse at the time of the event not current time
        """
        """ Get mouse posiiton """
        pt = mouse_event.pos()

        if mouse_event.button() == Qt.LeftButton:
            self.kinect.last_click[0] = pt.x()
            self.kinect.last_click[1] = pt.y()
            self.kinect.new_click = True
        elif mouse_event.button() == Qt.RightButton:
            self.kinect.last_rclick[0] = pt.x()
            self.kinect.last_rclick[1] = pt.y()
            self.kinect.new_rclick = True

    def initRexarm(self):
        """!
        @brief      Initializes the rexarm.
        """
        self.sm.set_next_state('initialize_rexarm')
        self.ui.SliderFrame.setEnabled(False)
        self.ui.chk_directcontrol.setChecked(False)
예제 #26
0
파일: showui.py 프로젝트: Winawer/jaabagui
class jaabaGUI(QMainWindow):
    """ controller for the blob labeling GUI"""
    def __init__(self,parent=None):
        QMainWindow.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        #add new slider
        # self.positionSlider=QSlider(Qt.Horizontal)
        # self.positionSlider.setGeometry (800,800,100,30)
        # self.positionSlider.setRange(0, 0)
        # self.positionSlider.sliderMoved.connect(self.setPosition)

        #setup Video
        #video player
        self.mediaPlayer1 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
        self.mediaPlayer2 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
        #self.mediaPlayer.metaDataChanged.connect(self.metaDataChanged)
        self.mediaPlayer1.durationChanged.connect(self.durationChanged)
        self.mediaPlayer1.positionChanged.connect(self.positionChanged)
        self.mediaPlayer2.positionChanged.connect(self.positionChanged)
        

        #visualizetion
        self.scene = QGraphicsScene()
        self.ui.graphicsView.setScene(self.scene)
        #self.scene.setBackgroundBrush(Qt.black)
        self.videoItem1 = QGraphicsVideoItem()
        self.videoItem2 = QGraphicsVideoItem()
        self.scene.addItem(self.videoItem1)
        self.scene.addItem(self.videoItem2)
        self.mediaPlayer1.setVideoOutput(self.videoItem1)
        self.mediaPlayer2.setVideoOutput(self.videoItem2)

        #slide bar
        print self.ui.horizontalSlider
        self.ui.horizontalSlider.setRange(0, 0)
        self.ui.horizontalSlider.sliderMoved.connect(self.setPosition)
        # self.ui.horizontalSlider.sliderPressed.connect(self.sliderPressed)



        #print self.ui.graphicsView.width()/2,self.ui.graphicsView.height()
        #self.videoItem1.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
        #self.videoItem2.setSize(QSizeF(self.ui.graphicsView.width()*10,self.ui.graphicsView.height()*10))
       # self.videoItem2.setSize(graphicsView.size())
        #self.videoItem2.setOffset(QPointF(500,500))
        #self.videoItem2.setOffset(QPointF(self.ui.graphicsView.width()/2,0))   
        #self.videoItem2.setPos(QPointF(0,0))
        # print self.ui.graphicsView.width(), self.ui.graphicsView.height()
        # print self.ui.graphicsView.size()
        # print self.videoItem2.boundingRect().width(), self.videoItem2.boundingRect().height()
        # print self.ui.graphicsView.sceneRect()
        #self.mediaPlayer.stateChanged.connect(self.mediaStateChanged)

        #callbacks
        self.ui.actionQuit.triggered.connect(self.quit)
        self.ui.actionLoad_Project.triggered.connect(self.loadVideo)
        #self.ui.buttonPlay.clicked[bool].connect(self.setToggleText)
        self.ui.buttonPlay.clicked.connect(self.play)
        #print self.ui.graphicsView.sizeHint()


        #initialization
        self.loaded = False
        self.videoFilename = None
        self.frame_count=None
        self.width=None
        self.height=None
        self.frame_trans=None



        
    # ###actions starts from here###
    def quit(self):
        QApplication.quit()

    def loadVideo(self):
        self.writeLog("Loading video...")

        self.videoFilename = QFileDialog.getOpenFileName(self, 'Open File', '.')[0]
        if not self.videoFilename:
            self.writeLog("User cancelled - no video loaded")
            return
        else:
       		cap=cv2.VideoCapture(self.videoFilename)
	    	self.frame_count=cap.get(cv2.CAP_PROP_FRAME_COUNT)
	    	self.width=cap.get(3)
	    	self.height=cap.get(4)
	        self.mediaPlayer2.setMedia(QMediaContent(QUrl.fromLocalFile(self.videoFilename )))
	        self.mediaPlayer1.setMedia(QMediaContent(QUrl.fromLocalFile(self.videoFilename )))
	        self.ui.buttonPlay.setEnabled(True)
            # self.mediaPlayer2.setVideoOutput(self.videoItem2)
            # self.mediaPlayer1.setVideoOutput(self.videoItem1)
            # size= self.videoItem2.nativeSize()
            # print size
            #print self.mediaPlayer.duration()
          
            #print self.mediaPlayer.metaData()
        self.writeLog("Video loaded!")

    def play(self):
    	
        self.videoItem1.setAspectRatioMode(0)
        self.videoItem2.setAspectRatioMode(0)
        self.scene.setSceneRect(0,0,self.ui.graphicsView.width(),self.ui.graphicsView.height())
        self.videoItem1.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
        self.videoItem2.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
        self.videoItem1.setPos(QPointF(0,0))
        self.videoItem2.setPos(QPointF(self.ui.graphicsView.width()/2,0))
        #self.ui.graphicsView.setGeometry(0,0, 600,800)
        #print 'graphicsView size', self.ui.graphicsView.size()
        #print 'graphicsScene size', self.scene.sceneRect()
        #self.videoItem2.setSize(QSizeF(1000,300))
        #print 'graphicsVideoItem size',self.videoItem2.size()
        # print 'item x',self.videoItem2.scenePos().x()
        # print 'item y', self.videoItem2.scenePos().y()
        # print 'item x',self.videoItem1.scenePos().x()
        # print 'item y', self.videoItem1.scenePos().y()

        if self.mediaPlayer1.state() == QMediaPlayer.PlayingState:
        	self.ui.buttonPlay.setIcon(self.ui.style().standardIcon(PyQt5.QtWidgets.QStyle.SP_MediaPlay))
        	self.ui.buttonPlay.setText("Play")
        	self.mediaPlayer1.pause()
        	self.writeLog("Video paused")
        else: 
        	self.ui.buttonPlay.setIcon(self.ui.style().standardIcon(PyQt5.QtWidgets.QStyle.SP_MediaPause))
	        self.ui.buttonPlay.setText("Stop")
	        self.mediaPlayer1.play()
	        self.writeLog("Playing video")

        if self.mediaPlayer2.state() == QMediaPlayer.PlayingState:
            self.mediaPlayer2.pause()
        else: 
            self.mediaPlayer2.play()


        
        #size= self.videoItem2.nativeSize()
        # print self.mediaPlayer.duration()
      
        #print self.mediaPlayer.metaData()
      

        # print self.ui.graphicsView.width(), self.ui.graphicsView.height()
        # print self.ui.graphicsView.size()
        # print self.videoItem2.boundingRect().width(), self.videoItem2.boundingRect().height()
        # print self.ui.graphicsView.sceneRect()
        # print self.scene.sceneRect()
        # print self.ui.graphicsView.sizeHint()

    

    def setPosition(self, position):
    	self.mediaPlayer1.setPosition(position) 
    	self.mediaPlayer2.setPosition(position)  

    # when position of media changed, set slider and text box accordingly.
    def positionChanged(self, position):
        self.ui.horizontalSlider.setValue(position)
        if isinstance(self.frame_trans,float):
	        # print type(position),position
	        # print type(self.frame_trans),self.frame_trans 
	        # print position/self.frame_trans
	     	self.ui.lineEdit.setText(str(int(round(position/self.frame_trans,0))))
	       
        self.writeLog(str(position))    
    
    def durationChanged(self, duration):
	    self.ui.horizontalSlider.setRange(0, duration) 
	    self.frame_trans=self.mediaPlayer1.duration()/self.frame_count
	    print self.frame_trans

    

    def writeLog(self,text):
        self.ui.log.setText(text)
예제 #27
0
class MainWindow(QtGui.QMainWindow):
    def __init__(self, N):
        super(MainWindow, self).__init__()

        # This is always the same
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        # no resizing
        self.setFixedSize(self.width(), self.height())

        
        from sph_demo import SPHDemo
        enable_advanced_rendering = not '--disable-advanced-rendering' in sys.argv
        self.sph_demo = SPHDemo(N, size=(self.ui.fluid.width(), self.ui.fluid.height()), enable_advanced_rendering=enable_advanced_rendering)

        # ui.fluid is the FluidWidget QGLWidget-widget.
        self.ui.fluid.init(self.sph_demo)
        self.ui.fluid.gl_initialized.connect(self.update_gui_from_params)

        self.setup_signals()
        statusBarInfo = QtGui.QLabel()
        statusBarInfo.setText("%s particles, initial density: %s, mass: %.02f, gas constant k: %s, timestep: %.05f" % (
                self.sph_demo.fluid_simulator.N,
                self.sph_demo.fluid_simulator.density0, 
                self.sph_demo.fluid_simulator.mass,
                self.sph_demo.fluid_simulator.k,
                self.sph_demo.fluid_simulator.dt
                ))

        self.statusBarFps = QtGui.QLabel()
        self.statusBar().addPermanentWidget(self.statusBarFps)
        self.statusBar().addWidget(statusBarInfo)

        if not self.sph_demo.enable_advanced_rendering:
            # disable ui elements dealing with advanced rendering
            self.ui.rm_points.setChecked(True)
            for el in (self.ui.rm_balls, 
                       self.ui.rm_advanced, 
                       self.ui.advanced,):
                el.setEnabled(False)

    def setup_signals(self):
        """
        Set up callbacks for the widgets.
        """

        ui = self.ui

        self.fps_timer = QtCore.QTimer()
        def callback():
            self.statusBarFps.setText("%.2f fps" % self.sph_demo.fps)
        self.fps_timer.timeout.connect(callback)
        self.fps_timer.start(1000)

        def callback(p):
            self.sph_demo.params.paused = p
        ui.paused.toggled.connect(callback)

        if self.sph_demo.enable_advanced_rendering:
            #from fluid_rendering.fluid_renderer import FluidRenderer

            def callback(p):
                self.sph_demo.params.blur_thickness_map = p
            ui.blur_thickness_map.toggled.connect(callback)
            def callback(p):
                self.sph_demo.params.render_mean_curvature = p
            ui.render_mean_curvature.toggled.connect(callback)        

            def callback():
                self.sph_demo.params.render_mode = 0
            ui.rm_points.pressed.connect(callback)
            def callback():
                self.sph_demo.params.render_mode = 1
            ui.rm_balls.pressed.connect(callback)
            def callback():
                self.sph_demo.params.render_mode = 2
            ui.rm_advanced.pressed.connect(callback)
            ui.rm_advanced.toggled.connect(ui.advanced.setEnabled)

            def callback(n):
                self.sph_demo.params.smoothing_iterations = n
            ui.smoothing_iterations.valueChanged.connect(callback)

            def callback(n):
                self.sph_demo.params.smoothing_z_contrib = n
            ui.smoothing_z_contrib.valueChanged.connect(callback)

    def update_gui_from_params(self):
        params = self.sph_demo.params
        if not params.dirty:
            return
        
        ui = self.ui
        ui.paused.setOn(params.paused)
        
        if self.sph_demo.enable_advanced_rendering:
            from fluid_rendering.fluid_renderer import FluidRenderer

            ui.blur_thickness_map.setOn(params.blur_thickness_map)
            ui.render_mean_curvature.setOn(params.render_mean_curvature)

            { FluidRenderer.RENDERMODE_POINTS: ui.rm_points,
              FluidRenderer.RENDERMODE_BALLS: ui.rm_balls,
              FluidRenderer.RENDERMODE_ADVANCED: ui.rm_advanced }[params.render_mode].setChecked(True)

            ui.smoothing_iterations.setValue(params.smoothing_iterations)
            ui.smoothing_z_contrib.setValue(params.smoothing_z_contrib)

            ui.advanced.setEnabled(ui.rm_advanced.isChecked())

        params.dirty = False
           
    def keyPressEvent(self, event):
        """
        Handle keyboard shortcuts
        """
        key = event.key()
        params = self.sph_demo.params
        if key == QtCore.Qt.Key_R:
            self.sph_demo.fluid_simulator.set_positions()
        elif key == QtCore.Qt.Key_P:
            params.paused = not params.paused
        elif key == QtCore.Qt.Key_B:
            params.blur_thickness_map = not params.blur_thickness_map
        elif key == QtCore.Qt.Key_N:
            params.render_mode = (params.render_mode+1) % self.sph_demo.fluid_renderer.number_of_render_modes
        elif key == QtCore.Qt.Key_C:
            params.render_mean_curvature = not params.render_mean_curvature
        else:
            super(MainWindow, self).keyPressEvent(event)

        self.update_gui_from_params()
예제 #28
0
class Gui(QtGui.QMainWindow):
    """ 
    Main GUI Class
    It contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self,parent=None):
        QtGui.QWidget.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        """ Main Variables Using Other Classes"""
        self.rex = Rexarm()
        self.video = Video(cv2.VideoCapture(0))
	self.world_coord = np.float32()

	""" Play and Repeat Variable """
	self.wayPoints = []
        self.wayPointsPos = []
	self.wayPointsSpeed = []
	self.wayPointsTime = []

        """ Other Variables """
        self.last_click = np.float32([-1,-1])
	self.define_template_flag = -1
        self.click_point1 = np.float32([-1,-1])
        self.click_point2 = np.float32([-1,-1])
	self.template = None
	self.targets = []
	self.waypointsfp = csv.writer(open("waypoint.csv","wb"))
        self.currtime = 0
        """ Set GUI to track mouse """
        QtGui.QWidget.setMouseTracking(self,True)

        """ 
        Video Function 
        Creates a timer and calls play() function 
        according to the given time delay (27mm) 
        """
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.play)
        self._timer.start(27)
       
        """ 
        LCM Arm Feedback
        Creates a timer to call LCM handler continuously
        No delay implemented. Reads all time 
        """  
        self._timer2 = QtCore.QTimer(self)
        self._timer2.timeout.connect(self.rex.get_feedback)
        self._timer2.start()

	"""
	ARM Plan and Command Thread
	Creates a timer to call REXARM.plan_command function continuously
	"""
	self._timer3 = QtCore.QTimer(self)
	self._timer3.timeout.connect(self.rex.plan_command)
	self._timer3.start()
        
	"""
        Connect Sliders to Function
        TO DO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI 
        """ 
        self.ui.sldrBase.valueChanged.connect(self.slider_change)
        self.ui.sldrShoulder.valueChanged.connect(self.slider_change)
        self.ui.sldrElbow.valueChanged.connect(self.slider_change)
        self.ui.sldrWrist.valueChanged.connect(self.slider_change)
        self.ui.sldrMaxTorque.valueChanged.connect(self.slider_change)
	self.ui.sldrSpeed.valueChanged.connect(self.slider_change)

        """ Commands the arm as the arm initialize to 0,0,0,0 angles """
        self.slider_change() 
        
        """ Connect Buttons to Functions """
        self.ui.btnLoadCameraCal.clicked.connect(self.load_camera_cal)
        self.ui.btnPerfAffineCal.clicked.connect(self.affine_cal)
        self.ui.btnTeachRepeat.clicked.connect(self.tr_initialize)
        self.ui.btnAddWaypoint.clicked.connect(self.tr_add_waypoint)
        self.ui.btnSmoothPath.clicked.connect(self.tr_smooth_path)
        self.ui.btnPlayback.clicked.connect(self.tr_playback)
	self.ui.btnLoadPlan.clicked.connect(self.tr_load)
        self.ui.btnDefineTemplate.clicked.connect(self.def_template)
        self.ui.btnLocateTargets.clicked.connect(self.template_match)
        self.ui.btnExecutePath.clicked.connect(self.exec_path)


    def play(self):
        """ 
        Play Funtion
        Continuously called by GUI 
        """

        """ Renders the Video Frame """
        try:
            self.video.captureNextFrame()
	    for t in self.targets:
	    	self.video.addTarget(t)
            self.ui.videoFrame.setPixmap(self.video.convertFrame())
            self.ui.videoFrame.setScaledContents(True)
	    cv2.imwrite("curretFrame.png", self.video.currentFrame)
        except TypeError:
            print "No frame"
        
        """ 
        Update GUI Joint Coordinates Labels
        TO DO: include the other slider labels 
        """
        self.ui.rdoutBaseJC.setText(str(self.rex.joint_angles_fb[0]*R2D))
        self.ui.rdoutShoulderJC.setText(str(self.rex.joint_angles_fb[1]*R2D))
        self.ui.rdoutElbowJC.setText(str(self.rex.joint_angles_fb[2]*R2D))
        self.ui.rdoutWristJC.setText(str(self.rex.joint_angles_fb[3]*R2D))

	fk_result = self.rex.rexarm_FK(3)
	#print fk_result
        self.ui.rdoutX.setText(repr(fk_result[0]))
        self.ui.rdoutY.setText(repr(fk_result[1]))
        self.ui.rdoutZ.setText(repr(fk_result[2]))
	self.ui.rdoutT.setText(repr(fk_result[3]))
        """ 
        Mouse position presentation in GUI
        TO DO: after getting affine calibration make the apprriate label
        to present the value of mouse position in world coordinates 
        """    
        x = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).x()
        y = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).y()
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x,y))
            if (self.video.aff_flag == 2):
                """ TO DO Here is where affine calibration must be used """
                self.ui.rdoutMouseWorld.setText("(%0.f,%0.f)" % (self.world_coord[0][0], self.world_coord[1][0]))
            else:
                self.ui.rdoutMouseWorld.setText("(-,-)")

        """ 
        Updates status label when rexarm playback is been executed.
        This will be extended to includ eother appropriate messages
        """ 
        if(self.rex.plan_status == 1):
            self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
                                    %(self.rex.wpt_number + 1))


    def slider_change(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position 
        TO DO: Implement for the other sliders
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rex.max_torque = self.ui.sldrMaxTorque.value()/100.0
	for i in xrange(4):
            self.rex.speed[i] = self.ui.sldrSpeed.value()/100.0
        self.rex.joint_angles[0] = self.ui.sldrBase.value()*D2R
        self.rex.joint_angles[1] = self.ui.sldrShoulder.value()*D2R
        self.rex.joint_angles[2] = self.ui.sldrElbow.value()*D2R
        self.rex.joint_angles[3] = self.ui.sldrWrist.value()*D2R
        self.rex.cmd_publish()

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for 
        affine calibration 
        """
 
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()

        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return

        """ Change coordinates to image axis """
        self.last_click[0] = x - MIN_X
        self.last_click[1] = y - MIN_Y
       
        """ If affine calibration is been performed """
        if (self.video.aff_flag == 1):
            """ Save last mouse coordinate """
	    self.video.mouse_coord[self.video.mouse_click_id] = [(x-MIN_X),(y-MIN_Y)]

            """ Update the number of used poitns for calibration """
            self.video.mouse_click_id += 1

            """ Update status label text """
            self.ui.rdoutStatus.setText("Affine Calibration: Click point %d" 
                                      %(self.video.mouse_click_id + 1))

            """ 
            If the number of click is equal to the expected number of points
            computes the affine calibration.
            TO DO: Change this code to use you programmed affine calibration
            and NOT openCV pre-programmed function as it is done now.
            """
            if(self.video.mouse_click_id == self.video.aff_npoints):
                """ 
                Update status of calibration flag and number of mouse
                clicks
                """
                self.video.aff_flag = 2
                self.video.mouse_click_id = 0
                
		print self.video.mouse_coord
		print self.video.real_coord
                """ Perform affine calibration with OpenCV """
                #self.video.aff_matrix = cv2.getAffineTransform(
                #                        self.video.mouse_coord,
                #                        self.video.real_coord)
		self.video.compute_affine_matrix()            

                """ Updates Status Label to inform calibration is done """ 
                self.ui.rdoutStatus.setText("Waiting for input")

                """ 
                Uncomment to gether affine calibration matrix numbers 
                on terminal
                """ 
                print self.video.aff_matrix
	if self.video.aff_flag == 2:
            mouse_coord = np.array([[(x-MIN_X)], [(y-MIN_Y)],[1]])
	    self.world_coord = np.dot(self.video.aff_matrix, mouse_coord)
	
	if self.define_template_flag == 0:
	    self.click_point1 = copy.deepcopy(self.last_click)
	    self.define_template_flag = 1
	elif self.define_template_flag == 1:
	    self.click_point2 = copy.deepcopy(self.last_click)
	    self.template = copy.deepcopy(self.video.bwFrame[2*self.click_point1[1]:2*self.click_point2[1],2*self.click_point1[0]:2*self.click_point2[0]])
	    print self.click_point1
            print self.click_point2
	    self.define_template_flag = -1
	    cv2.imwrite('./template.png', self.template)

    def affine_cal(self):
        """ 
        Function called when affine calibration button is called.
        Note it only chnage the flag to record the next mouse clicks
        and updates the status text label 
        """
        self.video.aff_flag = 1 
        self.ui.rdoutStatus.setText("Affine Calibration: Click point %d" 
                                    %(self.video.mouse_click_id + 1))

    def load_camera_cal(self):
        print "Load Camera Cal"
	self.video.loadCalibration()

    def tr_initialize(self):
	self.wayPointsPos = []
	self.wayPointsSpeed = []
	self.wayPointsTime = []
        print "Teach and Repeat"

    def tr_add_waypoint(self):
	#waypoints1 = copy.deepcopy(self.rex.joint_angles_fb)
	#waypoints2 = copy.deepcopy(self.rex.joint_angles_fb)
	#self.wayPointsPos.append(waypoints1)
	#self.wayPointsTime.append([self.currtime, self.currtime, self.currtime, self.currtime])
        #self.wayPointsSpeed.append([0.1, 0.1, 0.1, 0.1])
        #self.currtime += 70000
	#waypoints2[1] -= 0.7
	#self.wayPointsPos.append(waypoints2)
	#self.wayPointsTime.append([self.currtime, self.currtime, self.currtime, self.currtime])
        #self.wayPointsSpeed.append([0.1, 0.1, 0.1, 0.1])
        #self.currtime += 70000
	#self.waypointsfp.writerow(waypoints1)
	#self.waypointsfp.writerow(waypoints2)
	#np.save("waypointsPos",self.wayPointsPos)
	#np.save("waypointsSpeed", self.wayPointsSpeed)
	#np.save("waypointsTime", self.wayPointsTime)
	self.wayPointsPos.append(copy.deepcopy(self.rex.joint_angles_fb))
	self.wayPointsSpeed.append(copy.deepcopy(self.rex.speed_fb))
	self.wayPointsTime.append(copy.deepcopy(self.rex.time_fb))
	np.save("waypointsPos",self.wayPointsPos)
	np.save("waypointsSpeed", self.wayPointsSpeed)
	np.save("waypointsTime", self.wayPointsTime)
	#print self.wayPoints
        print "Add Waypoint"
    
    def cubic_spline(self, q0, q0dot, t0, qf, qfdot, tf, stepnum):
	a0 = q0
	a1 = q0dot
	a2 = (3*(qf-q0) - (2*q0dot+qfdot)*float(tf-t0)) / float((tf-t0)*(tf-t0))
	a3 = (2*(q0-qf) + (q0dot+qfdot)*float(tf-t0)) / float((tf-t0)*(tf-t0)*(tf-t0))
	stepsize = float(tf-t0)/float(stepnum)
	currtime = t0 + stepsize
	pos_interpolated = []
	speed_interpolated = []
	for i in xrange(stepnum-1):
	   pos_interpolated.append(a0 + a1*currtime + a2*currtime*currtime + \
				   a3*currtime*currtime*currtime)
	   speed_interpolated.append(np.abs(a1 + 2*a2*currtime + 3*a3*currtime*currtime))
	   currtime += stepsize
	#print q0, qf
	#print pos_interpolated
	return (pos_interpolated, speed_interpolated) 

    def tr_smooth_path(self):
	if len(self.wayPointsPos) != len(self.wayPointsSpeed) and len(self.wayPointsPos) != len(self.wayPointsTime):
	    print "Error on waypoints number, cannot smooth path"
	    return
	for i in xrange(len(self.wayPointsTime)):
	    for j in xrange(4):
		self.wayPointsTime[i][j] *= US2S 
	for i in xrange(len(self.wayPointsSpeed)):
	    for j in xrange(4):
		self.wayPointsSpeed[i][j] *= percent2rads 
	interpolated_waypoints_pos = []
	interpolated_waypoints_speed = []
	for i in xrange(len(self.wayPointsPos)-1):
	    time_offset = self.wayPointsTime[i]
            startPos = self.wayPointsPos[i]
	    endPos = self.wayPointsPos[i+1]
	    startSpeed = self.wayPointsSpeed[i]
	    endSpeed = self.wayPointsSpeed[i+1]
	    startTime = self.wayPointsTime[i] 
	    endTime = self.wayPointsTime[i+1] 
	    stepnum = 3
	    four_joint_interpolated_pos = []
	    four_joint_interpolated_speed = []
	    for j in xrange(4):
		q0 = startPos[j]
		q0dot = startSpeed[j]
		t0 = startTime[j] - time_offset[j]
		qf = endPos[j]
		qfdot = endSpeed[j]
		tf = endTime[j] - time_offset[j]
		res = self.cubic_spline(q0, q0dot, t0, qf, qfdot, tf, stepnum)
		four_joint_interpolated_pos.append(res[0])
		four_joint_interpolated_speed.append(res[1])
	    
	    interpolated_waypoints_pos.append(startPos)
	    for i in xrange(len(four_joint_interpolated_pos[0])):
	        pos = []
		for j in xrange(len(four_joint_interpolated_pos)):
		    pos.append(four_joint_interpolated_pos[j][i])
		interpolated_waypoints_pos.append(pos)
	    interpolated_waypoints_pos.append(endPos)

	    interpolated_waypoints_speed.append(startSpeed)
	    for i in xrange(len(four_joint_interpolated_speed[0])):
	        speed = []
		for j in xrange(len(four_joint_interpolated_speed)):
		    speed.append(four_joint_interpolated_speed[j][i])
		interpolated_waypoints_speed.append(speed)
	    interpolated_waypoints_speed.append(endSpeed)

	self.wayPointsPos = interpolated_waypoints_pos
	self.wayPointsSpeed = interpolated_waypoints_speed
	for i in xrange(len(self.wayPointsSpeed)):
	    for j in xrange(len(self.wayPointsSpeed[0])):
	        self.wayPointsSpeed[i][j] /= percent2rads
	#pprint.pprint(self.wayPointsPos)
	#pprint.pprint(self.wayPointsSpeed)
	np.save("interpolated_waypoints_pos", self.wayPointsPos)
	np.save("interpolated_waypoints_speed", self.wayPointsSpeed)
        print "Smooth Path"

    def tr_playback(self):
	#print self.wayPoints
	self.rex.planPos = self.wayPointsPos
	self.rex.planSpeed = self.wayPointsSpeed 
	#print self.rex.plan
        self.rex.save_data = True
	self.rex.plan_status = 1
	self.rex.wpt_number = 0
	self.rex.wpt_total = len(self.rex.planPos)
        print "Playback"

    def tr_load(self):
        self.wayPointsPos = np.load("waypointsPos.npy")
	self.wayPointsSpeed = np.load("waypointsSpeed.npy")
	self.wayPointsTime = np.load("waypointsTime.npy")
	print "Load waypoints"

    def def_template(self):
        print "Define Template"
	self.define_template_flag = 0

    @do_cprofile
    def template_match(self):
        print "Template Match"
        self.targets = []
	result_pq = Queue.PriorityQueue()
	template = cv2.resize(self.template, None, fx=0.6, fy=0.6, interpolation=cv2.INTER_AREA)
	frame = cv2.resize(self.video.bwFrame, None, fx=0.6, fy=0.6, interpolation=cv2.INTER_AREA)
	height, width = template.shape	
	for i in xrange(0, frame.shape[0] - height):
	    for j in xrange(0, frame.shape[1] - width):
               	center_x = (i + height/2.0)/0.6
		center_y = (j + width/2.0)/0.6
		to_compare = frame[i:i+height,j:j+width]
		num = la.norm(to_compare - template) 
		result_pq.put((num, center_y, center_x))		
        result = []
	#for i in xrange(40):
	#    t = result_pq.get()
        #    print t[0]
        #    result.append([int(t[1]), int(t[2])])
        for i in xrange((frame.shape[0]-height)*(frame.shape[1]-width)):
            t = result_pq.get()
            if t[0] > 350:
                break
            else:
                result.append([int(t[1]), int(t[2])])
	distort = sys.maxint 
	cluster_size = 1
        #print result
	while distort > 20:
            clustered_result, distort = scv.kmeans(np.array(result), cluster_size)
            cluster_size += 1
        clustered_result, distort = scv.kmeans(np.array(result), 4)
        print "cluster_size: ", cluster_size-1
        print "distort", distort
        for r in clustered_result:
	    print r
	    self.targets.append((r[0], r[1]))
        #circles = cv2.HoughCircles(self.video.bwFrame, cv2.cv.CV_HOUGH_GRADIENT, 1, minDist=5, param1=50, param2=50, minRadius=1, maxRadius=30) 
        #for c in circlesi[0,:]:
        #    print c
        #    self.targets.append((c[0],c[1]))
 	#img = np.zeros((1000,1000,3), np.uint8)	
	#for t in self.targets:
	#    img = cv2.circle(img, t, 10, (255,255,255), -1)	
	#cv2.imwrite("./result.png", img)

    def exec_path(self):
        #waypoints = [(-80.0, 90.0, 350.0), (70.0, 80.0, 400.0), (-180.0, -250.0, 125.0),(240.0, -190.0, 95.0), (-80.0, 90.0, 350.0)]
        #waypoints = [(-100.0, 100.0, 5.0), (-100.0, 100.0, 200.0), (100.0, 100.0, 200.0),(100.0, 100.0, 5.0), (100.0, -100.0, 5.0), (100.0, -100.0, 200.0), (100.0, 100.0, 200.0)]
        waypoints = []
        for t in xrange(0,12,1):
            x = 150 * np.cos(t)
            y = 150 * np.sin(t)
            z = 5.0 + 30.0 * t
            waypoints.append((x, y, z))
        #waypoints = []
        #if not self.targets or len(self.targets) == 0:
        #    return
        #for t in self.targets:
        #    img_coord = np.array([[(t[0]/2.0)], [(t[1]/2.0)], [1]]) 
	#    world_coord = np.dot(self.video.aff_matrix, img_coord)
        #    print "img coord: ", img_coord
        #    print "world coord: ", world_coord
        #    waypoints.append((world_coord[0], world_coord[1], 5)) 
        print waypoints
	self.wayPointsPos = []
	self.wayPointsSpeed = []
	self.wayPointsTime = []
	for i in xrange(0, 3*len(waypoints), 3):
	    x, y, z = waypoints[i/3]
            self.wayPointsPos.append(self.rex.rexarm_IK_Search((x,y,z)))
            self.wayPointsSpeed.append(copy.deepcopy([0.15, 0.15, 0.15, 0.15]))
	    #jointsUp = self.rex.rexarm_IK_Search((x, y, 50.0))
	    #jointsDown = self.rex.rexarm_IK_Search((x, y, 0.0))
            #if jointsUp:
	    #    self.wayPointsPos.append(copy.deepcopy(jointsUp))
            #    self.wayPointsSpeed.append(copy.deepcopy([0.15, 0.15, 0.15, 0.15]))
            #    self.wayPointsTime.append([2000000*i,2000000*i,2000000*i,2000000*i])
            #
            #if jointsDown:
            #    self.wayPointsPos.append(copy.deepcopy(jointsDown))
	    #    self.wayPointsSpeed.append(copy.deepcopy([0.05, 0.05, 0.05, 0.05]))
	    #    self.wayPointsTime.append([2000000*(i+1),2000000*(i+1),2000000*(i+1),2000000*(i+1)])
            
            #if jointsUp:
	    #    self.wayPointsPos.append(copy.deepcopy(jointsUp))
	    #    self.wayPointsSpeed.append(copy.deepcopy([0.05, 0.05, 0.05, 0.05]))
	    #    self.wayPointsTime.append([2000000*(i+2),2000000*(i+2),2000000*(i+2),2000000*(i+2)])

	np.save("funPathPos",self.wayPointsPos)
	np.save("funPathSpeed", self.wayPointsSpeed)
	np.save("funPathTime", self.wayPointsTime)

	self.rex.planPos = self.wayPointsPos
	self.rex.planSpeed = self.wayPointsSpeed 
	print self.rex.planPos
        self.rex.save_data = True
	self.rex.plan_status = 1
	self.rex.wpt_number = 0
	self.rex.wpt_total = len(self.rex.planPos)
예제 #29
0
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_XL(port_num, 1)
        shld = DXL_XL(port_num, 2)
        elbw = DXL_XL(port_num, 3)
        wrst = DXL_XL(port_num, 4)
        grip = DXL_XL(port_num, 5)
        """Objects Using Other Classes"""
        self.camera = PiCamera()
        self.rexarm = Rexarm((base, shld, elbw, wrst, grip))
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp)
        self.taskThread = TaskThread(self.sm)
        self.rgb_image = None
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_current_state, "calibrate"))

        self.ui.btn_task1.clicked.connect(
            partial(self.taskThread.set_task_num, 1))
        self.ui.btn_task2.clicked.connect(
            partial(self.taskThread.set_task_num, 2))
        self.ui.btn_task3.clicked.connect(
            partial(self.taskThread.set_task_num, 3))
        self.ui.btn_task4.clicked.connect(
            partial(self.taskThread.set_task_num, 4))

        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.camera)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.updateAprilTags.connect(self.updateAprilTags)
        self.videoThread.updateImage.connect(self.updateImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.taskThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)
예제 #30
0
class Gui(QMainWindow):
    """ 
    Main GUI Class
    contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self,parent=None):
        QWidget.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self,True)

        """dynamixel bus -- add other motors here"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 0)
        shld = DXL_MX(port_num, 1)
        elbw = DXL_MX(port_num, 2)
        wrst = DXL_AX(port_num, 3)
        wrst2 = DXL_XL(port_num, 4)
        grip = DXL_XL(port_num, 5)

        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,grip),0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
    
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state, "execute"))
        self.ui.btn_cali.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
        # self.ui.btnUser1.setText("Calibrate")
        # self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
        
        self.ui.btnUser1.setText("Teach and Repeat")
        self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "teachNRepeat"))
        self.ui.btnUser2.setText("Record Waypoints")
        self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "recordWaypoint"))
        self.ui.btnUser3.setText("Play")
        self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))

        self.ui.btnUser4.setText("Block Detection Start")
        self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "blockDetectionStart"))
        self.ui.btnUser5.setText("Block Message")
        self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "blockMessage"))
        self.ui.btnUser6.setText("Block Detection End")
        self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "blockDetectionEnd"))

        self.ui.btnUser7.setText("Click and Grab")
        self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "clickNGrab"))
        self.ui.btnUser8.setText("Pick n' Place")
        self.ui.btnUser8.clicked.connect(partial(self.sm.set_next_state, "pickNPlace"))
        self.ui.btnUser9.setText("Pick n' Stack")
        self.ui.btnUser9.clicked.connect(partial(self.sm.set_next_state, "pickNStack"))
        self.ui.btnUser10.setText("Line 'em Up!")
        self.ui.btnUser10.clicked.connect(partial(self.sm.set_next_state, "lineUp"))
        self.ui.btnUser11.setText("Stack 'em High!")
        self.ui.btnUser11.clicked.connect(partial(self.sm.set_next_state, "stackHigh"))
        self.ui.btnUser12.setText("Pyramid Builder!")
        self.ui.btnUser12.clicked.connect(partial(self.sm.set_next_state, "buildPyramid"))
        


        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")

        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)

        """initalize rexarm"""
        self.rexarm.initialize()

        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)        
        self.videoThread.start()

        
        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()
        

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
        self.displayThread.start()

        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)

    """ Slots attach callback functions to signals emitted from threads"""

    @pyqtSlot(QImage, QImage)
    def setImage(self, rgb_image, depth_image):
        if(self.ui.radioVideo.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
        if(self.ui.radioDepth.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0]*R2D)))
        self.ui.rdoutShoulderJC.setText(str("%+.2f" % ((joints[1]*R2D)+90.0)))
        self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2]*R2D)))
        self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3]*R2D)))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)


    """ Other callback functions attached to GUI elements"""

    def estop(self):
        self.rexarm.estop = True
        self.sm.set_next_state("estop")

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
        # self.ui.sldrGrip1.setText(str(self.ui.sldrGrip1.value()))
        # self.ui.sldrGrip2.setText(str(self.ui.sldrGrip2.value()))
        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value()/100.0]*self.rexarm.num_joints, update_now = False)
        self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value()/100.0, update_now = False)
        joint_positions = np.array([self.ui.sldrBase.value()*D2R, 
                           self.ui.sldrShoulder.value()*D2R,
                           self.ui.sldrElbow.value()*D2R,
                           self.ui.sldrWrist.value()*D2R,
                           self.ui.sldrGrip1.value()*D2R,
                           self.ui.sldrGrip2.value()*D2R])
        self.rexarm.set_positions(joint_positions, update_now = False)

    def directControlChk(self, state):
        if state == Qt.Checked:
            self.sm.set_next_state("manual")
            self.ui.SliderFrame.setEnabled(True)
        else:
            self.sm.set_next_state("idle")
            self.ui.SliderFrame.setEnabled(False)

    def trackMouse(self):
        """ 
        Mouse position presentation in GUI
        After implementing workspace calibration 
        display the world coordinates the mouse points to 
        in the RGB video image.
        """

        x = QWidget.mapFromGlobal(self,QCursor.pos()).x()
        y = QWidget.mapFromGlobal(self,QCursor.pos()).y()
        if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            if(self.kinect.currentDepthFrame.any() != 0):
                z = self.kinect.currentDepthFrame[y][x]
                mouse_coord = [x,y,1]
                # affine = [[1,1,1],[1,1,1],[0,0,1]]
                
                self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x,y,z))
                if (self.kinect.kinectCalibrated):
                    # Z = self.kinect.worldHeight - 0.1236 * 1000 * np.tan(z/2842.5 + 1.1863)
                    # world_coord = np.matmul(self.kinect.convert_to_world, mouse_coord)
                    world_coord = self.kinect.world_coord(x,y)
                    self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (world_coord[0],world_coord[1],world_coord[2]))
                else:
                    self.ui.rdoutMouseWorld.setText("(-,-,-)")
                


    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
 
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()

        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return

        """ Change coordinates to image axis """
        self.kinect.last_click[0] = x - MIN_X 
        self.kinect.last_click[1] = y - MIN_Y
        self.kinect.new_click = True
예제 #31
0
class Gui(QtGui.QMainWindow):
    """ 
    Main GUI Class
    It contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self, parent=None):

        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Main Variables Using Other Classes"""
        self.rex = Rexarm(self.ui)
        self.video = Video(cv2.VideoCapture(0))
        """
        Zhentao added Here.
        Initialize the statemachine.
        """

        self.statemanager = StateManager(self.rex, self.video)
        """ Other Variables """
        self.last_click = np.float32([0, 0])
        """ Set GUI to track mouse """
        QtGui.QWidget.setMouseTracking(self, True)
        """ 
        Video Function 
        Creates a timer and calls play() function 
        according to the given time delay (27mm) 
        """
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.play)
        self._timer.start(27)
        """ 
        LCM Arm Feedback
        Creates a timer to call LCM handler continuously
        No delay implemented. Reads all time 
        """
        self._timer2 = QtCore.QTimer(self)
        self._timer2.timeout.connect(self.rex.get_feedback)
        self._timer2.start()
        """ 
        Connect Sliders to Function
        LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI 
        """
        ## TODO: IMPLEMENT GRIP VALUE CONTROLS ##
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        """ Commands the arm as the arm initialize to 0,0,0,0 angles """
        self.sliderChange()
        """ Connect Buttons to Functions 
        LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
        """
        #This is needed.
        self.ui.btnUser1.setText("Affine Calibration")
        self.ui.btnUser1.clicked.connect(self.affine_cal)

        self.ui.btnUser2.setText("SM Test")
        self.ui.btnUser2.clicked.connect(self.iTestSM)

        self.ui.btnUser3.setText("Reset Position")
        self.ui.btnUser3.clicked.connect(self.rex.iResetPosition)

        self.ui.btnUser4.setText("Replay")
        self.ui.btnUser4.clicked.connect(self.iReplay)
        """
        self.ui.btnUser4.setText("OpenGripper")
        self.ui.btnUser4.clicked.connect(self.iTestGripperOpen)

        self.ui.btnUser5.setText("CloseGripper")
        self.ui.btnUser5.clicked.connect(self.iTestGripperClose)

        
        """
        """

        
        self.ui.btnUser2.setText("STEP1: Reset Position")
        self.ui.btnUser2.clicked.connect(self.iResetPosition)
        self.ui.btnUser3.clicked.connect(self.iResetTorqueAndSpeed)
        
        self.ui.btnUser3.setText("STEP2: Reset Torque and Speed")
        self.ui.btnUser4.setText("STEP3: Train Begin")
        self.ui.btnUser4.clicked.connect(self.iTrainBegin)
        self.ui.btnUser5.setText("STEP4(r): GetWayPoint")
        self.ui.btnUser5.clicked.connect(self.iGetWayPoint)
        self.ui.btnUser5.setEnabled(False)
        self.ui.btnUser6.setText("STEP5: Stop Recording")
        self.ui.btnUser6.clicked.connect(self.iTrainStop)
        self.ui.btnUser6.setEnabled(False)
        self.ui.btnUser7.clicked.connect(self.iReplayBegin)
        self.ui.btnUser7.setText("Replay WholeWay")
        self.ui.btnUser8.clicked.connect(self.iReplayWPTBegin_FAST)
        self.ui.btnUser8.setText("Replay WayPoint(FAST)")
        self.ui.btnUser9.clicked.connect(self.iReplayWPTBegin_SLOW)
        self.ui.btnUser9.setText("Replay WayPoint(SLOW)")
        self.ui.btnUser10.setText("PlayStop")
        self.ui.btnUser10.clicked.connect(self.iReplayStop)
        self.ui.btnUser11.setText("Save WPT Data")
        self.ui.btnUser11.clicked.connect(self.iSaveData)
        self.ui.btnUser12.setText("Load WPT Data")
        self.ui.btnUser12.clicked.connect(self.iLoadData)
        
        """

    def play(self):
        self.statemanager.Statemanager_MoveToNextState()
        """ 
        Play Funtion
        Continuously called by GUI 
        """
        """ Renders the Video Frame """
        try:
            self.video.captureNextFrame()
            #self.video.blobDetector()
            self.ui.videoFrame.setPixmap(self.video.convertFrame())
            self.ui.videoFrame.setScaledContents(True)
        except TypeError:
            print "No frame"
        """ 
        Update GUI Joint Coordinates Labels on Sliders
        LAB TASK: include the other slider labels 
        """
        self.ui.rdoutBaseJC.setText(
            str("%.2f" % (self.rex.joint_angles_fb[0] * R2D)))
        self.ui.rdoutShoulderJC.setText(
            str("%.2f" % (self.rex.joint_angles_fb[1] * R2D)))
        self.ui.rdoutElbowJC.setText(
            str("%.2f" % (self.rex.joint_angles_fb[2] * R2D)))
        self.ui.rdoutWristJC.setText(
            str("%.2f" % (self.rex.joint_angles_fb[3] * R2D)))
        """ 
        Mouse position presentation in GUI
        TO DO: after getting affine calibration make the apprriate label
        to present the value of mouse position in world coordinates 
        """
        x = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).x()
        y = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).y()
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x, y))
            if (self.video.aff_flag == 2):
                """
                ZHENTAO: STATE CONTROL: after finishing the camera calibration, move to the next states.
                """
                """
                ######################################## 
                TED added world_coords label to frame
                ######################################## 
                """
                world_coords = np.dot(self.video.aff_matrix,
                                      np.array([[x], [y], [1]]))
                self.ui.rdoutMouseWorld.setText(
                    "(%.0f,%.0f)" % (world_coords[0], world_coords[1]))
            else:
                self.ui.rdoutMouseWorld.setText("(-,-)")
        """
        Set button avalibity.
        """
        #self.iPrintStatusTerminal()########################################Here should be activated.
        self.iSetButtonAbility()
        self.iUpdateStatusBar()
        """ 
        Updates status label when rexarm playback is been executed.
        This will be extended to include other appropriate messages
        """
        """
        if(self.rex.plan_status == 1):
            self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
                                    %(self.rex.wpt_number + 1))
        if (self.rex.plan_status == 0 and self.rex.way_total == 0):
            self.ui.rdoutStatus.setText("Click [Train Begin] button to start train.")


        if (self.rex.plan_status == 2):
            self.ui.rdoutStatus.setText("Click [Get Way Point] button to record way point. Click [Stop Recording] to stop recording.")
        if (self.rex.plan_status == 0 and self.rex.way_total != 0 and self.rex.way_number == 0):
            self.ui.rdoutStatus.setText("Click [Replay Wholeway] to play the whole way. Click [Save Data] to Save the data.")
        if (self.rex.plan_status == 5):
            self.ui.rdoutStatus.setText("Click [Play Stop] to stop")
        """
        """###############################################
        Frank Added Here
        ###############################################"""

        if (self.rex.plan_status == 2):
            self.ui.sldrBase.setProperty("value",
                                         self.rex.joint_angles_fb[0] * R2D)
            self.ui.sldrShoulder.setProperty("value",
                                             self.rex.joint_angles_fb[1] * R2D)
            self.ui.sldrElbow.setProperty("value",
                                          self.rex.joint_angles_fb[2] * R2D)
            self.ui.sldrWrist.setProperty("value",
                                          self.rex.joint_angles_fb[3] * R2D)
            self.iTrain_AddOneWay()

# replay continuously recorded waypoints
        if (self.rex.plan_status == 5):
            self.iReplay_PlayOneWay()

# replay manually recorded waypoints
        if (self.rex.plan_status == 3 or self.rex.plan_status == 4):
            self.iReplayWPT_PlayOneWay()

        self.iShowFK()

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position 
        TO DO: Implement for the other sliders
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
        self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rex.max_torque = self.ui.sldrMaxTorque.value() / 100.0
        self.rex.speed = self.ui.sldrSpeed.value() / 100.0
        #self.rex.joint_angles[0] = self.ui.sldrBase.value()*D2R
        #self.rex.joint_angles[1] = self.ui.sldrShoulder.value()*D2R
        #self.rex.joint_angles[2] = self.ui.sldrElbow.value()*D2R
        #elf.rex.joint_angles[3] = self.ui.sldrWrist.value()*D2R
        self.rex.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
        self.rex.cmd_publish()

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for 
        affine calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        """ Change coordinates to image axis """
        self.last_click[0] = x - MIN_X
        self.last_click[1] = y - MIN_Y

        ### TESTING BLOB DETECTION TESTING ###
        #self.video.blobDetector()
        ### TESTING BLOB DETECTION TESTING ###

        # if aff_flag is 2, affine transform has been performed
        if (self.video.aff_flag == 2):

            ik_wcoords = np.dot(self.video.aff_matrix,
                                np.array([[x - MIN_X], [y - MIN_Y], [1]]))

            #self.iTestIK(ik_wcoords[0], ik_wcoords[1], 40,4*PI/4)
            """
            #TODO: Temperary here to take the place of Camera that Ted will write.
            """
            self.iMimicCamera(ik_wcoords[0], ik_wcoords[1])
        """ If affine calibration is being performed """
        if (self.video.aff_flag == 1):
            """ Save last mouse coordinate """
            self.video.mouse_coord[self.video.mouse_click_id] = [(x - MIN_X),
                                                                 (y - MIN_Y)]
            """ Update the number of used poitns for calibration """
            self.video.mouse_click_id += 1
            """ Update status label text """
            self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
                                        (self.video.mouse_click_id + 1))
            """ 
            If the number of click is equal to the expected number of points
            computes the affine calibration.
            
            LAB TASK: Change this code to use your affine calibration routine
            and NOT openCV pre-programmed function as it is done now.
            """
            if (self.video.mouse_click_id == self.video.aff_npoints):
                """ Perform affine calibration with OpenCV
                self.video.aff_matrix = cv2.getAffineTransform(
                                        self.video.mouse_coord,
                                        self.video.real_coord)
                """
                """
                ######################################## 
                TED added affineTransform function here
                ######################################## 
                """

                self.video.aff_matrix = self.video.affineTransform()
                self.video.aff_flag = 2
                self.video.mouse_click_id = 0

                self.video.calculateBoundaryMask()
                self.video.calculateArmMask()
                self.video.calculateBaseMask()
                self.video.calculatePokeballMask()
                self.video.generateBoundaryMask()
                """ 
                Update status of calibration flag and number of mouse
                clicks
                """
                """ Updates Status Label to inform calibration is done """
                self.ui.rdoutStatus.setText("Affine Transform Completed")
                """ 
                print affine calibration matrix numbers to terminal
                """
                print("[Msg]: Affine Calibration Finished.")
                print("[Msg]: Affine Calibration Matrix: "),
                print self.video.aff_matrix

    def affine_cal(self):
        """ 
        Function called when affine calibration button is called.
        Note it only chnage the flag to record the next mouse clicks
        and updates the status text label 
        """
        self.video.aff_flag = 1
        self.video.mouse_click_id = 0
        self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
                                    (self.video.mouse_click_id + 1))

    """
    Button 1: Reset Position
    """

    def iResetTorqueAndSpeed(self):
        self.rex.iSetTorque(0.0)
        self.rex.iSetSpeed(0.1)
        self.rex.cmd_publish()

    """
    Button 3: Start Train.
    """

    def iTrain_ClearRecord(self):
        self.rex.wpt = []
        self.rex.wpt_number = 0
        self.rex.wpt_total = 0
        self.rex.way = []
        self.rex.way_number = 0
        self.rex.way_total = 0

    def iTrainBegin(self):
        self.rex.iSetTorque(0.0)
        self.iTrain_ClearRecord()
        self.rex.plan_status = 2

    """
    In Play, keep recording.
    """

    def iTrain_FetchSensorData(self):
        return [
            self.rex.joint_angles_fb[0], self.rex.joint_angles_fb[1],
            self.rex.joint_angles_fb[2], self.rex.joint_angles_fb[3]
        ]

# fetch list of sensor data and append to the list of waypoints continuously

    def iTrain_AddOneWay(self):
        SensorData = self.iTrain_FetchSensorData()
        self.rex.way.append(SensorData)
        self.rex.way_total = self.rex.way_total + 1  # Have such data point up to now.

    """
    Get Way Point:
    """

    def iGetWayPoint(self):
        SensorData = self.iTrain_FetchSensorData()
        currentTime = self.iGetTime_now()
        SensorData.append(currentTime)
        self.rex.wpt.append(SensorData)
        self.rex.wpt_total = self.rex.wpt_total + 1  # Have such data point up to now.
        print self.rex.wpt_number

    """
    Train stop:
    """

    def iTrainStop(self):
        self.rex.plan_status = 0
        print("Stop Recording!")

    """
    Replay
    """

    # Begin replaying waypoints that were continuously set
    def iReplayBegin(self):
        self.rex.iSetTorque(0.5)
        self.rex.plan_status = 5
        self.rex.way_number = 0
        print("Replay Start")

    """
    """

    # Stop replaying waypoints and reset waypoint number to 0
    def iReplayStop(self):
        self.rex.plan_status = 0
        self.rex.way_number = 0
        self.rex.wpt_number = 0
### TESTING TED ADDED ###

    def iReplay_SetOneSensorData(self, valueIndex):
        sensorData = self.rex.way[valueIndex]
        self.rex.iSetJointAngle(0, sensorData[0])
        self.rex.iSetJointAngle(1, sensorData[1])
        self.rex.iSetJointAngle(2, sensorData[2])
        self.rex.iSetJointAngle(3, sensorData[3])
        self.rex.cmd_publish()

# TODO: add comments

    def iReplay_PlayOneWay(self):
        if (self.rex.way_number == self.rex.way_total):
            self.iReplayStop()
        else:
            self.iReplay_SetOneSensorData(self.rex.way_number)
            self.rex.way_number = self.rex.way_number + 1

    "Replay WPT"

    # begin replaying manually set waypoints at a slow speed
    #Slow: plan_status = 3.

    def iReplayWPTBegin_SLOW(self):

        self.rex.iSetTorque(0.7)
        self.rex.iSetSpeed(GLOBALSLOWSPEED)
        self.rex.plan_status = 3
        self.rex.wpt_number = 0
        self.rex.recslow_total = 0
        self.rex.recslow = []
        #self.calcCubicCoeffs()

# begin replaying manually set waypoints at a fast speed
# Fast mode: plan_status = 4.

    def iReplayWPTBegin_FAST(self):

        self.rex.iSetTorque(0.7)
        self.rex.iSetSpeed(GLOBALFASTSPEED)
        self.rex.plan_status = 4
        self.rex.wpt_number = 0
        self.rex.recfast_total = 0
        self.rex.recfast = []
        #self.calcCubicCoeffs()

    def iReplayWPT_GetSensorData(self):
        return [
            self.rex.joint_angles_fb[0], self.rex.joint_angles_fb[1],
            self.rex.joint_angles_fb[2], self.rex.joint_angles_fb[3]
        ]

    def iCheckIfArrived(self, target, errorTorrance):
        sensorData = self.iReplayWPT_GetSensorData()
        error0 = abs(target[0] - sensorData[0])
        error1 = abs(target[1] - sensorData[1])
        error2 = abs(target[2] - sensorData[2])
        error3 = abs(target[3] - sensorData[3])

        if (error0 < errorTorrance and error1 < errorTorrance
                and error2 < errorTorrance and error3 < errorTorrance):
            return True
        else:
            return False

    """
    def iReplayWPT_PlayOneWay(self):
        if (self.rex.wpt_number == self.rex.wpt_total):
            self.iReplayStop()
        else:
            target = self.rex.wpt[self.rex.wpt_number]
            arrived = self.iCheckIfArrived(target,GLOBALERRORTORRANCE)
            

            if (arrived):  
                self.rex.wpt_number = self.rex.wpt_number + 1
                print "CURRENT WAYPOINT SENT TO CALC CUBIC"
                print(self.rex.wpt_number),
                self.calcCubicCoeffs()

            else:
            	self.cubicPoly()
                self.rex.cmd_publish()
    """

    def iReplayWPT_PlayOneWay(self):
        if (self.rex.wpt_number == self.rex.wpt_total):
            self.iReplayStop()
        else:
            target = self.rex.wpt[self.rex.wpt_number]
            arrived = self.iCheckIfArrived(target, GLOBALERRORTORRANCE)

            #Record one real time data into self.rex.rec list and self.rex.rec_total += 1.

            #SLOW
            if (self.rex.plan_status == 3):
                mode = 0
                #Fast:
            elif (self.rex.plan_status == 4):
                mode = 1
            else:
                print("Error: iReplayWPT_PlayOneWay: Undefined status.")

            self.iRecord_JointAngleFB(mode)

            if (arrived):
                self.rex.wpt_number = self.rex.wpt_number + 1
            else:
                self.rex.iSetJointAngle(0, target[0])
                self.rex.iSetJointAngle(1, target[1])
                self.rex.iSetJointAngle(2, target[2])
                self.rex.iSetJointAngle(3, target[3])
                self.rex.cmd_publish()

                #self.ui.sldrBase.setProperty("value",self.rex.joint_angles[0]*R2D)
                #self.ui.sldrShoulder.setProperty("value",self.rex.joint_angles[1]*R2D)
                #self.ui.sldrElbow.setProperty("value",self.rex.joint_angles[2]*R2D)
                #self.ui.sldrWrist.setProperty("value",self.rex.joint_angles[3]*R2D)

    """
    Button Availibity
    """

    def iSetButtonAbility(self):

        if (self.statemanager.currentState == STATE_CODE_INIT):
            self.ui.btnUser1.setEnabled(1)
        else:
            self.ui.btnUser1.setEnabled(0)

        if (self.statemanager.currentState == STATE_CODE_END):
            self.ui.btnUser4.setEnabled(1)
        else:
            self.ui.btnUser4.setEnabled(0)

    """
        if (self.rex.plan_status == 0):
            self.ui.btnUser4.setEnabled(True)
            self.ui.btnUser5.setEnabled(False)
            self.ui.btnUser6.setEnabled(False)
            

        if (self.rex.plan_status == 2):
            self.ui.btnUser4.setEnabled(False)
            self.ui.btnUser5.setEnabled(True)
            self.ui.btnUser6.setEnabled(True)

        '''Replay way'''

        if (self.rex.plan_status == 0 and self.rex.way_total != 0):
            self.ui.btnUser7.setEnabled(True)
        else:
            self.ui.btnUser7.setEnabled(False)
        

        '''
        replay way point.
        '''
        if (self.rex.plan_status == 0 and self.rex.wpt_total != 0):
            self.ui.btnUser8.setEnabled(True)
            self.ui.btnUser9.setEnabled(True)
        else:
            self.ui.btnUser8.setEnabled(False)
            self.ui.btnUser9.setEnabled(False)

        '''
        save data.
        '''
        if (self.rex.plan_status == 0 and self.rex.way_total != 0):
            self.ui.btnUser11.setEnabled(True)
        else:
            self.ui.btnUser11.setEnabled(False)
        
        '''
        Load Data
        '''
        if (self.rex.plan_status == 0):
            self.ui.btnUser12.setEnabled(True)
        else:
            self.ui.btnUser12.setEnabled(False)
    """

    def iPrintStatusTerminal(self):
        print("[Status = "),
        print(self.rex.plan_status),

        print(",#way="),
        print(self.rex.way_total),
        print(",#way_now="),
        print(self.rex.way_number),

        print(",#wpt="),
        print(self.rex.wpt_total),
        print(",#wpt_now="),
        print(self.rex.wpt_number),

        print(",#recslow="),
        print(self.rex.recslow_total),

        print(",#recfast="),
        print(self.rex.recfast_total),

        print("]")

    def iShowFK(self):
        self.rex.rexarm_FK(self.rex.joint_angles_fb)
        """
        print("[x]:"),
        print(P0[0][0])
        print("[y]:"),
        print(P0[1][0])
        print("[z]:"),
        print(P0[2][0])
        print("\n")
        """
        self.ui.rdoutX.setText(str(float("{0:.2f}".format(self.rex.P0[0]))))
        self.ui.rdoutY.setText(str(float("{0:.2f}".format(self.rex.P0[1]))))
        self.ui.rdoutZ.setText(str(float("{0:.2f}".format(self.rex.P0[2]))))
        self.ui.rdoutT.setText(str(float("{0:.2f}".format(self.rex.T * R2D))))

    # function which sets the self.rex.joint_angles for each joint to the values
    # calculated by the cubic polynomials as a function of time
    def cubicPoly(self):
        if self.rex.wpt_number == self.rex.wpt_total:
            self.iReplayStop()
            # TODO: break from function here

        # TODO: set the start time when at the first waypoint!

        for i in range(0, 4):
            t = self.iGetTime_now() - self.rex.st
            self.rex.joint_angles[i] = (
                self.rex.cubic_coeffs[i]
            )[0]  #+((self.rex.cubic_coeffs[i])[1]*t)+((self.rex.cubic_coeffs[i])[2]*(t**2))+((self.rex.cubic_coeffs[i])[3]*(t**3))

    # function which calculates the coefficients for the cubic polynomial function
    def calcCubicCoeffs(self):
        #NOTE: each array index corresponds to the joint
        #TODO: Forward Kinematics + Calculate Time + Set time for moving + set tf.

        v0 = [0, 0, 0, 0]
        vf = [0, 0, 0, 0]
        t0 = [0, 0, 0, 0]
        tf = [1, 1, 1, 1]

        current_wpt = self.rex.wpt_number
        next_wpt = self.rex.wpt_number + 1

        q0 = [
            self.rex.wpt[current_wpt][0], self.rex.wpt[current_wpt][1],
            self.rex.wpt[current_wpt][2], self.rex.wpt[current_wpt][3]
        ]
        qf = [
            self.rex.wpt[next_wpt][0], self.rex.wpt[next_wpt][1],
            self.rex.wpt[next_wpt][2], self.rex.wpt[next_wpt][3]
        ]

        A = []
        b = []
        A = list()
        b = list()

        # NOTE: format is Ax=b where A is the constant waypoint relation matrix,
        # x is the unknown coefficients and bi is the [q0,v0,qf,vf] column vector
        # for each joint
        # For each joint create arrays A and b
        # A: list of 4 numpy arrays that are 4x4
        # b: list of 4 numpy arrays that are 4x1
        # self.rex.cubic_coeffs: list of 4 numpy arrays that are 4x1

        for i in range(0, 4):
            A.append(
                np.array([[1, t0[i], t0[i]**2, t0[i]**3],
                          [0, 1, 2 * t0[i], 3 * (t0[i]**2)],
                          [1, tf[i], tf[i]**2, tf[i]**3],
                          [0, 1, 2 * tf[i], 3 * (tf[i]**2)]]))
            b.append(np.array([q0[i], v0[i], qf[i], vf[i]]))
            self.rex.cubic_coeffs[i] = np.dot(np.linalg.inv(A[i]), b[i])

        #set the start time used by the cubic
        self.rex.st = self.iGetTime_now()

    def iSaveData(self):
        """
        The data variable to save is: 
           
            self.wpt = []
            self.wpt_total = 0
            
            self.way = []
            self.way_total = 0

        """
        """
        f = open(GLOBALDFILENAME_WAYNUM,'wt')
        writer = csv.writer(f)
        writer.writerow([self.rex.way_total])
        f.close()
        """

        f = open(GLOBALDFILENAME_WAY, 'wt')
        writer = csv.writer(f)

        for ii in range(self.rex.way_total):
            writer.writerow(self.rex.way[ii])
        f.close()
        """
        f = open(GLOBALDFILENAME_WPTNUM,'wt')
        writer = csv.writer(f)
        writer.writerow([self.rex.wpt_total])
        f.close()
        """
        f = open(GLOBALDFILENAME_WPT, 'wt')
        writer = csv.writer(f)

        for ii in range(self.rex.wpt_total):
            writer.writerow(self.rex.wpt[ii])
        f.close()

        f = open(GLOBALDFILENAME_RECSLOW, 'wt')
        writer = csv.writer(f)
        for ii in range(self.rex.recslow_total):
            writer.writerow(self.rex.recslow[ii])
        f.close()

        f = open(GLOBALDFILENAME_RECFAST, 'wt')
        writer = csv.writer(f)
        for ii in range(self.rex.recfast_total):
            writer.writerow(self.rex.recfast[ii])
        f.close()

        print("Saving")

    def iLoadData(self):
        """
        the data variable to load is:
            self.wpt = []
            self.wpt_total = 0
            
            self.way = []
            self.way_total = 0

        """

        self.rex.wpt = []
        self.rex.way = []

        self.rex.wpt_total = 0
        self.rex.way_total = 0

        self.rex.wpt_number = 0
        self.rex.way_number = 0

        datafile = open(GLOBALDFILENAME_WAY, 'r')
        datafileReader = csv.reader(datafile)
        for ii in datafileReader:
            ii = map(float, ii)
            self.rex.way.append(ii)
            self.rex.way_total = self.rex.way_total + 1

        datafile = open(GLOBALDFILENAME_WPT, 'r')
        datafileReader = csv.reader(datafile)
        for ii in datafileReader:
            ii = map(float, ii)
            self.rex.wpt.append(ii)
            self.rex.wpt_total = self.rex.wpt_total + 1

        print("Loading Finished.")

    def iGetTime_now(self):
        return int(time.time() * 1E6)

    # Record the real time joint angle feedback when replaying the way points.
    # mdoe == 0: slow mode, 1: fast mode
    def iRecord_JointAngleFB(self, mode):
        if (mode == 0):
            temp = []
            #0: Time
            temp.append(self.iGetTime_now())
            #1-4: Joint angles.
            temp.append(self.rex.joint_angles_fb[0])
            temp.append(self.rex.joint_angles_fb[1])
            temp.append(self.rex.joint_angles_fb[2])
            temp.append(self.rex.joint_angles_fb[3])

            #5-7: Forward kinematics.
            temp.append(self.rex.P0[0])
            temp.append(self.rex.P0[1])
            temp.append(self.rex.P0[2])
            temp.append(self.rex.T)

            print(len(self.rex.recslow))
            print(temp)
            self.rex.recslow.append(temp)

            self.rex.recslow_total = self.rex.recslow_total + 1
        elif (mode == 1):
            temp = []
            temp.append(self.iGetTime_now())
            temp.append(self.rex.joint_angles_fb[0])
            temp.append(self.rex.joint_angles_fb[1])
            temp.append(self.rex.joint_angles_fb[2])
            temp.append(self.rex.joint_angles_fb[3])
            temp.append(self.rex.P0[0])
            temp.append(self.rex.P0[1])
            temp.append(self.rex.P0[2])
            temp.append(self.rex.T)
            self.rex.recfast.append(temp)
            print(temp)
            self.rex.recfast_total = self.rex.recfast_total + 1

    """
    A function to test the IK, not need for final competition.
    """

    def iMimicCamera(self, x, y):

        print("[Msg]: MimicCam is called.")

        self.video.numPokRemain = 1
        self.video.whetherFinishedCam = True
        self.video.nextLocationofPokmon = [x, y]
        """
        self.numPokRemain  = 0
        self.whetherFinishedCam = False;
        self.nextLocationofPokmon = [0,0];
        """

    def iTestIK(self, x, y, z, phi):
        phi = self.rex.rexarm_IK_CatchAnglePlaner([x, y, z])

        print("[Msg]: IK is called.")
        [
            validity_1, IK_conf_1, validity_2, IK_conf_2, validity_3,
            IK_conf_3, validity_4, IK_conf_4
        ] = self.rex.rexarm_IK([x, y, z, phi], 1)

        if (validity_1):
            self.rex.iSetJointAngle(0, IK_conf_1[0])
            self.rex.iSetJointAngle(1, IK_conf_1[1])
            self.rex.iSetJointAngle(2, IK_conf_1[2])
            self.rex.iSetJointAngle(3, IK_conf_1[3])
            self.rex.cmd_publish()
        else:
            print("[Msg]: IK is not reachable.")

    """
    State manager Testing.
    """

    def iTestSM(self):
        self.statemanager.StateManager_Test()

    """
    Update the name of the state to the state bar.
    """

    def iUpdateStatusBar(self):
        if (self.video.aff_flag == 1):
            pass
        if (self.statemanager.currentState == STATE_CODE_INIT):
            self.ui.rdoutStatus.setText("STATE_CODE_INIT")
        if (self.statemanager.currentState == STATE_CODE_RP):
            self.ui.rdoutStatus.setText("STATE_CODE_RP")
        if (self.statemanager.currentState == STATE_CODE_MTB):
            self.ui.rdoutStatus.setText("STATE_CODE_MTB")
        if (self.statemanager.currentState == STATE_CODE_MTFT):
            self.ui.rdoutStatus.setText("STATE_CODE_MTFT")
        if (self.statemanager.currentState == STATE_CODE_CP):
            self.ui.rdoutStatus.setText("STATE_CODE_CP")
        if (self.statemanager.currentState == STATE_CODE_OG):
            self.ui.rdoutStatus.setText("STATE_CODE_OG")
        if (self.statemanager.currentState == STATE_CODE_CCACFP):
            self.ui.rdoutStatus.setText("STATE_CODE_CCACFP")
        if (self.statemanager.currentState == STATE_CODE_END):
            self.ui.rdoutStatus.setText("STATE_CODE_END")
        if (self.statemanager.currentState == STATE_CODE_RAP):
            self.ui.rdoutStatus.setText("STATE_CODE_RAP")
            """
            STATE_CODE_INIT  = 1
            STATE_CODE_CCACFP = 2
            STATE_CODE_OG = 3
            STATE_CODE_MTFT = 4
            STATE_CODE_CP = 5
            STATE_CODE_MTB = 6
            STATE_CODE_RP = 7
            STATE_END = 8


            """

    def iTestGripperOpen(self):
        self.rex.rexarm_gripper_grab(1)

    def iTestGripperClose(self):
        self.rex.rexarm_gripper_grab(0)

    def iReplay(self):
        if (self.statemanager.currentState == STATE_CODE_END):
            self.statemanager.currentState = STATE_CODE_INIT
class App(QtWidgets.QMainWindow):
    
    path1 = ""
    path2 = ""
    path3 = ""
    
    
    def __init__(self):
        super(App, self).__init__()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        
        self.ui.b1.clicked.connect(self.loaddata1)
        self.ui.b3.clicked.connect(self.loaddata2)
        self.ui.b4.clicked.connect(self.loaddata3)
        self.ui.b2.clicked.connect(self.apply_harris)
        self.ui.b5.clicked.connect(self.matching)
    
    
    
    def loaddata1(self):
        filename = QFileDialog.getOpenFileName(self)
        if filename[0]:
            self.path1 = filename[0]
        
        image = cv2.imread(self.path1)
        self.ui.w1.show()
        self.ui.w1.setImage(np.rot90(image,1))
        
    def loaddata2(self):
        filename = QFileDialog.getOpenFileName(self)
        if filename[0]:
            self.path2 = filename[0]
        
        image = cv2.imread(self.path2)
        self.ui.w3.show()
        self.ui.w3.setImage(np.rot90(image,1))
        self.ui.w5.show()
        self.ui.w5.setImage(np.rot90(image,1))
        
    def loaddata3(self):
        filename = QFileDialog.getOpenFileName(self)
        if filename[0]:
            self.path3 = filename[0]
        
        image = cv2.imread(self.path3)
        self.ui.w4.show()
        self.ui.w4.setImage(np.rot90(image,1))
        self.ui.w6.show()
        self.ui.w6.setImage(np.rot90(image,1))
    
    def apply_harris(self):
        image = cv2.imread(self.path1)
        img_plot = np.copy(image)
        gray = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
        gray_img = np.float32(gray)
        points = self.harris(gray_img)
        for point in points:
            cv2.circle(img_plot, (point[1],point[0]), 5 , [255,0,255], -1)
        cv2.imwrite("out/harris_points.png", img_plot)
        out_harris = cv2.imread("out/harris_points.png")
        self.ui.w2.show()
        self.ui.w2.setImage(np.rot90(out_harris,1))
    
    def harris(self,gray_img):
        
        smothed_image = signal.convolve2d(gray_img, self.gaussian_kernel2d(7,1) ,'same')
        Ix =  cv2.Sobel(smothed_image,cv2.CV_64F,1,0,ksize=3)
        Iy = cv2.Sobel(smothed_image,cv2.CV_64F,0,1,ksize=3)
        Ixx =  np.multiply( Ix, Ix) 
        Iyy =  np.multiply( Iy, Iy)
        Ixy =  np.multiply( Ix, Iy)
        Ixx_hat = signal.convolve2d( Ixx , self.box_filter(3) ,'same') 
        Iyy_hat = signal.convolve2d( Iyy , self.box_filter(3) ,'same') 
        Ixy_hat = signal.convolve2d( Ixy , self.box_filter(3) ,'same')
        k = 0.04
        detM = np.multiply(Ixx_hat,Iyy_hat) - np.multiply(Ixy_hat,Ixy_hat) 
        trM = Ixx_hat + Iyy_hat
        R = detM - (k * (trM**2))
        corners = R > 0.01*R.max()
        points_x = np.where(corners==True)[0].reshape(len(np.where(corners==True)[0]),-1)
        points_y = np.where(corners==True)[1].reshape(len(np.where(corners==True)[1]),-1)
        points = np.concatenate((points_x,points_y),axis = 1)
        return points
        
    def apply_sift(self):  
        
        image1 = cv2.imread(self.path2)
        image1 = cv2.resize(image1, (1000,1000), interpolation = cv2.INTER_AREA)
        gray1 = cv2.cvtColor(image1,cv2.COLOR_BGR2GRAY)
        gray_img1 = np.float32(gray1)
        points1 = self.harris(gray_img1)
        discreptors1_points ,discreptors1  = self.sift(points1 , gray_img1,[8,980])
        
        image2 = cv2.imread(self.path3)
        image2 = cv2.resize(image2, (300,300), interpolation = cv2.INTER_AREA)
        gray2 = cv2.cvtColor(image2,cv2.COLOR_BGR2GRAY)
        gray_img2 = np.float32(gray2)
        points2 = self.harris(gray_img2)
        discreptors2_points ,discreptors2 = self.sift(points2 , gray_img2,[8,280])
        
        return image1 , image2 , discreptors1_points , discreptors1 , discreptors2_points , discreptors2
        
    def sift(self, points,gray_img,limit):
        direction_list = [0, 45, 90, 135,180,225,270,315,360]
        gaussian_img  = convolve2d( gray_img , self.gaussian_kernel2d(7,1.5) , 'same', 'symm') 
        gauss_kernal = self.gaussian_kernel2d(std = 1.5, kernlen = 16)   
        gx,gy,magnitude,direction = self.sift_gradient(gaussian_img)

        ## orinatation & rotate 
        magnitude = self.patch_orientation(direction,magnitude,limit,points)

        ## Gaussian with patch sigma=1.5
        for x , y in points:
            if((x > limit[0] and x < limit[1] ) and ( y > limit[0] and y < limit[1])):
                lT_point_x = x - 7 
                lT_point_y = y - 7
                magnitude[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16] = magnitude[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16] * gauss_kernal

        ## Get the 4*4 parts & compute descreptors
        discreptors_points = []
        flag2 = 0
        for x , y in points:
            if((x > 15 and x < 980 ) and ( y > 15 and y < 980)):
                flag1 = 0

                for part in self.get_4parts(magnitude , x , y):
                    if (flag1 == 0):
                        hist_arr = np.histogram(part, bins = direction_list)[0]
                        flag1 = 1
                    else:
                        hist_arr = np.concatenate((hist_arr,np.array(np.histogram(part, bins = direction_list)[0])))
                if(flag2 == 0):
                    discreptors = hist_arr.reshape(128,-1)
                    flag2 = 1
                else:
                    discreptors = np.concatenate((discreptors ,hist_arr.reshape(128,-1)),axis = 1)
                discreptors_points.append((x,y)) 

        return discreptors_points,discreptors.T
        
    
    def matching(self):
        
        image1 , image2 , discreptors1_points , discreptors1 , discreptors2_points , discreptors2 = self.apply_sift()
        match_arr = np.zeros((discreptors1.shape[0],discreptors2.shape[0]))
        for i in range(discreptors1.shape[0]):
            for j in range(discreptors2.shape[0]):
                match_arr[i,j] = np.sum(np.square(discreptors1[i] - discreptors2[j]))
        ## SSD match      
        plot_img1 = np.copy(image1)
        plot_img2 = np.copy(image2)

        point_des_1 = np.where(match_arr < np.min(match_arr)+80)[0]
        point_des_2 = np.where(match_arr < np.min(match_arr)+80)[1]

        for index in point_des_1:
            cv2.circle(plot_img1, (discreptors1_points[index][1],discreptors1_points[index][0]), 2, [255,0,255], -1)
        cv2.imwrite("out/SSD_match_main_image.png", plot_img1)

        for index in point_des_2:
            cv2.circle(plot_img2, (discreptors2_points[index][1],discreptors2_points[index][0]), 1, [255,0,255], -1)
        cv2.imwrite("out/SSD_match_temp_image.png", plot_img2)
        
        out_main_ssd = cv2.imread("out/SSD_match_main_image.png")
        self.ui.w3.show()
        self.ui.w3.setImage(np.rot90(out_main_ssd,1))
        out_temp_ssd = cv2.imread("out/SSD_match_temp_image.png")
        self.ui.w4.show()
        self.ui.w4.setImage(np.rot90(out_temp_ssd,1))
        
        # Normalize cross correlation match 
        match_arr_norm = np.zeros((discreptors1.shape[0],discreptors2.shape[0]))
        for i in range(discreptors1.shape[0]):
            for j in range(discreptors2.shape[0]):
                num =((np.sum(np.square(self.norm_data(discreptors1[i]))))*(np.sum(np.square(self.norm_data(discreptors2[j])))))
                dom = (np.sum(self.norm_data(discreptors1[i]) * self.norm_data(discreptors2[j])))
                match_arr_norm[i,j] = num / dom if dom else 0
        plot_n1 = np.copy(image1)
        plot_n2 = np.copy(image2)
        
        point_norm_1 = np.where(match_arr_norm > np.max(match_arr_norm)-(4*pow(10,19)))[0]
        point_norm_2 = np.where(match_arr_norm > np.max(match_arr_norm)-(4*pow(10,19)))[1]
        
        print(point_norm_1.shape)
        
        for index in point_norm_1:
            cv2.circle(plot_n1, (discreptors1_points[index][1],discreptors1_points[index][0]), 2, [255,0,255], -1)
        cv2.imwrite("out/norm_match_main_image.png", plot_n1)

        for index in point_norm_2:
            cv2.circle(plot_n2, (discreptors2_points[index][1],discreptors2_points[index][0]), 1, [255,0,255], -1)
        cv2.imwrite("out/norm_match_temp_image.png", plot_n2)
        
        out_main_norm = cv2.imread("out/norm_match_main_image.png")
        self.ui.w5.show()
        self.ui.w5.setImage(np.rot90(out_main_norm,1))
        out_temp_norm = cv2.imread("out/norm_match_temp_image.png")
        self.ui.w6.show()
        self.ui.w6.setImage(np.rot90(out_temp_norm,1))
        
        
    def gaussian_kernel1d(self,kernlen=7, std=1.5):
        kernel1d = signal.gaussian(kernlen, std=std)
        kernel1d = kernel1d.reshape(kernlen, 1)
        return kernel1d / kernel1d.sum()

    def gaussian_kernel2d(self,kernlen=7, std=1.5):
        gkern1d = self.gaussian_kernel1d(kernlen,std)
        gkern2d = np.outer(gkern1d, gkern1d)
        return gkern2d

    def box_filter(self,f):
        kernel = np.ones((f,f)) / (f**2)
        return kernel
    
    def sift_gradient(self,img):
        dx = np.array([[-1,0,1],
                      [-2,0,2],
                      [-1,0,1]])
        dy = dx.T
        gx = signal.convolve2d( img , dx , boundary='symm', mode='same' )
        gy = signal.convolve2d( img , dy , boundary='symm', mode='same' )
        magnitude = np.sqrt( gx * gx + gy * gy )
        direction = np.rad2deg( np.arctan2( gy , gx )) % 360
        return gx,gy,magnitude,direction
    
    def get_4parts(self,img,x,y):
        parts = []
        counter = 0
        lT_point_x = x - 7 
        lT_point_y = y - 7
        for i in range(16):
            parts.append(img[lT_point_x+counter:lT_point_x+counter+4,lT_point_y+counter:lT_point_y+counter+4])
            counter = counter + 4 
        return parts
    
    def patch_orientation(self,direction,magnitude,limit,points):
        direction_list = [0, 45, 90, 135,180,225,270,315,360]
        points_orientation = []
        for x , y in points:
            if((x > limit[0] and x < limit[1] ) and ( y > limit[0] and y < limit[1])):
                lT_point_x = x - 7 
                lT_point_y = y - 7
                window = direction[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16]
                orientation = direction_list[np.argmax(np.histogram(window, bins = direction_list)[0])]
                magnitude[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16] = ndimage.rotate(magnitude   [lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16], orientation, reshape=False)
                return magnitude
            
    def norm_data(self,data):

        mean_data=np.mean(data)
        std_data=np.std(data, ddof=1)
        return (data-mean_data)/(std_data)
예제 #33
0
class Gui(QMainWindow):
    """ 
    Main GUI Class
    contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        print(self.dxlbus)
        port_num = self.dxlbus.port()
        print(port_num)
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        grip = DXL_XL(port_num, 6)
        wrst3 = DXL_XL(port_num, 7)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip)
        #self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2), 0)
        self.tp = TrajectoryPlanner(self.rexarm, self.kinect)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(self.execute)
        self.ui.btn_task1.clicked.connect(self.operation)
        self.ui.btn_task2.clicked.connect(self.record)
        self.ui.btn_task3.clicked.connect(self.opex)
        self.ui.btn_task4.clicked.connect(self.opplay)
        self.ui.btn_task5.clicked.connect(self.FK_check)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.setText("Block Detection")
        self.ui.btnUser2.clicked.connect(self.block_detect)
        self.ui.btnUser3.setText("Task 1")
        self.ui.btnUser3.clicked.connect(self.task_1)
        self.ui.btnUser4.setText("Task 2")
        self.ui.btnUser4.clicked.connect(self.task_2)
        self.ui.btnUser5.setText("Task 3")
        self.ui.btnUser5.clicked.connect(self.task_2)
        self.ui.btnUser6.setText("Traj Collect")
        self.ui.btnUser6.clicked.connect(self.collect_traj_data)
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)

    """ Slots attach callback functions to signals emitted from threads"""

    @pyqtSlot(QImage, QImage)
    def setImage(self, rgb_image, depth_image):
        if (self.ui.radioVideo.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
        if (self.ui.radioDepth.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
        self.ui.rdoutShoulderJC.setText(
            str("%+.2f" % ((joints[1] * R2D) + 90.0)))
        self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
        self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
        self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D)))

        if (len(joints) > 5):
            self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))

        else:
            self.ui.rdoutWrist3JC.setText(str("N.A."))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        # self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
        # self.ui.rdoutG.setText(str("%+.2f" % (pos[4])))
        # self.ui.rdoutP.setText(str("%+.2f" % (pos[5])))

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)

    """ Other callback functions attached to GUI elements"""

    def estop(self):
        self.rexarm.estop = True
        self.sm.set_next_state("estop")

    def execute(self):
        self.sm.set_next_state("execute")
        self.rexarm.pause(5)

    def operation(self):
        if path.exists("op_joints.csv"):
            os.remove("op_joints.csv")
        self.sm.set_next_state("operation")

    def opex(self):
        self.sm.set_next_state("idle")

    def opplay(self):
        self.sm.set_next_state("opplay")

    def FK_check(self):
        self.sm.set_next_state("FK_check")

    def block_detect(self):
        self.sm.set_next_state("block_detect")

    def task_1(self):
        self.sm.set_next_state("Task 1")

    def task_2(self):
        self.sm.set_next_state("Task 2")

    def task_3(self):
        self.sm.set_next_state("Task 3")

    def collect_traj_data(self):
        self.sm.set_next_state("Collect Traj")

    def record(self):
        if self.sm.current_state == "operation":
            rec_joints = self.rexarm.get_positions()
            strec = str(rec_joints)[1:-1] + "\n"
            if path.exists("op_joints.csv"):
                with open('op_joints.csv', 'a') as f:
                    f.write(strec)
            else:
                with open('op_joints.csv', 'w') as f:
                    f.write(strec)

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))

        self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))
        self.ui.rdoutWrist3.setText(str(self.ui.sldrWrist3.value()))
        self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
                                      self.rexarm.num_joints,
                                      update_now=False)
        self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
                                                 100.0,
                                                 update_now=False)
        joint_positions = np.array([
            self.ui.sldrBase.value() * D2R,
            self.ui.sldrShoulder.value() * D2R,
            self.ui.sldrElbow.value() * D2R,
            self.ui.sldrWrist.value() * D2R,
            self.ui.sldrWrist2.value() * D2R,
            self.ui.sldrWrist3.value() * D2R
        ])
        self.rexarm.set_positions(joint_positions, update_now=False)
        self.rexarm.gripper.set_position(
            np.array([self.ui.sldrGrip1.value() * D2R]))

    def directControlChk(self, state):
        if state == Qt.Checked:
            self.sm.set_next_state("manual")
            self.ui.SliderFrame.setEnabled(True)
        else:
            self.sm.set_next_state("idle")
            self.ui.SliderFrame.setEnabled(False)

    def trackMouse(self):
        """ 
        Mouse position presentation in GUI
        TODO: after implementing workspace calibration 
        display the world coordinates the mouse points to 
        in the RGB video image.
        """

        x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
        y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
        count = 0
        if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-,-)")
            # posesall = self.rexarm.get_positions()
            # endeffectorpos = FK_dh(posesall,0)
            # if path.exists("traj_fast_not_smooth.txt"):
            #     with open('traj_fast_not_smooth.txt','a') as f:
            #         f.write(str(self.rexarm.get_wrist_pose())+'\n')
            # else :
            #     with open('traj_fast_not_smooth.txt','w') as f:
            #         f.write("Traj Not Smooth\n")

        else:
            # Subtracting the X and Y distance corresponding to image origin frame to find cursor location with reference to imae frame
            x = x - MIN_X
            y = y - MIN_Y

            # Checking if the Kinect depth camera is producing output
            if (self.kinect.currentDepthFrame.any() != 0):
                z = self.kinect.currentDepthFrame[y][x]
                # Display the x,y (pixels), z (10 bit number) coordinates
                self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
                                                 (x, y, z))

                # Checking if the calibration has been done
                if self.sm.calibration_state() == True:

                    #############################################
                    # 		CAMERA FRAME TO DEPTH FRAME         #
                    #############################################

                    # Taking in the pixel values in camera frame and transforming to the kinect depth frame
                    pixel_value = np.array([x, y])
                    # Converting 10 bit depth to real distance using provided analytical function
                    z = self.kinect.currentDepthFrame[int(
                        pixel_value.item(1))][int(pixel_value.item(0))]
                    Z = 12.36 * np.tan(float(z) / 2842.5 + 1.1863)
                    # 95 cm marks the z location of the base plane wrt to the camera. Subtracting 95 to measure +z from the base plane
                    Z_modified = 95 - Z

                    #############################################
                    # 		CAMERA FRAME TO WORLD FRAME         #
                    #############################################

                    # Extracting the origin of the camera frame (Following 4 quadrant system)
                    pix_center = self.sm.pixel_center_loc()
                    # X and Y locations in the RGB space in pixels with (0,0) at the robot base center
                    x = x - pix_center.item(0)
                    y = pix_center.item(1) - y
                    # Taking in the pixel values in camera frame and transforming to the world frame
                    pixel_value = np.array([x, y])
                    pixel_value = np.transpose(pixel_value)
                    # Extracting the affine matrix computed during camera calibration
                    affine = self.sm.return_affine()
                    affine = affine[0:2, 0:2]
                    # World x,y location corresponding to iamge frame x,y location
                    world_value = np.matmul(affine, pixel_value)

                    #############################################
                    # 				SOLVE PNP					#
                    #############################################

                    # rot,trans = self.sm.return_solvepnp()
                    # cam = self.sm.return_intrinsic()

                    # xyz_c = Z*rgb_pt.T
                    # xyz_c = np.linalg.inv(cam).dot(xyz_c)
                    # xyz_c = xyz_c - trans
                    # world_value = xyz_c*rot
                    # -0.197*float(z) + 142.772
                    # self.kinect.detectBlocksInDepthImage()
                    # self.kinect.processVideoFrame()

                    # Displaying the World X,Y and Z coordinates in GUI
                    self.ui.rdoutMouseWorld.setText(
                        "(%.0f,%.0f,%.1f)" %
                        (world_value.item(0), world_value.item(1), Z_modified))

                    # self.sm.WC = [world_value.item(0)*10,world_value.item(1)*10,ZZ*10]
                else:
                    self.ui.rdoutMouseWorld.setText("(-,-,-)")

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        """ Change coordinates to image axis """
        self.kinect.last_click[0] = x - MIN_X
        self.kinect.last_click[1] = y - MIN_Y
        self.kinect.new_click = True
예제 #34
0
class MainWindow(QtGui.QMainWindow):
    def __init__(self, settings, application=None):
        '''
        UI Object that draws the main window.
        
        Listens to the following settings:
        - delay: 
        '''
        QtGui.QMainWindow.__init__(self)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        self._renderwidgets = [
            RenderWidget(self),
            RenderWidget(self),
            RenderWidget(self),
            RenderWidget(self),
        ]

        self.update()
        self.application = application

        self.settings = settings
        self.settings.settingChanged.connect(self.settingChanged)
        self.ui.delay.setText(str(settings.getSetting("delay")))
        self._loadScreens(
            self.settings.getSetting("layouts")[self.settings.getSetting(
                "selectedlayout")]["screen"])
        self._bindings = {}
        self._windowstate = None
        self._loadBindings(settings.getSetting('keybinding'))

    # end __init__()

    # signals
    incDelay = Signal()  #+>=. (while running)
    decDelay = Signal()  #-<,  (while running)
    incFrame = Signal()  #+>=. (while paused)
    decFrame = Signal()  #-<,  (while paused)
    help = Signal()  #F1
    edit = Signal()  #F2
    togglePlay = Signal()  #F7 <space>
    recordBuffer = Signal()  #F12

    # signals (new)
    processFrame = Signal(str, object)
    processGroup = Signal(str, object)
    resized = Signal()

    def resizeEvent(self, *args, **kwargs):
        returncode = QtGui.QMainWindow.resizeEvent(self, *args, **kwargs)
        self.resized.emit()
        return returncode

    @Slot(str, object)
    def settingChanged(self, name, value):
        if name == "delay":
            self.ui.delay.setText(str(value))
        elif name == "keybinding":
            self._loadBindings(value)
        elif name == "selectedlayout":
            self._loadScreens(
                self.settings.getSetting("layouts")[value]["screen"])

    ## reimplemented ##
    def showEvent(self, *args, **kwargs):
        if self._windowstate is None:
            self._windowstate = self.settings.getSetting("delayanalysisui")
            self.setGeometry(*self._windowstate['geometry'])
            if self._windowstate['mode'] == 'fullscreen':
                self.showFullScreen()
            elif self._windowstate['mode'] == 'maximised':
                self.showMaximized()
        return QtGui.QMainWindow.showEvent(self, *args, **kwargs)

    def closeEvent(self, *args, **kwargs):
        if self._windowstate is not None:
            self._windowstate = self.settings.getSetting("delayanalysisui")
            if self._windowstate['savemode'] == True:
                if self.windowState() == QtCore.Qt.WindowFullScreen:
                    self._windowstate['mode'] = "fullscreen"
                elif self.windowState() == QtCore.Qt.WindowMaximized:
                    self._windowstate['mode'] = "maximised"
                else:
                    self._windowstate['mode'] = "normal"
            if self._windowstate['savegeometry'] == True:
                self._windowstate['geometry'] = self.geometry().getRect()
            self.settings.setSetting('delayanalysisui', self._windowstate)

        return QtGui.QMainWindow.closeEvent(self, *args, **kwargs)

    ## support ##
    def _loadScreens(self, screens):
        self._screens = screens
        self._layouts = min(4, len(self._screens))
        #         print "Layouts: %s" % self._layouts
        #         print "Screens: %s" % self._screens
        for i in range(self._layouts):
            self._renderwidgets[i].setLayout(screens[i])

    def _loadBindings(self, bindings):
        '''
        Extracts the actual key ids from the string representations of them provided.
        
        @param bindings: dict containing keybindings {'<keyname>': [('<group>', '<function>', <optionargs>,...), ...]} 
        '''
        try:

            groups = {
                'core': None,
                'processframe': lambda m, c: self.processFrame.emit(m, c),
                'processgroup': lambda m, c: self.processGroup.emit(m, c),
            }
            corefuncs = {
                'quit': lambda a, b: self.close(),
                'play': lambda a, b: self.togglePlay.emit(),
                'incdelay': lambda a, b: self.incDelay.emit(),
                'decdelay': lambda a, b: self.decDelay.emit(),
                'incframe': lambda a, b: self.incFrame.emit(),
                'decframe': lambda a, b: self.decFrame.emit(),
                'fullscreen': lambda a, b: self.toggleFullScreen(),
                'edit': lambda a, b: self.edit.emit(),
                'help': lambda a, b: self.help.emit(),
            }

            errors = []
            self._bindings = {}
            for keystr, events in bindings.items():
                for event in events:
                    try:
                        # convert to integer
                        keyint = getattr(QtCore.Qt, "Key_%s" % str(keystr))

                        # get functions to perform tasks
                        func = groups[event[0]]
                        if func is None:
                            func = corefuncs[event[1]]

                        if keyint in self._bindings:
                            self._bindings[keyint].append(
                                (func, event[1], event[2:]))
                        else:
                            self._bindings[keyint] = [(func, event[1],
                                                       event[2:])]
                    except AttributeError:
                        errors.append("Key: %s" % keystr)
                    except KeyError:
                        errors.append("Group: %s" % keystr)

            if len(errors) > 0:
                print "Ignoring the following invalid key bindings (%s)" % (
                    ", ".join(errors))
        except:
            pass

    def toggleFullScreen(self):
        '''Toggles between fullscreen and normal mode'''
        if not self.isFullScreen():
            self.showFullScreen()
        else:
            self.showNormal()

    def keyPressEvent(self, e):
        '''Perform tasks for various key events'''

        if e.key() in self._bindings:
            for binding in self._bindings[e.key()]:
                func, action, config = binding
                func(action, config)
        else:
            print "key: %s" % (e.key(), )

    def renderFrameset(self, frameset):
        '''
        Renders the relevent frames from the frameset to screen
        '''
        for i in range(self._layouts):
            self._renderwidgets[i].process(frameset)

#     def updateView(self, vid, pixmap):
#         '''Updates a view to display given pixmap'''
#         self.ui.videoFrame.setPixmap(pixmap)
#         self.ui.videoFrame.setScaledContents(True)

    def setFrameId(self, frameid):
        self.ui.frameNum.setText(frameid)

    def setFrameRate(self, framerate):
        self.ui.frameRate.setText(framerate)
예제 #35
0
class Gui(QMainWindow):
    """ 
    Main GUI Class
    contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_XL(port_num, 1)
        shld = DXL_XL(port_num, 2)
        elbw = DXL_XL(port_num, 3)
        wrst = DXL_XL(port_num, 4)
        grip = DXL_XL(port_num, 5)
        """Objects Using Other Classes"""
        self.camera = PiCamera()
        self.rexarm = Rexarm((base, shld, elbw, wrst, grip))
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp)
        self.taskThread = TaskThread(self.sm)
        self.rgb_image = None
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_current_state, "calibrate"))

        self.ui.btn_task1.clicked.connect(
            partial(self.taskThread.set_task_num, 1))
        self.ui.btn_task2.clicked.connect(
            partial(self.taskThread.set_task_num, 2))
        self.ui.btn_task3.clicked.connect(
            partial(self.taskThread.set_task_num, 3))
        self.ui.btn_task4.clicked.connect(
            partial(self.taskThread.set_task_num, 4))

        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.camera)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.updateAprilTags.connect(self.updateAprilTags)
        self.videoThread.updateImage.connect(self.updateImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.taskThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)

    """ Slots attach callback functions to signals emitted from threads"""

    # Convert image to Qt image for display.
    def convertImage(self, image):
        qimg = QImage(image, image.shape[1], image.shape[0],
                      QImage.Format_RGB888)
        return QPixmap.fromImage(qimg)

    # TODO: Add more QImage in the list for visualization and debugging
    @pyqtSlot(list)
    def setImage(self, image_list):
        rgb_image = image_list[0]
        apriltag_image = image_list[1]
        block_image = image_list[2]
        if (self.ui.radioVideo.isChecked()):
            # self.ui.videoDisplay.setPixmap(self.convertImage(rgb_image))
            self.rgb_image = rgb_image
            self.sm.rgb_image = rgb_image

        if (self.ui.radioApril.isChecked()):
            self.ui.videoDisplay.setPixmap(self.convertImage(apriltag_image))
        if (self.ui.radioUsr1.isChecked()):
            self.ui.videoDisplay.setPixmap(self.convertImage(block_image))

    @pyqtSlot(list)
    def updateAprilTags(self, tags):
        # print('UPDATING APRIL TAGS')
        self.sm.tags = tags

    @pyqtSlot(list)
    def updateImage(self, image):
        # self.sm.image = image
        # detectColor(self.sm, self.sm.image)
        pass

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
        self.ui.rdoutShoulderJC.setText(str("%+.2f" % (joints[1] * R2D)))
        self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
        self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))

        if (len(joints) > 5):
            self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))

        else:
            self.ui.rdoutWrist3JC.setText(str("N.A."))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)

    """ Other callback functions attached to GUI elements"""

    def estop(self):
        self.rexarm.estop = True
        self.sm.set_current_state("estop")

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))

        self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))
        self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
                                      self.rexarm.num_joints,
                                      update_now=False)
        self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
                                                 100.0,
                                                 update_now=False)
        joint_positions = np.array([
            self.ui.sldrBase.value() * D2R,
            self.ui.sldrShoulder.value() * D2R,
            self.ui.sldrElbow.value() * D2R,
            self.ui.sldrWrist.value() * D2R,
            self.ui.sldrGrip1.value() * D2R
        ])
        self.rexarm.set_positions(joint_positions, update_now=False)

    def directControlChk(self, state):
        if state == Qt.Checked:
            self.sm.set_current_state("manual")
            self.ui.SliderFrame.setEnabled(True)
        else:
            self.sm.set_current_state("idle")
            self.ui.SliderFrame.setEnabled(False)

    def trackMouse(self):
        """ 
        Mouse position presentation in GUI
        TODO: Display the rgb/hsv value
        """

        x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
        y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
        if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-,-)")
            self.ui.rdoutRGB.setText("(-,-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))

            rgb = self.rgb_image[y, x]  # [r, g, b] of this pixel
            hsv_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2HSV)
            hsv = hsv_image[y, x]
            # self.ui.rdoutRGB.setText("({},{},{})".format(*rgb))
            # self.ui.rdoutRGB.setText("({},{},{})".format(*hsv))
            self.ui.rdoutRGB.setText(hue_to_classification(hsv[0]))
            print(hsv[0])

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
예제 #36
0
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        # wrst3 = DXL_XL(port_num, 6)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm([base, shld, elbw, wrst, wrst2], 0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        # Currently Changed into Pick & Place
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(self.execute)

        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.setText("Wait4Action")
        self.ui.btnUser2.clicked.connect(self.wait4action)
        self.ui.btnUser3.setText("Detection")
        self.ui.btnUser3.clicked.connect(self.detection)
        self.ui.btnUser4.setText("Pick")
        self.ui.btnUser4.clicked.connect(self.pick)
        self.ui.btnUser5.setText("Place")
        self.ui.btnUser5.clicked.connect(self.place)
        self.ui.btnUser6.setText("QuitTask")
        self.ui.btnUser6.clicked.connect(self.quittask)
        self.ui.btnUser7.setText("LoadCalibration")
        self.ui.btnUser7.clicked.connect(self.loadcalibration)

        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)
예제 #37
0
class Gui(QMainWindow):
    """ 
    Main GUI Class
    contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        # wrst3 = DXL_XL(port_num, 6)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm([base, shld, elbw, wrst, wrst2], 0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        # Currently Changed into Pick & Place
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(self.execute)

        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.setText("Wait4Action")
        self.ui.btnUser2.clicked.connect(self.wait4action)
        self.ui.btnUser3.setText("Detection")
        self.ui.btnUser3.clicked.connect(self.detection)
        self.ui.btnUser4.setText("Pick")
        self.ui.btnUser4.clicked.connect(self.pick)
        self.ui.btnUser5.setText("Place")
        self.ui.btnUser5.clicked.connect(self.place)
        self.ui.btnUser6.setText("QuitTask")
        self.ui.btnUser6.clicked.connect(self.quittask)
        self.ui.btnUser7.setText("LoadCalibration")
        self.ui.btnUser7.clicked.connect(self.loadcalibration)

        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)

    """ Slots attach callback functions to signals emitted from threads"""

    @pyqtSlot(QImage, QImage)
    def setImage(self, rgb_image, depth_image):
        if (self.ui.radioVideo.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
        if (self.ui.radioDepth.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
        self.ui.rdoutShoulderJC.setText(str("%+.2f" % ((joints[1] * R2D))))
        self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
        self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
        self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D)))

        if (len(joints) > 5):
            self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))

        else:
            self.ui.rdoutWrist3JC.setText(str("N.A."))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
        self.ui.rdoutG.setText(str("%+.2f" % (pos[4])))
        self.ui.rdoutP.setText(str("%+.2f" % (pos[5])))

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)

    """ Other callback functions attached to GUI elements"""

    def estop(self):
        self.rexarm.estop = True
        self.sm.set_next_state("estop")

    def create(self):
        self.sm.set_next_state("wait4record")

    def record(self):
        self.sm.set_next_state("record")

    def exit(self):
        self.sm.set_next_state("idle")

    def execute(self):
        print('callback func')
        self.sm.set_next_state("execute")

    # Pick & Place Relavant Functions
    def wait4action(self):
        self.sm.set_next_state("wait4action")

    def pick(self):
        self.sm.set_next_state("pick")

    def place(self):
        self.sm.set_next_state("place")

    def detection(self):
        self.sm.set_next_state("detection")

    def quittask(self):
        self.sm.set_next_state("quittask")

    def loadcalibration(self):
        # load the calibration result
        depth2rgb_affine = np.load("./data/depth2rgb_affine.npy")
        extrinsic = np.load("./data/extrinsic.npy")
        self.kinect.depth2rgb_affine = depth2rgb_affine
        self.kinect.extrinsic = extrinsic
        self.kinect.kinectCalibrated = True

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))

        self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
                                      self.rexarm.num_joints,
                                      update_now=False)
        self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
                                                 100.0,
                                                 update_now=False)
        joint_positions = np.array([
            self.ui.sldrBase.value() * D2R,
            self.ui.sldrShoulder.value() * D2R,
            self.ui.sldrElbow.value() * D2R,
            self.ui.sldrWrist.value() * D2R,
            self.ui.sldrWrist2.value() * D2R,
            self.ui.sldrWrist3.value() * D2R
        ])
        self.rexarm.set_positions(joint_positions, update_now=False)

    def directControlChk(self, state):
        if state == Qt.Checked:
            self.sm.set_next_state("manual")
            self.ui.SliderFrame.setEnabled(True)
        else:
            self.sm.set_next_state("idle")
            self.ui.SliderFrame.setEnabled(False)

    def trackMouse(self):
        """ 
        Mouse position presentation in GUI
        TODO: after implementing workspace calibration 
        display the world coordinates the mouse points to 
        in the RGB video image.
        """

        x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
        y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
        if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            if (self.kinect.currentDepthFrame.any() != 0):
                z = self.kinect.currentDepthFrame[y][x]
                self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
                                                 (x, y, z))

                if not self.kinect.kinectCalibrated:
                    self.ui.rdoutMouseWorld.setText("(-,-,-)")
                else:
                    w_x, w_y, w_z = self.kinect.toWorldCoord(x, y, z)
                    self.ui.rdoutMouseWorld.setText("(%.1f,%.1f,%.1f)" %
                                                    (w_x, w_y, w_z))
                    #

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        """ Change coordinates to image axis """
        self.kinect.last_click[0] = x - MIN_X
        self.kinect.last_click[1] = y - MIN_Y
        self.kinect.new_click = True
예제 #38
0
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Groups of ui commonents """
        self.error_lcds = [
            self.ui.rdoutBaseErrors, self.ui.rdoutShoulderErrors,
            self.ui.rdoutElbowErrors, self.ui.rdoutWristErrors,
            self.ui.rdoutWrist2Errors, self.ui.rdoutWrist3Errors
        ]
        self.joint_readouts = [
            self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC,
            self.ui.rdoutWristJC, self.ui.rdoutWrist2JC, self.ui.rdoutWrist3JC
        ]
        self.joint_slider_rdouts = [
            self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow,
            self.ui.rdoutWrist, self.ui.rdoutWrist2, self.ui.rdoutWrist3
        ]
        self.joint_sliders = [
            self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow,
            self.ui.sldrWrist, self.ui.sldrWrist2, self.ui.sldrWrist3
        ]
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm()
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        # Video
        self.ui.videoDisplay.setMouseTracking(True)
        self.ui.videoDisplay.mouseMoveEvent = self.trackMouse
        self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress
        # Buttons
        # Handy lambda function that can be used with Partial to only set the new state if the rexarm is initialized
        nxt_if_arm_init = lambda next_state: self.sm.set_next_state(
            next_state if self.rexarm.initialized else None)
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_init_arm.clicked.connect(self.initRexarm)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate'))
        self.ui.btn_exec.clicked.connect(self.execute)
        self.ui.btnUser2.setText("Reset Waypoints")
        self.ui.btnUser2.clicked.connect(self.reset)
        self.ui.btnUser3.setText("Add Waypoint")
        self.ui.btnUser3.clicked.connect(self.teach)
        self.ui.btnUser4.setText("Gripper Toggle")
        self.ui.btnUser4.clicked.connect(self.toggle_gripper)
        self.ui.btnUser5.setText("Add Color Points")
        self.ui.btnUser5.clicked.connect(self.colPoints)
        self.ui.btnUser6.setText("Click and Grab")
        self.ui.btnUser6.clicked.connect(self.click_grab)
        # Sliders
        for sldr in self.joint_sliders:
            sldr.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        # Direct Control
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        # Status
        self.ui.rdoutStatus.setText("Waiting for input")
        # Auto exposure
        self.ui.chkAutoExposure.stateChanged.connect(self.autoExposureChk)
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """Setup Threads"""
        # Rexarm runs its own thread

        # Video
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        # State machine
        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        # Display
        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.updateJointErrors.connect(self.updateJointErrors)
        self.displayThread.start()
예제 #39
0
class ScoreboardWindow(QtWidgets.QMainWindow):
    def __init__(self):
        super(ScoreboardWindow,self).__init__()

        self.t1=[]
        self.t2=[]
        self.t3=[]
        self.clock=0

        self.ui=Ui_MainWindow()
        self.ui.setupUi(self)
        # 设置表头内容
        header1=["iusse","read","execution","write"]
        # header2=["LD F6 34,R2",'LD F2 45,R3','MULT F0 F2,F4',\
        #     'SUBD F8 F6,F2','DIVD F10 F0,F6','ADDD F6 F8,F2']
        self.ui.table1.setHorizontalHeaderLabels(header1)
        # self.ui.table1.setVerticalHeaderLabels(header2)
        self.ui.table1.horizontalHeader().setSectionResizeMode(QtWidgets.QHeaderView.Stretch)

        header3=["busy","op","Fi","Fj",'Fk','Qj','Qk','Rj','Rk']
        header4=['Integer','Mult1','Mult2','Add','Divide']
        self.ui.table2.setHorizontalHeaderLabels(header3)
        self.ui.table2.setVerticalHeaderLabels(header4)
        self.ui.table2.horizontalHeader().setSectionResizeMode(QtWidgets.QHeaderView.Stretch)

        header5=['F0','F2','F4','F6','F8','F10']
        header6=['item']
        self.ui.table3.setHorizontalHeaderLabels(header5)
        self.ui.table3.setVerticalHeaderLabels(header6)
        self.ui.table3.horizontalHeader().setSectionResizeMode(QtWidgets.QHeaderView.Stretch)

        self.ui.button1.clicked.connect(self.click_next_step)
        self.ui.button_run.clicked.connect(self.run_all)
        self.ui.button_reset_current.clicked.connect(self.reset_current_run)
        self.ui.button_run_to.clicked.connect(self.run_to_step)
        self.ui.button_load.clicked.connect(self.load_instructions)
        self.ui.button_load_fromtxt.clicked.connect(self.load_instructions_fromtxt)

    def load_instructions_fromtxt(self):
        instructions = []
        # 初始化这个实例,设置一些基本属性
        dlg = QtWidgets.QFileDialog(directory='./',filter="*.txt")
        dlg.setFileMode(QtWidgets.QFileDialog.AnyFile)
        dlg.setFilter(QtCore.QDir.Files)
        # 当选择器关闭的时候
        if dlg.exec_():
            # 拿到所选择的的文本
            filenames = dlg.selectedFiles()
            # 读取文本内容设置到TextEdit当中来
            # print(filenames)#可以选择多个文件,返回的是一个list
            f = open(filenames[0], 'r')
            with f:
                x=f.readlines()
                # print(x)
                # .strip('\n').split('\n')
                for i in x:
                    i = i.strip()
                    if i:
                        i_list = i.split(',')
                        if len(i_list) == 4:
                            i_list.append(2)
                            i_list.append(0)
                            instructions.append(i_list)
                        elif len(i_list) == 5:
                            i_list[4] = int(i_list[4])
                            i_list.append(0)
                            instructions.append(i_list)
                        else:
                            instructions=[]
                            break
        if instructions:
            self.t1, self.t2, self.t3 = scoreboard(instructions)
            self.ui.table1.setRowCount(len(instructions))
            header_row=[(','.join(i[0:4]))for i in instructions]
            self.ui.table1.setVerticalHeaderLabels(header_row)
            QtWidgets.QMessageBox.information(self, '提示', '代码加载成功', QtWidgets.QMessageBox.Yes)
        else:
            QtWidgets.QMessageBox.warning(self, "警告", "指令读入错误或者文件中没有代码,请重新加载",
                                          QtWidgets.QMessageBox.Yes)

    def load_instructions(self):
        instructions=self.read_instructions()
        if instructions:
            self.t1, self.t2, self.t3 = scoreboard(instructions)
            self.ui.table1.setRowCount(len(instructions))
            header_row=[(','.join(i[0:4]))for i in instructions]
            self.ui.table1.setVerticalHeaderLabels(header_row)
            QtWidgets.QMessageBox.information(self, '提示', '代码加载成功', QtWidgets.QMessageBox.Yes)

    def run_to_step(self):
        if self.t1==[]:
            QtWidgets.QMessageBox.warning(self,'警告','当前面板没有代码,请点击加载代码',QtWidgets.QMessageBox.Yes)
        else:
            step=self.ui.step_to.text()
            if step=='':
                QtWidgets.QMessageBox.information(self, "提示", '请输入步数', QtWidgets.QMessageBox.Yes)
            else:
                step=int(step)
                if step>len(self.t1):
                    QtWidgets.QMessageBox.information(
                        self, "提示", "输入的步数"+str(step)+"大于当前代码运行的最大步数,请重新输入", QtWidgets.QMessageBox.Yes
                    )
                elif step==self.clock:
                    QtWidgets.QMessageBox.information(
                        self, "提示", "已经运行到"+str(step)+"步,请重新输入步数", QtWidgets.QMessageBox.Yes
                    )
                else:
                    self.clock=0
                    self.ui.table1.clearContents()
                    self.ui.table2.clearContents()
                    self.ui.table3.clearContents()
                    for i in range(step):
                        self.update_table()

    def reset_current_run(self):
        self.clock=0
        self.ui.table1.clearContents()
        self.ui.table2.clearContents()
        self.ui.table3.clearContents()
        self.ui.step_to.setText('')

    def read_instructions(self):
        result=[]
        x=self.ui.instructions.toPlainText().strip('\n').split('\n')
        for i in x:
            i=i.strip()
            i_list=i.split(',')
            if len(i_list)==4:
                i_list.append(2)
                i_list.append(0)
                result.append(i_list)
            elif len(i_list)==5:
                i_list[4]=int(i_list[4])
                i_list.append(0)
                result.append(i_list)
            else:
                QtWidgets.QMessageBox.warning(self,"警告","指令输入错误"+str(i),
                                              QtWidgets.QMessageBox.Yes)
                return []

        return result

    def run_all(self):
        if len(self.t1)==0:
            QtWidgets.QMessageBox.warning(self, '警告', '当前面板没有代码,请点击加载代码', QtWidgets.QMessageBox.Yes)
        else:
            clock=self.clock
            if clock>=len(self.t1):
                QtWidgets.QMessageBox.information(self, "提示", "代码运行完成,如果需要请重新开始",
                                                  QtWidgets.QMessageBox.Yes)
            else:
                while clock<len(self.t1):
                    self.update_table()
                    clock+=1

    def update_table(self):
        clock = self.clock
        if clock < len(self.t1):
            count = 0
            while count < len(self.t1[clock]):
                if self.t1[clock][count][0]:
                    a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][0]))
                    a.setTextAlignment(QtCore.Qt.AlignCenter)
                    self.ui.table1.setItem(count, 0, a)
                if self.t1[clock][count][1]:
                    a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][1]))
                    a.setTextAlignment(QtCore.Qt.AlignCenter)
                    self.ui.table1.setItem(count, 1, a)
                if self.t1[clock][count][2]:
                    a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][2]))
                    a.setTextAlignment(QtCore.Qt.AlignCenter)
                    self.ui.table1.setItem(count, 2, a)
                if self.t1[clock][count][3]:
                    a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][3]))
                    a.setTextAlignment(QtCore.Qt.AlignCenter)
                    self.ui.table1.setItem(count, 3, a)
                count += 1

            for i, item in enumerate(self.t2[clock][0]):
                a = QtWidgets.QTableWidgetItem(str(item))
                a.setTextAlignment(QtCore.Qt.AlignCenter)
                self.ui.table2.setItem(0, i, a)
            for i, item in enumerate(self.t2[clock][1]):
                a = QtWidgets.QTableWidgetItem(str(item))
                a.setTextAlignment(QtCore.Qt.AlignCenter)
                self.ui.table2.setItem(1, i, a)
            for i, item in enumerate(self.t2[clock][2]):
                a = QtWidgets.QTableWidgetItem(str(item))
                a.setTextAlignment(QtCore.Qt.AlignCenter)
                self.ui.table2.setItem(2, i, a)
            for i, item in enumerate(self.t2[clock][3]):
                a = QtWidgets.QTableWidgetItem(str(item))
                a.setTextAlignment(QtCore.Qt.AlignCenter)
                self.ui.table2.setItem(3, i, a)
            # if self.t2[clock]['Divide'][0]:
            for i, item in enumerate(self.t2[clock][4]):
                a = QtWidgets.QTableWidgetItem(str(item))
                a.setTextAlignment(QtCore.Qt.AlignCenter)
                self.ui.table2.setItem(4, i, a)

            a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F0']))
            a.setTextAlignment(QtCore.Qt.AlignCenter)
            self.ui.table3.setItem(0, 0, a)
            a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F2']))
            a.setTextAlignment(QtCore.Qt.AlignCenter)
            self.ui.table3.setItem(0, 1, a)
            a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F4']))
            a.setTextAlignment(QtCore.Qt.AlignCenter)
            self.ui.table3.setItem(0, 2, a)
            a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F6']))
            a.setTextAlignment(QtCore.Qt.AlignCenter)
            self.ui.table3.setItem(0, 3, a)
            a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F8']))
            a.setTextAlignment(QtCore.Qt.AlignCenter)
            self.ui.table3.setItem(0, 4, a)
            a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F10']))
            a.setTextAlignment(QtCore.Qt.AlignCenter)
            self.ui.table3.setItem(0, 5, a)

            self.ui.text3.setText(str(clock + 1))
            self.clock += 1
        else:
            QtWidgets.QMessageBox.information(self, "提示", "代码运行完成,如果需要请重新开始",
                                          QtWidgets.QMessageBox.Yes)

    def click_next_step(self):
        if self.t1==[]:
            QtWidgets.QMessageBox.warning(self,'警告','当前面板没有代码,请点击加载代码',QtWidgets.QMessageBox.Yes)
        else:
            self.update_table()
예제 #40
0
class Gui(QMainWindow):
    """ 
    Main GUI Class
    contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        gripper1 = DXL_XL(port_num, 6)
        gripper2 = DXL_XL(port_num, 7)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm(
            (base, shld, elbw, wrst, wrst2, gripper1, gripper2), 0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(self.execute)
        self.ui.btn_teach.clicked.connect(self.teach)
        self.ui.btn_repeat.clicked.connect(self.repeat)
        self.ui.btn_rls.clicked.connect(self.rls)
        self.ui.btn_grab.clicked.connect(self.grab)
        self.ui.btn_drop_off.clicked.connect(self.drop_off)
        self.ui.btn_move.clicked.connect(self.move)

        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.clicked.connect(
            partial(self.sm.set_next_state, "event_1"))
        self.ui.btnUser3.clicked.connect(
            partial(self.sm.set_next_state, "event_2"))
        self.ui.btnUser4.clicked.connect(
            partial(self.sm.set_next_state, "event_3"))
        self.ui.btnUser5.clicked.connect(
            partial(self.sm.set_next_state, "event_4"))
        self.ui.btnUser6.clicked.connect(
            partial(self.sm.set_next_state, "event_5"))
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)

    """ Slots attach callback functions to signals emitted from threads"""

    @pyqtSlot(QImage, QImage)
    def setImage(self, rgb_image, depth_image):
        if (self.ui.radioVideo.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
        if (self.ui.radioDepth.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
        self.ui.rdoutShoulderJC.setText(str("%+.2f" % ((joints[1] * R2D))))
        self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
        self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
        self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D)))

        if (len(joints) > 5):
            self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))

        else:
            self.ui.rdoutWrist3JC.setText(str("N.A."))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
        self.ui.rdoutG.setText(str("%+.2f" % (pos[4])))
        self.ui.rdoutP.setText(str("%+.2f" % (pos[5])))

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)

    """ Other callback functions attached to GUI elements"""

    def move(self):
        self.sm.set_next_state("move")

    def grab(self):
        self.sm.set_next_state("grab")

    def drop_off(self):
        self.sm.set_next_state("drop_off")

    def estop(self):
        self.rexarm.estop = True
        self.sm.set_next_state("estop")

    def execute(self):
        self.sm.set_next_state("execute")

    def teach(self):
        self.sm.set_next_state("teach")

    def repeat(self):
        self.sm.set_next_state("repeat")

    def rls(self):
        self.sm.set_next_state("rls")

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))

        self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
                                      self.rexarm.num_joints,
                                      update_now=False)
        self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
                                                 100.0,
                                                 update_now=False)
        joint_positions = np.array([
            self.ui.sldrBase.value() * D2R,
            self.ui.sldrShoulder.value() * D2R,
            self.ui.sldrElbow.value() * D2R,
            self.ui.sldrWrist.value() * D2R,
            self.ui.sldrWrist2.value() * D2R
        ])
        self.rexarm.set_positions(joint_positions, update_now=False)

    def directControlChk(self, state):
        if state == Qt.Checked:
            self.sm.set_next_state("manual")
            self.ui.SliderFrame.setEnabled(True)
        else:
            self.sm.set_next_state("idle")
            self.ui.SliderFrame.setEnabled(False)

    def trackMouse(self):
        """ 
        Mouse position presentation in GUI
        TODO: after implementing workspace calibration 
        display the world coordinates the mouse points to 
        in the RGB video image.
        """

        x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
        y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
        if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            if (self.kinect.currentDepthFrame.any() != 0):
                d = self.kinect.currentDepthFrame[y][x]  # depth value
                d = np.clip(d, 0, 2**10 - 1)
                Zc = 0.1236 * math.tan(d / 2842.5 + 1.1863)
                # print Zc
                # print('----control station intrinsic_matrix')
                # print self.kinect.intrinsic_matrix
                XYZ_camera = Zc * np.matmul(
                    np.linalg.inv(self.kinect.intrinsic_matrix),
                    np.array([x, y, 1]))
                # print('----control station co_eff_camera_2_world')
                # print self.kinect.co_eff_camera_2_world
                W = np.matmul(self.kinect.co_eff_camera_2_world,
                              np.append(XYZ_camera, 1))
                self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
                                                 (x, y, d))

                # W = np.matmul(self.sm.coeff_rgb_2_world, np.array([x, y, 1]))
                #linear fitting
                # d = 2047-z
                # 2047 - 718 --> 0
                # 2047 - 705 --> 38
                # 2047 - 688 --> 38*2
                # 2047 - 668 --> 38*3
                # 2047 - 646 --> 38*4
                # 2047 - 624 --> 38*5
                # 2047 - 598 --> 38*6
                # 2047 - 570 --> 38*7
                # 2047 - 538 --> 38*8
                # 2047 - 501 --> 38*9
                # 2047 - 462 --> 38*10
                # need better calibration function

                # W[2] =  (8.00506778e-06)*d**3-(3.79099906e-02)*d**2 + (6.08296089e+01)*d - (3.26712433e+04)
                #W[2] = (1.46565657e+00)*d - (1.91508256e+03)
                #W[2] = (-4.15178471e-08)*d**4 + (2.49769770e-04)*d**3 - (5.65159066e-01)*d**2 + (5.71205622e+02)*d - (2.17696573e+05)
                self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" %
                                                (W[0], W[1], W[2]))

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        """ Change coordinates to image axis """
        self.kinect.last_click[0] = x - MIN_X
        self.kinect.last_click[1] = y - MIN_Y
        self.kinect.new_click = True
        #print(self.kinect.last_click)
        d = self.kinect.currentDepthFrame[self.kinect.last_click[1]][
            self.kinect.last_click[0]]  # depth value
        d = np.clip(d, 0, 2**10 - 1)
        Zc = 0.1236 * math.tan(d / 2842.5 + 1.1863)
        XYZ_camera = Zc * np.matmul(
            np.linalg.inv(self.kinect.intrinsic_matrix),
            np.array([self.kinect.last_click[0], self.kinect.last_click[1], 1
                      ]))
        W = np.matmul(self.kinect.co_eff_camera_2_world,
                      np.append(XYZ_camera, 1))
        self.sm.world_mouse = deepcopy(W[:3])
예제 #41
0
 def __init__(self):
     super(TranslateText, self).__init__()
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)
     self.init_UI()
예제 #42
0
class QMain(QMainWindow):
    def __init__(self):
        super(QMain, self).__init__()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        # 过滤掉未获取到IP地址的适配器
        self.ip_address_list = sorted(
            [a for a in self.get_all_ip_address() if a[:3] != '169'])
        self.ui.listenaddress.addItems(self.ip_address_list)
        self.ui.connectaddress.addItems(self.ip_address_list)
        self.update_network_list = MySignals()
        # 信号连接槽,需要更新端口转发地址时发送信号触发函数调用
        self.update_network_list.signal.connect(self.get_all_network)  # noqa
        self.get_all_network()
        self.check_service_status()

    @staticmethod
    def get_all_ip_address():
        """
        获取本机所有的IP地址

        :return: IP地址列表
        """
        return [
            ifaddresses(a)[AF_INET][0]['addr'] for a in interfaces()
            if (AF_INET in ifaddresses(a))
        ]

    def start(self):
        """
        运行程序

        :return: None
        """
        network = self.ui.network.currentText()
        listen_address = self.ui.listenaddress.currentText()
        listen_port = self.ui.listenport.text()
        connect_address = self.ui.connectaddress.currentText()
        connect_port = self.ui.connectport.text()
        if self.check_ip_port(listen_address, listen_port, connect_address,
                              connect_port):
            command = 'netsh interface portproxy add %s listenaddress=%s listenport=%s connectaddress=%s connectport=%s'  # noqa
            command = command % (network, listen_address, listen_port,
                                 connect_address, connect_port)
            self.run_and_check_result(command)
            # 执行命令结束发出信号更新network_list
            self.update_network_list.signal.emit(True)  # noqa

    def check_ip_port(self, listen_address, listen_port, connect_address,
                      connect_port):
        """
        检测IP地址和端口是否合法

        :param listen_address: 监听地址
        :param listen_port: 监听端口
        :param connect_address: 连接地址
        :param connect_port: 连接端口
        :return: bool
        """
        if not (listen_address and listen_port and connect_address
                and connect_port):
            text = ''
            if not listen_address:
                text += '监听地址,'
            if not listen_port:
                text += '监听端口,'
            if not connect_address:
                text += '连接地址,'
            if not connect_port:
                text += '连接端口,'
            QMessageBox.warning(self, '提示', f'{text[:-1]}不能为空!',
                                QMessageBox.Ok, QMessageBox.Ok)
            return False
        rule_ipaddress = re.compile(
            r'^((2(5[0-5]|[0-4]\d))|[0-1]?\d{1,2})(\.((2(5[0-5]|[0-4]\d))|[0-1]?\d{1,2})){3}$'
        )
        rule_port = re.compile(
            r'^[1-9]$|(^[1-9][0-9]$)|(^[1-9][0-9][0-9]$)|(^[1-9][0-9][0-9][0-9]$)|(^[1-6][0-9][0-9][0-9][0-9]$)'
        )
        if rule_ipaddress.match(listen_address) and rule_ipaddress.match(connect_address) and \
                rule_port.match(listen_port) and rule_port.match(connect_port):
            if 1 <= int(listen_port) <= 65535 and 1 <= int(
                    connect_port) <= 65535:
                return True
            else:
                if not (1 <= int(listen_port) <= 65535):
                    QMessageBox.warning(self, '提示', '监听端口不合法!', QMessageBox.Ok,
                                        QMessageBox.Ok)
                    return False
                if not (1 <= int(connect_port) <= 65535):
                    QMessageBox.warning(self, '提示', '连接端口不合法!', QMessageBox.Ok,
                                        QMessageBox.Ok)
                    return False
        else:
            text = ''
            if not rule_ipaddress.match(listen_address):
                text += '监听地址,'
            if not rule_ipaddress.match(connect_address):
                text += '连接地址,'
            if not rule_port.match(listen_port):
                text += '监听端口,'
            if not rule_port.match(connect_port):
                text += '连接端口,'
            QMessageBox.warning(self, '提示', f'{text[:-1]}不合法!', QMessageBox.Ok,
                                QMessageBox.Ok)
            return False

    def run_and_check_result(self, command):
        """
        运行程序并且检测返回结果

        :param command: 需要运行的命令
        :return: None
        """
        stdout_data, stderr_data = subprocess.Popen(
            command,
            stdout=subprocess.PIPE,
            stderr=subprocess.PIPE,
            stdin=subprocess.PIPE,  # GUI程序中记得设置标准输入
            shell=True).communicate()
        stdout_data = stdout_data.decode('gbk')
        stderr_data = stderr_data.decode('gbk')
        if stdout_data == '请求的操作需要提升(作为管理员运行)。\n\r\n':
            QMessageBox.warning(self, '提示', '请使用管理员权限运行程序!', QMessageBox.Ok,
                                QMessageBox.Ok)
        elif stdout_data == '\r\n' and stderr_data == '':
            QMessageBox.information(self, '提示', 'OK!', QMessageBox.Ok,
                                    QMessageBox.Ok)
        else:
            QMessageBox.information(self, '提示',
                                    f'{stdout_data}\r\n{stderr_data}',
                                    QMessageBox.Ok, QMessageBox.Ok)

    def edit_item(self, item: QListWidgetItem):
        """
        编辑IP和端口

        :param item: 选中的item
        :return: None
        """
        src, des = item.text().split(' -> ')
        src_ip, src_port = src.split(':')
        des_ip, des_port = des.split(':')
        self.ui.listenaddress.setCurrentText(src_ip)
        self.ui.listenport.setText(src_port)
        self.ui.connectaddress.setCurrentText(des_ip)
        self.ui.connectport.setText(des_port)

    def get_all_network(self):
        """
        获取所有的网络适配器中包含的地址

        :return: None
        """
        # 先清空之前的值,再更新
        self.ui.network_list.clear()
        parent = r'(\S+?)\s+?(\d+?) \s+?(\S+?)\s+?(\d+?)\s+?'
        get_ip_port = re.compile(parent, re.VERBOSE)
        command = 'netsh interface portproxy show all'  # noqa
        stdout_data, stderr_data = subprocess.Popen(command,
                                                    stdout=subprocess.PIPE,
                                                    stderr=subprocess.PIPE,
                                                    stdin=subprocess.PIPE,
                                                    shell=True).communicate()
        stdout_data, stderr_data = stdout_data.decode(
            'gbk'), stderr_data.decode('gbk')
        result = re.findall(get_ip_port, stdout_data)
        for a in result:
            self.ui.network_list.addItem(f'{a[0]}:{a[1]} -> {a[2]}:{a[3]}')

    def my_listwidget_context(self, position):  # noqa
        """
        右键菜单

        :param position: 选中的条目索引
        :return: None
        """
        # 只有在选中item时才会出现菜单
        if self.ui.network_list.itemAt(position):
            menu = QMenu()
            del_act = QAction("删除", self)
            del_act.triggered.connect(self.del_network)  # noqa
            menu.addAction(del_act)
            menu.exec_(self.ui.network_list.mapToGlobal(position))

    def del_network(self):
        """
        删除数据

        :return: None
        """
        ip, port = self.ui.network_list.currentItem().text().split(
            ' -> ')[0].split(':')
        command = f'netsh interface portproxy delete {self.ui.network.currentText()} listenaddress={ip} listenport={port}'  # noqa
        self.run_and_check_result(command)
        # 执行命令结束发出信号更新network_list
        self.update_network_list.signal.emit(True)  # noqa

    def check_service_status(self):
        """
        检查端口转发所需服务状态

        :return: None
        """
        command = 'sc query iphlpsvc'  # noqa
        stdout_data, stderr_data = subprocess.Popen(command,
                                                    stdin=subprocess.PIPE,
                                                    stdout=subprocess.PIPE,
                                                    stderr=subprocess.PIPE,
                                                    shell=True).communicate()
        stdout_data, stderr_data = stdout_data.decode(
            'gbk'), stderr_data.decode('gbk')
        print(stdout_data, stderr_data)
        if stdout_data:
            service_status = re.compile(r'STATE\s+?:\s+?(\d)').search(
                stdout_data).group(1)
            if service_status == '4':
                self.setWindowTitle(f'{self.windowTitle()}  服务状态:运行')
                return
            if service_status == '1':
                self.setWindowTitle(f'{self.windowTitle()}  服务状态:停止')
                return
            else:
                self.setWindowTitle(f'{self.windowTitle()}  服务状态:未知')
                return
        else:
            self.setWindowTitle(f'{self.windowTitle()}  服务状态:未知')
예제 #43
0
    def __init__(self,parent=None):
        QWidget.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self,True)

        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        """
        467TODO:
        Based on motors we are using, modify ports and definition of rexarm
        
        Assume only use shld, elbw, wrst for now
        """
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        #wrst2 = DXL_AX(port_num, 1)

        """Objects Using Other Classes"""
        self.kinect = None #Kinect()
        #self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2),0)
        self.rexarm = Rexarm((base,shld,elbw, wrst),0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
    
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")

        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)

        """initalize rexarm"""
        self.rexarm.initialize()

        """Setup Threads"""
        # self.videoThread = VideoThread(self.kinect)
        # self.videoThread.updateFrame.connect(self.setImage)        
        # self.videoThread.start()

        
        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()
        

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
        self.displayThread.start()

        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)
예제 #44
0
import sys
from PyQt5.QtCore import *
from PyQt5.QtGui import *
from PyQt5.QtWidgets import *
from ui import Ui_MainWindow

QApplication.setAttribute(Qt.AA_EnableHighDpiScaling)
app = QApplication(sys.argv)

window = QMainWindow()

Ui_MainWindow().setupUi(window)
window.show()

sys.exit(app.exec_())
예제 #45
0
    def __init__(self,parent=None):
        QWidget.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self,True)

        """dynamixel bus -- add other motors here"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 0)
        shld = DXL_MX(port_num, 1)
        elbw = DXL_MX(port_num, 2)
        wrst = DXL_AX(port_num, 3)
        wrst2 = DXL_XL(port_num, 4)
        grip = DXL_XL(port_num, 5)

        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,grip),0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
    
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state, "execute"))
        self.ui.btn_cali.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
        # self.ui.btnUser1.setText("Calibrate")
        # self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
        
        self.ui.btnUser1.setText("Teach and Repeat")
        self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "teachNRepeat"))
        self.ui.btnUser2.setText("Record Waypoints")
        self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "recordWaypoint"))
        self.ui.btnUser3.setText("Play")
        self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))

        self.ui.btnUser4.setText("Block Detection Start")
        self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "blockDetectionStart"))
        self.ui.btnUser5.setText("Block Message")
        self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "blockMessage"))
        self.ui.btnUser6.setText("Block Detection End")
        self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "blockDetectionEnd"))

        self.ui.btnUser7.setText("Click and Grab")
        self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "clickNGrab"))
        self.ui.btnUser8.setText("Pick n' Place")
        self.ui.btnUser8.clicked.connect(partial(self.sm.set_next_state, "pickNPlace"))
        self.ui.btnUser9.setText("Pick n' Stack")
        self.ui.btnUser9.clicked.connect(partial(self.sm.set_next_state, "pickNStack"))
        self.ui.btnUser10.setText("Line 'em Up!")
        self.ui.btnUser10.clicked.connect(partial(self.sm.set_next_state, "lineUp"))
        self.ui.btnUser11.setText("Stack 'em High!")
        self.ui.btnUser11.clicked.connect(partial(self.sm.set_next_state, "stackHigh"))
        self.ui.btnUser12.setText("Pyramid Builder!")
        self.ui.btnUser12.clicked.connect(partial(self.sm.set_next_state, "buildPyramid"))
        


        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")

        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)

        """initalize rexarm"""
        self.rexarm.initialize()

        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)        
        self.videoThread.start()

        
        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()
        

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
        self.displayThread.start()

        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)
예제 #46
0
class MainWindow:
    def __init__(self, studentIDS):
        global prereq
        global isPrereqFor
        isPrereqFor = {}
        prereq = {}
        self.main_win = QMainWindow()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self.main_win)
        self.ui.listWidget.addItems(studentIDS)
        self.ui.pushButton.clicked.connect(self.pushButton_click)
        prereq_file = pd.read_excel(PREREQ_LIST, skiprows=1)
        self.formPrereq()

    def show(self):
        self.main_win.show()

    def pushButton_click(self):
        global student_id
        student_id = self.ui.listWidget.currentItem().text().split('-')[1]
        global student_name
        student_name = self.ui.listWidget.currentItem().text().split('-')[0]
        global main_win
        main_win = SecondPage()
        main_win.show()

    def formPrereq(self):
        prereq_file = pd.read_excel(PREREQ_LIST, skiprows=1)
        or_and = ""
        for i in range(len(prereq_file['Subject'])):
            course1 = prereq_file['Subject'][i].strip(
            ) + prereq_file['Catalog'][i].strip()

            if str(prereq_file['preq1 subject'][i]) == 'nan':
                continue
            l = prereq_file['preq1 subject'][i].strip(
            ) + prereq_file['preq1 catalog'][i].strip()
            prereq[course1] = [l]
            if l not in isPrereqFor:
                isPrereqFor[l] = [course1]
            else:
                isPrereqFor[l].append(course1)
#######################################################################
            if str(prereq_file['preq2 sub'][i]) == 'nan':
                continue
            l = prereq_file['preq2 sub'][i].strip(
            ) + prereq_file['preq2 cat'][i].strip()
            prereq[course1].append(l)
            if l not in isPrereqFor:
                isPrereqFor[l] = [course1]
            else:
                isPrereqFor[l].append(course1)
#######################################################################

            if str(prereq_file['preq3 no'][i]) == 'nan':
                continue
            l = prereq_file['preq3 no'][i].strip(
            ) + prereq_file['preq3 cat'][i].strip()
            prereq[course1].append(l)
            if l not in isPrereqFor:
                isPrereqFor[l] = [course1]
            else:
                isPrereqFor[l].append(course1)


#######################################################################

            if str(prereq_file['preq4 no'][i]) == 'nan':
                continue
            l = prereq_file['preq4 no'][i].strip(
            ) + prereq_file['preq4 cat'][i].strip()
            prereq[course1].append(l)
            if l not in isPrereqFor:
                isPrereqFor[l] = [course1]
            else:
                isPrereqFor[l].append(course1)
예제 #47
0
    def __init__(self, parent=None):

        QtGui.QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Main Variables Using Other Classes"""
        self.rex = Rexarm(self.ui)
        self.video = Video(cv2.VideoCapture(0))
        """
        Zhentao added Here.
        Initialize the statemachine.
        """

        self.statemanager = StateManager(self.rex, self.video)
        """ Other Variables """
        self.last_click = np.float32([0, 0])
        """ Set GUI to track mouse """
        QtGui.QWidget.setMouseTracking(self, True)
        """ 
        Video Function 
        Creates a timer and calls play() function 
        according to the given time delay (27mm) 
        """
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.play)
        self._timer.start(27)
        """ 
        LCM Arm Feedback
        Creates a timer to call LCM handler continuously
        No delay implemented. Reads all time 
        """
        self._timer2 = QtCore.QTimer(self)
        self._timer2.timeout.connect(self.rex.get_feedback)
        self._timer2.start()
        """ 
        Connect Sliders to Function
        LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI 
        """
        ## TODO: IMPLEMENT GRIP VALUE CONTROLS ##
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        """ Commands the arm as the arm initialize to 0,0,0,0 angles """
        self.sliderChange()
        """ Connect Buttons to Functions 
        LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
        """
        #This is needed.
        self.ui.btnUser1.setText("Affine Calibration")
        self.ui.btnUser1.clicked.connect(self.affine_cal)

        self.ui.btnUser2.setText("SM Test")
        self.ui.btnUser2.clicked.connect(self.iTestSM)

        self.ui.btnUser3.setText("Reset Position")
        self.ui.btnUser3.clicked.connect(self.rex.iResetPosition)

        self.ui.btnUser4.setText("Replay")
        self.ui.btnUser4.clicked.connect(self.iReplay)
        """
        self.ui.btnUser4.setText("OpenGripper")
        self.ui.btnUser4.clicked.connect(self.iTestGripperOpen)

        self.ui.btnUser5.setText("CloseGripper")
        self.ui.btnUser5.clicked.connect(self.iTestGripperClose)

        
        """
        """
예제 #48
0
class Gui(QMainWindow):
    """ 
    Main GUI Class
    contains the main function and interfaces between 
    the GUI and functions
    """
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        wrst3 = DXL_XL(port_num, 6)
        grip = DXL_XL(port_num, 7)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.setText("Waypoint")
        self.ui.btnUser2.clicked.connect(
            partial(self.sm.set_next_state, "waypoint"))
        self.ui.btnUser3.setText("Teach")
        self.ui.btnUser3.clicked.connect(
            partial(self.sm.set_next_state, "teach"))
        self.ui.btnUser4.setText("Record")
        self.ui.btnUser4.clicked.connect(
            partial(self.sm.set_next_state, "record"))
        self.ui.btnUser5.setText("Repeat")
        self.ui.btnUser5.clicked.connect(
            partial(self.sm.set_next_state, "repeat"))
        self.ui.btn_task1.setText("Pick n Stack")
        self.ui.btn_task1.clicked.connect(partial(self.sm.set_next_state,
                                                  "IK"))
        self.ui.btnUser6.setText("Click and Place")
        self.ui.btnUser6.clicked.connect(
            partial(self.sm.set_next_state, "clickgrab"))
        self.ui.btnUser7.setText("Open Gripper")
        self.ui.btnUser7.clicked.connect(
            partial(self.sm.set_next_state, "open_gripper"))
        self.ui.btnUser8.setText("Close Gripper")
        self.ui.btnUser8.clicked.connect(
            partial(self.sm.set_next_state, "close_gripper"))
        self.ui.btnUser9.setText("Incline")
        self.ui.btnUser9.clicked.connect(
            partial(self.sm.set_next_state, "incline"))
        self.ui.btn_task2.setText("Line Em Up")
        self.ui.btn_task2.clicked.connect(
            partial(self.sm.set_next_state, "color_sort"))
        self.ui.btn_task3.setText("Stack Em High")
        self.ui.btn_task3.clicked.connect(
            partial(self.sm.set_next_state, "color_stack"))
        self.ui.btn_task4.setText("Block Mover")
        self.ui.btn_task4.clicked.connect(
            partial(self.sm.set_next_state, "square"))
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
        #self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)

    """ Slots attach callback functions to signals emitted from threads"""

    @pyqtSlot(QImage, QImage, QImage)
    def setImage(self, rgb_image, depth_image, gray_depth_image):
        if (self.ui.radioVideo.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
        if (self.ui.radioDepth.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))
        if (self.ui.radioUsr1.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(gray_depth_image))

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
        self.ui.rdoutShoulderJC.setText(
            str("%+.2f" % ((joints[1] * R2D) + 90.0)))
        self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
        self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
        self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D)))

        if (len(joints) > 5):
            self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))

        else:
            self.ui.rdoutWrist3JC.setText(str("N.A."))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
        self.ui.rdoutG.setText(str("%+.2f" % (pos[4])))
        self.ui.rdoutP.setText(str("%+.2f" % (pos[5])))

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)

    """ Other callback functions attached to GUI elements"""

    def estop(self):
        self.rexarm.estop = True
        self.sm.set_next_state("estop")

    #def waypoint(self):
    # self.sm.set_next_state("waypoint")

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))

        self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))
        self.ui.rdoutWrist3.setText(str(self.ui.sldrWrist3.value()))
        #self.ui.sldrGrip1.setText(str(self.ui.sldrGrip1.value()))
        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
                                      self.rexarm.num_joints,
                                      update_now=False)
        self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
                                                 100.0,
                                                 update_now=False)
        joint_positions = np.array([
            self.ui.sldrBase.value() * D2R,
            self.ui.sldrShoulder.value() * D2R,
            self.ui.sldrElbow.value() * D2R,
            self.ui.sldrWrist.value() * D2R,
            self.ui.sldrWrist2.value() * D2R,
            self.ui.sldrWrist3.value() * D2R
        ])
        self.rexarm.set_positions(joint_positions, update_now=False)

    def directControlChk(self, state):
        if state == Qt.Checked:
            self.sm.set_next_state("manual")
            self.ui.SliderFrame.setEnabled(True)
        else:
            self.sm.set_next_state("idle")
            self.ui.SliderFrame.setEnabled(False)

    def trackMouse(self):
        """ 
        Mouse position presentation in GUI
        TODO: after implementing workspace calibration 
        display the world coordinates the mouse points to 
        in the RGB video image.
        """

        x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
        y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
        if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            if (self.kinect.currentDepthFrame.any() != 0):
                z = self.kinect.currentDepthFrame[y][x]
                self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
                                                 (x, y, z))

                if self.kinect.calibrated == True:
                    mouse = np.array([[x], [y], [1]], dtype=float)
                    depth = (0.1236 * np.tan((z) / 2842.5 + 1.1863))

                    camera = depth * np.matmul(inv(self.kinect.camera_matrix),
                                               mouse)
                    points = np.array([[camera[0]], [camera[1]], [1]])
                    world = np.matmul(self.kinect.affineB.astype(float),
                                      points.astype(float))

                    #world = np.matmul(self.kinect.affineB.astype(float),mouse)

                    #depth = -2.5333*z + 1836.7
                    board_depth = 0.1236 * np.tan(718 / 2842.5 + 1.1863)

                    #depth = (1-depth/board_depth)*1000
                    depth = (board_depth - depth) * 1000
                    """
                    correction_factor = 0.000626
                    world[0]=world[0]*(1-correction_factor*depth)
                    world[1]=world[1]*(1-correction_factor*depth)
                    """
                    #self.ui.rdoutMouseWorld.setText("(%3.2f,%3.2f,%3.2f)" % (camera[0],camera[1],camera[2]))
                    self.ui.rdoutMouseWorld.setText(
                        "(%.0f,%.0f,%.0f)" % (world[0], world[1], depth))

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        """ Change coordinates to image axis """
        self.kinect.last_click[0] = x - MIN_X
        self.kinect.last_click[1] = y - MIN_Y
        self.kinect.new_click = True
예제 #49
0
class mywindow(QtWidgets.QMainWindow):

    # включение базы данных
    db = Databass()

    def __init__(self):
        super(mywindow, self).__init__()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tableWidget.setColumnCount(5)
        self.ui.tableWidget.setHorizontalHeaderLabels(
            ("ID", "Название", "Режисер", "год выпуска", "Страна"))

        # добавление значений в комбо боксы
        self.ui.comboBox.addItems(("название", "режисер", "год", "страна"))
        self.ui.pole.addItems(("название", "режисер", "год", "страна"))

        # подключение кнопочек
        self.ui.save.clicked.connect(self.saveClicked)
        self.ui.load.clicked.connect(self.loadClicked)
        self.ui.change.clicked.connect(self.changeClicked)
        self.ui.find.clicked.connect(self.findClicked)
        self.ui.delete_2.clicked.connect(self.deleteClicked)
        self.ui.pushButton.clicked.connect(self.addClicked)

    def saveClicked(self):
        self.db.file_name = self.ui.file_name.text()
        self.db.save()

    def loadClicked(self):
        self.db.file_name = self.ui.file_name.text()
        self.db.load()
        self.ui.tableWidget.setRowCount(len(self.db.get_id()))
        for i in self.db.get_id():
            self.ui.id.addItem(str(i))
            self.ui.id_2.addItem(str(i))

        for i in range(len(self.db.get_id())):
            info = QtWidgets.QTableWidgetItem(str(self.db.get_id()[i]))
            self.ui.tableWidget.setItem(i, 0, info)
            info = QtWidgets.QTableWidgetItem(self.db.get_name()[i])
            self.ui.tableWidget.setItem(i, 1, info)
            info = QtWidgets.QTableWidgetItem(self.db.get_rej()[i])
            self.ui.tableWidget.setItem(i, 2, info)
            info = QtWidgets.QTableWidgetItem(self.db.get_year()[i])
            self.ui.tableWidget.setItem(i, 3, info)
            info = QtWidgets.QTableWidgetItem(self.db.get_country()[i])
            self.ui.tableWidget.setItem(i, 4, info)
            for j in range(5):
                self.ui.tableWidget.item(i, j).setBackground(
                    QtGui.QColor(255, 255, 255))

    def changeClicked(self):
        id = int(self.ui.id.currentText())
        pole = self.ui.pole.currentText()
        print(pole)
        word = self.ui.word.text()
        print(word)
        self.db.change(id, pole, word)
        self.ui.id.clear()
        self.ui.id_2.clear()
        self.loadClicked()

    def findClicked(self):
        for i in range(len(self.db.get_id())):
            for j in range(5):
                self.ui.tableWidget.item(i, j).setBackground(
                    QtGui.QColor(255, 255, 255))
        pole = self.ui.comboBox.currentText()
        word = self.ui.lineEdit.text()
        id = self.db.find(pole, word)
        for i in range(5):
            self.ui.tableWidget.item(id,
                                     i).setBackground(QtGui.QColor(0, 0, 255))

    def deleteClicked(self):
        id = int(self.ui.id_2.currentText())
        self.db.delete(id)
        self.ui.id.clear()
        self.ui.id_2.clear()
        self.loadClicked()

    def addClicked(self):
        name = self.ui.Name.text()
        rej = self.ui.Rej.text()
        year = self.ui.Year.text()
        country = self.ui.Country.text()
        self.db.append(name, rej, year, country)
        self.ui.Name.clear()
        self.ui.Rej.clear()
        self.ui.Year.clear()
        self.ui.Country.clear()
        self.ui.id.clear()
        self.ui.id_2.clear()
        self.loadClicked()
예제 #50
0
class Gui(QMainWindow):
    """
	Main GUI Class
	contains the main function and interfaces between
	the GUI and functions
	"""
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
		Dynamixel bus
		TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)

        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        #wrst3 = DXL_XL(port_num, 6)
        #grip = DXL_XL(port_num, 7)
        #wrst2.set_compliance(8,64)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), 0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """
		Attach Functions to Buttons & Sliders
		TODO: NAME AND CONNECT BUTTONS AS NEEDED
		"""
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(self.execute)
        self.ui.btn_task1.clicked.connect(self.record)
        self.ui.btn_task2.clicked.connect(self.clear_waypoints)
        self.ui.btn_task3.clicked.connect(self.toggle_gripper)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.setText("Block Detector")
        self.ui.btnUser2.clicked.connect(
            partial(self.sm.set_next_state, "block detection"))
        self.ui.btnUser2.setText("Color Buckets")
        self.ui.btnUser2.clicked.connect(
            partial(self.sm.set_next_state, "color buckets"))
        self.ui.btnUser3.setText("Click Grab/Drop Mode")
        self.ui.btnUser3.clicked.connect(
            partial(self.sm.set_next_state, "click grab drop"))
        self.ui.btnUser4.setText("Pick n' Stack")
        self.ui.btnUser4.clicked.connect(
            partial(self.sm.set_next_state, "pick and stack"))
        self.ui.btnUser5.setText("Line 'em up")
        self.ui.btnUser5.clicked.connect(
            partial(self.sm.set_next_state, "line them up"))
        self.ui.btnUser6.setText("Stack 'em high")
        self.ui.btnUser6.clicked.connect(
            partial(self.sm.set_next_state, "stack them high"))
        self.ui.btnUser7.setText("Block slider")
        self.ui.btnUser7.clicked.connect(
            partial(self.sm.set_next_state, "block slider"))
        self.ui.btnUser8.setText("Hot swap")
        self.ui.btnUser8.clicked.connect(
            partial(self.sm.set_next_state, "hot swap"))
        self.ui.btnUser9.setText("Calibration Accuracy")
        self.ui.btnUser9.clicked.connect(
            partial(self.sm.set_next_state, "Calibration Accuracy"))

        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """
		Setup Timer
		this runs the trackMouse function every 50ms
		"""
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)

    """ Slots attach callback functions to signals emitted from threads"""

    @pyqtSlot(QImage, QImage, QImage, QImage, QImage, QImage)
    def setImage(self, rgb_image, depth_image, blocks_depth_image, h_image,
                 s_image, v_image):
        if (self.ui.radioVideo.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
        if (self.ui.radioDepth.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))
        if (self.ui.radioUsr1.isChecked()):
            self.ui.videoDisplay.setPixmap(
                QPixmap.fromImage(blocks_depth_image))
        if (self.ui.radioUsr2.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(h_image))
        if (self.ui.radioUsr3.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(s_image))
        if (self.ui.radioUsr4.isChecked()):
            self.ui.videoDisplay.setPixmap(QPixmap.fromImage(v_image))

    @pyqtSlot(list)
    def updateJointReadout(self, joints):
        self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
        self.ui.rdoutShoulderJC.setText(
            str("%+.2f" % ((joints[1] * R2D) + 90.0)))
        self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
        self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
        self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D)))

        if (len(joints) > 5):
            self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))

        else:
            self.ui.rdoutWrist3JC.setText(str("N.A."))

    @pyqtSlot(list)
    def updateEndEffectorReadout(self, pos):
        self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
        self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
        self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
        self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
        self.ui.rdoutG.setText(str("%+.2f" % (pos[4])))
        self.ui.rdoutP.setText(str("%+.2f" % (pos[5])))

    @pyqtSlot(str)
    def updateStatusMessage(self, msg):
        self.ui.rdoutStatus.setText(msg)

    """ Other callback functions attached to GUI elements"""

    def estop(self):
        self.rexarm.estop = True
        self.sm.set_next_state("estop")

    def execute(self):
        self.sm.set_next_state("execute")
        self.ui.sldrMaxTorque.setValue(50)

    def toggle_gripper(self):
        self.rexarm.toggle_gripper()
        #self.rexarm.pause(1)

    def record(self):
        self.sm.set_next_state("record")

    def clear_waypoints(self):
        self.sm.waypoints = []

    def sliderChange(self):
        """
		Function to change the slider labels when sliders are moved
		and to command the arm to the given position
		"""
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))

        self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
                                      self.rexarm.num_joints,
                                      update_now=False)
        self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
                                                 100.0,
                                                 update_now=False)
        joint_positions = np.array([
            self.ui.sldrBase.value() * D2R,
            self.ui.sldrShoulder.value() * D2R,
            self.ui.sldrElbow.value() * D2R,
            self.ui.sldrWrist.value() * D2R,
            self.ui.sldrWrist2.value() * D2R,
            self.ui.sldrWrist3.value() * D2R
        ])
        self.rexarm.set_positions(joint_positions, update_now=False)

    def directControlChk(self, state):
        if state == Qt.Checked:
            self.sm.set_next_state("manual")
            self.ui.SliderFrame.setEnabled(True)
            self.ui.sldrMaxTorque.setValue(0)
        else:
            self.sm.set_next_state("idle")
            self.ui.SliderFrame.setEnabled(False)
            self.ui.sldrMaxTorque.setValue(50)

    def trackMouse(self):
        """
		Mouse position presentation in GUI
		TODO: after implementing workspace calibration
		display the world coordinates the mouse points to
		in the RGB video image.
		"""
        x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
        y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
        if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
            self.ui.rdoutMousePixels.setText("(-,-,-)")
            self.ui.rdoutMouseWorld.setText("(-,-,-)")
        else:
            x = x - MIN_X
            y = y - MIN_Y
            if (self.kinect.currentDepthFrame.any() != 0):
                z = self.kinect.currentDepthFrame[y][x]
                self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
                                                 (x, y, z))
                if (self.kinect.kinectCalibrated):
                    z_w = .1236 * np.tan(z / 2842.5 + 1.1863) - 0.94
                    xy_world = self.sm.worldCoordinates(x, y)
                    self.ui.rdoutMouseWorld.setText(
                        "(%.3f,%.3f,%.3f)" %
                        (xy_world[0], xy_world[1], xy_world[2]))

    def mousePressEvent(self, QMouseEvent):
        """
		Function used to record mouse click positions for calibration
		"""
        """ Get mouse posiiton """
        #print("mouse event")
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        if self.sm.current_state == "click grab drop":
            if not self.sm.grab_position:
                self.sm.grab_position = (x, y)
                print("Have grab position")
                return
            if self.sm.grab_position:
                self.sm.drop_position = (x, y)
                print("Have drop position")
        if self.sm.current_state == "calibrate":
            """ Change coordinates to image axis """
            self.kinect.last_click[0] = x - MIN_X
            self.kinect.last_click[1] = y - MIN_Y
            self.kinect.new_click = True
        if self.sm.current_state == "Calibration Accuracy":
            self.sm.calibrateaccuracypt = (x, y)
예제 #51
0
    def __init__(self,parent=None):
        QtGui.QWidget.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        """ Main Variables Using Other Classes"""
        self.rex = Rexarm()
        self.video = Video(cv2.VideoCapture(0))
	self.world_coord = np.float32()

	""" Play and Repeat Variable """
	self.wayPoints = []
        self.wayPointsPos = []
	self.wayPointsSpeed = []
	self.wayPointsTime = []

        """ Other Variables """
        self.last_click = np.float32([-1,-1])
	self.define_template_flag = -1
        self.click_point1 = np.float32([-1,-1])
        self.click_point2 = np.float32([-1,-1])
	self.template = None
	self.targets = []
	self.waypointsfp = csv.writer(open("waypoint.csv","wb"))
        self.currtime = 0
        """ Set GUI to track mouse """
        QtGui.QWidget.setMouseTracking(self,True)

        """ 
        Video Function 
        Creates a timer and calls play() function 
        according to the given time delay (27mm) 
        """
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.play)
        self._timer.start(27)
       
        """ 
        LCM Arm Feedback
        Creates a timer to call LCM handler continuously
        No delay implemented. Reads all time 
        """  
        self._timer2 = QtCore.QTimer(self)
        self._timer2.timeout.connect(self.rex.get_feedback)
        self._timer2.start()

	"""
	ARM Plan and Command Thread
	Creates a timer to call REXARM.plan_command function continuously
	"""
	self._timer3 = QtCore.QTimer(self)
	self._timer3.timeout.connect(self.rex.plan_command)
	self._timer3.start()
        
	"""
        Connect Sliders to Function
        TO DO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI 
        """ 
        self.ui.sldrBase.valueChanged.connect(self.slider_change)
        self.ui.sldrShoulder.valueChanged.connect(self.slider_change)
        self.ui.sldrElbow.valueChanged.connect(self.slider_change)
        self.ui.sldrWrist.valueChanged.connect(self.slider_change)
        self.ui.sldrMaxTorque.valueChanged.connect(self.slider_change)
	self.ui.sldrSpeed.valueChanged.connect(self.slider_change)

        """ Commands the arm as the arm initialize to 0,0,0,0 angles """
        self.slider_change() 
        
        """ Connect Buttons to Functions """
        self.ui.btnLoadCameraCal.clicked.connect(self.load_camera_cal)
        self.ui.btnPerfAffineCal.clicked.connect(self.affine_cal)
        self.ui.btnTeachRepeat.clicked.connect(self.tr_initialize)
        self.ui.btnAddWaypoint.clicked.connect(self.tr_add_waypoint)
        self.ui.btnSmoothPath.clicked.connect(self.tr_smooth_path)
        self.ui.btnPlayback.clicked.connect(self.tr_playback)
	self.ui.btnLoadPlan.clicked.connect(self.tr_load)
        self.ui.btnDefineTemplate.clicked.connect(self.def_template)
        self.ui.btnLocateTargets.clicked.connect(self.template_match)
        self.ui.btnExecutePath.clicked.connect(self.exec_path)
예제 #52
0
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
		Dynamixel bus
		TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)

        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        #wrst3 = DXL_XL(port_num, 6)
        #grip = DXL_XL(port_num, 7)
        #wrst2.set_compliance(8,64)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), 0)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """
		Attach Functions to Buttons & Sliders
		TODO: NAME AND CONNECT BUTTONS AS NEEDED
		"""
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btn_exec.clicked.connect(self.execute)
        self.ui.btn_task1.clicked.connect(self.record)
        self.ui.btn_task2.clicked.connect(self.clear_waypoints)
        self.ui.btn_task3.clicked.connect(self.toggle_gripper)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.setText("Block Detector")
        self.ui.btnUser2.clicked.connect(
            partial(self.sm.set_next_state, "block detection"))
        self.ui.btnUser2.setText("Color Buckets")
        self.ui.btnUser2.clicked.connect(
            partial(self.sm.set_next_state, "color buckets"))
        self.ui.btnUser3.setText("Click Grab/Drop Mode")
        self.ui.btnUser3.clicked.connect(
            partial(self.sm.set_next_state, "click grab drop"))
        self.ui.btnUser4.setText("Pick n' Stack")
        self.ui.btnUser4.clicked.connect(
            partial(self.sm.set_next_state, "pick and stack"))
        self.ui.btnUser5.setText("Line 'em up")
        self.ui.btnUser5.clicked.connect(
            partial(self.sm.set_next_state, "line them up"))
        self.ui.btnUser6.setText("Stack 'em high")
        self.ui.btnUser6.clicked.connect(
            partial(self.sm.set_next_state, "stack them high"))
        self.ui.btnUser7.setText("Block slider")
        self.ui.btnUser7.clicked.connect(
            partial(self.sm.set_next_state, "block slider"))
        self.ui.btnUser8.setText("Hot swap")
        self.ui.btnUser8.clicked.connect(
            partial(self.sm.set_next_state, "hot swap"))
        self.ui.btnUser9.setText("Calibration Accuracy")
        self.ui.btnUser9.clicked.connect(
            partial(self.sm.set_next_state, "Calibration Accuracy"))

        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)

        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)

        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """
		Setup Timer
		this runs the trackMouse function every 50ms
		"""
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)
예제 #53
0
 def __init__(self, pod):
     super().__init__()
     self._pod = pod
     self._ui = Ui_MainWindow()
     self._ui.setupUi(self)
     self._ui.actionNetwork.triggered.connect(self.networkDialog)
예제 #54
0
파일: main.py 프로젝트: mohamedkamal77/DSP
class ApplicationWindow(QtWidgets.QMainWindow):
    def __init__(self):
        super(ApplicationWindow, self).__init__()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.index = int(0)
        self.on = 1
        self.k2 = 42.1
        self.D = 0.05
        self.Uo = 8
        self.hieght = 1.7
        self.add_mask = 1
        self.t = []
        self.Xp = []
        self.d_display = 1
        self.image = [[]] * 2
        self.ax = []
        self.fig = []
        self.cax = []
        self.divider = []
        self.plotWidget = []
        self.is_mask = 0
        self.planes = ["sagittal", "coronal"]
        self.current_img = [np.zeros((251, 251)), np.zeros((251, 251))]
        self.img = [np.zeros((251, 251)), np.zeros((251, 251))]
        self.initialize_plt()
        self.ui.start_simulation.clicked.connect(self.start)
        self.ui.save_video.clicked.connect(self.save_video)
        self.ui.stop_simulation.clicked.connect(self.stop_restart)
        self.ui.distance_slider.valueChanged.connect(self.change_dist)
        self.ui.hide_plot.clicked.connect(self.ui.frame.hide)

    def change_dist(self):
        temp = self.ui.distance_slider.value()
        self.ui.source_distance.setText(f" {temp} m From Source")

    def initialize_plt(self):
        plt.ioff()
        for i in range(3):
            fig, ax = plt.subplots()
            self.fig.append(fig)
            self.ax.append(ax)
            if i != 2:
                divider = make_axes_locatable(self.ax[i])
                self.divider.append(divider)
                cax = self.divider[i].append_axes('right', size='3%', pad=0)
                self.cax.append(cax)
                self.ax[i].set_title(f"{self.planes[i]} plane ")
                ax = self.ax[i].imshow(np.zeros((251, 251)),
                                       cmap=plt.cm.jet,
                                       origin='lower')
                self.fig[i].colorbar(ax,
                                     cax=self.cax[i],
                                     orientation='vertical')
                self.plotWidget.append(FigureCanvas(self.fig[i]))
                self.ui.display_layout.addWidget(self.plotWidget[i])
            else:
                self.plotWidget.append(FigureCanvas(self.fig[i]))
                self.ui.diplay_on_click_layout.addWidget(self.plotWidget[i])
        cid = self.fig[0].canvas.mpl_connect(
            'button_press_event', lambda event: self.on_press(event, 0))
        cid2 = self.fig[1].canvas.mpl_connect(
            'button_press_event', lambda event: self.on_press(event, 1))

    def start(self):
        self.get_parameters()
        self.ui.stop_simulation.setText("Stop")
        self.ui.frame.hide()
        self.ui.hide_plot.hide()
        self.on = 1
        if self.image[0] != []:
            self.timer.stop()
            self.image = [[]] * 2
            self.img = [np.zeros((251, 251)), np.zeros((251, 251))]
            self.index = 0
            for i in range(2):
                self.ax[i].cla()
                self.cax[i].cla()
                self.fig[i].delaxes(self.cax[i])
                divider = make_axes_locatable(self.ax[i])
                self.divider.append(divider)
                cax = self.divider[i].append_axes('right', size='3%', pad=0)
                self.cax[i] = cax
                ax = self.ax[i].imshow(self.img[i],
                                       cmap=plt.cm.jet,
                                       origin='lower')
                self.ax[i].set_title(f"{self.planes[i]} plane ")
                self.fig[i].colorbar(ax,
                                     cax=self.cax[i],
                                     orientation='vertical')
                self.fig[i].canvas.draw_idle()

        self.timer = QtCore.QTimer()
        self.timer.setInterval(50)
        self.timer.timeout.connect(self.start_simulation)
        self.timer.start()

    def stop_restart(self):

        if self.on == 1:
            self.timer.stop()
            self.on = 0
            self.ui.stop_simulation.setText("Restart")
        else:
            self.timer.start()
            self.on = 1
            self.ui.stop_simulation.setText("Stop")

    def get_parameters(self):
        self.d_display = self.ui.distance_slider.value()
        person_hieght = self.ui.person_hieght.text()
        discharge_velocity = self.ui.discharge_velocity.text()
        if discharge_velocity != '':
            self.Uo = float(discharge_velocity)
        if person_hieght != '':
            self.hieght = float(person_hieght)
        temp = [3, 5]
        if self.ui.add_mask.isChecked():
            self.is_mask = 1
            self.add_mask = temp[self.ui.select_mask.currentIndex()]
        else:
            self.is_mask = 0
            self.add_mask = 1

    def start_simulation(self):
        if self.index < 251:
            temp_d = ((self.index + 1) / 250) * self.d_display
            if temp_d <= 0.272:
                temp_t = temp_d / self.Uo
                self.ui.time_display.setText(f"Time: {np.round(temp_t,5)} s")
                central_velocity = self.Uo * ((1 - self.is_mask * 0.97) /
                                              (self.is_mask * self.index + 1))
            else:
                temp_t = ((temp_d**2) + 0.074) / (0.544 * self.Uo)
                self.ui.time_display.setText(f"Time: {np.round(temp_t,5)} s")
                central_velocity = (2.176 / temp_d) * (1 - self.is_mask)
            self.ui.distance_display.setText(
                f"Distance: {np.round(temp_d,4)} m")
            self.t.append(temp_t)
            limit_pixel = int(
                np.round(125 - self.index *
                         np.tan(self.add_mask * np.pi * 15 / 180) - 2))

            self.img[0][125, self.index] = central_velocity
            self.img[1][125, 125] = central_velocity
            if limit_pixel > 250:
                limit_pixel = 250
            for n in range(limit_pixel, 126):
                dist_vertical = ((125 - n) / 125) * self.hieght
                vertial_v = central_velocity * np.exp(-self.k2 * 0.2 *
                                                      (dist_vertical)**2)
                self.img[0][n, self.index] = vertial_v
                self.img[0][250 - n, self.index] = vertial_v
                y = np.sqrt((125 - limit_pixel)**2 - (125 - n)**2)
                y = int(np.round(y))

                for m in range(125, 125 + y + 1):
                    dist = np.sqrt((n - 125)**2 + (m - 125)**2)
                    dist_vertical = ((dist) / 125) * self.hieght
                    vertial_v2 = central_velocity * np.exp(-self.k2 * 0.2 *
                                                           (dist_vertical)**2)
                    self.img[1][m, 250 - n] = vertial_v2
                    self.img[1][250 - m, 250 - n] = vertial_v2
                    self.img[1][m, n] = vertial_v2
                    self.img[1][250 - m, n] = vertial_v2

            for i in range(2):
                self.ax[i].cla()
                self.ax[i].set_title(
                    f"{self.planes[i]} plane at {np.round(temp_t,5)} s")
                ax = self.ax[i].imshow(self.img[i],
                                       cmap=plt.cm.jet,
                                       origin='lower')
                self.cax[i].cla()
                self.fig[i].colorbar(ax,
                                     cax=self.cax[i],
                                     orientation='vertical')
                self.fig[i].savefig(f"imgs/img{i}_{self.index}.jpeg")
                self.fig[i].canvas.draw_idle()
                temp = np.empty_like(self.img[i])
                temp[:, :] = self.img[i]
                self.image[i].append(temp)
            self.index += 1

        else:
            self.timer.stop()

    def on_press(self, point, index):
        if point.xdata != None and point.ydata != None:
            self.ui.hide_plot.show()
            x_data = point.xdata
            y_data = point.ydata
            area = [0]
            if index == 0:
                temp_d = ((x_data + 1) / 251) * self.d_display
                dist_center = (abs(y_data - 125) / 125) * self.hieght

                if temp_d <= 0.272:
                    t_start = temp_d / self.Uo
                    max_velocit = self.Uo * ((1 - self.is_mask * 0.97) /
                                             (self.is_mask * self.index + 1))
                else:
                    t_start = ((temp_d**2) + 0.074) / (0.544 * self.Uo)
                    max_velocit = (2.176 / temp_d) * (1 - self.is_mask)

            else:
                temp_d = ((self.index + 1) / 251) * self.d_display
                if temp_d <= 0.272:
                    t_start = temp_d / self.Uo
                    max_velocit = self.Uo * ((1 - self.is_mask * 0.97) /
                                             (self.is_mask * self.index + 1))
                else:
                    t_start = ((temp_d**2) + 0.074) / (0.544 * self.Uo)
                    max_velocit = (2.176 / temp_d) * (1 - self.is_mask)
                dist_center = (np.sqrt((y_data - 125)**2 +
                                       (x_data - 125)**2) / 125) * self.hieght
                x_data = self.index

            self.draw(max_velocit, dist_center, t_start, temp_d, x_data,
                      y_data)

    def draw(self, max_velocity, dist_center, t_start, x_dist, xdata, ydata):
        self.ui.frame.show()
        self.ax[2].cla()
        t = np.linspace(0.0001, 5.0001, 50001)
        if np.abs(ydata - 125) > (np.abs(xdata) * np.tan(np.pi / 12) + 4):
            f = np.zeros((np.size(t)))
        else:
            index_of_t_start = int(np.round(t_start / 0.0001))
            f = max_velocity * np.exp(-10 * t / ((0.272 + (
                (self.d_display**2) - 0.073984) / 0.544)) / self.Uo) * np.exp(
                    -self.k2 * (dist_center)**2)
            f[0:index_of_t_start] = np.repeat(0, index_of_t_start)
        self.ax[2].plot(t, f)
        self.ax[2].set(title="V at poin Versus Time",
                       xlabel="time (s)",
                       ylabel="V (m/s)")
        self.fig[2].canvas.draw()

    def save_video(self):
        self.frame_no = int(self.index) - 1
        self.ui.save_video.setText("Saving Progressing")
        self.directory = self.get_video_directory()
        if self.directory == None:
            self.directory = "kimo"
        frame = cv2.imread("imgs/img0_0.jpeg")
        height, width, layers = frame.shape
        width = int(np.round(width * 0.7)) * 2
        self.ui.videoprogress.show()
        self.video = cv2.VideoWriter(f"{self.directory}.avi", 0, 5,
                                     (width, height))
        self.index_2 = 0
        self.timer1 = QtCore.QTimer()
        self.timer1.setInterval(50)
        self.timer1.timeout.connect(
            lambda: self.save_video_iterate(width, height))
        self.timer1.start()

    def save_video_iterate(self, width, height):
        width = int(0.5 * width)
        if self.index_2 <= self.frame_no:
            self.video.write(
                np.concatenate(
                    (cv2.resize(cv2.imread(f"imgs/img{0}_{self.index_2}.jpeg"),
                                (width, height)),
                     cv2.resize(cv2.imread(f"imgs/img{1}_{self.index_2}.jpeg"),
                                (width, height))),
                    axis=1))
            self.index_2 += 1
            self.ui.videoprogress.setValue(
                int(np.round(100 * self.index_2 / self.frame_no)))
        else:
            self.ui.save_video.setText("Save Video")
            self.timer1.stop()
            cv2.destroyAllWindows()
            self.video.release()
            self.ui.videoprogress.setValue(100)
            time.sleep(0.5)
            self.ui.videoprogress.hide()

    def get_video_directory(self):
        options = QtWidgets.QFileDialog.Options()
        options |= QtWidgets.QFileDialog.DontUseNativeDialog
        fileName, _ = QtWidgets.QFileDialog.getSaveFileName(
            self,
            "Choose Directory and Name",
            "",
            "All Files (*);;Python Files (*.py)",
            options=options)
        if fileName:
            return fileName
예제 #55
0
파일: showui.py 프로젝트: Winawer/jaabagui
    def __init__(self,parent=None):
        QMainWindow.__init__(self,parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)

        #add new slider
        # self.positionSlider=QSlider(Qt.Horizontal)
        # self.positionSlider.setGeometry (800,800,100,30)
        # self.positionSlider.setRange(0, 0)
        # self.positionSlider.sliderMoved.connect(self.setPosition)

        #setup Video
        #video player
        self.mediaPlayer1 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
        self.mediaPlayer2 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
        #self.mediaPlayer.metaDataChanged.connect(self.metaDataChanged)
        self.mediaPlayer1.durationChanged.connect(self.durationChanged)
        self.mediaPlayer1.positionChanged.connect(self.positionChanged)
        self.mediaPlayer2.positionChanged.connect(self.positionChanged)
        

        #visualizetion
        self.scene = QGraphicsScene()
        self.ui.graphicsView.setScene(self.scene)
        #self.scene.setBackgroundBrush(Qt.black)
        self.videoItem1 = QGraphicsVideoItem()
        self.videoItem2 = QGraphicsVideoItem()
        self.scene.addItem(self.videoItem1)
        self.scene.addItem(self.videoItem2)
        self.mediaPlayer1.setVideoOutput(self.videoItem1)
        self.mediaPlayer2.setVideoOutput(self.videoItem2)

        #slide bar
        print self.ui.horizontalSlider
        self.ui.horizontalSlider.setRange(0, 0)
        self.ui.horizontalSlider.sliderMoved.connect(self.setPosition)
        # self.ui.horizontalSlider.sliderPressed.connect(self.sliderPressed)



        #print self.ui.graphicsView.width()/2,self.ui.graphicsView.height()
        #self.videoItem1.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
        #self.videoItem2.setSize(QSizeF(self.ui.graphicsView.width()*10,self.ui.graphicsView.height()*10))
       # self.videoItem2.setSize(graphicsView.size())
        #self.videoItem2.setOffset(QPointF(500,500))
        #self.videoItem2.setOffset(QPointF(self.ui.graphicsView.width()/2,0))   
        #self.videoItem2.setPos(QPointF(0,0))
        # print self.ui.graphicsView.width(), self.ui.graphicsView.height()
        # print self.ui.graphicsView.size()
        # print self.videoItem2.boundingRect().width(), self.videoItem2.boundingRect().height()
        # print self.ui.graphicsView.sceneRect()
        #self.mediaPlayer.stateChanged.connect(self.mediaStateChanged)

        #callbacks
        self.ui.actionQuit.triggered.connect(self.quit)
        self.ui.actionLoad_Project.triggered.connect(self.loadVideo)
        #self.ui.buttonPlay.clicked[bool].connect(self.setToggleText)
        self.ui.buttonPlay.clicked.connect(self.play)
        #print self.ui.graphicsView.sizeHint()


        #initialization
        self.loaded = False
        self.videoFilename = None
        self.frame_count=None
        self.width=None
        self.height=None
        self.frame_trans=None
예제 #56
0
파일: test2.py 프로젝트: xj962458/Python
from ui import Ui_MainWindow
from PyQt5.QtWidgets import QApplication, QMainWindow

app = QApplication([])
window = QMainWindow()
ui = Ui_MainWindow()
ui.setupUi(window)
app.exec_()
예제 #57
0
 def __init__(self):
     super(CurrencyConv, self).__init__()
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)
     self.init_UI()
예제 #58
0
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        """ Set GUI to track mouse """
        QWidget.setMouseTracking(self, True)
        """
        Dynamixel bus
        TODO: add other motors here as needed with their correct address"""
        self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
        port_num = self.dxlbus.port()
        base = DXL_MX(port_num, 1)
        shld = DXL_MX(port_num, 2)
        elbw = DXL_MX(port_num, 3)
        wrst = DXL_AX(port_num, 4)
        wrst2 = DXL_AX(port_num, 5)
        wrst3 = DXL_XL(port_num, 6)
        grip = DXL_XL(port_num, 7)
        """Objects Using Other Classes"""
        self.kinect = Kinect()
        self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip)
        self.tp = TrajectoryPlanner(self.rexarm)
        self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
        """ 
        Attach Functions to Buttons & Sliders
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btn_estop.clicked.connect(self.estop)
        self.ui.btnUser1.setText("Calibrate")
        self.ui.btnUser1.clicked.connect(
            partial(self.sm.set_next_state, "calibrate"))
        self.ui.btnUser2.setText("Waypoint")
        self.ui.btnUser2.clicked.connect(
            partial(self.sm.set_next_state, "waypoint"))
        self.ui.btnUser3.setText("Teach")
        self.ui.btnUser3.clicked.connect(
            partial(self.sm.set_next_state, "teach"))
        self.ui.btnUser4.setText("Record")
        self.ui.btnUser4.clicked.connect(
            partial(self.sm.set_next_state, "record"))
        self.ui.btnUser5.setText("Repeat")
        self.ui.btnUser5.clicked.connect(
            partial(self.sm.set_next_state, "repeat"))
        self.ui.btn_task1.setText("Pick n Stack")
        self.ui.btn_task1.clicked.connect(partial(self.sm.set_next_state,
                                                  "IK"))
        self.ui.btnUser6.setText("Click and Place")
        self.ui.btnUser6.clicked.connect(
            partial(self.sm.set_next_state, "clickgrab"))
        self.ui.btnUser7.setText("Open Gripper")
        self.ui.btnUser7.clicked.connect(
            partial(self.sm.set_next_state, "open_gripper"))
        self.ui.btnUser8.setText("Close Gripper")
        self.ui.btnUser8.clicked.connect(
            partial(self.sm.set_next_state, "close_gripper"))
        self.ui.btnUser9.setText("Incline")
        self.ui.btnUser9.clicked.connect(
            partial(self.sm.set_next_state, "incline"))
        self.ui.btn_task2.setText("Line Em Up")
        self.ui.btn_task2.clicked.connect(
            partial(self.sm.set_next_state, "color_sort"))
        self.ui.btn_task3.setText("Stack Em High")
        self.ui.btn_task3.clicked.connect(
            partial(self.sm.set_next_state, "color_stack"))
        self.ui.btn_task4.setText("Block Mover")
        self.ui.btn_task4.clicked.connect(
            partial(self.sm.set_next_state, "square"))
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
        #self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
        self.ui.rdoutStatus.setText("Waiting for input")
        """initalize manual control off"""
        self.ui.SliderFrame.setEnabled(False)
        """initalize rexarm"""
        self.rexarm.initialize()
        """Setup Threads"""
        self.videoThread = VideoThread(self.kinect)
        self.videoThread.updateFrame.connect(self.setImage)
        self.videoThread.start()

        self.logicThread = LogicThread(self.sm)
        self.logicThread.start()

        self.displayThread = DisplayThread(self.rexarm, self.sm)
        self.displayThread.updateJointReadout.connect(self.updateJointReadout)
        self.displayThread.updateEndEffectorReadout.connect(
            self.updateEndEffectorReadout)
        self.displayThread.updateStatusMessage.connect(
            self.updateStatusMessage)
        self.displayThread.start()
        """ 
        Setup Timer 
        this runs the trackMouse function every 50ms
        """
        self._timer = QTimer(self)
        self._timer.timeout.connect(self.trackMouse)
        self._timer.start(50)
예제 #59
0
class App(QtWidgets.QMainWindow):
    """
    main app window
    """

    #logger = Logger("c:\\temp\\price_bot.log").get_logger()

    # params to init bot with
    params = []

    username = ""
    password = ""

    selected_site = ""
    selected_exec_type = ""
    selected_frequency = ""

    thresh = ""

    bot_state = "off"

    def __init__(self):
        """
        constructor
        """

        logging.debug(" init apps ")

        # ui init
        super(App, self).__init__()
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.mgr = Manager(self.ui)

        self.selected_google_auth_file = self.ui.get_google_auth_file_name()

        # init starting events
        self.ui.activateButton.setEnabled(False)
        self.ui.deactivateButton.setEnabled(False)
        self.ui.refreshButton.setEnabled(False)
        self.ui.activateButton.clicked.connect(self.mgr.activate)
        self.ui.deactivateButton.clicked.connect(self.mgr.deactivate)
        self.ui.quitButton.clicked.connect(self.quit)

        self.ui.refreshButton.clicked.connect(self.refresh)

        # init and make selections
        self.ui.cmbbox_site.activated[str].connect(self.on_site_select)
        self.ui.exec_type_box.activated[str].connect(self.on_exec_type_select)
        self.ui.cmbbox_freq.activated[str].connect(self.on_frequency_select)

    def capture_thresh(self):
        """
        capture threshold
        """

        print(" *** IN CAPTURE REFRESH ")
        thresh_val = self.ui.reduct_field.text()

        if (isinstance(thresh_val, str)):
            if (thresh_val.isdigit()):
                if (isinstance(int(thresh_val), int)):

                    if (int(thresh_val) >= 0 and int(thresh_val) <= 100):
                        self.thresh = thresh_val
                    else:
                        logging.error("frequency must be greater than zero")
                        self.reset_form()
                else:
                    logging.error("frequency is not numeric")
                    self.reset_form()
            else:
                logging.error("frequency is not numeric")
                self.reset_form()
        else:
            logging.error("frequency is not numeric")
            self.reset_form()

    def refresh(self):
        """
        tries to re-init the bot
        """

        print(" *** IN REFRESH ")
        self.ui.activateButton.setEnabled(False)
        self.params = []
        self.capture_thresh()
        self.enable_buttons()

    def on_site_select(self, site):
        """
        selecting site
        """
        self.selected_site = site
        if (self.bot_state == "off"):
            self.capture_thresh()
            self.enable_buttons()
        else:
            self.ui.refreshButton.setEnabled(True)
            self.ui.activateButton.setEnabled(False)
            self.bot_state = "off"

    def on_exec_type_select(self, exec_type):
        """
        exec type select
        """
        self.selected_exec_type = exec_type
        if (self.bot_state == "off"):
            self.capture_thresh()
            self.enable_buttons()
        else:
            self.ui.refreshButton.setEnabled(True)
            self.ui.activateButton.setEnabled(False)
            self.bot_state = "off"

    def on_frequency_select(self, freq):
        """
        frequency select
        """

        self.selected_frequency_type = freq
        if (self.bot_state == "off"):
            self.frequency = str(self.ui.freq_value_box.text())

            if (isinstance(self.frequency, str)):
                if (self.frequency.isdigit()):
                    if (isinstance(int(self.frequency), int)):

                        if (int(self.frequency) > 0):

                            self.capture_thresh()
                            self.enable_buttons()
                        else:
                            logging.error(
                                "frequency must be greater than zero")
                            self.ui.reduct_field.setText("")
                    else:
                        logging.error("frequency is not numeric")
                        self.ui.reduct_field.setText("")
                else:
                    logging.error("frequency is not numeric")
                    self.ui.reduct_field.setText("")
            else:
                logging.error("frequency is not numeric")
                self.ui.reduct_field.setText("")
        else:
            self.ui.refreshButton.setEnabled(True)
            self.ui.activateButton.setEnabled(False)
            self.bot_state = "off"

    def reset_form(self):
        """
        reset freq 
        """

        #self.capture_thresh()
        self.ui.freq_value_box.setText("")
        self.ui.cmbbox_freq.setCurrentIndex(0)

    def enable_buttons(self):
        """
        enable buttons
        """

        try:
            print(" *** ENABLING BUTTONS ")
            if (self.selected_exec_type != ""
                    and self.selected_frequency_type != ""
                    and self.selected_site != ""):

                self.username = self.ui.username_field.text()
                self.password = self.ui.password_field.text()
                self.frequency = self.ui.freq_value_box.text()

                self.params.append(self.username)
                self.params.append(self.password)
                self.params.append(self.selected_site)
                self.params.append(self.selected_frequency_type)
                self.params.append(self.frequency)
                self.params.append(self.selected_exec_type)
                self.params.append(self.selected_google_auth_file)
                self.params.append(self.thresh)

                #self.ui.refreshButton.setEnabled(False)

                print(self.params)

                self.mgr.init_bot(self.params)
                self.bot_state = "on"

            else:

                print(" CANNOT ENABLE ")

        except Exception as e:

            print(" *** EXCEPTION WHEN ENABLING ")
            print(e)

            self.ui.refreshButton.setEnabled(True)
            self.ui.activateButton.setEnabled(False)
            logging.debug(e)

    def quit(self):

        self.mgr.deactivate()
        sys.exit()
예제 #60
0
파일: main.py 프로젝트: handy505/playground
 def __init__(self):
     super(MainWindow, self).__init__()
     self.ui = Ui_MainWindow()
     self.ui.setupUi(self)
     self.ui.label.setText('Hello World!')