class QCameraView(QGraphicsView): """ Accepts an image of a teleop compressed image type and draws that in the scene/view. """ def __init__(self, parent=None): super(QCameraView, self).__init__(parent) self.scene = QGraphicsScene(self) self.setScene(self.scene) self._load_default_image() self.connect(self, SIGNAL("load_default_image"), self._load_default_image) def _load_default_image(self): joystick_icon = os.path.join( rospkg.RosPack().get_path('rocon_bubble_icons'), 'icons', 'joystick.png') pixmap = QPixmap(joystick_icon, format="png") if self.scene: self.scene.addPixmap(pixmap) self.scene.update() self.fitInView( QRectF(0, 0, self.scene.width(), self.scene.height()), Qt.KeepAspectRatio) def load_default_image(self): self.emit(SIGNAL("load_default_image")) @pyqtSlot(sensor_msgs.CompressedImage, name='image_received') def on_compressed_image_received(self, image): ''' :param sensor_msgs.CompressedImage image: convert and display this in the QGraphicsView. ''' if len(self.scene.items()) > 1: self.scene.removeItem(self.scene.items()[0]) pixmap = QPixmap() pixmap.loadFromData(image.data, format=image.format) self.scene.addPixmap(pixmap) self.scene.update() # setting fitInvView seems sensitive to here or prior to scene update self.fitInView(QRectF(0, 0, self.scene.width(), self.scene.height()), Qt.KeepAspectRatio) def resizeEvent(self, evt=None): self.fitInView(QRectF(0, 0, self.scene.width(), self.scene.height()), Qt.KeepAspectRatio)
class TeleopApp(Plugin): _update_robot_list_signal = Signal() CAMERA_FPS = (1000 / 20) D2R = 3.141592 / 180 R2D = 180 / 3.141592 LINEAR_V = 1.5 ANGULAR_V = 60 * D2R def __init__(self, context): self._context = context super(TeleopApp, self).__init__(context) self.initialised = False self.setObjectName('Teleop App') self.is_setting_dlg_live = False self._widget = QWidget() rospack = rospkg.RosPack() ui_file = os.path.join(rospack.get_path('concert_teleop_app'), 'ui', 'teleop_app.ui') self._widget.setObjectName('TeleopAppUi') loadUi(ui_file, self._widget, {}) if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) #list item click event self._widget.robot_list_tree_widget.itemClicked.connect( self._select_robot_list_tree_item) self._widget.robot_list_tree_widget.itemDoubleClicked.connect( self._dbclick_robot_list_item) #button event connection self._widget.backward_btn.setAutoRepeat(True) self._widget.forward_btn.setAutoRepeat(True) self._widget.left_turn_btn.setAutoRepeat(True) self._widget.right_turn_btn.setAutoRepeat(True) self._widget.backward_btn.clicked.connect(self._backward) self._widget.forward_btn.clicked.connect(self._forward) self._widget.left_turn_btn.clicked.connect(self._left_turn) self._widget.right_turn_btn.clicked.connect(self._right_turn) self._widget.backward_btn.released.connect(self._stop) self._widget.forward_btn.released.connect(self._stop) self._widget.left_turn_btn.released.connect(self._stop) self._widget.right_turn_btn.released.connect(self._stop) self._widget.capture_teleop_btn.pressed.connect(self._capture_teleop) self._widget.release_teleop_btn.pressed.connect(self._release_teleop) #signal event connection self._widget.destroyed.connect(self._exit) self._update_robot_list_signal.connect(self._update_robot_list) self.connect(self, SIGNAL("capture"), self._show_capture_teleop_message) self.connect(self, SIGNAL("release"), self._show_release_teleop_message) self.connect(self, SIGNAL("error"), self._show_error_teleop_message) context.add_widget(self._widget) #init self.scene = QGraphicsScene() self._widget.camera_view.setScene(self.scene) self.timer = QTimer(self._widget) self.timer.timeout.connect(self._display_image) self.timer.start(self.CAMERA_FPS) self._widget.release_teleop_btn.setEnabled(False) self.teleop_app_info = TeleopAppInfo() self.teleop_app_info._reg_event_callback(self._refresh_robot_list) self.teleop_app_info._reg_capture_event_callback( self._capture_event_callback) self.teleop_app_info._reg_release_event_callback( self._release_event_callback) self.teleop_app_info._reg_error_event_callback( self._error_event_callback) self.robot_item_list = {} self.current_robot = None self.current_captured_robot = None def _exit(self): if self.current_captured_robot: self.teleop_app_info._release_teleop( self.current_captured_robot["rocon_uri"]) def _show_capture_teleop_message(self, rtn): if rtn: QMessageBox.warning(self._widget, 'SUCCESS', "CAPTURE!!!!", QMessageBox.Ok | QMessageBox.Ok) for k in self.robot_item_list.keys(): if self.robot_item_list[k] == self.current_robot: k.setBackground(0, QBrush(Qt.SolidPattern)) k.setBackgroundColor(0, QColor(0, 255, 0, 255)) robot_name = k.text(0) k.setText(0, str(robot_name) + " (captured)") else: k.setBackground(0, QBrush(Qt.NoBrush)) k.setBackgroundColor(0, QColor(0, 0, 0, 0)) robot_name = k.text(0) else: QMessageBox.warning(self._widget, 'FAIL', "FAIURE CAPTURE!!!!", QMessageBox.Ok | QMessageBox.Ok) self._widget.capture_teleop_btn.setEnabled(False) self._widget.release_teleop_btn.setEnabled(True) self._widget.setDisabled(False) self.current_captured_robot = self.current_robot def _show_release_teleop_message(self, rtn): if rtn: QMessageBox.warning(self._widget, 'SUCCESS', "RELEASE!!!!", QMessageBox.Ok | QMessageBox.Ok) for k in self.robot_item_list.keys(): if self.robot_item_list[k] == self.current_captured_robot: k.setBackground(0, QBrush(Qt.NoBrush)) k.setBackgroundColor(0, QColor(0, 0, 0, 0)) robot_name = k.text(0) k.setText(0, robot_name[:robot_name.find(" (captured)")]) else: QMessageBox.warning(self._widget, 'FAIL', "FAIURE RELEASE!!!!", QMessageBox.Ok | QMessageBox.Ok) self._widget.setDisabled(False) self._widget.capture_teleop_btn.setEnabled(True) self._widget.release_teleop_btn.setEnabled(False) self.current_captured_robot = None def _show_error_teleop_message(self, err): QMessageBox.warning(self._widget, 'ERROR', err, QMessageBox.Ok | QMessageBox.Ok) self._widget.setDisabled(False) self._widget.capture_teleop_btn.setEnabled(True) self._widget.release_teleop_btn.setEnabled(True) def _capture_event_callback(self, rtn): try: self.emit(SIGNAL("capture"), rtn) except: pass def _release_event_callback(self, rtn): try: self.emit(SIGNAL("release"), rtn) except: pass def _error_event_callback(self, err): try: self.emit(SIGNAL("error"), err) except: pass def _release_teleop(self): if self.current_robot != self.current_captured_robot: print "NO Capture robot (captured: %s)" % self.current_captured_robot[ 'name'].string return self.teleop_app_info._release_teleop(self.current_robot["rocon_uri"]) self._widget.setDisabled(True) def _capture_teleop(self): if self.current_robot == None: print "NO Select robot" return elif self.current_captured_robot: print "Already captured robot" return self.teleop_app_info._capture_teleop(self.current_robot["rocon_uri"]) self._widget.setDisabled(True) pass def _dbclick_robot_list_item(self): if self.current_captured_robot == None: self._capture_teleop() else: self._release_teleop() def _update_robot_list(self): self._widget.robot_list_tree_widget.clear() robot_list = self.teleop_app_info.robot_list for k in robot_list.values(): robot_item = QTreeWidgetItem(self._widget.robot_list_tree_widget) robot_item.setText(0, k["name"].string) self.robot_item_list[robot_item] = k def _backward(self): if self._widget.backward_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(-self.LINEAR_V, 0) else: self._stop() def _forward(self): if self._widget.forward_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(self.LINEAR_V, 0) else: self._stop() def _left_turn(self): if self._widget.left_turn_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(0, self.ANGULAR_V) else: self._stop() def _right_turn(self): if self._widget.right_turn_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(0, -self.ANGULAR_V) else: self._stop() def _stop(self): self.teleop_app_info._request_teleop_cmd_vel(0, 0) def _select_robot_list_tree_item(self, Item): if not Item in self.robot_item_list.keys(): print "HAS NO KEY" else: self.current_robot = self.robot_item_list[Item] if self.current_robot == self.current_captured_robot: self._widget.release_teleop_btn.setEnabled(True) else: self._widget.release_teleop_btn.setEnabled(False) def _refresh_robot_list(self): self._update_robot_list_signal.emit() pass def _display_image(self): image = self.teleop_app_info.image_data if image: if len(self.scene.items()) > 1: self.scene.removeItem(self.scene.items()[0]) image_data = self.teleop_app_info.image_data.data pixmap = QPixmap() pixmap.loadFromData( image_data, format="PNG", ) self._widget.camera_view.fitInView( QRectF(0, 0, pixmap.width(), pixmap.height()), Qt.KeepAspectRatio) self.scene.addPixmap(pixmap) self.scene.update() else: self.scene.clear()