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)
