class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", rw_cache=sys.path[1]+"/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0): """Set up the device reader""" # Set values for input from config (advanced) self._jr.set_thrust_limits( self._p2t(Config().get("min_thrust")), self._p2t(Config().get("max_thrust"))) self._jr.set_rp_limit(Config().get("max_rp")) self._jr.set_yaw_limit(Config().get("max_yaw")) self._jr.set_thrust_slew_limiting( self._p2t(Config().get("slew_rate")), self._p2t(Config().get("slew_limit"))) # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connectionFailed.add_callback(self._connection_failed) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() sys.exit(-1) def _input_dev_error(self, message): """Callback for an input device error""" print "Error when reading device: {}".format(message) sys.exit(-1) def _p2t(self, percentage): """Convert a percentage to raw thrust""" return int(65000 * (percentage / 100.0))
class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", rw_cache=sys.path[1]+"/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0, xmode=False): """Set up the device reader""" # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) print "Client side X-mode: %s" % xmode if (xmode): self._cf.commander.set_client_xmode(xmode) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def controller_connected(self): """ Return True if a controller is connected""" return True if (len(self._jr.getAvailableDevices()) > 0) else False def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connection_failed.add_callback(self._connection_failed) self._cf.param.add_update_callback(group="imu_sensors", name="HMC5883L", cb=(lambda name, found: self._jr.setAltHoldAvailable(eval(found)))) self._jr.althold_updated.add_callback( lambda enabled: self._cf.param.set_value("flightmode.althold", enabled)) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() sys.exit(-1) def _input_dev_error(self, message): """Callback for an input device error""" print "Error when reading device: {}".format(message) sys.exit(-1)
class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0, xmode=False): """Set up the device reader""" # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) print "Client side X-mode: %s" % xmode if (xmode): self._cf.commander.set_client_xmode(xmode) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def controller_connected(self): """ Return True if a controller is connected""" return True if (len(self._jr.getAvailableDevices()) > 0) else False def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connection_failed.add_callback(self._connection_failed) self._cf.param.add_update_callback( group="imu_sensors", name="HMC5883L", cb=(lambda name, found: self._jr.setAltHoldAvailable(eval(found)))) self._jr.althold_updated.add_callback( lambda enabled: self._cf.param.set_value("flightmode.althold", enabled)) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() sys.exit(-1) def _input_dev_error(self, message): """Callback for an input device error""" print "Error when reading device: {}".format(message) sys.exit(-1)
class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0): """Set up the device reader""" # Set values for input from config (advanced) self._jr.set_thrust_limits(self._p2t(Config().get("min_thrust")), self._p2t(Config().get("max_thrust"))) self._jr.set_rp_limit(Config().get("max_rp")) self._jr.set_yaw_limit(Config().get("max_yaw")) self._jr.set_thrust_slew_limiting( self._p2t(Config().get("slew_rate")), self._p2t(Config().get("slew_limit"))) # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connectionFailed.add_callback(self._connection_failed) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() sys.exit(-1) def _input_dev_error(self, message): """Callback for an input device error""" print "Error when reading device: {}".format(message) sys.exit(-1) def _p2t(self, percentage): """Convert a percentage to raw thrust""" return int(65000 * (percentage / 100.0))
class MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers( enable_debug_driver=GuiConfig().get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader(cf=self.cf) self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked( GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(GuiConfig().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s ..." % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() for c in self._menu_mappings.actions(): c.setEnabled(False) devs = self.joystickReader.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked GuiConfig().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: %s", checked) def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) GuiConfig().set("link_uri", linkURI) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") self.cf.log.add_config(lg) if lg.valid: lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() else: logger.warning("Could not setup loggingblock!") def _logging_error(self, log_conf, msg): QMessageBox.about( self, "Log error", "Error when starting log config" " [%s]: %s" % (log_conf.name, msg)) def connectionLost(self, linkURI, msg): if not self._auto_reconnect_enabled: if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def connectionFailed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def closeEvent(self, event): self.hide() self.cf.close_link() GuiConfig().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self.joystickReader.stop_input() self._active_config = str(config) self._active_device = str(device) GuiConfig().set("input_device", self._active_device) GuiConfig().get("device_config_mapping")[ self._active_device] = self._active_config self.joystickReader.start_input(self._active_device, self._active_config) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [%s] - " "No input config selected" % (self._active_device)) else: self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._active_config)) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stop_input() sender = self.sender() self._active_device = sender.text() device_config_mapping = GuiConfig().get("device_config_mapping") if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text( ) GuiConfig().set("input_device", str(self._active_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.start_input(str(sender.text()), self._current_input_config) self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return self._update_input(self._active_device, self.sender().text()) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == GuiConfig().get("input_device")): self._active_device = d["name"] if (len(self._active_device) == 0): self._active_device = self._menu_devices.actions()[0].text() device_config_mapping = GuiConfig().get("device_config_mapping") if (device_config_mapping): if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions( )[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text( ) # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._active_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(GuiConfig().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl( QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
class MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers(enable_debug_driver=GuiConfig() .get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader(cf=self.cf) self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback( self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(GuiConfig().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s ..." % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() for c in self._menu_mappings.actions(): c.setEnabled(False) devs = self.joystickReader.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked GuiConfig().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: %s", checked) def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) cfclient.ui.pluginhelper.inputDeviceReader.inputdevice.setBatteryData(int(data["pm.vbat"] * 1000)) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) GuiConfig().set("link_uri", linkURI) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") self.cf.log.add_config(lg) if lg.valid: lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() else: logger.warning("Could not setup loggingblock!") def _logging_error(self, log_conf, msg): QMessageBox.about(self, "Log error", "Error when starting log config" " [%s]: %s" % (log_conf.name, msg)) def connectionLost(self, linkURI, msg): if not self._auto_reconnect_enabled: if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def connectionFailed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def closeEvent(self, event): self.hide() self.cf.close_link() GuiConfig().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self.joystickReader.stop_input() self._active_config = str(config) self._active_device = str(device) GuiConfig().set("input_device", self._active_device) GuiConfig().get( "device_config_mapping" )[self._active_device] = self._active_config self.joystickReader.start_input(self._active_device, self._active_config) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [%s] - " "No input config selected" % (self._active_device)) else: self._statusbar_label.setText("Using [%s] with config [%s]" % (self._active_device, self._active_config)) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stop_input() sender = self.sender() self._active_device = sender.text() device_config_mapping = GuiConfig().get("device_config_mapping") if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() GuiConfig().set("input_device", str(self._active_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.start_input(str(sender.text()), self._current_input_config) self._statusbar_label.setText("Using [%s] with config [%s]" % ( self._active_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return self._update_input(self._active_device, self.sender().text()) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == GuiConfig().get("input_device")): self._active_device = d["name"] if (len(self._active_device) == 0): self._active_device = self._menu_devices.actions()[0].text() device_config_mapping = GuiConfig().get("device_config_mapping") if (device_config_mapping): if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text() # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._active_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(GuiConfig().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl(QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
class MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) self.cfg = Config() self.cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", rw_cache=sys.path[1]+"/cache") cflib.crtp.init_drivers() # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("No inputdevice connected!") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() # Populate combo box with available input device configurations group = QActionGroup(self._menu_mappings, exclusive=True) for c in self.joystickReader.getListOfConfigs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) group.addAction(node) self._menu_mappings.addAction(node) self.joystickReader.start() self._current_input_config = "" self._current_input_device = "" # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connectionFailed.add_callback(self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self.joystickReader.inputDeviceErrorSignal.connect(self.inputDeviceError) self.joystickReader.discovery_signal.connect(self.device_discovery) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) # Do not queue data from the controller output to the Crazyflie wrapper to avoid latency self.joystickReader.sendControlSetpointSignal.connect(self.cf.commander.send_setpoint, Qt.DirectConnection) # Connection callbacks and signal wrappers for UI protection self.cf.connectSetupFinished.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connectionLost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connectionInitiated.add_callback(self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect(lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) # Connect link quality feedback self.cf.linkQuality.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect(lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader() self.logConfigReader.readConfigFiles() # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) #Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] #Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda :self.toggleToolbox(False)) #Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tmpTabList = {} for newTab in cfclient.ui.tabs.available: t = newTab(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(t.getMenuName(), self) item.setCheckable(True) item.toggled.connect(t.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tmpTabList[t.getTabName()] = item # TODO: Fix this dirty hack! # Without this the signal never arrives is t.toggleVisibility when # clicked in menu ?! t.fakeIt() # First instantiate all the tabs and then open them in the correct order try: for tName in Config().get("open_tabs").split(","): t = tmpTabList[tName] if (t != None): t.toggle() # Toggle though menu so it's also marked as open there except Exception as e: logger.warning("Exception while opening tabs [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(Config().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._current_input_device = "" for c in self._menu_mappings.actions(): c.setEnabled(False) devs = self.joystickReader.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, data): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) Config().set("link_uri", linkURI) lg = LogConfig ("Battery", 1000) lg.addVariable(LogVariable("pm.vbat", "float")) self.log = self.cf.log.create_log_packet(lg) if (self.log != None): self.log.dataReceived.add_callback(self.batteryUpdatedSignal.emit) self.log.error.add_callback(self.loggingError) self.log.start() else: logger.warning("Could not setup loggingblock!") def loggingError(self, error): logger.warn("logging error %s", error) def connectionLost(self, linkURI, msg): if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self,warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) def connectionFailed(self, linkURI, error): msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self,warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) def closeEvent(self, event): self.hide() self.cf.close_link() Config().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self,"Input device error", error) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stopInput() sender = self.sender() self._current_input_device = sender.text() device_config_mapping = Config().get("device_config_mapping") if (self._current_input_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._current_input_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() Config().set("input_device", str(self._current_input_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.startInput(str(sender.text()), self._current_input_config) self._statusbar_label.setText("Using [%s] with config [%s]" % ( self._current_input_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return sender = self.sender() self._current_input_config = sender.text() Config().get("device_config_mapping")[str(self._current_input_device)] = str(self._current_input_config) self.joystickReader.startInput(str(self._current_input_device), self._current_input_config) self._statusbar_label.setText("Using [%s] with config [%s]" % ( self._current_input_device, self._current_input_config)) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == Config().get("input_device")): self._current_input_device = d["name"] if (len(self._current_input_device) == 0): self._current_input_device = self._menu_devices.actions()[0].text() device_config_mapping = Config().get("device_config_mapping") if (device_config_mapping): if (self._current_input_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._current_input_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text() # Now we know what device to use and what mapping, trigger the events to # change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._current_input_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(Config().get("link_uri")) except KeyError: self.cf.open_link("") def closeAppRequest(self): self.close() app.exit(0);
class Teleop(): """Crazyflie Joystick Controller""" def __init__(self): """Initialize the joystick reader""" self._althold = False self._jr = JoystickReader(do_device_discovery=False) self._jr.device_error.add_callback(self._input_dev_error) self._jr.althold_updated.add_callback(self._althold_updated) self._jr.input_updated.add_callback(self._input_updated) self._command_pub = rospy.Publisher('cfjoy', CFJoyMsg) def start_controller(self, input_config, input_device=0): """Set up the device reader""" # Frequency of joystick messages is handled by joystick driver # TODO: make that configurable or write handler for ROS joystick # Frequency of sending to flie is handled by crazyflie_node #TODO: connect to a config message from the flie? self._jr.setAltHoldAvailable(False) devs = self._jr.getAvailableDevices() if len(devs) < 1: raise ControllerNotFound("Device count: %d" % len(devs)) dev_name = devs[input_device]["name"] rospy.loginfo("Using [%s] for input with [%s]", dev_name, input_config) self._jr.start_input(dev_name, input_config) def controller_connected(self): """ Return True if a controller is connected""" return True if (len(self._jr.getAvailableDevices()) > 0) else False def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print("Controller #%s: %s" % (dev["id"], dev["name"])) def _althold_updated(self, enabled): #TODO: enabled is a string! Stop that! self._althold = (enabled == 'True') rospy.logdebug("Althold set: enabled=%i", self._althold) def _input_updated(self, roll, pitch, yaw, thrust): #Sent from joystickreader: #self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust) msg = self._create_joy_msg(roll, pitch, yaw, thrust, self._althold); self._command_pub.publish(msg); rospy.logdebug("Updated: r=%.4f, p=%.4f, y=%.4f, t=%.4f, h=%i", roll, pitch, yaw, thrust, self._althold) @staticmethod def _input_dev_error(message): """Callback for an input device error""" rospy.logerr("Error when reading device: %s", message) rospy.signal_shutdown("Error when reading device: %s" % message) sys.exit(-1) @staticmethod def _create_joy_msg(roll, pitch, yaw, thrust, althold): h = std_msgs.msg.Header() h.stamp = rospy.Time.now() return CFJoyMsg(h, roll, pitch, yaw, thrust, althold)