def createColors(self):
'''
Create QColor objects from RGB values.
'''
self.grayBlueColor = QColor(89,120,137); # Letter buttons
self.offWhiteColor = QColor(206,230,243); # Background
self.darkGray = QColor(65,88,101); # Central buttons
self.wordListFontColor = QColor(62,143,185); # Darkish blue.
self.purple = QColor(147,124,195); # Gesture button pressed
def _update_color_of_line_edit(self):
global currently_in_collision
palette = main_window.lineEdit.palette()
if currently_in_collision:
palette.setColor(QPalette.Text, QColor(255, 0, 0))
else:
palette.setColor(QPalette.Text, default_color)
main_window.lineEdit.setPalette(palette)
def _redraw_graph_view(self):
self._scene.clear()
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
POINTS_PER_INCH = 72
nodes = {}
for node in self._graph.nodes_iter():
# decrease rect by one so that edges do not reach inside
bounding_box = QRectF(
0, 0,
POINTS_PER_INCH * float(node.attr['width']) - 1.0,
POINTS_PER_INCH * float(node.attr['height']) - 1.0)
pos = node.attr['pos'].split(',')
bounding_box.moveCenter(QPointF(float(pos[0]), -float(pos[1])))
color_name = node.attr.get('color', None)
if color_name is None:
color = None
else:
color_name = color_name.lower()
if color_name in x11_colors:
color = QColor(*x11_colors[color_name])
else:
print 'Unrecognized color: %s' % color_name
color = None
name = str(node)
label = node.attr['label']
node_item = NodeItem(highlight_level, bounding_box, label,
node.attr.get('shape', 'ellipse'), color)
#node_item.setToolTip(self._generate_tool_tip(node.attr.get('URL', None)))
nodes[name] = node_item
edges = {}
for edge in self._graph.edges_iter():
label = None
label_center = None
source_node = edge[0]
destination_node = edge[1]
# create edge with from-node and to-node
edge_item = EdgeItem(highlight_level, edge.attr['pos'],
label_center, label, nodes[source_node],
nodes[destination_node])
# symmetrically add all sibling edges with same label
if label is not None:
if label not in edges:
edges[label] = []
for sibling in edges[label]:
edge_item.add_sibling_edge(sibling)
sibling.add_sibling_edge(edge_item)
edges[label].append(edge_item)
edge_item.setToolTip(
self._generate_tool_tip(edge.attr.get('URL', None)))
edge_item.add_to_scene(self._scene)
for node_item in nodes.itervalues():
self._scene.addItem(node_item)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
class TBoard(QWidget):
# Ids for checkboxes in the 'add-new-word' dialog:
RARELY_BUTTON_ID = 0;
OCCCASIONALLY_BUTTON_ID = 1;
CONSTANTLY_BUTTON_ID = 2;
def __init__(self):
super(TBoard, self).__init__();
self.setWindowTitle("TBoard");
# Find QtCreator's XML file in the PYTHONPATH:
currDir = os.path.realpath(__file__);
relPathQtCreatorFile = "tboard_ui/tboard_ui.ui";
qtCreatorXMLFilePath = Utilities.findFile(relPathQtCreatorFile);
if qtCreatorXMLFilePath is None:
raise ValueError("Can't find QtCreator user interface file %s" % relPathQtCreatorFile);
# Make QtCreator generated UI a child if this instance:
python_qt_binding.loadUi(qtCreatorXMLFilePath, self);
self.letterWidgets = [self.ABCWidget, self.DEFWidget, self.GHIWidget, self.JKLWidget,
self.MNOWidget, self.PQRWidget, self.STUVWidget, self.WXYZWidget];
self.createColors();
# Populate all empty letter board button widgets with
# GestureButton instances:
self.populateGestureButtons();
self.preparePixmaps();
# Increase the width of the word area scrollbar:
self.wordList.verticalScrollBar().setFixedWidth(WORD_LIST_SCROLLBAR_WIDTH) # pixels
# Where we accumulate evolving words in encoded form
# (see symbolToEnc dict in word_collection.py):
self.encEvolvingWord = "";
self.currButtonUsedForFlick = False;
self.wordCollection = TelPadEncodedWordCollection();
# Timer to ensure that a crossed-out button doesn't
# stay crossed out forever:
self.crossOutTimer = QTimer();
self.crossOutTimer.setSingleShot(True);
self.crossedOutButtons = [];
# Popup dialog for adding new words to dictionary:
self.initNewDictWordDialog();
# Gesture buttons all in Dialpad (speed-write mode):
self.buttonEditMode = ButtonEditMode.DIALPAD;
# Disable selecting for the remaining-words panel:
self.wordList.setFocusPolicy(Qt.NoFocus);
# Mutex for keeping very fast flicking gestures
# out of each others' hair:
self.mutex = QMutex();
# The system clipboard for copy:
self.clipboard = QApplication.clipboard();
# Speak-button not working yet:
self.speakButton.setDisabled(True);
self.connectWidgets();
#self.setGeometry(500, 500, 300, 100);
self.show();
# -------------------------------------- UI Setup Methods -------------------------
def initNewDictWordDialog(self):
'''
Initializes dialog window for user to add a new word to the dictionary.
'''
# Find QtCreator's XML file in the PYTHONPATH:
currDir = os.path.realpath(__file__);
relPathQtCreatorFile = "tboard_ui/addWord_dialog/addWordDialog.ui";
qtCreatorXMLFilePath = Utilities.findFile(relPathQtCreatorFile);
if qtCreatorXMLFilePath is None:
raise ValueError("Can't find QtCreator user interface file for 'new dictionary word' dialog file %s" % relPathQtCreatorFile);
#****self.addWordDialog = QWidget();
self.addWordDialog = QDialog();
python_qt_binding.loadUi(qtCreatorXMLFilePath, self.addWordDialog);
# Assign int IDs to the frequency checkboxes:
rareButton = self.addWordDialog.useRarelyButton;
occasionButton = self.addWordDialog.useOccasionallyButton;
constantButton = self.addWordDialog.useConstantlyButton;
self.addWordButtonGroup = QButtonGroup();
self.addWordButtonGroup.addButton(rareButton, TBoard.RARELY_BUTTON_ID);
self.addWordButtonGroup.addButton(occasionButton, TBoard.OCCCASIONALLY_BUTTON_ID);
self.addWordButtonGroup.addButton(constantButton, TBoard.CONSTANTLY_BUTTON_ID);
self.addWordDialog.hide();
def populateGestureButtons(self):
'''
Creates GestureButton instances for each telephone pad button.
Creates convenience data structures:
- C{letterButtons} is an array of all GestureButton instances
- C{letterButtonToID} maps GestureButton instances to the buttons'
IDs, which happen to be their label strings ('ABC', 'DEF', etc.).
- C{idToLetterButton} is the reverse: a dictionary mapping button labels,
like "ABC" to the corresponding button instance.
This function also sets style sheets for the all GestureButton instances.
'''
# Sorted array of all letter button objs:
self.letterButtons = [];
# Letter button object to button label: "ABC", "DEF", etc:
self.letterButtonToID = {};
self.idToLetterButton = {};
for buttonID in ButtonID.legalValues:
self.letterButtons.append(GestureButton(ButtonID.toString(buttonID),
parent=self.letterWidgets[buttonID]));
self.letterButtonToID[self.letterButtons[-1]] = self.letterButtons[-1].text();
self.letterButtons[-1].setGeometry(0,0,200,100);
self.idToLetterButton[self.letterButtons[-1].text()] = self.letterButtons[-1];
for buttonObj in self.letterButtons:
self.setGestureButtonStyle(buttonObj, StyleID.RELEASED);
#****buttonObj.setFocusPolicy(Qt.FocusPolicy.NoFocus);
buttonObj.setFocusPolicy(Qt.NoFocus);
def connectWidgets(self):
'''
Connect signals and button slots to their handlers.
'''
CommChannel.getSignal('GestureSignals.flickSig').connect(self.handleButtonFlicks);
CommChannel.getSignal('GestureSignals.buttonEnteredSig').connect(self.handleButtonEntered);
CommChannel.getSignal('GestureSignals.buttonExitedSig').connect(self.handleButtonExited);
self.eraseWordButton.clicked.connect(self.handleEraseWordButton);
self.addWordButton.clicked.connect(self.handleSaveWordButton);
self.crossOutTimer.timeout.connect(self.handleCrossoutTimeout);
# Number pad:
for button in set([self.numPad1, self.numPad2, self.numPad3, self.numPad4, self.numPad5,
self.numPad6, self.numPad7, self.numPad8, self.numPad9, self.numPad0]):
button.clicked.connect(partial(self.handleNumPad, button));
# Special characters:
for button in set([self.commaButton, self.colonButton, self.questionButton, self.atButton, self.leftParenButton,
self.periodButton, self.backspaceButton, self.slashButton, self.spaceButton, self.rightParenButton]):
button.clicked.connect(partial(self.handleSpecialChars, button));
# Gesture button clicks (switching between dialpad and letter mode:
for buttonObj in self.letterButtons:
buttonObj.clicked.connect(partial(self.handleGestureButtonClick, buttonObj));
# Add word dialog box capitalization checkbox state changed:
self.addWordDialog.capitalizeCheckbox.stateChanged.connect(self.handleAddDictWordCapitalizeStateChanged);
# Add word dialog box OK or Cancel botton clicked:
self.addWordDialog.cancelButton.clicked.connect(partial(self.handleAddDictWordOK_Cancel, self.addWordDialog.cancelButton));
self.addWordDialog.addWordButton.clicked.connect(partial(self.handleAddDictWordOK_Cancel, self.addWordDialog.addWordButton));
# CopyAll button:
self.copyButton.clicked.connect(self.handleCopyAll);
def preparePixmaps(self):
'''
Pull icons from the file system, and turn them into pixmaps.
'''
imgDirPath = os.path.join(os.path.dirname(__file__), "img/");
self.buttonBackGroundPixmaps = [];
buttonWidth = self.letterButtons[0].width();
buttonHeight = self.letterButtons[0].height();
for backgroundImgNum in range(NUM_LETTER_BUTTONS):
buttonPixmap = QPixmap();
#****imgPath = os.path.join(imgDirPath, "tboardButtonBackgroundTrail" + str(backgroundImgNum + 1) + ".png");
imgPath = os.path.join(imgDirPath, "tboardButtonBackgroundsSmall" + str(backgroundImgNum + 1) + ".png");
if not buttonPixmap.load(imgPath):
raise IOError("Could not find button background icon at " + imgPath);
#scaledPixmap = buttonPixmap.scaled(buttonWidth, buttonHeight);
#*****scaledPixmap = buttonPixmap.scaled(buttonWidth + 50, buttonHeight);
scaledPixmap = buttonPixmap;
#*****
self.buttonBackGroundPixmaps.append(scaledPixmap);
self.crossedOutButtonBackground = QPixmap();
imgPath = os.path.join(imgDirPath, "tboardButtonBackgroundCrossedOut.png");
if not self.crossedOutButtonBackground.load(imgPath):
raise IOError("Could not find crossed-out button background icon at " + imgPath);
self.crossedOutButtonBackground = self.crossedOutButtonBackground.scaled(buttonWidth + 50, buttonHeight);
# Initialize all buttons to a background with
# trail spot 8 (darkest):
for button in self.letterButtons:
self.setButtonImage(button, self.buttonBackGroundPixmaps[NUM_LETTER_BUTTONS - 1]);
# Initialize dictionary that tracks button background icons.
# Keys are button objs, values are ints that index into the
# self.buttonBackgroundPixmaps array. None means the button
# is so old that it has a plain background, or that it was never
# used so far:
self.currentButtonBackgrounds = {};
for buttonObj in self.letterButtons:
self.currentButtonBackgrounds[buttonObj] = None;
self.setStyleSheet("QWidget{background-color: %s}" % self.offWhiteColor.name());
def createColors(self):
'''
Create QColor objects from RGB values.
'''
self.grayBlueColor = QColor(89,120,137); # Letter buttons
self.offWhiteColor = QColor(206,230,243); # Background
self.darkGray = QColor(65,88,101); # Central buttons
self.wordListFontColor = QColor(62,143,185); # Darkish blue.
self.purple = QColor(147,124,195); # Gesture button pressed
def setGestureButtonStyle(self, buttonObj, styleID):
'''
Style a gesture button.
@param buttonObj: the button to style
@type buttonObj: GestureButton
@param styleID: whether button is pressed or released
@type styleID: StyleID
'''
if styleID == StyleID.RELEASED:
buttonObj.setStyleSheet("background-color: %s; color: %s; border: 2px outset %s; border-radius: 15; font-size: 18px" %
(self.grayBlueColor.name(),
self.offWhiteColor.name(),
self.offWhiteColor.name()));
elif styleID == StyleID.PRESSED:
buttonObj.setStyleSheet("background-color: %s; color: %s; border-radius: 15; font-size: 22px" %
(self.purple.name(),
self.offWhiteColor.name()));
self.setFocus();
# -------------------------------------- Signal Handlers -------------------------
@Slot(GestureButton, int)
def handleButtonFlicks(self, gestureButton, flickDirection):
'''
Action on flicking in and out of a gesture button.
@param gestureButton: button that was flicked
@type gestureButton: GestureButton
@param flickDirection: cursor flicked North, South, East, or West
@type flickDirection: GestureButton.FlickDirection
'''
#print "Flick direction: " + FlickDirection.toString(flickDirection);
# Protect against re-entry. Not that
# QtCore.QMutexLocker locks when created, and
# unlocks when destroyed (i.e. when function
# is left; no explicit unlocking needed);
myMutexLocker = QMutexLocker(self.mutex);
# West flick: Undo last letter (in Dialpad mode) or
# highlight next letter (in Letter_Edit mod):
if flickDirection == FlickDirection.WEST:
if self.buttonEditMode == ButtonEditMode.DIALPAD:
self.erasePreviousLetter();
self.showRemainingWords();
self.updateTickerTape();
else: # individual-letter-input mode:
self.highlightNextLetter(gestureButton, FlickDirection.WEST);
# North flick: Scroll word list up:
elif flickDirection == FlickDirection.NORTH:
currRemainingWordsRow = self.wordList.currentRow();
if currRemainingWordsRow == 0 or self.buttonEditMode == ButtonEditMode.LETTER_INPUT:
pass;
else:
self.wordList.setCurrentRow(currRemainingWordsRow - 1);
# South flick: Scroll word list down:
elif flickDirection == FlickDirection.SOUTH:
currRemainingWordsRow = self.wordList.currentRow();
if currRemainingWordsRow >= self.wordList.count() or self.buttonEditMode == ButtonEditMode.LETTER_INPUT:
pass;
else:
self.wordList.setCurrentRow(currRemainingWordsRow + 1);
# East flick: Accept word that is selected in remaining words list:
else:
if self.buttonEditMode == ButtonEditMode.LETTER_INPUT:
self.highlightNextLetter(gestureButton, FlickDirection.EAST);
else:
# No word in word list?
count = self.wordList.count()
currItem = self.wordList.currentItem();
if count <= 0 or currItem is None:
pass;
else:
self.outputPanel.insertPlainText(" " + currItem.text());
# Word entry done for this word:
self.eraseCurrentWord();
# Remember that we just used this button
# for a flick action.
self.currButtonUsedForFlick = True;
@Slot(GestureButton)
def handleButtonEntered(self, gestureButtonObj):
#print "Button %s entered" % str(gestureButtonObj);
pass;
@Slot(GestureButton)
def handleButtonExited(self, gestureButtonObj):
'''
Handler for cursor having entered a gesture button.
@param gestureButtonObj: button object that was entered
@type gestureButtonObj: GestureButton
'''
# Protect against re-entry. Not that
# QtCore.QMutexLocker locks when created, and
# unlocks when destroyed (i.e. when function
# is left; no explicit unlocking needed);
myMutexLocker = QMutexLocker(self.mutex);
# If we are in letter entry mode, return this button
# to Dialpad mode:
if self.buttonEditMode == ButtonEditMode.LETTER_INPUT:
self.switchButtonMode(gestureButtonObj, ButtonEditMode.DIALPAD);
return;
# If button being left was just used for a flick,
# don't count the exit:
if self.currButtonUsedForFlick:
self.currButtonUsedForFlick = False;
return;
# Get 'ABC', or 'DEF', etc representation from the button:
buttonLabelAsStr = str(gestureButtonObj);
newEncLetter = self.wordCollection.encodeTelPadLabel(buttonLabelAsStr);
self.encEvolvingWord += newEncLetter;
self.shiftButtonTrails(HistoryShiftDir.OLDER, newHead=gestureButtonObj);
#print self.encEvolvingWord;
self.showRemainingWords();
self.updateTickerTape();
def handleEraseWordButton(self):
'''
Handler for erase-word button clicked.
'''
self.eraseCurrentWord();
self.showRemainingWords();
self.updateTickerTape();
def handleCrossoutTimeout(self):
'''
Timeout handler that detects crossing out a letter by running through a gesture button and back.
'''
for buttonObj in self.crossedOutButtons:
self.setButtonImage(buttonObj, self.buttonBackGroundPixmaps[self.currentButtonBackgrounds[buttonObj]]);
self.crossedOutButtons = [];
def handleNumPad(self, numButton):
'''
Handler for number pad button pressed.
@param numButton: button object
@type numButton: QPushButton
'''
buttonLabel = numButton.text();
self.outputPanel.insertPlainText(buttonLabel);
def handleSpecialChars(self, specCharButton):
'''
Handler: special character button pushed.
@param specCharButton: button object
@type specCharButton: QPushButton
'''
if specCharButton == self.backspaceButton:
self.outputPanel.textCursor().deletePreviousChar();
return;
elif specCharButton == self.spaceButton:
char = " ";
else:
char = specCharButton.text();
self.outputPanel.insertPlainText(char);
def handleGestureButtonClick(self, buttonObj):
'''
Handler for gesture button click.
@param buttonObj: Button object
@type buttonObj: GestureButton
'''
# If button is in default dialpad mode, switch to letter-input mode:
if self.buttonEditMode == ButtonEditMode.DIALPAD:
self.switchButtonMode(buttonObj, ButtonEditMode.LETTER_INPUT);
# If button is in letter edit mode, add the currently
# capitalized letter to the output panel, and switch the
# button back into default dialpad mode:
elif self.buttonEditMode == ButtonEditMode.LETTER_INPUT:
labelBeingEdited = buttonObj.text();
capLetter = labelBeingEdited[self.findCapitalLetter(labelBeingEdited)];
self.outputPanel.insertPlainText(capLetter.lower());
self.switchButtonMode(buttonObj, ButtonEditMode.DIALPAD);
def handleSaveWordButton(self):
# Get content of output panel:
currOutput = self.outputPanel.toPlainText();
# If noth'n there, done:
if len(currOutput) == 0:
QMessageBox.information(self, "Dictionary addition", "Output panel has no content; so there is no word to save.", QMessageBox.Ok, QMessageBox.NoButton);
return;
# Get the last word in the output panel:
newWord = re.split("[.;:?! @()]", currOutput)[-1];
# Ask user about capitalization, and expected word frequency.
# This call will raise a modal dialog box. Signal handlers
# handleAddDictWordCapitalizeStateChanged(), and handleAddDictWordOK_Cancel()
# take it from there:
self.getAddWordUserInfo(newWord);
def handleAddDictWordCapitalizeStateChanged(self, newCapsState):
dialog = self.addWordDialog;
newWord = dialog.newWord.text();
if newCapsState == 0:
dialog.newWord.setText(newWord.lower());
else:
dialog.newWord.setText(newWord.capitalize());
def handleAddDictWordOK_Cancel(self, button):
if button == self.addWordDialog.cancelButton:
self.addWordDialog.hide();
return;
frequencyCheckboxID = self.addWordButtonGroup.checkedId();
if frequencyCheckboxID == TBoard.RARELY_BUTTON_ID:
freqRank = UseFrequency.RARELY;
elif frequencyCheckboxID == TBoard.OCCCASIONALLY_BUTTON_ID:
freqRank = UseFrequency.OCCASIONALLY;
elif frequencyCheckboxID == TBoard.CONSTANTLY_BUTTON_ID:
freqRank = UseFrequency.CONSTANTLY;
else:
raise ValueError("Unknown use frequency checkbox ID in add word to dictionary dialog handling: " + str(frequencyCheckboxID));
self.doAddWordButton(self.addWordDialog.newWord.text(), freqRank);
self.addWordDialog.hide();
def handleCopyAll(self):
self.clipboard.setText(self.outputPanel.toPlainText());
# -------------------------------------- UI Manipulation -------------------------
def doAddWordButton(self, newWord, rank):
additionResult = self.wordCollection.addToUserDict(newWord, rankInt=rank);
if additionResult:
QMessageBox.information(self, # dialog parent
"Dictionary addition", "Word '%s' has been saved in user dictionary." % newWord,
QMessageBox.Ok,
QMessageBox.NoButton);
else:
QMessageBox.information(self, # dialog parent
"Dictionary addition", "Word '%s' was already in the dictionary. No action taken" % newWord,
QMessageBox.Ok,
QMessageBox.NoButton);
def switchButtonMode(self, buttonObj, newEditMode):
if newEditMode == ButtonEditMode.DIALPAD:
self.setGestureButtonStyle(buttonObj, StyleID.RELEASED);
self.buttonEditMode = ButtonEditMode.DIALPAD;
buttonObj.setText(buttonObj.text().upper());
elif newEditMode == ButtonEditMode.LETTER_INPUT:
self.setGestureButtonStyle(buttonObj, StyleID.PRESSED);
self.buttonEditMode = ButtonEditMode.LETTER_INPUT;
buttonObj.setText(buttonObj.text().capitalize());
def highlightNextLetter(self, buttonObj, flickDirection):
label = buttonObj.text();
capitalLetterPos = self.findCapitalLetter(label);
label = label.lower();
if flickDirection == FlickDirection.EAST:
newCapPos = (capitalLetterPos + 1) % len(label);
else:
newCapPos = (capitalLetterPos - 1) % len(label);
#label = label[:newCapPos] + label[newCapPos].upper() + label[min(newCapPos + 1, len(label) - 1):];
label = label[:newCapPos] + label[newCapPos].upper() + label[newCapPos + 1:] if newCapPos < len(label) else "";
buttonObj.setText(label);
def findCapitalLetter(self, word):
for (pos, char) in enumerate(word):
if char.isupper():
return pos;
raise ValueError("No capital letter found.");
def updateTickerTape(self):
if len(self.encEvolvingWord) == 0:
self.tickerTape.setText("");
return;
visibleEncoding = "";
for encChar in self.encEvolvingWord:
dialpadButtonLabel = self.wordCollection.decodeTelPadLabel(encChar);
buttonID = ButtonID.toButtonID(dialpadButtonLabel);
visibleEncoding += ButtonID.idToStringable(buttonID);
self.tickerTape.setText(visibleEncoding);
def showRemainingWords(self):
remainingWords = self.wordCollection.prefix_search(self.encEvolvingWord);
rankSortedWords = sorted(remainingWords, key=self.wordCollection.rank);
self.wordList.clear();
self.wordList.addItems(rankSortedWords);
self.wordList.setCurrentRow(0);
#print self.wordCollection.prefix_search(self.encEvolvingWord);
def eraseCurrentWord(self):
self.encEvolvingWord = "";
self.updateTickerTape();
self.eraseTrail();
self.wordList.clear();
def erasePreviousLetter(self):
if len(self.encEvolvingWord) == 0:
# Just to make sure, erase all history trail:
self.eraseTrail();
return;
oldNewestButton = self.getButtonFromEncodedLetter(self.encEvolvingWord[-1]);
self.encEvolvingWord = self.encEvolvingWord[0:-1];
if len(self.encEvolvingWord) > 0:
newNewestButton = self.getButtonFromEncodedLetter(self.encEvolvingWord[-1]);
else:
newNewestButton = None;
self.shiftButtonTrails(HistoryShiftDir.YOUNGER, newHead=newNewestButton);
self.crossOutButton(oldNewestButton);
if len(self.encEvolvingWord) == 0:
self.eraseTrail();
def crossOutButton(self, buttonObj):
'''
Show the given button crossed out. Update the
currentButtonBackgrounds dict to show that this
button now has a different background (not one of the
trails.
@param buttonObj: GestureButton object to cross out.
@type buttonObj: QPushButton
'''
self.setButtonImage(buttonObj, self.crossedOutButtonBackground);
self.crossedOutButtons.append(buttonObj);
# Take the crossout away in 2 seconds:
self.crossOutTimer.start(2000);
def eraseTrail(self):
'''
Erase the history trail.
'''
for buttonObj in self.letterButtons:
self.setButtonImage(buttonObj, self.buttonBackGroundPixmaps[-1]);
self.currentButtonBackgrounds[buttonObj] = NUM_LETTER_BUTTONS - 1;
def shiftButtonTrails(self, direction, newHead=None):
if direction == HistoryShiftDir.OLDER:
# Every button gets one older:
for buttonObj in self.letterButtons:
currPixmapIndex = self.currentButtonBackgrounds[buttonObj];
if currPixmapIndex is None:
continue;
if currPixmapIndex >= NUM_LETTER_BUTTONS - 1:
# Button already as old as it gets:
continue;
buttonObj.setIcon(QIcon(self.buttonBackGroundPixmaps[currPixmapIndex + 1]));
self.currentButtonBackgrounds[buttonObj] = currPixmapIndex + 1;
if newHead is not None:
self.setButtonImage(newHead, self.buttonBackGroundPixmaps[0]);
self.currentButtonBackgrounds[newHead] = 0;
else:
# Make everyone younger:
for buttonObj in self.letterButtons:
currPixmapIndex = self.currentButtonBackgrounds[buttonObj];
if currPixmapIndex is None:
# Button has a special, temporary background, like being crossed out:
continue;
if currPixmapIndex <= 0:
# Button already as young as it gets. Make it the oldest:
self.setButtonImage(buttonObj,self.buttonBackGroundPixmaps[NUM_LETTER_BUTTONS - 1]);
self.setButtonImage(buttonObj, self.buttonBackGroundPixmaps[currPixmapIndex - 1]);
self.currentButtonBackgrounds[buttonObj] = currPixmapIndex - 1;
def setButtonImage(self, gestureButtonObj, pixmap):
# PySide: gestureButtonObj.setIcon(pixmap);
gestureButtonObj.setIcon(QIcon(pixmap));
gestureButtonObj.setIconSize(pixmap.rect().size());
def getAddWordUserInfo(self, newWord):
'''
Prepare the Add New Word dialog, and show it:
@param newWord:
@type newWord:
'''
# New word capitalized? Pre-set the Capitalize checkbox accordingly:
if newWord.istitle():
self.addWordDialog.capitalizeCheckbox.setChecked(True);
else:
self.addWordDialog.capitalizeCheckbox.setChecked(False);
# Init the label in the dialog box that shows the word to be added:
self.addWordDialog.newWord.setText(newWord);
# Place the dialog somewhere over the application window:
tboardGeoRect = self.geometry();
dialogGeoRect = self.addWordDialog.geometry();
newDialogGeo = QRect(tboardGeoRect.x() + 50,
tboardGeoRect.y() + 50,
dialogGeoRect.width(),
dialogGeoRect.height());
self.addWordDialog.setGeometry(newDialogGeo);
self.addWordDialog.show();
# -------------------------------------- Handy Methods -------------------------
def getButtonFromEncodedLetter(self, encLetter):
# From the encoded letter, get the corresponding
# "ABC", "PQR", etc label:
buttonLabel = self.wordCollection.decodeTelPadLabel(encLetter);
buttonID = ButtonID.toButtonID(buttonLabel);
return self.letterButtons[buttonID];
def init_ui(self):
# type combobox
ui_type_lable = QtGui.QLabel('Robot')
ui_type_lable.setStyleSheet('QLabel {font: bold}')
ui_type_lable.setMaximumWidth(70)
# ui_type_lable.setFont(QtGui.QFont(QtGui.QFont.Bold))
ui_type_lable.setAlignment(QtCore.Qt.AlignCenter)
self.ui_type = QtGui.QComboBox()
self.ui_type.addItem('MTMR')
self.ui_type.addItem('MTML')
self.ui_type.addItem('PSM1')
self.ui_type.addItem('PSM2')
self.ui_type.addItem('PSM3')
# self.ui_type.addItem('ECM')
ui_hbox_type = QtGui.QHBoxLayout()
ui_hbox_type.addWidget(ui_type_lable)
ui_hbox_type.addWidget(self.ui_type)
self.ui_type.currentIndexChanged.connect(self.slot_type_changed)
# control mode
# todo: use a for loop
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Gravity')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_grav)
ui_btn_vfix = QtGui.QPushButton('Teleop')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control = QtGui.QGroupBox('Control MTM')
self.ui_gbox_control.setLayout(gbox)
# ---- PSM Control Box ----
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Teleop')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_vfix = QtGui.QPushButton('Manual')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_manual)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control_psm = QtGui.QGroupBox('Control PSM')
self.ui_gbox_control_psm.setLayout(gbox)
self.ui_gbox_control_psm.setVisible(False)
# ---- gripper box -----
ui_hbox_gripper = QtGui.QHBoxLayout()
ui_gripper_label = QtGui.QLabel('Gripper Angle:')
ui_hbox_gripper.addWidget(ui_gripper_label)
ui_gbox_gripper = QtGui.QGroupBox('Gripper')
ui_gbox_gripper.setLayout(ui_hbox_gripper)
# ---- joint position group -----
jnt_pos_hbox = QtGui.QHBoxLayout()
jp_widgets = []
jn_widgets = []
for i in range(7):
pos_vbox = QtGui.QVBoxLayout()
ui_ppos = QwtThermo()
ui_ppos.setScalePosition(QwtThermo.NoScale)
ui_ppos.setAutoFillBackground(True)
ui_ppos.setAlarmLevel(0.8)
ui_ppos.setPipeWidth(20)
ui_ppos.setValue(0.0)
ui_ppos.setMinimumSize(0, 40)
ui_ppos.setRange(0.0, 1.0, False)
ui_npos = QwtThermo()
ui_npos.setScalePosition(QwtThermo.NoScale)
ui_npos.setAlarmLevel(0.8)
ui_npos.setPipeWidth(20)
ui_npos.setValue(0.9)
ui_npos.setMinimumSize(0, 40)
ui_npos.setRange(1.0, 0.0, False)
ui_npos.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
pos_vbox.addWidget(ui_ppos)
pos_vbox.addWidget(ui_npos)
pos_vbox.addWidget(ui_label_jnt)
jnt_pos_hbox.addLayout(pos_vbox)
jp_widgets.append(ui_ppos)
jn_widgets.append(ui_npos)
# ui_btn_jnt_pos = QPushButton('J1')
ui_gbox_jnt_pos = QtGui.QGroupBox('Joint Positions (normalized)')
ui_gbox_jnt_pos.setLayout(jnt_pos_hbox)
self.joint_widgets = zip(jp_widgets, jn_widgets)
# joint torque group
jnt_eff_hbox = QtGui.QHBoxLayout()
tp_widgets = []
tn_widgets = []
for i in range(7):
eff_vbox = QtGui.QVBoxLayout()
ui_peff = QwtThermo()
ui_peff.setScalePosition(QwtThermo.NoScale)
ui_peff.setAutoFillBackground(True)
ui_peff.setAlarmLevel(0.8)
ui_peff.setPipeWidth(20)
ui_peff.setValue(0.0)
ui_peff.setMinimumSize(0, 30)
ui_peff.setRange(0.0, 1.0, False)
ui_neff = QwtThermo()
ui_neff.setScalePosition(QwtThermo.NoScale)
ui_neff.setAlarmLevel(0.8)
ui_neff.setPipeWidth(20)
ui_neff.setValue(0.9)
ui_neff.setMinimumSize(0, 30)
ui_neff.setRange(1.0, 0.0, False)
ui_neff.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
eff_vbox.addWidget(ui_peff)
eff_vbox.addWidget(ui_neff)
eff_vbox.addWidget(ui_label_jnt)
jnt_eff_hbox.addLayout(eff_vbox)
tp_widgets.append(ui_peff)
tn_widgets.append(ui_neff)
ui_gbox_jnt_eff = QtGui.QGroupBox('Joint Torques (normalized)')
ui_gbox_jnt_eff.setLayout(jnt_eff_hbox)
self.torque_widgets = zip(tp_widgets, tn_widgets)
# make widgets colorful
self.dvrk_green = QColor(87, 186, 142)
self.dvrk_green_dark = self.dvrk_green.darker()
self.dvrk_green_light = self.dvrk_green.lighter()
self.dvrk_blue = QColor(80, 148, 204)
self.dvrk_blue_dark = self.dvrk_blue.darker()
self.dvrk_blue_light = self.dvrk_blue.lighter()
self.dvrk_red = QColor(232, 47, 47)
self.dvrk_red_dark = self.dvrk_red.darker()
self.dvrk_red_light = self.dvrk_red.lighter()
self.dvrk_orange = QColor(255, 103, 43)
self.dvrk_orange_dark = self.dvrk_orange.darker()
# joint_bg_color = self.dvrk_blue_dark.darker()
joint_fill_color = self.dvrk_blue
joint_alarm_color = self.dvrk_blue_light # self.dvrk_blue_light
# torque_bg_color = self.dvrk_green_dark.darker()
torque_fill_color = self.dvrk_green
torque_alarm_color = self.dvrk_orange # self.dvrk_green_light
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
# p.setColor(ui_ppos.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
# p.setColor(ui_peff.backgroundRole(), torque_bg_color)
w.setPalette(p)
# main layout
main_layout = QtGui.QVBoxLayout()
main_layout.addLayout(ui_hbox_type)
main_layout.addWidget(self.ui_gbox_control)
main_layout.addWidget(self.ui_gbox_control_psm)
main_layout.addWidget(ui_gbox_gripper)
main_layout.addWidget(ui_gbox_jnt_pos)
main_layout.addWidget(ui_gbox_jnt_eff)
self._widget.setLayout(main_layout)
pass
class dvrkDashboard(Plugin):
def __init__(self, context):
super(dvrkDashboard, self).__init__(context)
# give QObjects reasonable names
self.setObjectName('dvrkDashboard')
# process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# create qwidget
self._widget = QtGui.QWidget()
self._widget.setObjectName('dvrkDashboardUI')
# serial number
if context.serial_number() > 1:
self._widget.setWindowTitle(
self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# add widget to the user interface
context.add_widget(self._widget)
self.context = context
# ---- Get Widget -----
self.init_ui()
# ---- States -----
self.namespace = 'dvrk_mtmr'
self.jnt_pos = []
self.jnt_eff = []
self.init_ros()
# ---- Timer -----
self.update_timer = QtCore.QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self.update_widget_values)
self.update_timer.start()
pass
def init_ui(self):
# type combobox
ui_type_lable = QtGui.QLabel('Robot')
ui_type_lable.setStyleSheet('QLabel {font: bold}')
ui_type_lable.setMaximumWidth(70)
# ui_type_lable.setFont(QtGui.QFont(QtGui.QFont.Bold))
ui_type_lable.setAlignment(QtCore.Qt.AlignCenter)
self.ui_type = QtGui.QComboBox()
self.ui_type.addItem('MTMR')
self.ui_type.addItem('MTML')
self.ui_type.addItem('PSM1')
self.ui_type.addItem('PSM2')
self.ui_type.addItem('PSM3')
# self.ui_type.addItem('ECM')
ui_hbox_type = QtGui.QHBoxLayout()
ui_hbox_type.addWidget(ui_type_lable)
ui_hbox_type.addWidget(self.ui_type)
self.ui_type.currentIndexChanged.connect(self.slot_type_changed)
# control mode
# todo: use a for loop
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Gravity')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_grav)
ui_btn_vfix = QtGui.QPushButton('Teleop')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control = QtGui.QGroupBox('Control MTM')
self.ui_gbox_control.setLayout(gbox)
# ---- PSM Control Box ----
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Teleop')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_vfix = QtGui.QPushButton('Manual')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_manual)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control_psm = QtGui.QGroupBox('Control PSM')
self.ui_gbox_control_psm.setLayout(gbox)
self.ui_gbox_control_psm.setVisible(False)
# ---- gripper box -----
ui_hbox_gripper = QtGui.QHBoxLayout()
ui_gripper_label = QtGui.QLabel('Gripper Angle:')
ui_hbox_gripper.addWidget(ui_gripper_label)
ui_gbox_gripper = QtGui.QGroupBox('Gripper')
ui_gbox_gripper.setLayout(ui_hbox_gripper)
# ---- joint position group -----
jnt_pos_hbox = QtGui.QHBoxLayout()
jp_widgets = []
jn_widgets = []
for i in range(7):
pos_vbox = QtGui.QVBoxLayout()
ui_ppos = QwtThermo()
ui_ppos.setScalePosition(QwtThermo.NoScale)
ui_ppos.setAutoFillBackground(True)
ui_ppos.setAlarmLevel(0.8)
ui_ppos.setPipeWidth(20)
ui_ppos.setValue(0.0)
ui_ppos.setMinimumSize(0, 40)
ui_ppos.setRange(0.0, 1.0, False)
ui_npos = QwtThermo()
ui_npos.setScalePosition(QwtThermo.NoScale)
ui_npos.setAlarmLevel(0.8)
ui_npos.setPipeWidth(20)
ui_npos.setValue(0.9)
ui_npos.setMinimumSize(0, 40)
ui_npos.setRange(1.0, 0.0, False)
ui_npos.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
pos_vbox.addWidget(ui_ppos)
pos_vbox.addWidget(ui_npos)
pos_vbox.addWidget(ui_label_jnt)
jnt_pos_hbox.addLayout(pos_vbox)
jp_widgets.append(ui_ppos)
jn_widgets.append(ui_npos)
# ui_btn_jnt_pos = QPushButton('J1')
ui_gbox_jnt_pos = QtGui.QGroupBox('Joint Positions (normalized)')
ui_gbox_jnt_pos.setLayout(jnt_pos_hbox)
self.joint_widgets = zip(jp_widgets, jn_widgets)
# joint torque group
jnt_eff_hbox = QtGui.QHBoxLayout()
tp_widgets = []
tn_widgets = []
for i in range(7):
eff_vbox = QtGui.QVBoxLayout()
ui_peff = QwtThermo()
ui_peff.setScalePosition(QwtThermo.NoScale)
ui_peff.setAutoFillBackground(True)
ui_peff.setAlarmLevel(0.8)
ui_peff.setPipeWidth(20)
ui_peff.setValue(0.0)
ui_peff.setMinimumSize(0, 30)
ui_peff.setRange(0.0, 1.0, False)
ui_neff = QwtThermo()
ui_neff.setScalePosition(QwtThermo.NoScale)
ui_neff.setAlarmLevel(0.8)
ui_neff.setPipeWidth(20)
ui_neff.setValue(0.9)
ui_neff.setMinimumSize(0, 30)
ui_neff.setRange(1.0, 0.0, False)
ui_neff.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
eff_vbox.addWidget(ui_peff)
eff_vbox.addWidget(ui_neff)
eff_vbox.addWidget(ui_label_jnt)
jnt_eff_hbox.addLayout(eff_vbox)
tp_widgets.append(ui_peff)
tn_widgets.append(ui_neff)
ui_gbox_jnt_eff = QtGui.QGroupBox('Joint Torques (normalized)')
ui_gbox_jnt_eff.setLayout(jnt_eff_hbox)
self.torque_widgets = zip(tp_widgets, tn_widgets)
# make widgets colorful
self.dvrk_green = QColor(87, 186, 142)
self.dvrk_green_dark = self.dvrk_green.darker()
self.dvrk_green_light = self.dvrk_green.lighter()
self.dvrk_blue = QColor(80, 148, 204)
self.dvrk_blue_dark = self.dvrk_blue.darker()
self.dvrk_blue_light = self.dvrk_blue.lighter()
self.dvrk_red = QColor(232, 47, 47)
self.dvrk_red_dark = self.dvrk_red.darker()
self.dvrk_red_light = self.dvrk_red.lighter()
self.dvrk_orange = QColor(255, 103, 43)
self.dvrk_orange_dark = self.dvrk_orange.darker()
# joint_bg_color = self.dvrk_blue_dark.darker()
joint_fill_color = self.dvrk_blue
joint_alarm_color = self.dvrk_blue_light # self.dvrk_blue_light
# torque_bg_color = self.dvrk_green_dark.darker()
torque_fill_color = self.dvrk_green
torque_alarm_color = self.dvrk_orange # self.dvrk_green_light
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
# p.setColor(ui_ppos.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
# p.setColor(ui_peff.backgroundRole(), torque_bg_color)
w.setPalette(p)
# main layout
main_layout = QtGui.QVBoxLayout()
main_layout.addLayout(ui_hbox_type)
main_layout.addWidget(self.ui_gbox_control)
main_layout.addWidget(self.ui_gbox_control_psm)
main_layout.addWidget(ui_gbox_gripper)
main_layout.addWidget(ui_gbox_jnt_pos)
main_layout.addWidget(ui_gbox_jnt_eff)
self._widget.setLayout(main_layout)
pass
def init_ros(self):
# pub
topic = '/' + self.namespace + '/set_robot_state'
self.pub_state = rospy.Publisher(topic, String)
# self.pub_state = rospy.Publisher('/dvrk_psm1/set_robot_state', String)
# sub
topic = '/' + self.namespace + '/joint_states'
self.sub_jnts = rospy.Subscriber(topic, JointState,
self.cb_ros_jnt_states)
# urdf
urdf_param = '/' + self.namespace + '/robot_description'
print urdf_param
self.urdf = ''
try:
self.urdf = rospy.get_param(urdf_param)
except KeyError:
rospy.logerr('robot_description not set')
print self.urdf
pass
def shutdown_plugin(self):
# unregister
self.pub_state.unregister()
self.sub_jnts.unregister()
# del pubs
del self.pub_state
del self.sub_jnts
pass
def reconfigure_ui(self):
if 'mtm' in self.namespace:
self.ui_gbox_control.setVisible(True)
self.ui_gbox_control_psm.setVisible(False)
elif 'psm' in self.namespace:
self.ui_gbox_control.setVisible(False)
self.ui_gbox_control_psm.setVisible(True)
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO: save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def slot_type_changed(self, index):
robot_namespaces = ('dvrk_mtmr', 'dvrk_mtml',
'dvrk_psm1', 'dvrk_psm2',
'dvrk_psm3', 'dvrk_ecm')
self.namespace = robot_namespaces[index]
rospy.loginfo('namespace = ' + self.namespace)
# update ros pub/sub
self.shutdown_plugin()
self.init_ros()
self.reconfigure_ui()
pass
def slot_btn_home(self, checked):
rospy.loginfo('home btn pressed')
self.pub_state.publish('Home')
pass
def slot_btn_idle(self, checked):
rospy.loginfo('idle btn pressed')
self.pub_state.publish('Idle')
pass
def slot_btn_grav(self, checked):
rospy.loginfo('grav btn pressed')
self.pub_state.publish('Gravity')
pass
def slot_btn_teleop(self, checked):
rospy.loginfo('teleop btn pressed')
self.pub_state.publish('Teleop')
pass
def slot_btn_manual(self, checked):
rospy.loginfo('manual btn pressed')
self.pub_state.publish('Manual')
pass
def cb_ros_jnt_states(self, msg):
# save to states
self.jnt_pos = []
self.jnt_eff = []
self._jnt_limit_effort = [1, 2, 3, 4, 5, 6, 7]
self.torque_norm = []
jnt_limit_min = (-pi/2, -pi/4, 0, -2.27, -pi/2, -1.40, 0, 0)
jnt_limit_max = (pi/2, pi/4, 0.235, 2.27, pi/2, 1.40, pi/2)
for i in range(7):
rng = jnt_limit_max[i] - jnt_limit_min[i]
pos = 2.0 * (msg.position[i] - jnt_limit_min[i]) / rng - 1.0
self.jnt_pos.append(pos)
# print rospy.Time.now()
pass
def update_widget_values(self):
# update btns
# print rospy.Time.now()
for (v, (jp, jn)) in zip(self.jnt_pos, self.joint_widgets):
jp.setEnabled(True)
jn.setEnabled(True)
jp.setValue(v if v >= 0 else 0)
jn.setValue(-v if v < 0 else 0)
# update status
pass
def init_ui(self):
# type combobox
ui_type_lable = QtGui.QLabel('Robot')
ui_type_lable.setStyleSheet('QLabel {font: bold}')
ui_type_lable.setMaximumWidth(70)
# ui_type_lable.setFont(QtGui.QFont(QtGui.QFont.Bold))
ui_type_lable.setAlignment(QtCore.Qt.AlignCenter)
self.ui_type = QtGui.QComboBox()
self.ui_type.addItem('MTMR')
self.ui_type.addItem('MTML')
self.ui_type.addItem('PSM1')
self.ui_type.addItem('PSM2')
self.ui_type.addItem('PSM3')
# self.ui_type.addItem('ECM')
ui_hbox_type = QtGui.QHBoxLayout()
ui_hbox_type.addWidget(ui_type_lable)
ui_hbox_type.addWidget(self.ui_type)
self.ui_type.currentIndexChanged.connect(self.slot_type_changed)
# control mode
# todo: use a for loop
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Gravity')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_grav)
ui_btn_vfix = QtGui.QPushButton('Teleop')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control = QtGui.QGroupBox('Control MTM')
self.ui_gbox_control.setLayout(gbox)
# ---- PSM Control Box ----
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Teleop')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_vfix = QtGui.QPushButton('Manual')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_manual)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control_psm = QtGui.QGroupBox('Control PSM')
self.ui_gbox_control_psm.setLayout(gbox)
self.ui_gbox_control_psm.setVisible(False)
# ---- gripper box -----
ui_hbox_gripper = QtGui.QHBoxLayout()
ui_gripper_label = QtGui.QLabel('Gripper Angle:')
ui_hbox_gripper.addWidget(ui_gripper_label)
ui_gbox_gripper = QtGui.QGroupBox('Gripper')
ui_gbox_gripper.setLayout(ui_hbox_gripper)
# ---- joint position group -----
jnt_pos_hbox = QtGui.QHBoxLayout()
jp_widgets = []
jn_widgets = []
for i in range(7):
pos_vbox = QtGui.QVBoxLayout()
ui_ppos = QwtThermo()
ui_ppos.setScalePosition(QwtThermo.NoScale)
ui_ppos.setAutoFillBackground(True)
ui_ppos.setAlarmLevel(0.8)
ui_ppos.setPipeWidth(20)
ui_ppos.setValue(0.0)
ui_ppos.setMinimumSize(0, 40)
ui_ppos.setRange(0.0, 1.0, False)
ui_npos = QwtThermo()
ui_npos.setScalePosition(QwtThermo.NoScale)
ui_npos.setAlarmLevel(0.8)
ui_npos.setPipeWidth(20)
ui_npos.setValue(0.9)
ui_npos.setMinimumSize(0, 40)
ui_npos.setRange(1.0, 0.0, False)
ui_npos.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
pos_vbox.addWidget(ui_ppos)
pos_vbox.addWidget(ui_npos)
pos_vbox.addWidget(ui_label_jnt)
jnt_pos_hbox.addLayout(pos_vbox)
jp_widgets.append(ui_ppos)
jn_widgets.append(ui_npos)
# ui_btn_jnt_pos = QPushButton('J1')
ui_gbox_jnt_pos = QtGui.QGroupBox('Joint Positions (normalized)')
ui_gbox_jnt_pos.setLayout(jnt_pos_hbox)
self.joint_widgets = zip(jp_widgets, jn_widgets)
# joint torque group
jnt_eff_hbox = QtGui.QHBoxLayout()
tp_widgets = []
tn_widgets = []
for i in range(7):
eff_vbox = QtGui.QVBoxLayout()
ui_peff = QwtThermo()
ui_peff.setScalePosition(QwtThermo.NoScale)
ui_peff.setAutoFillBackground(True)
ui_peff.setAlarmLevel(0.8)
ui_peff.setPipeWidth(20)
ui_peff.setValue(0.0)
ui_peff.setMinimumSize(0, 30)
ui_peff.setRange(0.0, 1.0, False)
ui_neff = QwtThermo()
ui_neff.setScalePosition(QwtThermo.NoScale)
ui_neff.setAlarmLevel(0.8)
ui_neff.setPipeWidth(20)
ui_neff.setValue(0.9)
ui_neff.setMinimumSize(0, 30)
ui_neff.setRange(1.0, 0.0, False)
ui_neff.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
eff_vbox.addWidget(ui_peff)
eff_vbox.addWidget(ui_neff)
eff_vbox.addWidget(ui_label_jnt)
jnt_eff_hbox.addLayout(eff_vbox)
tp_widgets.append(ui_peff)
tn_widgets.append(ui_neff)
ui_gbox_jnt_eff = QtGui.QGroupBox('Joint Torques (normalized)')
ui_gbox_jnt_eff.setLayout(jnt_eff_hbox)
self.torque_widgets = zip(tp_widgets, tn_widgets)
# make widgets colorful
self.dvrk_green = QColor(87, 186, 142)
self.dvrk_green_dark = self.dvrk_green.darker()
self.dvrk_green_light = self.dvrk_green.lighter()
self.dvrk_blue = QColor(80, 148, 204)
self.dvrk_blue_dark = self.dvrk_blue.darker()
self.dvrk_blue_light = self.dvrk_blue.lighter()
self.dvrk_red = QColor(232, 47, 47)
self.dvrk_red_dark = self.dvrk_red.darker()
self.dvrk_red_light = self.dvrk_red.lighter()
self.dvrk_orange = QColor(255, 103, 43)
self.dvrk_orange_dark = self.dvrk_orange.darker()
# joint_bg_color = self.dvrk_blue_dark.darker()
joint_fill_color = self.dvrk_blue
joint_alarm_color = self.dvrk_blue_light # self.dvrk_blue_light
# torque_bg_color = self.dvrk_green_dark.darker()
torque_fill_color = self.dvrk_green
torque_alarm_color = self.dvrk_orange # self.dvrk_green_light
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
# p.setColor(ui_ppos.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
# p.setColor(ui_peff.backgroundRole(), torque_bg_color)
w.setPalette(p)
# main layout
main_layout = QtGui.QVBoxLayout()
main_layout.addLayout(ui_hbox_type)
main_layout.addWidget(self.ui_gbox_control)
main_layout.addWidget(self.ui_gbox_control_psm)
main_layout.addWidget(ui_gbox_gripper)
main_layout.addWidget(ui_gbox_jnt_pos)
main_layout.addWidget(ui_gbox_jnt_eff)
self._widget.setLayout(main_layout)
pass
class dvrkDashboard(Plugin):
def __init__(self, context):
super(dvrkDashboard, self).__init__(context)
# give QObjects reasonable names
self.setObjectName('dvrkDashboard')
# process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# create qwidget
self._widget = QtGui.QWidget()
self._widget.setObjectName('dvrkDashboardUI')
# serial number
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# add widget to the user interface
context.add_widget(self._widget)
self.context = context
# ---- Get Widget -----
self.init_ui()
# ---- States -----
self.namespace = 'dvrk_mtmr'
self.jnt_pos = []
self.jnt_eff = []
self.init_ros()
# ---- Timer -----
self.update_timer = QtCore.QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self.update_widget_values)
self.update_timer.start()
pass
def init_ui(self):
# type combobox
ui_type_lable = QtGui.QLabel('Robot')
ui_type_lable.setStyleSheet('QLabel {font: bold}')
ui_type_lable.setMaximumWidth(70)
# ui_type_lable.setFont(QtGui.QFont(QtGui.QFont.Bold))
ui_type_lable.setAlignment(QtCore.Qt.AlignCenter)
self.ui_type = QtGui.QComboBox()
self.ui_type.addItem('MTMR')
self.ui_type.addItem('MTML')
self.ui_type.addItem('PSM1')
self.ui_type.addItem('PSM2')
self.ui_type.addItem('PSM3')
# self.ui_type.addItem('ECM')
ui_hbox_type = QtGui.QHBoxLayout()
ui_hbox_type.addWidget(ui_type_lable)
ui_hbox_type.addWidget(self.ui_type)
self.ui_type.currentIndexChanged.connect(self.slot_type_changed)
# control mode
# todo: use a for loop
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Gravity')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_grav)
ui_btn_vfix = QtGui.QPushButton('Teleop')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control = QtGui.QGroupBox('Control MTM')
self.ui_gbox_control.setLayout(gbox)
# ---- PSM Control Box ----
gbox = QtGui.QGridLayout()
ui_btn_idle = QtGui.QPushButton('Idle')
ui_btn_idle.setCheckable(True)
ui_btn_idle.setChecked(True)
gbox.addWidget(ui_btn_idle, 0, 0)
ui_btn_idle.clicked[bool].connect(self.slot_btn_idle)
ui_btn_home = QtGui.QPushButton('Home')
ui_btn_home.setCheckable(True)
gbox.addWidget(ui_btn_home, 0, 1)
ui_btn_home.clicked[bool].connect(self.slot_btn_home)
ui_btn_grav = QtGui.QPushButton('Teleop')
ui_btn_grav.setCheckable(True)
gbox.addWidget(ui_btn_grav, 1, 0)
ui_btn_grav.clicked[bool].connect(self.slot_btn_teleop)
ui_btn_vfix = QtGui.QPushButton('Manual')
ui_btn_vfix.setCheckable(True)
gbox.addWidget(ui_btn_vfix, 1, 1)
ui_btn_vfix.clicked[bool].connect(self.slot_btn_manual)
ui_btn_group = QtGui.QButtonGroup(self._widget)
ui_btn_group.addButton(ui_btn_idle)
ui_btn_group.addButton(ui_btn_home)
ui_btn_group.addButton(ui_btn_grav)
ui_btn_group.addButton(ui_btn_vfix)
ui_btn_group.setExclusive(True)
# connect here
self.ui_gbox_control_psm = QtGui.QGroupBox('Control PSM')
self.ui_gbox_control_psm.setLayout(gbox)
self.ui_gbox_control_psm.setVisible(False)
# ---- gripper box -----
ui_hbox_gripper = QtGui.QHBoxLayout()
ui_gripper_label = QtGui.QLabel('Gripper Angle:')
ui_hbox_gripper.addWidget(ui_gripper_label)
ui_gbox_gripper = QtGui.QGroupBox('Gripper')
ui_gbox_gripper.setLayout(ui_hbox_gripper)
# ---- joint position group -----
jnt_pos_hbox = QtGui.QHBoxLayout()
jp_widgets = []
jn_widgets = []
for i in range(7):
pos_vbox = QtGui.QVBoxLayout()
ui_ppos = QwtThermo()
ui_ppos.setScalePosition(QwtThermo.NoScale)
ui_ppos.setAutoFillBackground(True)
ui_ppos.setAlarmLevel(0.8)
ui_ppos.setPipeWidth(20)
ui_ppos.setValue(0.0)
ui_ppos.setMinimumSize(0, 40)
ui_ppos.setRange(0.0, 1.0, False)
ui_npos = QwtThermo()
ui_npos.setScalePosition(QwtThermo.NoScale)
ui_npos.setAlarmLevel(0.8)
ui_npos.setPipeWidth(20)
ui_npos.setValue(0.9)
ui_npos.setMinimumSize(0, 40)
ui_npos.setRange(1.0, 0.0, False)
ui_npos.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
pos_vbox.addWidget(ui_ppos)
pos_vbox.addWidget(ui_npos)
pos_vbox.addWidget(ui_label_jnt)
jnt_pos_hbox.addLayout(pos_vbox)
jp_widgets.append(ui_ppos)
jn_widgets.append(ui_npos)
# ui_btn_jnt_pos = QPushButton('J1')
ui_gbox_jnt_pos = QtGui.QGroupBox('Joint Positions (normalized)')
ui_gbox_jnt_pos.setLayout(jnt_pos_hbox)
self.joint_widgets = zip(jp_widgets, jn_widgets)
# joint torque group
jnt_eff_hbox = QtGui.QHBoxLayout()
tp_widgets = []
tn_widgets = []
for i in range(7):
eff_vbox = QtGui.QVBoxLayout()
ui_peff = QwtThermo()
ui_peff.setScalePosition(QwtThermo.NoScale)
ui_peff.setAutoFillBackground(True)
ui_peff.setAlarmLevel(0.8)
ui_peff.setPipeWidth(20)
ui_peff.setValue(0.0)
ui_peff.setMinimumSize(0, 30)
ui_peff.setRange(0.0, 1.0, False)
ui_neff = QwtThermo()
ui_neff.setScalePosition(QwtThermo.NoScale)
ui_neff.setAlarmLevel(0.8)
ui_neff.setPipeWidth(20)
ui_neff.setValue(0.9)
ui_neff.setMinimumSize(0, 30)
ui_neff.setRange(1.0, 0.0, False)
ui_neff.setValue(0.0)
ui_label_jnt = QtGui.QLabel('J' + str(i))
eff_vbox.addWidget(ui_peff)
eff_vbox.addWidget(ui_neff)
eff_vbox.addWidget(ui_label_jnt)
jnt_eff_hbox.addLayout(eff_vbox)
tp_widgets.append(ui_peff)
tn_widgets.append(ui_neff)
ui_gbox_jnt_eff = QtGui.QGroupBox('Joint Torques (normalized)')
ui_gbox_jnt_eff.setLayout(jnt_eff_hbox)
self.torque_widgets = zip(tp_widgets, tn_widgets)
# make widgets colorful
self.dvrk_green = QColor(87, 186, 142)
self.dvrk_green_dark = self.dvrk_green.darker()
self.dvrk_green_light = self.dvrk_green.lighter()
self.dvrk_blue = QColor(80, 148, 204)
self.dvrk_blue_dark = self.dvrk_blue.darker()
self.dvrk_blue_light = self.dvrk_blue.lighter()
self.dvrk_red = QColor(232, 47, 47)
self.dvrk_red_dark = self.dvrk_red.darker()
self.dvrk_red_light = self.dvrk_red.lighter()
self.dvrk_orange = QColor(255, 103, 43)
self.dvrk_orange_dark = self.dvrk_orange.darker()
# joint_bg_color = self.dvrk_blue_dark.darker()
joint_fill_color = self.dvrk_blue
joint_alarm_color = self.dvrk_blue_light # self.dvrk_blue_light
# torque_bg_color = self.dvrk_green_dark.darker()
torque_fill_color = self.dvrk_green
torque_alarm_color = self.dvrk_orange # self.dvrk_green_light
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
# p.setColor(ui_ppos.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
# p.setColor(ui_peff.backgroundRole(), torque_bg_color)
w.setPalette(p)
# main layout
main_layout = QtGui.QVBoxLayout()
main_layout.addLayout(ui_hbox_type)
main_layout.addWidget(self.ui_gbox_control)
main_layout.addWidget(self.ui_gbox_control_psm)
main_layout.addWidget(ui_gbox_gripper)
main_layout.addWidget(ui_gbox_jnt_pos)
main_layout.addWidget(ui_gbox_jnt_eff)
self._widget.setLayout(main_layout)
pass
def init_ros(self):
# pub
topic = '/' + self.namespace + '/set_robot_state'
self.pub_state = rospy.Publisher(topic, String)
# self.pub_state = rospy.Publisher('/dvrk_psm1/set_robot_state', String)
# sub
topic = '/' + self.namespace + '/joint_states'
self.sub_jnts = rospy.Subscriber(topic, JointState,
self.cb_ros_jnt_states)
# urdf
urdf_param = '/' + self.namespace + '/robot_description'
print urdf_param
self.urdf = ''
try:
self.urdf = rospy.get_param(urdf_param)
except KeyError:
rospy.logerr('robot_description not set')
print self.urdf
pass
def shutdown_plugin(self):
# unregister
self.pub_state.unregister()
self.sub_jnts.unregister()
# del pubs
del self.pub_state
del self.sub_jnts
pass
def reconfigure_ui(self):
if 'mtm' in self.namespace:
self.ui_gbox_control.setVisible(True)
self.ui_gbox_control_psm.setVisible(False)
elif 'psm' in self.namespace:
self.ui_gbox_control.setVisible(False)
self.ui_gbox_control_psm.setVisible(True)
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO: save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def slot_type_changed(self, index):
robot_namespaces = ('dvrk_mtmr', 'dvrk_mtml', 'dvrk_psm1', 'dvrk_psm2',
'dvrk_psm3', 'dvrk_ecm')
self.namespace = robot_namespaces[index]
rospy.loginfo('namespace = ' + self.namespace)
# update ros pub/sub
self.shutdown_plugin()
self.init_ros()
self.reconfigure_ui()
pass
def slot_btn_home(self, checked):
rospy.loginfo('home btn pressed')
self.pub_state.publish('Home')
pass
def slot_btn_idle(self, checked):
rospy.loginfo('idle btn pressed')
self.pub_state.publish('Idle')
pass
def slot_btn_grav(self, checked):
rospy.loginfo('grav btn pressed')
self.pub_state.publish('Gravity')
pass
def slot_btn_teleop(self, checked):
rospy.loginfo('teleop btn pressed')
self.pub_state.publish('Teleop')
pass
def slot_btn_manual(self, checked):
rospy.loginfo('manual btn pressed')
self.pub_state.publish('Manual')
pass
def cb_ros_jnt_states(self, msg):
# save to states
self.jnt_pos = []
self.jnt_eff = []
self._jnt_limit_effort = [1, 2, 3, 4, 5, 6, 7]
self.torque_norm = []
jnt_limit_min = (-pi / 2, -pi / 4, 0, -2.27, -pi / 2, -1.40, 0, 0)
jnt_limit_max = (pi / 2, pi / 4, 0.235, 2.27, pi / 2, 1.40, pi / 2)
for i in range(7):
rng = jnt_limit_max[i] - jnt_limit_min[i]
pos = 2.0 * (msg.position[i] - jnt_limit_min[i]) / rng - 1.0
self.jnt_pos.append(pos)
# print rospy.Time.now()
pass
def update_widget_values(self):
# update btns
# print rospy.Time.now()
for (v, (jp, jn)) in zip(self.jnt_pos, self.joint_widgets):
jp.setEnabled(True)
jn.setEnabled(True)
jp.setValue(v if v >= 0 else 0)
jn.setValue(-v if v < 0 else 0)
# update status
pass
