"""

This software was designed by Alexander Thiel

Github handle: https://github.com/apockill

Email: Alex.D.Thiel@Gmail.com

The software was designed originaly for use with a robot arm, particularly uArm (Made by uFactory, ufactory.cc)

It is completely open source, so feel free to take it and use it as a base for your own projects.

If you make any cool additions, feel free to share!

License:

This file is part of uArmCreatorStudio.

uArmCreatorStudio is free software: you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by

the Free Software Foundation, either version 3 of the License, or

(at your option) any later version.

uArmCreatorStudio is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

GNU General Public License for more details.

You should have received a copy of the GNU General Public License

along with uArmCreatorStudio. If not, see <http://www.gnu.org/licenses/>.

"""

import re

import Paths

import Logic.RobotVision as rv

import numpy as np

from time import time

from PyQt5 import QtCore, QtWidgets, QtGui

from CameraGUI import CameraWidget, CameraSelector, cvToPixFrame

from CommandsGUI import CommandMenuWidget

from CommonGUI import centerScreen

from ControlPanelGUI import CommandList

from Logic.Global import printf,getOSType,WINDOWS,MACOSX,LINUX

from Logic.Resources import TrackableObject, MotionPath, Function

from Logic.RobotVision import MIN_POINTS_TO_LEARN_OBJECT

__author__ = "Alexander Thiel"

class ObjectManagerWindow(QtWidgets.QDialog):

def __init__(self, environment, parent):

super(ObjectManagerWindow, self).__init__(parent)

self.env = environment

self.vision = environment.getVision()

self.objManager = environment.getObjectManager()

self.cameraWidget = CameraWidget(self.env.getVStream(), parent=self)

# Global UI Variables

self.selLayout = QtWidgets.QVBoxLayout()

self.objTree = QtWidgets.QTreeWidget()

# Initialize the UI

self.initUI()

self.cameraWidget.play()

self.refreshTreeWidget()

def initUI(self):

self.objTree.setIndentation(10)

self.objTree.setHeaderLabels([""])

self.objTree.header().close()

# CREATE OBJECTS AND LAYOUTS FOR ROW 1 COLUMN (ALL)

newObjBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","New Vision Object"))

newGrpBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","New Vision Group"))

newRecBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","New Move Recording"))

newFncBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","New Function"))

# Fix Button font color

if getOSType() == MACOSX:

newObjBtn.setStyleSheet('QPushButton { color: Black;}')

newGrpBtn.setStyleSheet('QPushButton { color: Black;}')

newRecBtn.setStyleSheet('QPushButton { color: Black;}')

newFncBtn.setStyleSheet('QPushButton { color: Black;}')

# Set the icons for the buttons

newObjBtn.setIcon(QtGui.QIcon(Paths.event_recognize))

newGrpBtn.setIcon(QtGui.QIcon(Paths.event_recognize))

newRecBtn.setIcon(QtGui.QIcon(Paths.record_start))

newFncBtn.setIcon(QtGui.QIcon(Paths.command_run_func))

newObjBtn.setFixedWidth(175)

newGrpBtn.setFixedWidth(175)

newRecBtn.setFixedWidth(175)

newFncBtn.setFixedWidth(175)

newObjBtn.setFixedHeight(35)

newGrpBtn.setFixedHeight(35)

newRecBtn.setFixedHeight(35)

newFncBtn.setFixedHeight(35)

# Connect everything up

newObjBtn.clicked.connect(lambda: self.openResourceMenu(MakeObjectWindow))

newGrpBtn.clicked.connect(lambda: self.openResourceMenu(MakeGroupWindow))

newRecBtn.clicked.connect(lambda: self.openResourceMenu(MakeRecordingWindow))

newFncBtn.clicked.connect(lambda: self.openResourceMenu(MakeFunctionWindow))

self.objTree.itemSelectionChanged.connect(self.refreshSelected)

# CREATE OBJECTS AND LAYOUTS FOR COLUMN 1

listGBox = QtWidgets.QGroupBox(QtCore.QCoreApplication.translate("ObjectManagerGUI","Loaded Objects"))

listVLayout = QtWidgets.QVBoxLayout()

listVLayout.addWidget(self.objTree)

listGBox.setLayout(listVLayout)

listGBox.setFixedWidth(325)

# CREATE OBJECTS AND LAYOUTS FOR COLUMN 2

selectedGBox = QtWidgets.QGroupBox(QtCore.QCoreApplication.translate("ObjectManagerGUI","Selected Resource"))

selectedGBox.setLayout(self.selLayout)

# Put everything into 1 row (top) and multiple columns just below the row

row1 = QtWidgets.QHBoxLayout()

col1 = QtWidgets.QVBoxLayout()

col2 = QtWidgets.QVBoxLayout()

col3 = QtWidgets.QVBoxLayout()

row1.addWidget(newObjBtn)

row1.addWidget(newGrpBtn)

row1.addWidget(newRecBtn)

row1.addWidget(newFncBtn)

row1.addStretch(1)

col1.addWidget(listGBox)

col2.addWidget(selectedGBox)

col3.addWidget(self.cameraWidget)

# Place the row into the main vertical layout

mainVLayout = QtWidgets.QVBoxLayout()

mainVLayout.addLayout(row1)

mainHLayout = QtWidgets.QHBoxLayout()

mainVLayout.addLayout(mainHLayout)

# Place the columns into the main horizontal layout

mainHLayout.addLayout(col1)

mainHLayout.addLayout(col2)

mainHLayout.addLayout(col3)

# Set the layout and customize the window

self.setLayout(mainVLayout)

self.setWindowTitle(QtCore.QCoreApplication.translate("ObjectManagerGUI",'Resource Manager'))

self.setWindowIcon(QtGui.QIcon(Paths.objectManager))

self.setMinimumHeight(700)

def refreshTreeWidget(self, selectedItem=None):

"""

Clear the objectList, and reload all object names from the environment

:param selectedItem: If selectedItem is a string name of an object, it will try to select the item in the TreeWidget

Clear the current objectList

"""

self.objTree.clear()

self.vision.endAllTrackers()

# Get a list for each section of the QTreeWidget that there will be

visObjs = self.objManager.getObjectNameList(self.objManager.TRACKABLEOBJ)

visObjs.sort()

grpObjs = self.objManager.getObjectNameList(self.objManager.TRACKABLEGROUP)

grpObjs.sort()

rcdObjs = self.objManager.getObjectNameList(self.objManager.MOTIONPATH)

rcdObjs.sort()

fncObjs = self.objManager.getObjectNameList(self.objManager.FUNCTION)

fncObjs.sort()

tree = [[ QtCore.QCoreApplication.translate("ObjectManagerGUI","Vision Objects"), visObjs],

[ QtCore.QCoreApplication.translate("ObjectManagerGUI","Vision Groups"), grpObjs],

[QtCore.QCoreApplication.translate("ObjectManagerGUI","Movement Recordings"), rcdObjs],

[ QtCore.QCoreApplication.translate("ObjectManagerGUI","Functions"), fncObjs]]

for section in tree:

# Create the Title

title = QtWidgets.QTreeWidgetItem(self.objTree)

title.setText(0, section[0])

for name in section[1]:

newItem = QtWidgets.QTreeWidgetItem(title, [name])

# Select the item specified in selectedItem arg

if name == selectedItem:

self.objTree.setCurrentItem(newItem)

self.refreshSelected()

self.objTree.expandAll()

def refreshSelected(self):

# Modifies self.selectedObjVLayout to show the currently selected object, it's name, description, etc.

self.clearSelectedLayout()

# Get the selected object

selObject = self.getSelected()

if selObject is None: return

obj = self.objManager.getObject(selObject)

self.vision.endAllTrackers()

if obj is None: return

# Disconnect "doubleClick" event from doing anything

try: self.objTree.itemDoubleClicked.disconnect()

except Exception: pass

# Make the SelectedObject window reflect the information about the object (and it's type) that is curr. selected

if isinstance(obj, self.objManager.TRACKABLEOBJ):

self.objTree.itemDoubleClicked.connect(lambda: self.openResourceMenu(MakeObjectWindow, editResource=obj))

self.setSelectionTrackable(obj)

self.vision.addTarget(obj)

return

if isinstance(obj, self.objManager.TRACKABLEGROUP):

self.objTree.itemDoubleClicked.connect(lambda: self.openResourceMenu(MakeGroupWindow, editResource=obj))

self.setSelectionGroup(obj)

self.vision.addTarget(obj)

return

if isinstance(obj, self.objManager.MOTIONPATH):

self.objTree.itemDoubleClicked.connect(lambda: self.openResourceMenu(MakeRecordingWindow, editResource=obj))

self.setSelectionPath(obj)

if isinstance(obj, self.objManager.FUNCTION):

self.objTree.itemDoubleClicked.connect(lambda: self.openResourceMenu(MakeFunctionWindow, editResource=obj))

self.setSelectionFunction(obj)

def setSelectionTrackable(self, trackableObj):

views = trackableObj.getViews()

selDescLbl = QtWidgets.QLabel("") # Description of selected object

selImgLbl = QtWidgets.QLabel("") # A small picture of the object

deleteBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Delete"))

addOrientationBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Add Orientation"))

# Connect any buttons

deleteBtn.clicked.connect(self.deleteSelected)

addOrientationBtn.clicked.connect(lambda: self.openResourceMenu(MakeObjectWindow, editResource=trackableObj))

# Create a pretty icon for the object, so it's easily recognizable. Use the first sample in the objectt

icon = cvToPixFrame(trackableObj.getIcon(150, 300))

selImgLbl.setPixmap(icon)

# Get the "Average" number of keypoints for this object

totalPoints = 0

for view in views:

target = self.vision.planeTracker.createTarget(view)

totalPoints += len(target.descrs)

avgPoints = int(totalPoints / len(views))

# Create and set the description for this object

selDescLbl.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Name: \n") + trackableObj.name + "\n\n" +

QtCore.QCoreApplication.translate("ObjectManagerGUI","Detail Points: \n") + str(avgPoints) + "\n\n" +

QtCore.QCoreApplication.translate("ObjectManagerGUI","Orientations: \n") + str(len(views)) + "\n\n" +

QtCore.QCoreApplication.translate("ObjectManagerGUI","Belongs To Groups:\n") + ''.join(['-' + tag + '\n' for tag in trackableObj.getTags()]) + "\n" +

QtCore.QCoreApplication.translate("ObjectManagerGUI","Image:"))

self.selLayout.addWidget(selDescLbl)

self.selLayout.addWidget(selImgLbl)

self.selLayout.addWidget(addOrientationBtn)

self.selLayout.addWidget(deleteBtn)

self.selLayout.addStretch(1)

def setSelectionGroup(self, trackableGrp):

selDescLbl = QtWidgets.QLabel("") # Description of selected object

deleteBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Delete"))

editBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Edit Group"))

# Connect any buttons

deleteBtn.clicked.connect(self.deleteSelected)

editBtn.clicked.connect(lambda: self.openResourceMenu(MakeGroupWindow, editResource=trackableGrp))

# Create the appropriate description

groupMembers = ['-' + obj.name + '\n' for obj in trackableGrp.getMembers()]

selDescLbl.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Name: \n") + trackableGrp.name + "\n\n" +

QtCore.QCoreApplication.translate("ObjectManagerGUI","Group Members: \n") + ''.join(groupMembers) + "\n")

self.selLayout.addWidget(selDescLbl)

self.selLayout.addWidget(editBtn)

self.selLayout.addWidget(deleteBtn)

self.selLayout.addStretch(1)

def setSelectionPath(self, pathObj):

selDescLbl = QtWidgets.QLabel("") # Description of selected object

deleteBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Delete"))

editBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Edit Recording"))

# Connect any buttons

deleteBtn.clicked.connect(self.deleteSelected)

editBtn.clicked.connect(lambda: self.openResourceMenu(MakeRecordingWindow, editResource=pathObj))

# Create the appropriate description

motionPath = pathObj.getMotionPath()

totalTime = round(motionPath[-1][0], 1)

if len(motionPath) == 0: return # That would be weird, but you never know...

selDescLbl.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Name: \n") + pathObj.name + "\n\n"+

QtCore.QCoreApplication.translate("ObjectManagerGUI","Move Count: \n") + str(len(motionPath)) + "\n\n"+

QtCore.QCoreApplication.translate("ObjectManagerGUI","Length: \n") + str(totalTime) + " seconds\n\n"+

QtCore.QCoreApplication.translate("ObjectManagerGUI","Moves/Second:\n") + str(round(len(motionPath) / totalTime, 1)))

self.selLayout.addWidget(selDescLbl)

self.selLayout.addWidget(editBtn)

self.selLayout.addWidget(deleteBtn)

self.selLayout.addStretch(1)

def setSelectionFunction(self, funcObj):

selDescLbl = QtWidgets.QLabel("") # Description of selected object

deleteBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Delete"))

editBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Edit Function"))

selDescLbl.setWordWrap(True)

# Connect any buttons

deleteBtn.clicked.connect(self.deleteSelected)

editBtn.clicked.connect(lambda: self.openResourceMenu(MakeFunctionWindow, editResource=funcObj))

# Create the appropriate description

commandList = funcObj.getCommandList()

description = funcObj.getDescription()

arguments = funcObj.getArguments()

selDescLbl.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Name: \n") + funcObj.name + "\n\n"+

QtCore.QCoreApplication.translate("ObjectManagerGUI","Description: \n") + description + "\n\n"+

QtCore.QCoreApplication.translate("ObjectManagerGUI","Length: \n") + str(len(commandList)) + QtCore.QCoreApplication.translate("ObjectManagerGUI"," Commands\n\n")+

QtCore.QCoreApplication.translate("ObjectManagerGUI","Arguments:\n") + ''.join(['-' + arg + '\n' for arg in arguments]) + "\n")

self.selLayout.addWidget(selDescLbl)

self.selLayout.addWidget(editBtn)

self.selLayout.addWidget(deleteBtn)

self.selLayout.addStretch(1)

def clearSelectedLayout(self):

"""

Delete/garbage collect every widget in the layout

:return:

"""

for cnt in reversed(range(self.selLayout.count())):

# takeAt does both the jobs of itemAt and removeWidget

# namely it removes an item and returns it

widget = self.selLayout.takeAt(cnt).widget()

if widget is not None:

# widget will be None if the item is a layout

widget.deleteLater()

def deleteSelected(self):

# Get the selected object

selObject = self.getSelected()

if selObject is None: return

# Warn the user of the consequences of continuing

reply = QtWidgets.QMessageBox.question(self, QtCore.QCoreApplication.translate("ObjectManagerGUI",'Warning'),

QtCore.QCoreApplication.translate("ObjectManagerGUI","Deleting this object will delete it permanently.\n") +

QtCore.QCoreApplication.translate("ObjectManagerGUI","Do you want to continue?"),

QtWidgets.QMessageBox.Yes | QtWidgets.QMessageBox.No, QtWidgets.QMessageBox.No)

if reply == QtWidgets.QMessageBox.Yes:

self.objManager.deleteObject(selObject)

self.refreshTreeWidget()

def getSelected(self):

"""Returns the selected resource, as a string of the resources name"""

selectedObjects = self.objTree.selectedItems()

if not len(selectedObjects): return None

selObject = selectedObjects[0].text(0)

return selObject

def openResourceMenu(self, resourceMenuType, editResource=None):

"""

This will open a resource menu window. Before this happens, it pauses the cameraWidget on the ObjectMenu,

and after the resource menu is closed, it will select the newly created item (if one was created).

:param resoureceMenuType: MakeGroupWindow, MakeRecordingWindow, MakeFunctionWindow, MakeObjectWindow, etc.

:param editResource: If there is an object (say, a vision object or recording object) that you want to open the

editing window for, then pass it in through here.

"""

self.cameraWidget.pause()

# Open the window (the code will only continue once the window has closed)

menuWindow = resourceMenuType(editResource, self.env, parent=self)

# If the menuWindow created a new object, then select

if menuWindow.newObject is not None:

self.refreshTreeWidget(selectedItem=menuWindow.newObject.name)

self.cameraWidget.play()

def closeEvent(self, event):

# This ensures that the cameraWidget will no longer be open when the window closes

self.vision.endAllTrackers()

self.cameraWidget.close()

# Make New Group Menu

class MakeGroupWindow(QtWidgets.QDialog):

"""

This opens up when "New Group" button is clicked or when "add objects to group" is clicked in ObjectManager

"""

def __init__(self, currentObj, env, parent):

super(MakeGroupWindow, self).__init__(parent)

self.newObject = currentObj

self.objManager = env.getObjectManager()

self.forbiddenNames = self.objManager.getForbiddenNames()

# Initialize UI variables

self.nameEdit = QtWidgets.QLineEdit()

self.objList = QtWidgets.QListWidget()

self.applyBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Apply"), self)

self.hintLbl = QtWidgets.QLabel("")

# Add trackablObjs to the objList

objNames = self.objManager.getObjectNameList(typeFilter=self.objManager.TRACKABLEOBJ)

for i, objID in enumerate(objNames):

self.objList.addItem(objID)

self.objList.setSelectionMode(QtWidgets.QAbstractItemView.MultiSelection)

# If this is in 'editing' mode, restore the last state

if self.newObject is not None:

self.nameEdit.setText(self.newObject.name)

self.nameEdit.setDisabled(True)

# Select any objets that are in the group

prevChosenIDs = [obj.name for obj in self.newObject.getMembers()]

for i, objID in enumerate(objNames):

if objID in prevChosenIDs:

self.objList.item(i).setSelected(True)

# Initialize UI Elements

self.initUI()

self.isComplete()

# Execute window and garbage collect afterwards

finished = self.exec_()

self.close()

self.deleteLater()

# If the window was valid, then create the object

if finished:

self.createNewObject()

def initUI(self):

nameLbl = QtWidgets.QLabel(QtCore.QCoreApplication.translate("ObjectManagerGUI","Group Name: "))

cancelBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Cancel"), self)

# Set up slots to check if the "Apply" button should be enabled or disabled

self.objList.itemSelectionChanged.connect(self.isComplete)

self.applyBtn.clicked.connect(self.accept)

cancelBtn.clicked.connect(self.reject)

bold = QtGui.QFont()

bold.setBold(True)

self.hintLbl.setFont(bold)

self.hintLbl.setWordWrap(True)

row1 = QtWidgets.QHBoxLayout()

row2 = QtWidgets.QHBoxLayout()

row3 = QtWidgets.QHBoxLayout()

row4 = QtWidgets.QHBoxLayout()

self.nameEdit.textChanged.connect(self.isComplete)

row1.addWidget(nameLbl)

row1.addWidget(self.nameEdit)

row2.addWidget(self.objList)

row3.addWidget(self.hintLbl)

row4.addWidget(cancelBtn)

row4.addStretch(1)

row4.addWidget(self.applyBtn)

mainVLayout = QtWidgets.QVBoxLayout()

mainVLayout.addLayout(row1)

mainVLayout.addLayout(row2)

mainVLayout.addLayout(row3)

mainVLayout.addLayout(row4)

self.setLayout(mainVLayout)

self.setMinimumHeight(400)

self.setWindowTitle(QtCore.QCoreApplication.translate("ObjectManagerGUI",'Add Objects to Group'))

def createNewObject(self):

name = self.nameEdit.text()

self.objManager.deleteObject(name)

# Get the name of every selected object

selectedItems = self.objList.selectedItems()

selectedObjs = []

for item in selectedItems:

selectedObjs.append(item.text())

# Add the appropriate tags to every object

for objID in selectedObjs:

trackableInGroup = self.objManager.getObject(objID)

trackableInGroup.addTag(name)

self.objManager.saveObject(trackableInGroup)

self.objManager.refreshGroups()

self.newObject = self.objManager.getObject(name)

def isComplete(self):

newHintText = ""

if self.nameEdit.isEnabled():

newName, newHintText = sanitizeName(self.nameEdit.text(), self.forbiddenNames)

self.nameEdit.setText(newName)

if len(self.objList.selectedItems()) == 0:

newHintText = QtCore.QCoreApplication.translate("ObjectManagerGUI","You must select at least one object")

self.hintLbl.setText(newHintText)

# Set the apply button enabled if everything is filled out correctly

setEnabled = len(newHintText) == 0 and len(self.objList.selectedItems())

self.applyBtn.setEnabled(setEnabled)

# Make New Motion Recording Menu

class MakeRecordingWindow(QtWidgets.QDialog):

"""

This opens up when "New Group" button is clicked or when "add objects to group" is clicked in ObjectManager

"""

def __init__(self, currentObj, env, parent):

super(MakeRecordingWindow, self).__init__(parent)

self.newObject = currentObj # This is where the created object will go after being made in objManager

self.robot = env.getRobot()

self.objManager = env.getObjectManager()

self.forbiddenNames = self.objManager.getForbiddenNames()

self.recording = False # State of recording

self.baseTime = None # When continuing a recording, this is the time of the last recording ending

self.startTime = None # Keeps track of *when* the recording started

self.lastTime = None # Used to keep track of time between recorded points

self.motionPath = [] # Format: [(time, gripperStatus, angleA, angleB, angleC, angleD), (...)]

# Initialize UI variables

self.timer = QtCore.QTimer()

self.nameEdit = QtWidgets.QLineEdit()

self.motionTbl = QtWidgets.QTableWidget()

self.recordBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Record"))

self.applyBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Apply"), self)

self.hintLbl = QtWidgets.QLabel("")

# If this is in 'editing' mode, restore the last state

if self.newObject is not None:

self.motionPath = self.newObject.getMotionPath()

self.nameEdit.setText(self.newObject.name)

self.nameEdit.setDisabled(True)

# Move to the last position of the current recording

lastPos = self.motionPath[-1][2:]

pos = self.robot.getFK(servo0=lastPos[0], servo1=lastPos[1], servo2=lastPos[2])

self.robot.setPos(coord=pos, wait=False)

self.robot.setServoAngles(servo3=lastPos[3])

# Initialize UI Elements

self.initUI()

self.refreshMotionList()

self.isComplete()

# Check if the robot is connected before running

robot = env.getRobot()

if not robot.connected():

message = QtCore.QCoreApplication.translate("ObjectManagerGUI","A robot must be connected to do movement recording.")

QtWidgets.QMessageBox.question(self, QtCore.QCoreApplication.translate("ObjectManagerGUI",'Error'), message, QtWidgets.QMessageBox.Ok)

return

# Execute window and garbage collect afterwards

finished = self.exec_()

self.close()

self.deleteLater()

# If the window was valid, then create the object

if finished:

self.createNewObject()

def initUI(self):

self.recordBtn.setMinimumWidth(150)

self.recordBtn.setIcon(QtGui.QIcon(Paths.record_start))

monospace = QtGui.QFont("Monospace")

monospace.setStyleHint(QtGui.QFont.TypeWriter)

self.motionTbl.setFont(monospace)

self.motionTbl.setColumnCount(3)

self.motionTbl.setHorizontalHeaderLabels((QtCore.QCoreApplication.translate("ObjectManagerGUI","Time"), QtCore.QCoreApplication.translate("ObjectManagerGUI","Servo Angles"), QtCore.QCoreApplication.translate("ObjectManagerGUI","Gripper Action")))

self.motionTbl.verticalHeader().hide()

# Create non global UI variables

nameLbl = QtWidgets.QLabel(QtCore.QCoreApplication.translate("ObjectManagerGUI","Recording Name: "))

pathLbl = QtWidgets.QLabel(QtCore.QCoreApplication.translate("ObjectManagerGUI","Recorded Path"))

hint2Lbl = QtWidgets.QLabel(QtCore.QCoreApplication.translate("ObjectManagerGUI","Press 'Record' to start recording robot movements.\n") +

QtCore.QCoreApplication.translate("ObjectManagerGUI","While recording, press the robots suction cup to activate the pump.\n") +

QtCore.QCoreApplication.translate("ObjectManagerGUI","When you press Apply, areas of no movement at the start and end\n") +

QtCore.QCoreApplication.translate("ObjectManagerGUI","will be trimmed out."))

cancelBtn = QtWidgets.QPushButton(QtCore.QCoreApplication.translate("ObjectManagerGUI","Cancel"), self)

# Connect everything

self.timer.timeout.connect(self.recordAction)

self.recordBtn.clicked.connect(self.toggleRecording)

self.applyBtn.clicked.connect(self.accept)

cancelBtn.clicked.connect(self.reject)

# Bolden the hintLbl

bold = QtGui.QFont()

bold.setBold(True)

self.hintLbl.setFont(bold)

self.hintLbl.setWordWrap(True)

self.nameEdit.textChanged.connect(self.isComplete)

# Create the rows then fill them

row1 = QtWidgets.QHBoxLayout()

row2 = QtWidgets.QHBoxLayout()

row3 = QtWidgets.QHBoxLayout()

row4 = QtWidgets.QHBoxLayout()

row5 = QtWidgets.QHBoxLayout()

row6 = QtWidgets.QHBoxLayout()

row7 = QtWidgets.QHBoxLayout()

row1.addWidget(nameLbl)

row1.addWidget(self.nameEdit)

row2.addWidget(self.recordBtn)

row3.addWidget(pathLbl)

row4.addWidget(self.motionTbl)

row5.addWidget(hint2Lbl)

row6.addWidget(self.hintLbl)

row7.addWidget(cancelBtn)

row7.addStretch(1)

row7.addWidget(self.applyBtn)

# Add everything to the main layout then touch it up a bit

mainVLayout = QtWidgets.QVBoxLayout()

mainVLayout.addLayout(row1)

mainVLayout.addLayout(row2)

mainVLayout.addLayout(row3)

mainVLayout.addLayout(row4)

mainVLayout.addLayout(row5)

mainVLayout.addLayout(row6)

mainVLayout.addLayout(row7)

self.setLayout(mainVLayout)

self.setMinimumHeight(550)

self.setMinimumWidth(500)

self.setWindowTitle(QtCore.QCoreApplication.translate("ObjectManagerGUI",'Create a Movement Recording'))

# Table events

def resizeEvent(self, event):

super(MakeRecordingWindow, self).resizeEvent(event)

# Modify the resize event to keep the columns evenly distributed

tableSize = self.motionTbl.width()

sideHeaderWidth = self.motionTbl.verticalHeader().width()

tableSize -= sideHeaderWidth

numberOfColumns = self.motionTbl.columnCount()

remainingWidth = tableSize % numberOfColumns

for columnNum in range(numberOfColumns):

if remainingWidth > 0:

self.motionTbl.setColumnWidth(columnNum, int(tableSize / numberOfColumns) + 1)

remainingWidth -= 1

else:

self.motionTbl.setColumnWidth(columnNum, int(tableSize / numberOfColumns))

def addActionToTable(self, action, loading=False):

# Add a single motionpath point to the self.motionTbl

row = self.motionTbl.rowCount()

time = str(round(action[0], 2))

gripper = str((False, True)[action[1]])

servos = str(round(action[2], 1)) + ", " + \

str(round(action[3], 1)) + ", " + \

str(round(action[4], 1)) + ", " + \

str(round(action[5], 1))

self.motionTbl.insertRow(row)

self.motionTbl.setItem(row, 0, QtWidgets.QTableWidgetItem(time))

self.motionTbl.setItem(row, 1, QtWidgets.QTableWidgetItem(servos))

self.motionTbl.setItem(row, 2, QtWidgets.QTableWidgetItem(gripper))

# To prevent noticible lag when loading the window, don't skip rows when loading the table

if not loading:

self.motionTbl.scrollToItem(self.motionTbl.item(row, 0))

def refreshMotionList(self):

# # Clear previous data and remove previous rows

# self.motionTbl.clear()

while self.motionTbl.rowCount() > 0: self.motionTbl.removeRow(0)

for r, action, in enumerate(self.motionPath):

self.addActionToTable(action, loading=True)

# Recording events

def toggleRecording(self):

self.lastTime = time()

if self.recording:

self.robot.setPump(False)

self.recordBtn.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Record"))

if len(self.motionPath):

self.recordBtn.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Continue Recording"))

self.timer.stop()

self.recordBtn.setIcon(QtGui.QIcon(Paths.record_start))

else:

self.baseTime = 0

if len(self.motionPath):

self.baseTime = self.motionPath[-1][0]

self.startTime = time()

self.lastTime = self.startTime

self.timer.start()

self.recordBtn.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Stop Recording"))

self.recordBtn.setIcon(QtGui.QIcon(Paths.record_end))

self.robot.setActiveServos(all=False)

self.recording = not self.recording

self.isComplete()

def recordAction(self):

# This is where a point is recorded from the robot

GRIPPER = 1

now = time()

if now - self.lastTime < 0.01: return # If 10 ms havent passed, ignore

print("Current FPS: ", 1.0 / (now - self.lastTime))

self.lastTime = now

# Every 10 times check the gripper status

if len(self.motionPath) == 0:

gripperStatus = 0

elif len(self.motionPath) % 15 == 0:

# If the robots tip is pressed, toggle the pump

if self.robot.getTipSensor():

gripperStatus = int(not self.motionPath[-1][GRIPPER])

self.robot.setPump(gripperStatus)

else:

gripperStatus = self.motionPath[-1][GRIPPER]

else:

gripperStatus = self.motionPath[-1][GRIPPER]

t = now - self.startTime + self.baseTime

angles = self.robot.getAngles()

tip = gripperStatus

newAction = [round(t, 3), int(tip), angles[0], angles[1], angles[2], angles[3]]

self.motionPath.append(newAction)

self.addActionToTable(newAction)

def trimPath(self):

"""

Gets rid of motionless parts of the motion path at the beginning and end, where the user is presumably pressing

"record" or pressing "stop recording"

"""

if len(self.motionPath) <= 20: return

TIME = 0

GRIPPER = 1

SERVO0 = 2

SERVO1 = 3

SERVO2 = 4

SERVO3 = 5

minDist = 1 # In degrees

startPoint = self.motionPath[0]

trimStart = 0

for i, p in enumerate(self.motionPath):

trimStart = i

# print("Start: ", startPoint, "curr: ", p)

if abs(p[SERVO0] - startPoint[SERVO0]) > minDist: break

if abs(p[SERVO1] - startPoint[SERVO1]) > minDist: break

if abs(p[SERVO2] - startPoint[SERVO2]) > minDist: break

if abs(p[SERVO3] - startPoint[SERVO3]) > minDist: break

if trimStart < len(self.motionPath) - 1:

self.motionPath = self.motionPath[trimStart:]

if len(self.motionPath) <= 20: return

endPoint = self.motionPath[-1]

trimEnd = 0

self.motionPath.reverse()

for i, p in enumerate(self.motionPath):

trimEnd = i

# print("Start: ", endPoint, "curr: ", p)

if abs(p[SERVO0] - endPoint[SERVO0]) > minDist: break

if abs(p[SERVO1] - endPoint[SERVO1]) > minDist: break

if abs(p[SERVO2] - endPoint[SERVO2]) > minDist: break

if abs(p[SERVO3] - endPoint[SERVO3]) > minDist: break

# Correct the order

self.motionPath.reverse()

if trimStart < len(self.motionPath) - 1:

self.motionPath = self.motionPath[:-trimEnd]

# Subtract the time from the start to every cell in the array now

startTime = self.motionPath[0][TIME] - .1

for i in range(len(self.motionPath)):

self.motionPath[i][TIME] -= startTime

if self.motionPath[i][TIME] < 0:

printf("GUI| ERROR: Time is negative in motionPath!")

startPoint[TIME] = 0

endPoint[TIME] = self.motionPath[-1][TIME] + .1

self.motionPath = [startPoint] + self.motionPath + [endPoint]

def optimizeMotionPath(self):

if len(self.motionPath) <= 20: return

TIME = 0

GRIPPER = 1

SERVO0 = 2

SERVO1 = 3

SERVO2 = 4

SERVO3 = 5

# Run the motion path through a gausian smoother

# Smooth the Time values, and all the servo values

degree = 8

toSmooth = np.asarray(self.motionPath[:])[:, [TIME, SERVO0, SERVO1, SERVO2, SERVO3]].tolist()

smooth = rv.smoothListGaussian(toSmooth, degree)

window = degree * 2 - 1

otherData = np.asarray(self.motionPath)[:, [GRIPPER]]

cutData = otherData[int(window / 2):-(int(window / 2) + 1), :]

smooth = np.asarray(smooth)

timeAndGripper = np.hstack((smooth[:, [0]], np.asarray(cutData)))

unrounded = np.hstack((timeAndGripper, smooth[:, 1:])).tolist()

self.motionPath = unrounded

self.roundMotionPath()

def roundMotionPath(self):

# Makes sure saves don't take a lot of space, by rounding any float errors

self.motionPath = list(map(lambda a: [float(round(a[0], 2)),

int(a[1]),

float(round(a[2], 1)),

float(round(a[3], 1)),

float(round(a[4], 1)),

float(round(a[5], 1))], self.motionPath))

def createNewObject(self):

if self.newObject is None:

self.optimizeMotionPath()

self.trimPath()

self.roundMotionPath()

# Create an actual TrackableObject with this information

if self.newObject is not None:

name = self.newObject.name

motionObj = self.objManager.getObject(name)

else:

name = self.nameEdit.text()

motionObj = MotionPath(name)

motionObj.setup(motionPath = self.motionPath)

self.objManager.saveObject(motionObj)

self.newObject = motionObj

def isComplete(self):

newHintText = ""

if self.nameEdit.isEnabled():

newName, newHintText = sanitizeName(self.nameEdit.text(), self.forbiddenNames)

self.nameEdit.setText(newName)

self.hintLbl.setText(newHintText)

if len(self.motionPath) <= 20:

self.hintLbl.setText(QtCore.QCoreApplication.translate("ObjectManagerGUI","Recording must be longer than 20 points of data"))

# Set the apply button enabled if everything is filled out correctly

setEnabled = len(newHintText) == 0 and len(self.motionPath) > 20 and not self.recording

self.applyBtn.setEnabled(setEnabled)

def close(self):

self.robot.setPump(False)

self.timer.stop()

# Make a Custom Function

class MakeFunctionWindow(QtWidgets.QDialog):

class ArgumentsList(QtWidgets.QWidget):

"""

This is a list where the user can add/delete elements, its meant for setting the arguments of the function

"""

def __init__(self, parent):

super().__init__(parent)

self.argList = QtWidgets.QListWidget()

self.argList.setMaximumHeight(55)

addBtn = QtWidgets.QPushButton()

delBtn = QtWidgets.QPushButton()

addBtn.setIcon(QtGui.QIcon(Paths.create))

delBtn.setIcon(QtGui.QIcon(Paths.delete))

addBtn.clicked.connect(self.addArgument)

delBtn.clicked.connect(self.deleteArgument)

col1 = QtWidgets.QVBoxLayout()

col2 = QtWidgets.QVBoxLayout()

col1.addWidget(self.argList)

col2.addWidget(addBtn)

col2.addWidget(delBtn)

col2.addStretch()

mainHLayout = QtWidgets.QHBoxLayout()

# mainHLayout.addStretch()

mainHLayout.addLayout(col1)

mainHLayout.addLayout(col2)

# mainHLayout.addStretch()