def setVideo(self, state):
"""
Change the state of the videostream. The state can be play, pause, or simply "toggle
:param state: "play", "pause", or "toggle"
:return:
"""
printf("GUI| Setting video to state: ", state)
# Don't change anything if no camera ID has been added yet
cameraID = self.env.getSetting("cameraID")
if cameraID is None: return
if state == "toggle":
if self.env.getVStream().paused:
self.setVideo("play")
return
else:
self.setVideo("pause")
return
vStream = self.env.getVStream()
if state == "play":
# Make sure the videoStream object has a camera, or if the cameras changed, change it
# if not vStream.connected() or not vStream.cameraID == cameraID:
# vStream.setNewCamera(cameraID)
self.cameraWidget.play()
if state == "pause":
self.cameraWidget.pause()
def is_ready(self):
if self.__serial.readline().startswith(b"@1"):
printf("Communication| Connected to port: {}".format(
self.__serial.port))
return True
else:
return False
def loadAllObjects(self):
# Load all objects into the ObjectManager
foldersAndItems = os.listdir(self.__directory)
for folder in foldersAndItems:
path = self.__directory + "\\" + folder
newObj = None
createObj = lambda objType, prefix, directory: objType(folder.replace(prefix + " ", ""), path)
if not os.path.isdir(path):
printf("ERROR: Could not find directory ", path)
continue
# Handle any TrackableObject loading
if "Trackable" in folder: newObj = createObj(TrackableObject, "Trackable", folder)
if "MotionPath" in folder: newObj = createObj(MotionPath, "MotionPath", folder)
# Check that loading is complete and add the object if it was created successfully
if newObj is None:
printf("ERROR: Could not find relevant object for folder: ", folder)
continue
if newObj.loadSuccess: self.__addObject(newObj)
self.refreshGroups()
def startThread(self):
if self.mainThread is None:
self.running = True
self.mainThread = Thread(target=self.__videoThread)
self.mainThread.start()
else:
printf("ERROR: Tried to create mainThread, but mainThread already existed.")
def mousePressEvent(self, event):
# If the user already has selected an image, leave.
self.takeAnother()
if event.button() == QtCore.Qt.LeftButton:
# Make sure the click was within the boundaries of the frame
width = self.frameLbl.pixmap().width()
height = self.frameLbl.pixmap().height()
pos = (event.pos().x(), event.pos().y())
if not 0 < pos[0] < width: return
if not 0 < pos[1] < height: return
# Get a frame from OpenCV, so that it can be cropped when the user releases the mouse button
self.selectedImage = self.vStream.getFrame()
if self.selectedImage is None:
printf("GUI| ERROR: getCVFrame() returned None Frame! ")
return
# Set the rectangles position and unhide the rectangle
self.origin = QtCore.QPoint(event.pos())
self.rectangle.setGeometry(
QtCore.QRect(self.origin, QtCore.QSize()))
self.rectangle.show()
def smoothListGaussian(list1, degree):
"""
Smooth a list using guassian smoothing, with degree Degree. Lists are smoothed by columns, not rows.
:param list1: [[column1, column2, column3... column n], ... [element n]]
:param degree: The gaussian smoothing constant.
"""
window = degree * 2 - 1
if len(list1) < window:
printf(
"RobotVision| ERROR: Attempted to smooth list that was too small to be smoothed"
)
return None
weight = np.array([1.0] * window)
weightGauss = []
for i in range(window):
i = i - degree + 1
frac = i / float(window)
gauss = 1 / (np.exp((4 * frac)**2))
weightGauss.append(gauss)
weight = np.array(weightGauss) * weight
smoothed = [0.0] * (len(list1) - window)
for i in range(len(smoothed)):
smoothing = [0.0 for i in range(len(list1[i]))] # [0.0, 0.0, 0.0]
for e, w in zip(list1[i:i + window], weight):
smoothing = smoothing + np.multiply(e, w)
smoothed[i] = smoothing / sum(weight)
return smoothed
def __programThread(self):
printf("Interpreter| ##### STARTING PROGRAM #####\n")
fpsTimer = FpsTimer(fps=30)
# Main program loop - where events are checked and run
while not self.isExiting():
# Maintain constant FPS using an FPSTimer
fpsTimer.wait()
if not fpsTimer.ready(): continue
# currRunning keeps track of what was run, so the GUI can draw it
self.currRunning = {"event": -1, "command": -1}
# Check every event, in order of the list
exitCommand = False # If the interpreter reached an "ExitProgram" command
for index, event in enumerate(self.events):
if self.isExiting(): break
if not event.isActive(): continue
# If the event has been activated, run the commandList
self.currRunning["event"] = self.events.index(event)
exitCommand = self.interpretCommandList(event.commandList)
if exitCommand: break
if exitCommand: break
self.events = []
printf("Interpreter| Interpreter Thread Ended")
self.mainThread = None
def run(self):
if len(self.errors): return
# Find the object using vision
_, tracked = self.vision.getObjectLatestRecognition(self.trackable)
if tracked is None: return False
camAngle = tracked.rotation[2]
robAngle = self.transform.cameraToRobotRotation(
camAngle) # Angle from the robots X axis
# Before doing any tracking, evaluate the "Relative" number to make sure its valid
startAngle, successL = self.interpreter.evaluateExpression(
self.parameters["start"])
endAngle, successU = self.interpreter.evaluateExpression(
self.parameters["end"])
# If the evaluation failed, cancel the command
if not (successL and successU):
printf(
"Commands| ERROR: Could not evaluate the lowerAngle or the upperAngle. Canceling command. "
)
return False
end = endAngle - startAngle + 360.0 if (
endAngle - startAngle) < 0.0 else endAngle - startAngle
mid = robAngle - startAngle + 360.0 if (
robAngle - startAngle) < 0.0 else robAngle - startAngle
isBetween = mid < end
return isBetween
def searchTrackedHistory(self, trackable=None, maxAge=0, minPoints=None):
"""
Search through trackedHistory to find an object that meets the criteria
:param trackableObject: Specify if you want to find a particular object
:param maxAge: Specify if you wannt to find an object that was found within X frames ago
:param minPoints: Specify if you want to find an object with a minimum amount of tracking points
"""
maxFrame = maxAge + 1
if maxFrame is None or maxFrame >= self.historyLen:
printf(
"Vision| ERROR: Tried to look further in the history than was possible!"
)
maxFrame = self.historyLen
# Safely pull the relevant trackedHistory from the tracker object
with self.workLock:
trackHistory = self.planeTracker.trackedHistory[:maxFrame]
# Check if the object was recognized in the most recent frame. Check most recent frames first.
for historyFromFrame in trackHistory:
for tracked in historyFromFrame:
# If the object meets the criteria
if trackable is not None and not trackable.equalTo(
tracked.view.name):
continue
if minPoints is not None and not tracked.ptCount > minPoints:
continue
return tracked
return None
def setVideo(self, state):
"""
Change the state of the videostream. The state can be play, pause, or simply "toggle
:param state: "play", "pause", or "toggle"
:return:
"""
printf("GUI| Setting video to state: ", state)
# Don't change anything if no camera ID has been added yet
cameraID = self.env.getSetting("cameraID")
if cameraID is None: return
if state == "toggle":
if self.env.getVStream().paused:
self.setVideo("play")
return
else:
self.setVideo("pause")
return
vStream = self.env.getVStream()
if state == "play":
# Make sure the videoStream object has a camera, or if the cameras changed, change it
# if not vStream.connected() or not vStream.cameraID == cameraID:
# vStream.setNewCamera(cameraID)
self.cameraWidget.play()
if state == "pause":
self.cameraWidget.pause()
def searchTrackedHistory(self, trackable=None, maxAge=0, minPoints=None):
"""
Search through trackedHistory to find an object that meets the criteria
:param trackableObject: Specify if you want to find a particular object
:param maxAge: Specify if you wannt to find an object that was found within X frames ago
:param minPoints: Specify if you want to find an object with a minimum amount of tracking points
"""
maxFrame = maxAge + 1
if maxFrame is None or maxFrame >= self.historyLen:
printf("Vision| ERROR: Tried to look further in the history than was possible!")
maxFrame = self.historyLen
# Safely pull the relevant trackedHistory from the tracker object
with self.workLock:
trackHistory = self.planeTracker.trackedHistory[:maxFrame]
# Check if the object was recognized in the most recent frame. Check most recent frames first.
for historyFromFrame in trackHistory:
for tracked in historyFromFrame:
# If the object meets the criteria
if trackable is not None and not trackable.equalTo(tracked.view.name): continue
if minPoints is not None and not tracked.ptCount > minPoints: continue
return tracked
return None
def saveTask(self, promptSaveLocation):
printf("GUI| Saving project")
# If there is no filename, ask for one
if promptSaveLocation or self.fileName is None:
Global.ensurePathExists(Paths.saves_dir)
filename, _ = QtWidgets.QFileDialog.getSaveFileName(parent=self,
caption="Save Task",
filter="Task (*.task)",
directory=Paths.saves_dir)
if filename == "": return False #If user hit cancel
self.fileName = filename
self.env.updateSettings("lastOpenedFile", self.fileName)
# Update the save file
saveData = self.controlPanel.getSaveData()
json.dump(saveData, open(self.fileName, 'w'), sort_keys=False, indent=3, separators=(',', ': '))
self.loadData = deepcopy(saveData) #Update what the latest saved changes are
self.setWindowTitle(self.programTitle + ' ' + self.fileName)
printf("GUI| Project Saved Successfully")
return True
def run(self):
# Special case: If the parameter is "" set the new val to None and success to True
if self.parameters['x'] == "":
newX, successX = None, True
else:
newX, successX = self.interpreter.evaluateExpression(self.parameters['x'])
if self.parameters['y'] == "":
newY, successY = None, True
else:
newY, successY = self.interpreter.evaluateExpression(self.parameters['y'])
if self.parameters['z'] == "":
newZ, successZ = None, True
else:
newZ, successZ = self.interpreter.evaluateExpression(self.parameters['z'])
if successX and successY and successZ:
self.robot.setPos(x=newX, y=newY, z=newZ, relative=self.parameters['relative'])
return True
else:
printf("Commands| ERROR in evaluating either X Y or Z: ", successX, successY, successZ)
return False
def refresh(self):
"""
Refresh which commandList is currently being displayed to the one the user has highlighted. It basically just
goes over certain things and makes sure that everything that should be displaying, is displaying.
"""
# Remove all widgets on the commandList stack (not delete, though!) The chosen event will be added later.
for c in range(0, self.commandListStack.count()):
widget = self.commandListStack.widget(c)
self.commandListStack.removeWidget(widget)
# Get the currently selected event on the eventList
selectedEvent = self.eventList.getSelectedEvent()
# If user has no event selected, make a clear commandList to view
self.deleteEventBtn.setDisabled(selectedEvent is None)
self.changeEventBtn.setDisabled(selectedEvent is None)
self.commandMenuWidget.setDisabled(selectedEvent is None)
self.commandGBox.setTitle("Command List")
if selectedEvent is None:
printf("GUI| No event selected. Hiding buttons.")
return
eventTitle = selectedEvent.title + " Command List"
self.commandGBox.setTitle(eventTitle)
# Add and display the correct widget
self.commandListStack.addWidget(selectedEvent.commandList)
self.commandListStack.setCurrentWidget(selectedEvent.commandList)
def run(self):
if len(self.errors): return
# Find the object using vision
_, tracked = self.vision.getObjectLatestRecognition(self.trackable)
if tracked is None: return False
camAngle = tracked.rotation[2]
robAngle = self.transform.cameraToRobotRotation(camAngle) # Angle from the robots X axis
# Before doing any tracking, evaluate the "Relative" number to make sure its valid
startAngle, successL = self.interpreter.evaluateExpression(self.parameters["start"])
endAngle, successU = self.interpreter.evaluateExpression(self.parameters["end"])
# If the evaluation failed, cancel the command
if not (successL and successU):
printf("Commands| ERROR: Could not evaluate the lowerAngle or the upperAngle. Canceling command. ")
return False
end = endAngle - startAngle + 360.0 if (endAngle - startAngle) < 0.0 else endAngle - startAngle
mid = robAngle - startAngle + 360.0 if (robAngle - startAngle) < 0.0 else robAngle - startAngle
isBetween = mid < end
return isBetween
def nextPressed(self, currentID):
# If next is pressed, warn the user about placing the robots head on the ground
if currentID == 2:
self.robot.setActiveServos(all=False)
warn = QtWidgets.QMessageBox()
warn.setWindowTitle(self.tr("Getting Height of Ground"))
warn.addButton(QtWidgets.QPushButton(self.tr('Yes, the end effector is touching the ground')),
QtWidgets.QMessageBox.YesRole)
warn.setText(self.tr("Important!\nBefore continuing, make sure the robots end effector is touching the "
"ground, and that it is centered below the camera. "
"The program will read the robots coordinates.\n\n"))
reply = warn.exec_()
# Get several samples of the robots current position, and put the average into self.groundCoords
samples = 10
sumCoords = np.float32([0, 0, 0])
for i in range(0, samples):
coord = self.robot.getCoords()
sumCoords += np.float32(coord)
self.groundCoords = list(map(float, sumCoords / samples))
self.robot.setPos(z=self.groundCoords[2] + .5)
self.robot.setActiveServos(servo0=False)
printf("GUI| New ground coordinates set: ", self.groundCoords)
def mousePressEvent(self, event):
# If the user already has selected an image, leave.
self.takeAnother()
if event.button() == QtCore.Qt.LeftButton:
# Make sure the click was within the boundaries of the frame
width = self.frameLbl.pixmap().width()
height = self.frameLbl.pixmap().height()
pos = (event.pos().x(), event.pos().y())
if not 0 < pos[0] < width: return
if not 0 < pos[1] < height: return
# Get a frame from OpenCV, so that it can be cropped when the user releases the mouse button
self.selectedImage = self.vStream.getFrame()
if self.selectedImage is None:
printf("GUI| ERROR: getCVFrame() returned None Frame! ")
return
# Set the rectangles position and unhide the rectangle
self.origin = QtCore.QPoint(event.pos())
self.rectangle.setGeometry(QtCore.QRect(self.origin, QtCore.QSize()))
self.rectangle.show()
def saveTask(self, promptSaveLocation):
printf("GUI| Saving project")
# If there is no filename, ask for one
if promptSaveLocation or self.fileName is None:
Global.ensurePathExists(Paths.saves_dir)
filename, _ = QtWidgets.QFileDialog.getSaveFileName(parent=self,
caption="Save Task",
filter="Task (*.task)",
directory=Paths.saves_dir)
if filename == "": return False #If user hit cancel
self.fileName = filename
self.env.updateSettings("lastOpenedFile", self.fileName)
# Update the save file
saveData = self.controlPanel.getSaveData()
json.dump(saveData, open(self.fileName, 'w'), sort_keys=False, indent=3, separators=(',', ': '))
self.loadData = deepcopy(saveData) #Update what the latest saved changes are
self.setWindowTitle(self.programTitle + ' ' + self.fileName)
printf("GUI| Project Saved Successfully")
return True
def run(self):
# Special case: If the parameter is "" set the new val to None and success to True
if self.parameters['x'] == "":
newX, successX = None, True
else:
newX, successX = self.interpreter.evaluateExpression(
self.parameters['x'])
if self.parameters['y'] == "":
newY, successY = None, True
else:
newY, successY = self.interpreter.evaluateExpression(
self.parameters['y'])
if self.parameters['z'] == "":
newZ, successZ = None, True
else:
newZ, successZ = self.interpreter.evaluateExpression(
self.parameters['z'])
if successX and successY and successZ:
self.robot.setPos(x=newX,
y=newY,
z=newZ,
relative=self.parameters['relative'])
return True
else:
printf("Commands| ERROR in evaluating either X Y or Z: ", successX,
successY, successZ)
return False
def startThread(self):
if self.mainThread is None:
self.running = True
self.mainThread = Thread(target=self.__videoThread) # Cannot be Daemon thread
self.mainThread.start()
else:
printf("Video| Tried to create mainThread, but mainThread already existed.")
def getObjectBruteAccurate(self, trackable, minPoints=-1, maxAge=0, maxAttempts=1):
"""
This will brute-force the object finding process somewhat, and ensure a good recognition, or nothing at all.
:param trackable: The TrackableObject you intend to find
:param minPoints: Minimum amount of recognition points that must be found in order to track. -1 means ignore
:param maxAge: How recent the recognition was, in "frames gotten from camera"
:param maxAttempts: How many frames it should wait for before quitting the search.
:return:
"""
# Get a super recent frame of the object
for i in range(0, maxAttempts):
if self.exiting:
printf("Exiting early!")
break
# If the frame is too old or marker doesn't exist or doesn't have enough points, exit the function
frameAge, trackedObj = self.getObjectLatestRecognition(trackable)
if trackedObj is None or frameAge > maxAge or trackedObj.ptCount < minPoints:
if i == maxAttempts - 1: break
self.waitForNewFrames()
continue
# If the object was successfully found with the correct attributes, return it
return trackedObj
return None
def run(self):
interpreter = self.interpreter
# Get the variable. If that doesn't work, quit
# variableValue, successVar = interpreter.getVariable(self.parameters['variable'])
# if not successVar: return False
# Evaluate the expression. If that doesn't work, quit
# compareValue, successExp = interpreter.evaluateExpression(self.parameters['expression'])
# print("Compare value: ", compareValue)
# if not successExp: return False
# Compare the value of the expression using the operator from the parameters
operations = ['==', '!=', '>', '<']
variable = self.parameters['variable']
operation = operations[self.parameters['test']]
expression = self.parameters["expression"]
scriptString = variable + operation + expression #
testResult, success = interpreter.evaluateExpression(scriptString)
if not success: return False
printf("Testing: ", scriptString)
print(testResult, success)
# If the expression was evaluated correctly, then return the testResult. Otherwise, return False
return testResult
def __programThread(self):
printf("Interpreter| ##### STARTING PROGRAM #####\n")
fpsTimer = FpsTimer(fps=30)
# Main program loop - where events are checked and run
while not self.isExiting():
# Maintain constant FPS using an FPSTimer
fpsTimer.wait()
if not fpsTimer.ready(): continue
# currRunning keeps track of what was run, so the GUI can draw it
self.currRunning = {"event": -1, "command": -1}
# Check every event, in order of the list
exitCommand = False # If the interpreter reached an "ExitProgram" command
for index, event in enumerate(self.events):
if self.isExiting(): break
if not event.isActive(): continue
# If the event has been activated, run the commandList
self.currRunning["event"] = self.events.index(event)
exitCommand = self.interpretCommandList(event.commandList)
if exitCommand: break
if exitCommand: break
self.events = []
printf("Interpreter| Interpreter Thread Ended")
self.mainThread = None
def refresh(self):
"""
Refresh which commandList is currently being displayed to the one the user has highlighted. It basically just
goes over certain things and makes sure that everything that should be displaying, is displaying.
"""
# Remove all widgets on the commandList stack (not delete, though!) The chosen event will be added later.
for c in range(0, self.commandListStack.count()):
widget = self.commandListStack.widget(c)
self.commandListStack.removeWidget(widget)
# Get the currently selected event on the eventList
selectedEvent = self.eventList.getSelectedEvent()
# If user has no event selected, make a clear commandList to view
self.deleteEventBtn.setDisabled(selectedEvent is None)
self.changeEventBtn.setDisabled(selectedEvent is None)
self.commandMenuWidget.setDisabled(selectedEvent is None)
self.commandGBox.setTitle("Command List")
if selectedEvent is None:
printf("GUI| No event selected. Hiding buttons.")
return
eventTitle = selectedEvent.title + " Command List"
self.commandGBox.setTitle(eventTitle)
# Add and display the correct widget
self.commandListStack.addWidget(selectedEvent.commandList)
self.commandListStack.setCurrentWidget(selectedEvent.commandList)
def deleteSelectedEvent(self):
printf("GUI| Removing selected event")
# Get the current item it's corresponding event
selectedItem = self.getSelectedEventItem()
if selectedItem is None:
QtWidgets.QMessageBox.question(
self, 'Error', 'You need to select an event to delete',
QtWidgets.QMessageBox.Ok)
return
# If there are commands inside the event, ask the user if they are sure they want to delete it
if len(self.getSelectedEvent().commandList.commands) > 0:
reply = QtWidgets.QMessageBox.question(
self, 'Message',
'Are you sure you want to delete this event and all its commands?',
QtWidgets.QMessageBox.Yes | QtWidgets.QMessageBox.No,
QtWidgets.QMessageBox.No)
if reply == QtWidgets.QMessageBox.No:
printf("GUI| User rejected deleting the event")
return
# Delete the event item and it's corresponding event
self.deleteEvent(selectedItem)
def saveObject(self, newObject):
# If not new, replace self
wasNew = self.__addObject(newObject)
if not wasNew:
printf("ObjectManager| Tried to add object that already existed, ", newObject.name)
newObject.save(self.__getDirectory(newObject))
def updateSettings(self, category, newSettings):
"""
Apply any new settings that have been changed. If they've been changed, also save the program.
"""
current = self.__settings[category]
# Create a quick function that will check if a setting has been changed. If it has, an action will be taken.
# If settings change, then save the changes to the config file, and update the self.__settings dictionary
if (current is None or not current == newSettings):
printf("Environment| Saving setting: ", category)
# Update the self.__settings dictionary
self.__settings[category] = deepcopy(newSettings)
# Save the settings to a file
json.dump(self.__settings, open(self.__settingsPath, 'w'),
sort_keys=False, indent=3, separators=(',', ': '))
# Now, perform any updates that might need doing
if category == "coordCalibrations":
ptPairs = self.__settings["coordCalibrations"]["ptPairs"]
self.__transform = Transform(ptPairs)
else:
printf("Environment| No settings changed: ", category)
def __parseArgs(self, message, command, arguments):
responseDict = {n: 0 for n in arguments} #Fill the dictionary with zero's
# Do error checking, in case communication didn't work
if message is False:
printf("Since an error occured in communication, returning 0's for all arguments!")
return responseDict
if command not in message:
printf("ERROR: The message did not come with the appropriate command: ", command)
return responseDict
# Get rid of the "command" part of the message, so it's just arguments and their numbers
message = message.replace(command, "")
# Get the arguments and place them into the array
for i, arg in enumerate(arguments):
if i < len(arguments) - 1:
responseDict[arg] = message[message.find(arg) + 1: message.find(arguments[i + 1])]
else:
responseDict[arg] = message[message.find(arg) + 1:]
responseDict[arg] = float(responseDict[arg])
return responseDict
def updateSettings(self, category, newSettings):
"""
Apply any new settings that have been changed. If they've been changed, also save the program.
"""
current = self.__settings[category]
# Create a quick function that will check if a setting has been changed. If it has, an action will be taken.
# If settings change, then save the changes to the config file, and update the self.__settings dictionary
if (current is None or not current == newSettings):
printf("Environment| Saving setting: ", category)
# Update the self.__settings dictionary
self.__settings[category] = deepcopy(newSettings)
# Save the settings to a file
json.dump(self.__settings,
open(self.__settingsPath, 'w'),
sort_keys=False,
indent=3,
separators=(',', ': '))
# Now, perform any updates that might need doing
if category == "coordCalibrations":
ptPairs = self.__settings["coordCalibrations"]["ptPairs"]
self.__transform = Transform(ptPairs)
else:
printf("Environment| No settings changed: ", category)
def addEvent(self, eventType, parameters=None, commandListSave=None):
if commandListSave is None: commandListSave = []
# If a file is loading, then optimize the process by changing it a bit.
isLoading = len(commandListSave) > 0
# Check if the event being added already exists in the self.events dictionary
if not isLoading:
for _, item in self.events.items():
if isinstance(item,
eventType) and (item.parameters == parameters
or parameters is None):
printf(
"GUI| Event already exists, disregarding user input.")
return None
newEvent = eventType(parameters)
newCommandList = CommandList(self.env, parent=self)
newCommandList.loadData(commandListSave)
newEvent.commandList = newCommandList
# Create the widget item to visualize the event
blankWidget = EventsGUI.EventWidget(self)
eventWidget = newEvent.dressWidget(blankWidget)
# Figure out where the event will be placed in the list by looking at the "priority" of the other events
placeIndex = 0
for index, event in enumerate(self.getEventsOrdered()):
if newEvent.priority < event.priority:
break
# If they are of the same priority, then put them in alphabetical order
if newEvent.priority == event.priority:
if min(newEvent.title, event.title) == newEvent.title: break
placeIndex = index + 1
# Create the list item to put the widget item inside of
listWidgetItem = QtWidgets.QListWidgetItem(self)
listWidgetItem.setSizeHint(
eventWidget.sizeHint()) # Widget will not appear without this line
# Do some finagling so pyQt will let me insert an item at 'placeIndex' row
self.addItem(listWidgetItem)
lastRow = self.indexFromItem(listWidgetItem).row()
takenlistWidgetItem = self.takeItem(lastRow)
self.insertItem(placeIndex, takenlistWidgetItem)
# Add the widget to the list item
self.setItemWidget(listWidgetItem, eventWidget)
self.events[eventWidget] = newEvent
if not isLoading:
self.setCurrentRow(placeIndex) # Select the newly added event
if self.count() > 0: self.hintLbl.hide()
return placeIndex # Returns where the object was placed
def __sendAndRecieve(self, cmnd):
"""
This command will send a command and receive the robots response. There must always be a response!
Responses should be recieved immediately after sending the command, after which the robot will proceed to
perform the action.
:param cmnd: a String command, to send to the robot
:return: The robots response
"""
if not self.connected():
printf(
"Communication| Tried to send a command while robot was not connected!"
)
return ""
# Prepare and send the command to the robot
cmndString = bytes("[" + cmnd + "]",
encoding='ascii') # "[" + cmnd + "]"
try:
self.__serial.write(cmndString)
except serial.serialutil.SerialException as e:
printf("Communication| ERROR ", e, "while sending command ", cmnd,
". Disconnecting Serial!")
self.__isConnected = False
return ""
# Read the response from the robot (THERE MUST ALWAYS BE A RESPONSE!)
response = ""
while True:
try:
response += str(self.__serial.read(), 'ascii')
response = response.replace(' ', '')
except serial.serialutil.SerialException as e:
printf("Communication| ERROR ", e, "while sending command ",
cmnd, ". Disconnecting Serial!")
self.__isConnected = False
return ""
if "[" in response and "]" in response:
response = str(response.replace("\n", ""))
response = str(response.replace("\r", ""))
break
printf("Communication| ",
"[" + cmnd + "]" + " " * (30 - len(cmnd)) + response)
# Clean up the response
response = response.replace("[", "")
response = response.replace("]", "")
# If the robot returned an error, print that out
if "error" in response:
printf("Communication| ERROR: received error from robot: ",
response)
return response
def addTarget(self, targetName):
for target in self.cascades:
if targetName == target.name:
if target not in self.targets:
self.targets.append(target)
else:
printf("ERROR: Tried to add a target that was already there!")
def promptUser(self):
# Open the eventPromptWindow to ask the user what event they wish to create
objManager = self.env.getObjectManager()
eventPrompt = EventsGUI.EventPromptWindow(objManager, parent=self)
if eventPrompt.accepted:
self.addEvent(eventPrompt.chosenEvent, parameters=eventPrompt.chosenParameters)
else:
printf("User rejected the prompt.")
def promptUser(self):
# Open the eventPromptWindow to ask the user what event they wish to create
objManager = self.env.getObjectManager()
eventPrompt = EventsGUI.EventPromptWindow(objManager, parent=self)
if eventPrompt.accepted:
self.addEvent(eventPrompt.chosenEvent, parameters=eventPrompt.chosenParameters)
else:
printf("GUI| User rejected the prompt.")
def saveObject(self, newObject):
# If not new, replace self
wasNew = self.__addObject(newObject)
if not wasNew:
printf("ObjectManager| Tried to add object that already existed, ",
newObject.name)
newObject.save(self.__getDirectory(newObject))
def setExiting(self, value):
printf("Interpreter| Setting Interpreter to Exiting mode.")
global exitingFlag
self.env.getRobot().setExiting(value)
self.env.getVision().setExiting(value)
exitingFlag = value
def setExiting(self, value):
printf("Interpreter| Setting Interpreter to Exiting mode.")
global exitingFlag
self.env.getRobot().setExiting(value)
self.env.getVision().setExiting(value)
exitingFlag = value
def getSaveData(self):
printf("Getting CommandList save data")
commandList = []
commandsOrdered = [self.getCommand(self.item(index)) for index in range(self.count())]
for command in commandsOrdered:
commandList.append(command.getSaveData())
return commandList
def run(self):
if len(self.errors): return
# Before doing any tracking, evaluate the "Relative" number to make sure its valid
relativeAngle, success = self.interpreter.evaluateExpression(self.parameters["angle"])
if not success:
printf("Could not determine the relative angle for the wrist. Canceling command. ")
return False
# Find the object using vision
_, tracked = self.vision.getObjectLatestRecognition(self.trackable)
if tracked is None:
printf("Could not find ", self.trackable.name, " in order to set wrist relative")
return False
# This is the rotation of the object in degrees, derived from the camera
targetAngle = math.degrees(tracked.rotation[2])
cntr = tracked.center
if self.relToBase:
# If the angle is relative to the base angle, do the math here and add it to target angle.
TOCC = cntr # Tracked Object Camera Coordinates
ROCC = rv.getPositionTransform((0, 0, 0), "toCam", self.ptPairs) # Robot Origin Camera Coordinates
baseAngle = 90 - math.degrees(math.atan( (ROCC[1] - TOCC[1]) / (ROCC[0] - TOCC[0])))
targetAngle += baseAngle - 90
else:
# If the angle is relative to x Axis, do different math and add it to target angle
a = rv.getPositionTransform((0, 0, 0), "toCam", self.ptPairs)
b = rv.getPositionTransform((10, 0, 0), "toCam", self.ptPairs)
# TORC = rv.getPositionTransform(cntr, "toRob", self.ptPairs) # Tracked Object Robot Coordinates
# RXCC = rv.getPositionTransform((0, cntr[1], cntr[2]), "toCam", self.ptPairs) # Robot XAxis Camera Coordinates
xOffset = math.degrees(math.atan( (a[1] - b[1]) / (a[0] - b[0])))
targetAngle += 90 - xOffset
print("XOffset: ", xOffset)
print("Target: ", targetAngle)
# Add the "Relative" angle to the wrist
targetAngle += relativeAngle
# Normalize the value so that it's between 0 and 180
while targetAngle < 0: targetAngle += 180
while targetAngle > 180: targetAngle -= 180
# Set the robots wrist to the new value
self.robot.setServoAngles(servo3 = targetAngle)
return True
def createTransformFunc(ptPairs, direction):
"""
Returns a function that takes a point (x,y,z) of a camera coordinate, and returns the robot (x,y,z) for that point
Direction is either "toRob" or "toCam".
If "toCam" it will return a Robot-->Camera transform
If "toRob" it will return a Camera-->Robot transform
"""
ptPairArray = np.asarray(ptPairs)
if direction == "toRob":
pts1 = ptPairArray[:, 0]
pts2 = ptPairArray[:, 1]
if direction == "toCam":
pts1 = ptPairArray[:, 1]
pts2 = ptPairArray[:, 0]
# Generate the transformation matrix
ret, M, mask = cv2.estimateAffine3D(np.float32(pts1),
np.float32(pts2),
confidence = .9999999)
if not ret: printf("RobotVision| ERROR: Transform failed!")
# Returns a function that will perform the transformation between pointset 1 and pointset 2 (if direction is == 1)
transformFunc = lambda x: np.array((M * np.vstack((np.matrix(x).reshape(3, 1), 1)))[0:3, :].reshape(1, 3))[0]
"""
Breakdown of function. Here's an example of transforming [95, -35, 530] cam which is [5, 15, 15] in the robot grid
First:
[95, -35, 530]
np.vstack((np.matrix(input).reshape(3, 1), 1)) is applied, converts it to a vertical array
[[ 95]
[-35]
[530]
[ 1]]
M * np.vstack((np.matrix(x).reshape(3, 1), 1)) (transformation matrix multiplied by the previous number)
[[ 5.8371337 ]
[ 15.72722685]
[ 14.5007519 ]]
There you go. The rest of it is simply converting that matrix into
[5.83, 15.72, 14.5]
but you know, without the rounding.
"""
return transformFunc
def stopMoving(self):
"""
Stop the robot's ongoing movement
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, canceling position change")
return
with self.__lock:
self.__uArm.setStop()
def run(self):
speed, success = self.interpreter.evaluateExpression(self.parameters['speed'])
# Split the wait into incriments of 0.1 seconds each, and check if the thread has been stopped at each incriment
if success:
self.robot.setSpeed(speed)
return True
else:
printf("Commands| ERROR: Expression ", self.parameters['speed'], " failed to evaluate correctly!")
return False
def stopMoving(self):
"""
Stop the robot's ongoing movement
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, canceling position change")
return
with self.__lock:
self.__uArm.setStop()
def run(self):
newAngle, success = self.interpreter.evaluateExpression(self.parameters['angle'])
if success:
# If relative, get the current wrist angle then add that to newAngle
self.robot.setServoAngles(servo3=newAngle, relative=self.parameters['relative'])
return True
else:
printf("Commands| ERROR in parsing new wrist angle. Expression: ", self.parameters['angle'])
return False
def startThread(self):
if self.mainThread is None:
self.running = True
self.mainThread = Thread(
target=self.__videoThread) # Cannot be Daemon thread
self.mainThread.start()
else:
printf(
"Video| Tried to create mainThread, but mainThread already existed."
)
def setActiveServos(self,
all=None,
servo0=None,
servo1=None,
servo2=None,
servo3=None):
"""
Attach or detach a servo. Simple do True to attach and False to detach. Don't specify any servo you don't
want to touch.
:param all: Attach or detach all servos
:param servo0:
:param servo1:
:param servo2:
:param servo3:
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, canceling servo change")
return
# If a positional servo is attached, get the robots current position and update the self.pos cache
oldServoStatus = self.servoAttachStatus[:]
def setServo(servoNum, status):
# Verify that it is a dfiferent servo position before sending
if self.servoAttachStatus[servoNum] == status: return
with self.__lock:
if status:
self.__uArm.setServoAttach(servoNum)
else:
self.__uArm.setServoDetach(servoNum)
self.servoAttachStatus[servoNum] = status
# If anything changed, set the appropriate newServoStatus to reflect that
if all is not None: servo0, servo1, servo2, servo3 = all, all, all, all
if servo0 is not None: setServo(0, servo0)
if servo1 is not None: setServo(1, servo1)
if servo2 is not None: setServo(2, servo2)
if servo3 is not None: setServo(3, servo3)
# Make an array of which servos have been newly attached.
attached = [
oldServoStatus[i] is False and self.servoAttachStatus[i] is True
for i in range(3)
]
# If any positional servos have been attached, update the self.pos cache with the robots current position
if any(attached):
with self.__lock:
curr = self.getCoords()
self.coord = list(curr)
self.servoAngleStatus = list(self.__uArm.getServoAngles())
def setExiting(self, exiting):
"""
When True, this will prevent the robot from completing any moves or lagging the program. The idea is that
when you are exiting a script, it has to happen quickly. This prevents any lag for waiting for the robot,
and cuts all communication. When False everything is normal and the robot communicates normally
:param exiting: True-> communication with robot is cut off. False -> Everything works normally
"""
if exiting:
printf("Setting robot to Exiting mode. All commands should be ignored")
self.exiting = exiting
def getFK(self, servo0, servo1, servo2):
"""
Get the forward kinematic calculations for the robots servos.
:return: [X, Y, Z] list
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, returning 0 for FK")
return [0.0, 0.0, 0.0]
with self.__lock:
return self.__uArm.getFK(servo0, servo1, servo2)
def getFK(self, servo0, servo1, servo2):
"""
Get the forward kinematic calculations for the robots servos.
:return: [X, Y, Z] list
"""
if not self.connected() or self.exiting:
printf("Robot not avaliable, returning 0 for FK")
return [0.0, 0.0, 0.0]
with self.lock:
return self.uArm.getFK(servo0, servo1, servo2)
def getIK(self, x, y, z):
"""
Get the inverse kinematic calculations for a certain XYZ location.
:return: [X, Y, Z] list
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, returning 0 for IK")
return [0.0, 0.0, 0.0]
with self.__lock:
return self.__uArm.getIK(x, y, z)
def getIK(self, x, y, z):
"""
Get the inverse kinematic calculations for a certain XYZ location.
:return: [X, Y, Z] list
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, returning 0 for IK")
return [0.0, 0.0, 0.0]
with self.__lock:
return self.__uArm.getIK(x, y, z)
def waitForNewFrames(self, numFrames=1):
# Useful for situations when you need x amount of new frames after the robot moved, before doing vision
for i in range(0, numFrames):
lastFrame = self.vStream.frameCount
while self.vStream.frameCount == lastFrame:
if self.exiting:
printf("Vision| Exiting early!")
break
sleep(.01)
def waitForNewFrames(self, numFrames=1):
# Useful for situations when you need x amount of new frames after the robot moved, before doing vision
for i in range(0, numFrames):
lastFrame = self.vStream.frameCount
while self.vStream.frameCount == lastFrame:
if self.exiting:
printf("Vision| Exiting early!")
break
sleep(.01)
def loadData(self, data):
printf("Loading CommandList")
self.commands = {}
self.clear()
# Fill the list with new data
for commandSave in data:
# Convert from string to an actual event obj
self.addCommand(getattr(CommandsGUI, commandSave['type']), parameters=commandSave['parameters'])
self.refreshIndents()
def setBuzzer(self, frequency, duration):
"""
Set the robots buzzer.
:param frequency: In HZ
:param duration: In seconds
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, canceling buzzer change")
return
with self.__lock:
self.__uArm.setBuzzer(frequency, duration)
def initializeScript(self, script):
"""
Initializes each event in the script, Initializes each command in the script,
places all the commands into the appropriate events,
and returns all errors that occured during the build process.
"script" is an already loaded script, of the format that EventList.getSaveData() would return.
It looks something like [{Event here, commandList here}, {event here, commandList here}]
To load a script from file, simply use Interpreter.loadScriptFromFile(filename)
:param env: Environment object
:param script: a loaded script from a .task file
:return: any errors that commands returned during instantiation
"""
errors = {
} # Keep track of missing components for commands that are initialized
# Create each event
for _, eventSave in enumerate(script):
# Get the "Logic" code for this event, stored in Events.py
eventType = eventClasses[eventSave['type']]
event = eventType(self.env,
self,
parameters=eventSave['parameters'])
self.addEvent(event)
# Record any errors related to the creation of this event
for error in event.errors:
if error not in errors: errors[error] = []
errors[error].append(eventSave['type'])
# Create the commandList for this event
for _, commandSave in enumerate(eventSave['commandList']):
# Get the "Logic" code for this command, stored in Commands.py
commandType = commandClasses[commandSave['type']]
command = commandType(self.env,
self,
parameters=commandSave['parameters'])
event.addCommand(command)
# Record any errors related to the creation of this command
for error in command.errors:
if error not in errors: errors[error] = []
errors[error].append(commandSave['type'])
# Get rid of duplicates in the errors
for error in errors:
errors[error] = list(set(errors[error]))
printf("Interpreter| ", len(errors),
" errors occured while initializing the script.")
return errors
def getAngles(self):
"""
Get a list of the robots current servo angle readings
:return: [servo0, servo1, servo2, servo3] angles
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, returning 0 for angle")
return [0.0, 0.0, 0.0, 0.0]
with self.__lock:
return self.__uArm.getServoAngles()
def getCoords(self):
"""
Get the current XYZ coordinates of the robot
:return: [X, Y, Z] List. If robot is not connect, [0.0, 0.0, 0.0]
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, return 0 for all coordinates")
return [0.0, 0.0, 0.0]
with self.__lock:
return self.__uArm.getXYZCoords()
def getMoving(self):
"""
Check if the robot is currently moving.
:return: True or False, if robot is moving
"""
if not self.connected() or self.__exiting:
printf("Robot| Robot not avaliable, returning False")
return False
with self.__lock:
return self.__uArm.getMoving()
