def on_reset_motors(self): # if any of the breakers is not enabled ask if they'd like to enable them if (self._dashboard_message is not None and self._dashboard_message.power_board_state_valid): all_breakers_enabled = reduce(lambda x, y: x and y, [ state == PowerBoardState.STATE_ON for state in self._dashboard_message.power_board_state.circuit_state ]) if (not all_breakers_enabled): if (QMessageBox.question( self._breakers[0], self.tr('Enable Breakers?'), self. tr("Resetting the motors may not work because not all breakers are enabled. Enable all the breakers first?" ), QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes) == QMessageBox.Yes): [breaker.set_enable() for breaker in self._breakers] reset = rospy.ServiceProxy("pr2_etherCAT/reset_motors", std_srvs.srv.Empty) try: reset() except rospy.ServiceException, e: QMessageBox.critical( self._breakers[0], "Error", "Failed to reset the motors: service call failed with error: %s" % (e))
def mergeTracks(self, unused=None): (track1Id, ok) = QInputDialog.getInteger( self.widget, "Merge two tracks", "Please enter the ID of the 1st person track you want to merge.") if not ok: return (track2Id, ok) = QInputDialog.getInteger( self.widget, "Merge two tracks", "Please enter the ID of the 2nd person track you want to merge.") if not ok: return if track1Id == track2Id: QMessageBox.critical(self.widget, "Merge two tracks", "Track IDs cannot be identical!") return if self.verifyTrackExists(track1Id) and self.verifyTrackExists( track2Id): self.editor.mergeTracks(track1Id, track2Id) self.updateTrackCount() QMessageBox.information( self.widget, "Merge two tracks", "Person tracks %d and %d have been merged!" % (track1Id, track2Id))
def verifyTrackExists(self, trackId): if not trackId in self.database.getTrackIds(): QMessageBox.critical( self.widget, "Wrong track ID", "Person track with ID %d does not exist!" % trackId) return False return True
def _handle_remove_clicked(self): name = self.name_edit.text() if name in self.places_dict.keys(): self.places_dict.pop(name) for item in self._sceneItems[name].keys(): self._scene.removeItem(self._sceneItems[name][item]) else: QMessageBox.critical(self, "Error!", "Place does not exist.")
def reset_motorstop_cb(self): reset_srv = rospy.ServiceProxy("/reset_motorstop", ResetMotorStop) try: reset_srv() except rospy.ServiceException, e: QMessageBox.critical( self._drive, "Error", "Failed to halt the motors: service call failed with error: %s" % (e))
def enable_motorstop_cb(self): stop_srv = rospy.ServiceProxy("/emergency_stop", EmergencyStop) try: stop_srv() except rospy.ServiceException, e: QMessageBox.critical( self._drive, "Error", "Failed to emergency stop: service call failed with error: %s" % (e))
def on_halt_motors(self): halt = rospy.ServiceProxy( "{}/halt_motors".format(self._motor_namespace), std_srvs.srv.Empty) try: halt() except rospy.ServiceException as e: QMessageBox.critical( self._motors, "Error", "Failed to halt the motors: service call failed with error: %s" % (e))
def on_halt_motors(self): halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors", std_srvs.srv.Empty) try: halt() except rospy.ServiceException, e: QMessageBox.critical( self._motors, "Error", "Failed to halt the motors: service call failed with error: %s" % (e))
def freerun_cb(self): self._freerun_state = not self._freerun_state freerun_srv = rospy.ServiceProxy("/enable_motors", EnableMotors) try: freerun_srv(not self._freerun_state) except rospy.ServiceException, e: QMessageBox.critical( self._drive, "Error", "Failed to enter freerun: service call failed with error: %s" % (e))
def _handle_get_clicked(self): name = self.name_edit.text() if (name in self.places_dict.keys()): self.x_spin.setValue(self.places_dict[name][0]) self.y_spin.setValue(self.places_dict[name][1]) theta = euler_from_quaternion( numpy.asarray(self.places_dict[name][3:7]))[2] self.theta_spin.setValue(degrees(theta)) else: QMessageBox.critical(self, "Error!", "This places has not been added.")
def on_reconnect_clicked(self): if (self._dashboard_message is not None): reconnect = rospy.ServiceProxy("/reconnect", std_srvs.srv.Empty) try: reconnect() except rospy.ServiceException, e: QMessageBox.critical( self.ethercat, "Error", "Failed to reconnect the driver: service call failed with error: %s" % (e))
def on_saveButton_clicked(self): ''' Saves the current document. This method is called if the C{save button} was clicked. ''' saved, errors, msg = self.tabWidget.currentWidget().save(True) if errors: QMessageBox.critical(self, "Error", msg) self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._error_icon) self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), msg) elif saved: self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._empty_icon) self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), '')
def on_saveButton_clicked(self): ''' Saves the current document. This method is called if the C{save button} was clicked. ''' saved, errors, msg = self.tabWidget.currentWidget().save(True) if errors: QMessageBox.critical(self, "Error", msg) self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._error_icon) self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), msg) elif saved: self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._empty_icon) self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), '')
def _handle_add_clicked(self): name = self.name_edit.text() if len(name) != 0: # remove and re-draw it if name in self.places_dict.keys(): self.places_dict.pop(name) for item in self._sceneItems[name].keys(): self._scene.removeItem(self._sceneItems[name][item]) try: # storing the values in the dict x = self.x_spin.value() y = self.y_spin.value() theta = self.theta_spin.value() q = quaternion_from_euler(0.0, 0.0, theta) self.places_dict[str(name)] = [ x, y, 0.0, float(q[0]), float(q[1]), float(q[2]), float(q[3]) ] # drawing the items self._sceneItems[name] = {"text": QGraphicsTextItem()} self._sceneItems[name]["text"].setDefaultTextColor( QColor(0, 0, 255)) self._sceneItems[name]["text"].setFont(QFont("Times", 10)) self._sceneItems[name]["text"].setPlainText(name) scene_pos = self._gridToScene(x, y, q) x_c = scene_pos[0] - self._sceneItems[name][ "text"].boundingRect().width() / 2.0 self._sceneItems[name]["text"].setPos(x_c, scene_pos[1]) self._scene.addItem(self._sceneItems[name]["text"]) self._sceneItems[name]["rec"] = self._scene.addRect( QRectF((scene_pos[0] - 2), (scene_pos[1] - 2), 4, 4)) line = QLineF( scene_pos[0], scene_pos[1], (scene_pos[0] - self._mag * cos(radians(scene_pos[3]))), (scene_pos[1] + self._mag * sin(radians(scene_pos[3])))) self._sceneItems[name]["line"] = self._scene.addLine( line, pen=QPen(Qt.red, 2)) except ValueError: QMessageBox.critical(self, "Error!", "You must insert a valid value.") else: QMessageBox.critical( self, "Error!", "You have to insert a name and a valid position.")
def focusInEvent(self, event): # check for file changes try: if self.filename and self.file_info: if self.file_info.lastModified() != QFileInfo(self.filename).lastModified(): self.file_info = QFileInfo(self.filename) result = QMessageBox.question(self, "File changed", "File was changed, reload?", QMessageBox.Yes | QMessageBox.No) if result == QMessageBox.Yes: f = QFile(self.filename) if f.open(QIODevice.ReadOnly | QIODevice.Text): self.setText(unicode(f.readAll(), "utf-8")) self.document().setModified(False) self.textChanged.emit() else: QMessageBox.critical(self, "Error", "Cannot open launch file%s" % self.filename) except: pass QTextEdit.focusInEvent(self, event)
def _handle_load_clicked(self): filename = QFileDialog.getOpenFileName( self, self.tr('Load from File'), '.', self.tr('YAML files {.yaml} (*.yaml)')) if filename[0] != '': with open(filename[0], 'r') as infile: try: self.places_dict = yaml.load(infile) for k in self._sceneItems.keys(): for item in self._sceneItems[k].keys(): self._scene.removeItem(self._sceneItems[k][item]) for name in self.places_dict.keys(): # q = numpy.asarray(self.places_dict[name][3:7]) scene_pos = self._gridToScene( self.places_dict[name][0], self.places_dict[name][1], q) # drawing the items self._sceneItems[name] = {"text": QGraphicsTextItem()} self._sceneItems[name]["text"].setDefaultTextColor( QColor(0, 0, 255)) self._sceneItems[name]["text"].setFont( QFont("Times", 10)) self._sceneItems[name]["text"].setPlainText(name) x_c = scene_pos[0] - self._sceneItems[name][ "text"].boundingRect().width() / 2.0 self._sceneItems[name]["text"].setPos( x_c, scene_pos[1]) self._scene.addItem(self._sceneItems[name]["text"]) self._sceneItems[name]["rec"] = self._scene.addRect( QRectF((scene_pos[0] - 2), (scene_pos[1] - 2), 4, 4)) line = QLineF(scene_pos[0], scene_pos[1], (scene_pos[0] - self._mag * cos(radians(scene_pos[3]))), (scene_pos[1] + self._mag * sin(radians(scene_pos[3])))) self._sceneItems[name]["line"] = self._scene.addLine( line, pen=QPen(Qt.red, 2)) except yaml.scanner.ScannerError: QMessageBox.critical(self, "Error!", "Invalid YAML file.")
def msg_type_changed(self, type_name): if not type_name: if hasattr(self, 'message_info'): self._widget.msg_type_combo_box.setEditText( self.message_info['type_name']) return # initialization with empty string; just ignore if self._create_message_instance(type_name) is None: QMessageBox.critical(self._widget, 'Change Message Type', 'Unrecognized message type', QMessageBox.Ok) if hasattr(self, 'message_info'): self._widget.msg_type_combo_box.setEditText( self.message_info['type_name']) else: self._widget.msg_type_combo_box.setEditText('') return if hasattr(self, 'message_info'): if self.message_info['type_name'] == type_name: return # selected same type as current, just ignore answer = QMessageBox.question(self._widget, 'Change Message Type', 'Are you sure you want to change current message type?\n'\ 'All changes will be discarded!', QMessageBox.Ok, QMessageBox.Cancel) if answer != QMessageBox.Ok: self._widget.msg_type_combo_box.setEditText( self.message_info['type_name']) return self.clean_up_message() self.message_info = { 'type_name': str(type_name), 'instance': self._create_message_instance(str(type_name)) } # Ask for filling the message with annotation's fields likely to be the same answer = QMessageBox.question(self._widget, "Fill Message's Fields", "Do you want to copy matching fields from the annotation?\n" \ "You can change the suggested values later", QMessageBox.Yes, QMessageBox.No) if answer == QMessageBox.Yes: self._set_message(self.message_info, self.annotation) else: self._set_message(self.message_info)
def msg_type_changed(self, type_name): if not type_name: if hasattr(self, 'message_info'): self._widget.msg_type_combo_box.setEditText(self.message_info['type_name']) return # initialization with empty string; just ignore if self._create_message_instance(type_name) is None: QMessageBox.critical(self._widget, 'Change Message Type', 'Unrecognized message type', QMessageBox.Ok) if hasattr(self, 'message_info'): self._widget.msg_type_combo_box.setEditText(self.message_info['type_name']) else: self._widget.msg_type_combo_box.setEditText('') return if hasattr(self, 'message_info'): if self.message_info['type_name'] == type_name: return # selected same type as current, just ignore answer = QMessageBox.question(self._widget, 'Change Message Type', 'Are you sure you want to change current message type?\n'\ 'All changes will be discarded!', QMessageBox.Ok, QMessageBox.Cancel) if answer != QMessageBox.Ok: self._widget.msg_type_combo_box.setEditText(self.message_info['type_name']) return self.clean_up_message() self.message_info = { 'type_name': str(type_name), 'instance': self._create_message_instance(str(type_name)) } # Ask for filling the message with annotation's fields likely to be the same answer = QMessageBox.question(self._widget, "Fill Message's Fields", "Do you want to copy matching fields from the annotation?\n" \ "You can change the suggested values later", QMessageBox.Yes, QMessageBox.No) if answer == QMessageBox.Yes: self._set_message(self.message_info, self.annotation) else: self._set_message(self.message_info)
def focusInEvent(self, event): # check for file changes try: if self.filename and self.file_info: if self.file_info.lastModified() != QFileInfo( self.filename).lastModified(): self.file_info = QFileInfo(self.filename) result = QMessageBox.question( self, "File changed", "File was changed, reload?", QMessageBox.Yes | QMessageBox.No) if result == QMessageBox.Yes: f = QFile(self.filename) if f.open(QIODevice.ReadOnly | QIODevice.Text): self.setText(unicode(f.readAll(), "utf-8")) self.document().setModified(False) self.textChanged.emit() else: QMessageBox.critical( self, "Error", "Cannot open launch file%s" % self.filename) except: pass QTextEdit.focusInEvent(self, event)
def _handle_read_map(self): try: self.map = rospy.wait_for_message("/map", OccupancyGrid, timeout=5) self.add_button.setEnabled(True) self.load_button.setEnabled(True) self.save_button.setEnabled(True) self.name_edit.setEnabled(True) self.x_spin.setEnabled(True) self.y_spin.setEnabled(True) self.theta_spin.setEnabled(True) self.get_button.setEnabled(True) self.remove_button.setEnabled(True) self.graphics_view.setEnabled(True) self.graphics_view.setTransformationAnchor( QGraphicsView.AnchorUnderMouse) self.graphics_view.setDragMode(QGraphicsView.ScrollHandDrag) self.graphics_view.setInteractive(True) self.map_read.emit() except rospy.exceptions.ROSException: QMessageBox.critical( self, "Error!", "Unable to read the map. Make sure to run the map_server node." )
def control(self, breaker, cmd): """ Sends a PowerBoardCommand srv to the pr2 :param breaker: breaker index to send command to :type breaker: int :param cmd: command to be sent to the pr2 breaker :type cmd: str """ if (not self._power_board_state): QMessageBox.critical( self, "Error", self. tr("Cannot control breakers until we have received a power board state message" )) return False if (not self._power_board_state.run_stop or not self._power_board_state.wireless_stop): if (cmd == "start"): QMessageBox.critical( self, "Error", self. tr("Breakers will not enable because one of the runstops is pressed" )) return False try: power_cmd = PowerBoardCommandRequest() power_cmd.breaker_number = breaker power_cmd.command = cmd power_cmd.serial_number = self._serial self._power_control(power_cmd) return True except rospy.ServiceException as e: QMessageBox.critical(self, "Error", "Service call failed with error: %s" % (e), "Error") return False return False
def on_motors_clicked(self, button_handle, component_name, data_index, mode_name): if (self._dashboard_message is not None and self._dashboard_message.power_board_state_valid): # if mode == "Off" ok_state = PowerBoardState.STATE_ENABLED switch_state = PowerBoardState.STATE_STANDBY if (mode_name == "On"): ok_state = PowerBoardState.STATE_STANDBY switch_state = PowerBoardState.STATE_ENABLED if (self._dashboard_message.power_board_state. circuit_state[data_index] == ok_state): switch_motor_state = rospy.ServiceProxy( "/" + component_name + "/switch" + mode_name + "Motors", std_srvs.srv.Empty) try: switch_motor_state() except rospy.ServiceException, e: QMessageBox.critical( button_handle, "Error", "Failed to switch " + mode_name + " " + component_name + " motors: service call failed with error: %s" % (e)) elif (self._dashboard_message.power_board_state. circuit_state[data_index] == switch_state): QMessageBox.critical( button_handle, "Error", component_name + " motors are already switched " + mode_name) elif (self._dashboard_message.power_board_state. circuit_state[data_index] == PowerBoardState.STATE_DISABLED): QMessageBox.critical(button_handle, "Error", component_name + " is not connected")