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 __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
self._devs = []
for d in self._jr.available_devices():
self._devs.append(d.name)
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)
def __init__(self, args):
self.args = args
self.optitrack_enabled = False
self.console_dump_enabled = False
cflib.crtp.init_drivers(enable_debug_driver=False)
self._cf = Crazyflie(rw_cache="./cache")
self._jr = JoystickReader(do_device_discovery=False)
# Set flight mode
if self.args["trajectory"] is None or \
self.args["trajectory"][0] == "none":
self.mode = Mode.DONT_FLY
print("Mode set to [DONT_FLY]")
elif self.args["trajectory"][0] == "manual":
self.mode = Mode.MANUAL
print("Mode set to [MANUAL]")
elif self.args["safetypilot"]:
self.mode = Mode.MODE_SWITCH
print("Mode set to [MODE_SWITCH]")
else:
self.mode = Mode.AUTO
print("Mode set to [AUTO]")
# Setup for specified mode
if self.mode == Mode.AUTO:
self.is_in_manual_control = False
# Make sure drone is setup to perform autonomous flight
if args["uwb"] == "none":
assert args["optitrack"] == "state", "OptiTrack state needed in absence of UWB"
assert args["estimator"] == "kalman", "OptiTrack state needs Kalman estimator"
elif self.mode == Mode.DONT_FLY:
self.is_in_manual_control = False
else:
# Check if controller is connected
assert self.controller_connected(), "No controller detected."
self.setup_controller(map="flappy")
self.is_in_manual_control = True
# Setup the logging framework
self.setup_logger()
# Setup optitrack if required
if not args["optitrack"] == "none":
self.setup_optitrack()
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 __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 __init__(self, link_uri, radio_port, serialport, xmode=True):
"""Initialize the headless client and libraries"""
from cfclient.utils.input import JoystickReader
self._cf = Crazyflie()
self._serial = serialport
self._cf.commander.set_client_xmode(xmode)
self._cf.open_link(link_uri)
self._port = radio_port
self.is_connected = True
self.running = True
self._console = ""
# Callbacks
self._cf.connected.add_callback(
lambda msg: print("Connected to " + msg))
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.console.receivedChar.add_callback(self._console_char)
self._cf.packet_received.add_callback(self._data_received)
# Search for controllers
self._jr = JoystickReader(do_device_discovery=False)
available_devices = self._jr.available_devices()
if len(available_devices):
used_device = available_devices[0]
print("Using input device: " + str(used_device))
self._jr.device_error.add_callback(
lambda msg: print("Input device error: " + str(msg)))
self._jr.start_input(used_device.name)
self._jr.set_input_map(used_device.name, "xbox360_mode1")
# Controller callback
self._jr.input_updated.add_callback(
self._cf.commander.send_setpoint)
else:
print("No input device detected")
self.serialthread = threading.Thread(target=self._serial_listener)
self.serialthread.start()
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=None,
rw_cache=cfclient.config_path + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
self._devs = []
for d in self._jr.available_devices():
self._devs.append(d.name)
def main():
# Creating a soca bridge
serial_dev = '/dev/ttyS11'
serial_dev_agent = '/dev/ttyS10'
socat_process = subprocess.Popen(
["socat", "PTY,link=/dev/ttyS10", "PTY,link=" + serial_dev])
time.sleep(1)
print("Use " + serial_dev_agent + " for micro-ROS agent connection")
# Set logging level
logging.basicConfig(level=logging.INFO)
# Open serial port
serialport = serial.Serial(serial_dev, baudrate=115200, timeout=0.1)
# Retrieve controller
jr = JoystickReader(do_device_discovery=False)
available_devices = jr.available_devices()
if len(available_devices):
used_device = available_devices[0]
print("Using input device: " + str(used_device))
jr.device_error.add_callback(
lambda msg: print("Input device error: " + str(msg)))
jr.start_input(used_device.name)
jr.set_input_map(used_device.name, "PS3_Mode_1")
else:
used_device = None
print("Using no input device")
# Configure radio link
cflib.crtp.radiodriver.set_retries_before_disconnect(-1)
cflib.crtp.radiodriver.set_retries(100)
cflib.crtp.init_drivers(enable_debug_driver=False)
link_uri = "radio://0/65/2M"
try:
while True:
cf = CrazyflieBridgedController(link_uri, serialport,
jr if used_device else None)
while cf.is_connected:
time.sleep(0.1)
print("Disconected from: " + str(link_uri))
except KeyboardInterrupt:
pass
os.killpg(os.getpgid(socat_process.pid), signal.SIGTERM)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
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)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
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._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# 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._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(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)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_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 Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not 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 [{}]".format(e))
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
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)
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
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)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
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._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# 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._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(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)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_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 Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not 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 [{}]".format(e))
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
def interfaceChanged(self, interface):
if interface == INTERFACE_PROMPT_TEXT:
self._selected_interface = None
else:
self._selected_interface = interface
self._update_ui_state()
def foundInterfaces(self, interfaces):
selected_interface = self._selected_interface
self.interfaceCombo.clear()
self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT)
formatted_interfaces = []
for i in interfaces:
if len(i[1]) > 0:
interface = "%s - %s" % (i[0], i[1])
else:
interface = i[0]
formatted_interfaces.append(interface)
self.interfaceCombo.addItems(formatted_interfaces)
if self._initial_scan:
self._initial_scan = False
try:
selected_interface = Config().get("link_uri")
except KeyError:
pass
newIndex = 0
if selected_interface is not None:
try:
newIndex = formatted_interfaces.index(selected_interface) + 1
except ValueError:
pass
self.interfaceCombo.setCurrentIndex(newIndex)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _update_ui_state(self):
if self.uiState == UIState.DISCONNECTED:
self.setWindowTitle("Not connected")
canConnect = self._selected_interface is not None
self.menuItemConnect.setText("Connect to Crazyflie")
self.menuItemConnect.setEnabled(canConnect)
self.connectButton.setText("Connect")
self.connectButton.setToolTip(
"Connect to the Crazyflie on the selected interface")
self.connectButton.setEnabled(canConnect)
self.scanButton.setText("Scan")
self.scanButton.setEnabled(True)
self.address.setEnabled(True)
self.batteryBar.setValue(3000)
self._menu_cf2_config.setEnabled(False)
self._menu_cf1_config.setEnabled(True)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
self.interfaceCombo.setEnabled(True)
elif self.uiState == UIState.CONNECTED:
s = "Connected on %s" % self._selected_interface
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Disconnect")
self.connectButton.setToolTip("Disconnect from the Crazyflie")
self.scanButton.setEnabled(False)
self.logConfigAction.setEnabled(True)
# Find out if there's an I2C EEPROM, otherwise don't show the
# dialog.
if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
self._menu_cf2_config.setEnabled(True)
self._menu_cf1_config.setEnabled(False)
elif self.uiState == UIState.CONNECTING:
s = "Connecting to {} ...".format(self._selected_interface)
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Cancel")
self.connectButton.setToolTip("Cancel connecting to the Crazyflie")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
elif self.uiState == UIState.SCANNING:
self.setWindowTitle("Scanning ...")
self.connectButton.setText("Connect")
self.menuItemConnect.setEnabled(False)
self.connectButton.setText("Connect")
self.connectButton.setEnabled(False)
self.scanButton.setText("Scanning...")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.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.available_devices()
# if (len(devs) > 0):
# self.device_discovery(devs)
def _show_input_device_config_dialog(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
Config().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: {}".format(checked))
def _show_connect_dialog(self):
self.logConfigDialogue.show()
def _update_battery(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
color = COLOR_BLUE
# TODO firmware reports fully-charged state as 'Battery',
# rather than 'Charged'
if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
color = COLOR_GREEN
elif data["pm.state"] == BatteryStates.LOW_POWER:
color = COLOR_RED
self.batteryBar.setStyleSheet(progressbar_stylesheet(color))
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
mem = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0]
mem.write_data(self._led_write_done)
# self._led_write_test = 0
# mem.leds[self._led_write_test] = [10, 20, 30]
# mem.write_data(self._led_write_done)
def _disconnected(self):
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _connection_initiated(self):
self.uiState = UIState.CONNECTING
self._update_ui_state()
def _led_write_done(self, mem, addr):
logger.info("LED write done callback")
# self._led_write_test += 1
# mem.leds[self._led_write_test] = [10, 20, 30]
# mem.write_data(self._led_write_done)
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error", "Error when starting log config"
" [{}]: {}".format(log_conf.name,
msg))
def _connection_lost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if self.isActiveWindow():
warningCaption = "Communication failure"
error = "Connection lost to {}: {}".format(linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def _connection_failed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on {}: {}".format(linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
Config().save_file()
def _connect(self):
if self.uiState == UIState.CONNECTED:
self.cf.close_link()
elif self.uiState == UIState.CONNECTING:
self.cf.close_link()
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self.cf.open_link(self._selected_interface)
def _scan(self):
self.uiState = UIState.SCANNING
self._update_ui_state()
self.scanner.scanSignal.emit(self.address.value())
def _display_input_device_error(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _mux_selected(self, checked):
"""Called when a new mux is selected. The menu item contains a
reference to the raw mux object as well as to the associated device
sub-nodes"""
if not checked:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(False)
else:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(True)
self.joystickReader.set_mux(mux=mux)
# Go though the tree and select devices/mapping that was
# selected before it was disabled.
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
dev_node.toggled.emit(True)
self._update_input_device_footer()
def _get_dev_status(self, device):
msg = "{}".format(device.name)
if device.supports_mapping:
map_name = "N/A"
if device.input_map:
map_name = device.input_map_name
msg += " ({})".format(map_name)
return msg
def _update_input_device_footer(self):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
msg = ""
if len(self.joystickReader.available_devices()) > 0:
mux = self.joystickReader._selected_mux
msg = "Using {} mux with ".format(mux.name)
for key in list(mux._devs.keys())[:-1]:
if mux._devs[key]:
msg += "{}, ".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A, "
# Last item
key = list(mux._devs.keys())[-1]
if mux._devs[key]:
msg += "{}".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A"
else:
msg = "No input device found"
self._statusbar_label.setText(msg)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu. The
data in the menu object is the associated map menu (directly under the
item in the menu) and the raw device"""
(map_menu, device, mux_menu) = self.sender().data()
if not checked:
if map_menu:
map_menu.setEnabled(False)
# Do not close the device, since we don't know exactly
# how many devices the mux can have open. When selecting a
# new mux the old one will take care of this.
else:
if map_menu:
map_menu.setEnabled(True)
(mux, sub_nodes) = mux_menu.data()
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
if device.id == dev_node.data()[1].id \
and dev_node is not self.sender():
dev_node.setChecked(False)
role_in_mux = str(self.sender().parent().title()).strip()
logger.info("Role of {} is {}".format(device.name,
role_in_mux))
Config().set("input_device", str(device.name))
self._mapping_support = self.joystickReader.start_input(
device.name,
role_in_mux)
self._update_input_device_footer()
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu. The
data in the menu object is a referance to the device QAction in parent
menu. This contains a referance to the raw device."""
if not checked:
return
selected_mapping = str(self.sender().text())
device = self.sender().data().data()[1]
self.joystickReader.set_input_map(device.name, selected_mapping)
self._update_input_device_footer()
def device_discovery(self, devs):
"""Called when new devices have been added"""
for menu in self._all_role_menus:
role_menu = menu["rolemenu"]
mux_menu = menu["muxmenu"]
dev_group = QActionGroup(role_menu, exclusive=True)
for d in devs:
dev_node = QAction(d.name, role_menu, checkable=True,
enabled=True)
role_menu.addAction(dev_node)
dev_group.addAction(dev_node)
dev_node.toggled.connect(self._inputdevice_selected)
map_node = None
if d.supports_mapping:
map_node = QMenu(" Input map", role_menu, enabled=False)
map_group = QActionGroup(role_menu, exclusive=True)
# Connect device node to map node for easy
# enabling/disabling when selection changes and device
# to easily enable it
dev_node.setData((map_node, d))
for c in ConfigManager().get_list_of_configs():
node = QAction(c, map_node, checkable=True,
enabled=True)
node.toggled.connect(self._inputconfig_selected)
map_node.addAction(node)
# Connect all the map nodes back to the device
# action node where we can access the raw device
node.setData(dev_node)
map_group.addAction(node)
# If this device hasn't been found before, then
# select the default mapping for it.
if d not in self._available_devices:
last_map = Config().get("device_config_mapping")
if d.name in last_map and last_map[d.name] == c:
node.setChecked(True)
role_menu.addMenu(map_node)
dev_node.setData((map_node, d, mux_menu))
# Update the list of what devices we found
# to avoid selecting default mapping for all devices when
# a new one is inserted
self._available_devices = ()
for d in devs:
self._available_devices += (d,)
# Only enable MUX nodes if we have enough devies to cover
# the roles
for mux_node in self._all_mux_nodes:
(mux, sub_nodes) = mux_node.data()
if len(mux.supported_roles()) <= len(self._available_devices):
mux_node.setEnabled(True)
# TODO: Currently only supports selecting default mux
if self._all_mux_nodes[0].isEnabled():
self._all_mux_nodes[0].setChecked(True)
# If the previous length of the available devies was 0, then select
# the default on. If that's not available then select the first
# on in the list.
# TODO: This will only work for the "Normal" mux so this will be
# selected by default
if Config().get("input_device") in [d.name for d in devs]:
for dev_menu in self._all_role_menus[0]["rolemenu"].actions():
if dev_menu.text() == Config().get("input_device"):
dev_menu.setChecked(True)
else:
# Select the first device in the first mux (will always be "Normal"
# mux)
self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True)
logger.info("Select first device")
self._update_input_device_footer()
def _open_config_folder(self):
QDesktopServices.openUrl(
QUrl("file:///" +
QDir.toNativeSeparators(cfclient.config_path)))
def closeAppRequest(self):
self.close()
sys.exit(0)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
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)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# 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
cfclient.ui.pluginhelper.mainUI = self
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(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)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
# self.tabsMenuItem.setMenu(QtWidgets.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.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 Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not 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 [{}]".format(e))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
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 = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
class MainUI(QtWidgets.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)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
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)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# 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
cfclient.ui.pluginhelper.mainUI = self
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(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)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
# self.tabsMenuItem.setMenu(QtWidgets.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.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 Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not 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 [{}]".format(e))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
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 = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
def disable_input(self, disable):
"""
Disable the gamepad input to be able to send setpoint from a tab
"""
self._disable_input = disable
def interfaceChanged(self, interface):
if interface == INTERFACE_PROMPT_TEXT:
self._selected_interface = None
else:
self._selected_interface = interface
self._update_ui_state()
def foundInterfaces(self, interfaces):
selected_interface = self._selected_interface
self.interfaceCombo.clear()
self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT)
formatted_interfaces = []
for i in interfaces:
if len(i[1]) > 0:
interface = "%s - %s" % (i[0], i[1])
else:
interface = i[0]
formatted_interfaces.append(interface)
self.interfaceCombo.addItems(formatted_interfaces)
if self._initial_scan:
self._initial_scan = False
try:
if len(Config().get("link_uri")) > 0:
formatted_interfaces.index(Config().get("link_uri"))
selected_interface = Config().get("link_uri")
except KeyError:
# The configuration for link_uri was not found
pass
except ValueError:
# The saved URI was not found while scanning
pass
if len(interfaces) == 1 and selected_interface is None:
selected_interface = interfaces[0][0]
newIndex = 0
if selected_interface is not None:
try:
newIndex = formatted_interfaces.index(selected_interface) + 1
except ValueError:
pass
self.interfaceCombo.setCurrentIndex(newIndex)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _update_ui_state(self):
if self.uiState == UIState.DISCONNECTED:
self.setWindowTitle("Not connected")
canConnect = self._selected_interface is not None
self.menuItemConnect.setText("Connect to Crazyflie")
self.menuItemConnect.setEnabled(canConnect)
self.connectButton.setText("Connect")
self.connectButton.setToolTip(
"Connect to the Crazyflie on the selected interface")
self.connectButton.setEnabled(canConnect)
self.scanButton.setText("Scan")
self.scanButton.setEnabled(True)
self.address.setEnabled(True)
self.batteryBar.setValue(3000)
self._menu_cf2_config.setEnabled(False)
self._menu_cf1_config.setEnabled(True)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
self.interfaceCombo.setEnabled(True)
elif self.uiState == UIState.CONNECTED:
s = "Connected on %s" % self._selected_interface
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Disconnect")
self.connectButton.setToolTip("Disconnect from the Crazyflie")
self.scanButton.setEnabled(False)
self.logConfigAction.setEnabled(True)
# Find out if there's an I2C EEPROM, otherwise don't show the
# dialog.
if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
self._menu_cf2_config.setEnabled(True)
self._menu_cf1_config.setEnabled(False)
elif self.uiState == UIState.CONNECTING:
s = "Connecting to {} ...".format(self._selected_interface)
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Cancel")
self.connectButton.setToolTip("Cancel connecting to the Crazyflie")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
elif self.uiState == UIState.SCANNING:
self.setWindowTitle("Scanning ...")
self.connectButton.setText("Connect")
self.menuItemConnect.setEnabled(False)
self.connectButton.setText("Connect")
self.connectButton.setEnabled(False)
self.scanButton.setText("Scanning...")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.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.available_devices()
# if (len(devs) > 0):
# self.device_discovery(devs)
def _show_input_device_config_dialog(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
Config().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: {}".format(checked))
def _show_connect_dialog(self):
self.logConfigDialogue.show()
def _update_battery(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
color = COLOR_BLUE
# TODO firmware reports fully-charged state as 'Battery',
# rather than 'Charged'
if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
color = COLOR_GREEN
elif data["pm.state"] == BatteryStates.LOW_POWER:
color = COLOR_RED
self.batteryBar.setStyleSheet(progressbar_stylesheet(color))
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
mems = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)
if len(mems) > 0:
mems[0].write_data(self._led_write_done)
def _disconnected(self):
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _connection_initiated(self):
self.uiState = UIState.CONNECTING
self._update_ui_state()
def _led_write_done(self, mem, addr):
logger.info("LED write done callback")
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error", "Error when starting log config"
" [{}]: {}".format(log_conf.name,
msg))
def _connection_lost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if self.isActiveWindow():
warningCaption = "Communication failure"
error = "Connection lost to {}: {}".format(linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def _connection_failed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on {}: {}".format(linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
Config().save_file()
def resizeEvent(self, event):
Config().set("window_size", [event.size().width(),
event.size().height()])
def _connect(self):
if self.uiState == UIState.CONNECTED:
self.cf.close_link()
elif self.uiState == UIState.CONNECTING:
self.cf.close_link()
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self.cf.open_link(self._selected_interface)
def _scan(self):
self.uiState = UIState.SCANNING
self._update_ui_state()
self.scanner.scanSignal.emit(self.address.value())
def _display_input_device_error(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _mux_selected(self, checked):
"""Called when a new mux is selected. The menu item contains a
reference to the raw mux object as well as to the associated device
sub-nodes"""
if not checked:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(False)
else:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(True)
self.joystickReader.set_mux(mux=mux)
# Go though the tree and select devices/mapping that was
# selected before it was disabled.
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
dev_node.toggled.emit(True)
self._update_input_device_footer()
def _get_dev_status(self, device):
msg = "{}".format(device.name)
if device.supports_mapping:
map_name = "No input mapping"
if device.input_map:
map_name = device.input_map_name
msg += " ({})".format(map_name)
return msg
def _update_input_device_footer(self):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
msg = ""
if len(self.joystickReader.available_devices()) > 0:
mux = self.joystickReader._selected_mux
msg = "Using {} mux with ".format(mux.name)
for key in list(mux._devs.keys())[:-1]:
if mux._devs[key]:
msg += "{}, ".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A, "
# Last item
key = list(mux._devs.keys())[-1]
if mux._devs[key]:
msg += "{}".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A"
else:
msg = "No input device found"
self._statusbar_label.setText(msg)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu. The
data in the menu object is the associated map menu (directly under the
item in the menu) and the raw device"""
(map_menu, device, mux_menu) = self.sender().data()
if not checked:
if map_menu:
map_menu.setEnabled(False)
# Do not close the device, since we don't know exactly
# how many devices the mux can have open. When selecting a
# new mux the old one will take care of this.
else:
if map_menu:
map_menu.setEnabled(True)
(mux, sub_nodes) = mux_menu.data()
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
if device.id == dev_node.data()[1].id \
and dev_node is not self.sender():
dev_node.setChecked(False)
role_in_mux = str(self.sender().parent().title()).strip()
logger.info("Role of {} is {}".format(device.name,
role_in_mux))
Config().set("input_device", str(device.name))
self._mapping_support = self.joystickReader.start_input(
device.name,
role_in_mux)
self._update_input_device_footer()
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu. The
data in the menu object is a referance to the device QAction in parent
menu. This contains a referance to the raw device."""
if not checked:
return
selected_mapping = str(self.sender().text())
device = self.sender().data().data()[1]
self.joystickReader.set_input_map(device.name, selected_mapping)
self._update_input_device_footer()
def device_discovery(self, devs):
"""Called when new devices have been added"""
for menu in self._all_role_menus:
role_menu = menu["rolemenu"]
mux_menu = menu["muxmenu"]
dev_group = QActionGroup(role_menu, exclusive=True)
for d in devs:
dev_node = QAction(d.name, role_menu, checkable=True,
enabled=True)
role_menu.addAction(dev_node)
dev_group.addAction(dev_node)
dev_node.toggled.connect(self._inputdevice_selected)
map_node = None
if d.supports_mapping:
map_node = QMenu(" Input map", role_menu, enabled=False)
map_group = QActionGroup(role_menu, exclusive=True)
# Connect device node to map node for easy
# enabling/disabling when selection changes and device
# to easily enable it
dev_node.setData((map_node, d))
for c in ConfigManager().get_list_of_configs():
node = QAction(c, map_node, checkable=True,
enabled=True)
node.toggled.connect(self._inputconfig_selected)
map_node.addAction(node)
# Connect all the map nodes back to the device
# action node where we can access the raw device
node.setData(dev_node)
map_group.addAction(node)
# If this device hasn't been found before, then
# select the default mapping for it.
if d not in self._available_devices:
last_map = Config().get("device_config_mapping")
if d.name in last_map and last_map[d.name] == c:
node.setChecked(True)
role_menu.addMenu(map_node)
dev_node.setData((map_node, d, mux_menu))
# Update the list of what devices we found
# to avoid selecting default mapping for all devices when
# a new one is inserted
self._available_devices = ()
for d in devs:
self._available_devices += (d,)
# Only enable MUX nodes if we have enough devies to cover
# the roles
for mux_node in self._all_mux_nodes:
(mux, sub_nodes) = mux_node.data()
if len(mux.supported_roles()) <= len(self._available_devices):
mux_node.setEnabled(True)
# TODO: Currently only supports selecting default mux
if self._all_mux_nodes[0].isEnabled():
self._all_mux_nodes[0].setChecked(True)
# If the previous length of the available devies was 0, then select
# the default on. If that's not available then select the first
# on in the list.
# TODO: This will only work for the "Normal" mux so this will be
# selected by default
if Config().get("input_device") in [d.name for d in devs]:
for dev_menu in self._all_role_menus[0]["rolemenu"].actions():
if dev_menu.text() == Config().get("input_device"):
dev_menu.setChecked(True)
else:
# Select the first device in the first mux (will always be "Normal"
# mux)
self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True)
logger.info("Select first device")
self._update_input_device_footer()
def _open_config_folder(self):
QDesktopServices.openUrl(
QUrl("file:///" +
QDir.toNativeSeparators(cfclient.config_path)))
def closeAppRequest(self):
self.close()
sys.exit(0)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(
enable_debug_driver=Config().get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice)
self._mux_group.setExclusive(True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.connect_input = QShortcut("Ctrl+I", self.connectButton,
self._connect)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
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.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or self.cf.commander.
send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or self.cf.commander.
send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or self.cf.commander.
send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.linkQualityBar.setTextVisible(False)
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# 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
cfclient.ui.pluginhelper.pose_logger = PoseLogger(self.cf)
cfclient.ui.pluginhelper.mainUI = self
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(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)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
# Set reference for plot-tab.
if isinstance(tab, cfclient.ui.tabs.PlotTab):
cfclient.ui.pluginhelper.plotTab = tab
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.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 Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not 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 [{}]".format(e))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes",
self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
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 = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice)
self._mux_group.setExclusive(True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node, )
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node, )
self._all_role_menus += ({
"muxmenu": node,
"rolemenu": sub_node
}, )
node.setData((m, mux_subnodes))
self._mapping_support = True
# Add checkbuttons for theme-selection.
self._theme_group = QActionGroup(self.menuThemes)
self._theme_group.setExclusive(True)
self._theme_checkboxes = []
for theme in UiUtils.THEMES:
node = QAction(theme, self.menuThemes, checkable=True)
node.setObjectName(theme)
node.toggled.connect(self._theme_selected)
self._theme_checkboxes.append(node)
self._theme_group.addAction(node)
self.menuThemes.addAction(node)
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)
self._devs = []
for d in self._jr.available_devices():
self._devs.append(d.name)
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.available_devices()
print("Will use [%s] for input" % self._devs[input_device])
self._jr.start_input(self._devs[input_device])
self._jr.set_input_map(self._devs[input_device], input_config)
def controller_connected(self):
""" Return True if a controller is connected"""
return True if (len(self._jr.available_devices()) > 0) else False
def list_controllers(self):
"""List the available controllers and input mapping"""
print("\nAvailable controllers:")
for i, dev in enumerate(self._devs):
print(" - Controller #{}: {}".format(i, dev))
print("\nAvailable input mapping:")
for map in os.listdir(sys.path[1] + '/input'):
print(" - " + map.split(".json")[0])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connection_failed.add_callback(self._connection_failed)
# 2014-11-25 chad: Add a callback for when we have a good connection.
self._cf.connected.add_callback(self._connected)
self._cf.param.add_update_callback(
group="imu_sensors", name="HMC5883L", cb=(
lambda name, found: self._jr.set_alt_hold_available(
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 _connected(self, link):
"""Callback for a successful Crazyflie connection."""
print("Connected to {}".format(link))
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print("Connection failed on {}: {}".format(link, message))
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 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 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):
"""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 LogFlight():
def __init__(self, args):
self.args = args
self.optitrack_enabled = False
self.console_dump_enabled = False
cflib.crtp.init_drivers(enable_debug_driver=False)
self._cf = Crazyflie(rw_cache="./cache")
self._jr = JoystickReader(do_device_discovery=False)
# Set flight mode
if self.args["trajectory"] is None or \
self.args["trajectory"][0] == "none":
self.mode = Mode.DONT_FLY
print("Mode set to [DONT_FLY]")
elif self.args["trajectory"][0] == "manual":
self.mode = Mode.MANUAL
print("Mode set to [MANUAL]")
elif self.args["safetypilot"]:
self.mode = Mode.MODE_SWITCH
print("Mode set to [MODE_SWITCH]")
else:
self.mode = Mode.AUTO
print("Mode set to [AUTO]")
# Setup for specified mode
if self.mode == Mode.AUTO:
self.is_in_manual_control = False
# Make sure drone is setup to perform autonomous flight
if args["uwb"] == "none":
assert args["optitrack"] == "state", "OptiTrack state needed in absence of UWB"
assert args["estimator"] == "kalman", "OptiTrack state needs Kalman estimator"
elif self.mode == Mode.DONT_FLY:
self.is_in_manual_control = False
else:
# Check if controller is connected
assert self.controller_connected(), "No controller detected."
self.setup_controller(map="flappy")
self.is_in_manual_control = True
# Setup the logging framework
self.setup_logger()
# Setup optitrack if required
if not args["optitrack"] == "none":
self.setup_optitrack()
def get_filename(self):
# create default fileroot if not provided
if self.args["fileroot"] is None:
date = datetime.today().strftime(r"%Y_%m_%d")
self.args["fileroot"] = "data/" + date
fileroot = self.args["fileroot"]
# create default filename if not provided
if self.args["filename"] is None:
estimator = self.args["estimator"]
uwb = self.args["uwb"]
optitrack = self.args["optitrack"]
trajectory = self.args["trajectory"]
date = datetime.today().strftime(r"%Y-%m-%d+%H:%M:%S")
traj = '_'.join(trajectory)
if optitrack == "logging":
options = "{}+{}+optitracklog+{}".format(estimator, uwb, traj)
elif optitrack == "state":
options = "{}+{}+optitrackstate+{}".format(estimator, uwb, traj)
else:
options = "{}+{}+{}".format(estimator, uwb, traj)
name = "{}+{}.csv".format(date, options)
fname = os.path.normpath(os.path.join(os.getcwd(), fileroot, name))
else:
# make sure provided filename is unique
if self.args["filename"].endswith(".csv"):
name = self.args["filename"][:-4]
else:
name = self.args["filename"]
new_name = name + ".csv"
fname = os.path.normpath(os.path.join(
os.getcwd(), fileroot, new_name))
i = 0
while os.path.isfile(fname):
i = i + 1
new_name = name + "_" + str(i) + ".csv"
fname = os.path.normpath(os.path.join(
os.getcwd(), fileroot, new_name))
return fname
def setup_logger(self):
# Create filename from options and date
self.log_file = self.get_filename()
# Create directory if not there
Path(self.args["fileroot"]).mkdir(exist_ok=True)
print("Log location: {}".format(self.log_file))
# Logger setup
logconfig = self.args["logconfig"]
self.flogger = FileLogger(self._cf, logconfig, self.log_file)
self.flogger.enableAllConfigs()
def setup_optitrack(self):
self.ot_id = self.args["optitrack_id"]
self.ot_position = np.zeros(3)
self.ot_attitude = np.zeros(3)
self.ot_quaternion = np.zeros(4)
self.filtered_pos = np.zeros(3)
self.ot_filter_sos = scipy.signal.butter(N=4, Wn=0.1, btype='low',
analog=False, output='sos')
self.pos_filter_zi = [scipy.signal.sosfilt_zi(self.ot_filter_sos),
scipy.signal.sosfilt_zi(self.ot_filter_sos),
scipy.signal.sosfilt_zi(self.ot_filter_sos)]
# Streaming client in separate thread
streaming_client = NatNetClient()
streaming_client.newFrameListener = self.ot_receive_new_frame
streaming_client.rigidBodyListener = self.ot_receive_rigidbody_frame
streaming_client.run()
self.optitrack_enabled = True
print("OptiTrack streaming client started")
# TODO: do we need to return StreamingClient?
def reset_estimator(self):
# Kalman
if self.args["estimator"] == "kalman":
self._cf.param.set_value("kalman.resetEstimation", "1")
time.sleep(1)
self._cf.param.set_value("kalman.resetEstimation", "0")
# Complementary (needs changes to firmware)
if self.args["estimator"] == "complementary":
try:
self._cf.param.set_value("complementaryFilter.reset", "1")
time.sleep(1)
self._cf.param.set_value("complementaryFilter.reset", "0")
except:
pass
def ot_receive_new_frame(self, *args, **kwargs):
pass
def ot_receive_rigidbody_frame(self, id, position, rotation):
# Check ID
if id in self.ot_id:
# get optitrack data in crazyflie global frame
pos_in_cf_frame = util.ot2control(position)
att_in_cf_frame = util.quat2euler(rotation)
quat_in_cf_frame = util.ot2control_quat(rotation)
idx = self.ot_id.index(id)
if idx==0:
# main drone
ot_dict = {
"otX0": pos_in_cf_frame[0],
"otY0": pos_in_cf_frame[1],
"otZ0": pos_in_cf_frame[2],
"otRoll0": att_in_cf_frame[0],
"otPitch0": att_in_cf_frame[1],
"otYaw0": att_in_cf_frame[2]
}
self.ot_position = pos_in_cf_frame
self.ot_attitude = att_in_cf_frame
self.ot_quaternion = quat_in_cf_frame
self.flogger.registerData("ot0", ot_dict)
(self.filtered_pos[0], self.pos_filter_zi[0]) = scipy.signal.sosfilt(
self.ot_filter_sos, [self.ot_position[0]], zi=self.pos_filter_zi[0]
)
(self.filtered_pos[1], self.pos_filter_zi[1]) = scipy.signal.sosfilt(
self.ot_filter_sos, [self.ot_position[1]], zi=self.pos_filter_zi[1]
)
(self.filtered_pos[2], self.pos_filter_zi[2]) = scipy.signal.sosfilt(
self.ot_filter_sos, [self.ot_position[2]], zi=self.pos_filter_zi[2]
)
elif idx==1:
ot_dict = {
"otX1": pos_in_cf_frame[0],
"otY1": pos_in_cf_frame[1],
"otZ1": pos_in_cf_frame[2],
"otRoll1": att_in_cf_frame[0],
"otPitch1": att_in_cf_frame[1],
"otYaw1": att_in_cf_frame[2]
}
self.flogger.registerData("ot1", ot_dict)
def do_taskdump(self):
self._cf.param.set_value("system.taskDump", "1")
def process_taskdump(self, console_log):
file = self.get_filename()
# Dataframe placeholders
label_data, load_data, stack_data = [], [], []
# Get headers
headers = []
for i, line in enumerate(console_log):
if "Task dump" in line:
headers.append(i)
# None indicates the end of the list
headers.append(None)
# Get one task dump
for i in range(len(headers) - 1):
dump = console_log[headers[i] + 2 : headers[i + 1]]
# Process strings: strip \n, \t, spaces, SYSLOAD:
loads, stacks, labels = [], [], []
for line in dump:
entries = line.strip("SYSLOAD: ").split("\t")
loads.append(entries[0].strip()) # no sep means strip all space, \n, \t
stacks.append(entries[1].strip())
labels.append(entries[2].strip())
# Store labels
if not label_data:
label_data = labels
# Append to placeholders
load_data.append(loads)
stack_data.append(stacks)
# Check if we have data at all
if headers[0] is not None and label_data:
# Put in dataframe
load_data = pd.DataFrame(load_data, columns=label_data)
stack_data = pd.DataFrame(stack_data, columns=label_data)
# Save dataframes
load_data.to_csv(file.strip(".csv") + "+load.csv", sep=",", index=False)
stack_data.to_csv(file.strip(".csv") + "+stackleft.csv", sep=",", index=False)
else:
print("No task dump data found")
def controller_connected(self):
""" Return True if a controller is connected """
return len(self._jr.available_devices()) > 0
def setup_controller(self, map="PS3_Mode_1"):
devs = []
for d in self._jr.available_devices():
devs.append(d.name)
if len(devs)==1:
input_device = 0
else:
print("Multiple controllers detected:")
for i, dev in enumerate(devs):
print(" - Controller #{}: {}".format(i, dev))
input_device = int(input("Select controller: "))
if not input_device in range(len(devs)):
raise ValueError
self._jr.start_input(devs[input_device])
self._jr.set_input_map(devs[input_device], map)
def connect_crazyflie(self, uri):
"""Connect to a Crazyflie on the given link uri"""
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
if self.mode == Mode.AUTO or self.mode == Mode.DONT_FLY:
self._cf.open_link(uri)
else:
# Add callbacks for manual control
self._cf.param.add_update_callback(
group="imu_sensors", name="AK8963", cb=(
lambda name, found: self._jr.set_alt_hold_available(
eval(found))))
# self._jr.assisted_control_updated.add_callback(
# lambda enabled: self._cf.param.set_value("flightmode.althold",
# enabled))
self._cf.open_link(uri)
self._jr.input_updated.add_callback(self.controller_input_cb)
if self.mode == Mode.MODE_SWITCH:
self._jr.alt1_updated.add_callback(self.mode_switch_cb)
def _connected(self, link):
"""This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print("Connected to %s" % link)
# set estimator
if args["estimator"]=="kalman":
self._cf.param.set_value("stabilizer.estimator", "2")
self.flogger.start()
print("logging started")
def _connection_failed(self, link_uri, msg):
print("Connection to %s failed: %s" % (link_uri, msg))
self.flogger.is_connected = False
def _connection_lost(self, link_uri, msg):
print("Connection to %s lost: %s" % (link_uri, msg))
self.flogger.is_connected = False
def _disconnected(self, link_uri):
print("Disconnected from %s" % link_uri)
self.flogger.is_connected = False
def ready_to_fly(self):
# Wait for connection
timeout = 10
while not self._cf.is_connected():
print("Waiting for Crazyflie connection...")
time.sleep(2)
timeout -= 1
if timeout<=0:
return False
# Wait for optitrack
if self.optitrack_enabled:
while (self.ot_position == 0).any():
print("Waiting for OptiTrack fix...")
time.sleep(2)
timeout -= 1
if timeout <= 0:
return False
print("OptiTrack fix acquired")
print("Reset Estimator...")
self.reset_estimator()
time.sleep(2) # wait for kalman to stabilize
return True
def start_flight(self):
if self.ready_to_fly():
if self.mode == Mode.MANUAL:
print("Manual Flight - Ready to fly")
self.manual_flight()
elif self.mode == Mode.DONT_FLY:
print("Ready to not fly")
try:
while True:
pass
except KeyboardInterrupt:
print("Flight stopped")
else:
# Build trajectory
setpoints = self.build_trajectory(self.args["trajectory"], self.args["space"])
# Do flight
if self.mode == Mode.AUTO:
print("Autonomous Flight - Starting flight")
self.follow_setpoints(self._cf, setpoints, self.args["optitrack"])
print("Flight complete.")
else:
print("Ready to fly")
self.manual_flight()
print("Starting Trajectory")
self.follow_setpoints(self._cf, setpoints, self.args["optitrack"])
else:
print("Timeout while waiting for flight ready.")
def controller_input_cb(self, *data):
# only forward control in manual mode
if self.is_in_manual_control:
self._cf.commander.send_setpoint(*data)
def mode_switch_cb(self, auto_mode):
if auto_mode:
print("Switching autonomous flight")
self.is_in_manual_control = False
else:
print("Switching to manual control")
self.is_in_manual_control = True
def manual_flight(self):
self.is_in_manual_control = True
while(self.is_in_manual_control):
if self.args["optitrack"]=="state":
# self._cf.extpos.send_extpos(
# self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2]
# )
self._cf.extpos.send_extpos(
self.ot_position[0], self.ot_position[1], self.ot_position[2]
)
# self._cf.extpos.send_extpose(
# self.ot_position[0], self.ot_position[1], self.ot_position[2],
# self.ot_quaternion[0], self.ot_quaternion[1], self.ot_quaternion[2], self.ot_quaternion[3]
# )
time.sleep(0.01)
def build_trajectory(self, trajectories, space):
# Load yaml file with space specification
with open(space, "r") as f:
space = yaml.full_load(f)
home = space["home"]
ranges = space["range"]
# Account for height offset
altitude = home["z"] + ranges["z"]
side_length = min([ranges["x"], ranges["y"]]) * 2
radius = min([ranges["x"], ranges["y"]])
x_bound = [home["x"] - ranges["x"], home["x"] + ranges["x"]]
y_bound = [home["y"] - ranges["y"], home["y"] + ranges["y"]]
# Build trajectory
# Takeoff
setpoints = takeoff(home["x"], home["y"], altitude, 0.0)
for trajectory in trajectories:
# If nothing, only nothing
if trajectory == "nothing":
setpoints = None
return setpoints
elif trajectory == "hover":
setpoints += hover(home["x"], home["y"], altitude)
elif trajectory == "hover_fw":
setpoints += hover_fw(home["x"], home["y"], altitude)
elif trajectory == "square":
setpoints += square(home["x"], home["y"], side_length, altitude)
elif trajectory == "square_fw":
setpoints += square_fw(home["x"], home["y"], side_length, altitude)
elif trajectory == "octagon":
setpoints += octagon(home["x"], home["y"], radius, altitude)
elif trajectory == "triangle":
setpoints += triangle(home["x"], home["y"], radius, altitude)
elif trajectory == "hourglass":
setpoints += hourglass(home["x"], home["y"], side_length, altitude)
elif trajectory == "random":
setpoints += randoms(home["x"], home["y"], x_bound, y_bound, altitude)
elif trajectory == "scan":
setpoints += scan(home["x"], home["y"], x_bound, y_bound, altitude)
else:
raise ValueError("{} is an unknown trajectory".format(trajectory))
# Add landing
setpoints += landing(home["x"], home["y"], altitude, 0.0)
return setpoints
def follow_setpoints(self, cf, setpoints, optitrack):
# Counter for task dump logging
time_since_dump = 0.0
# Start
try:
print("Flight started")
# Do nothing, just sit on the ground
if setpoints is None:
while True:
time.sleep(0.05)
time_since_dump += 0.05
# Task dump
if time_since_dump > 2:
print("Do task dump")
self.do_taskdump()
time_since_dump = 0.0
# Do actual flight
else:
for i, point in enumerate(setpoints):
print("Next setpoint: {}".format(point))
# Compute time based on distance
# Take-off
if i == 0:
distance = point[2]
# No take-off
else:
distance = np.sqrt(
((np.array(point[:3]) - np.array(setpoints[i - 1][:3])) ** 2).sum()
)
# If zero distance, at least some wait time
if distance == 0.0:
wait = 5
else:
wait = distance * 2
# Send position and wait
time_passed = 0.0
while time_passed < wait:
if self.is_in_manual_control:
self.manual_flight()
# If we use OptiTrack for control, send position to Crazyflie
if optitrack == "state":
cf.extpos.send_extpos(
self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2]
)
cf.commander.send_position_setpoint(*point)
time.sleep(0.05)
time_passed += 0.05
time_since_dump += 0.05
# Task dump
# if time_since_dump > 2:
# print("Do task dump")
# self.do_taskdump()
# time_since_dump = 0.0
# Finished
cf.commander.send_stop_setpoint()
# Prematurely break off flight / quit doing nothing
except KeyboardInterrupt:
if setpoints is None:
print("Quit doing nothing!")
else:
print("Emergency landing!")
wait = setpoints[i][2] * 2
cf.commander.send_position_setpoint(setpoints[i][0], setpoints[i][1], 0.0, 0.0)
time.sleep(wait)
cf.commander.send_stop_setpoint()
def setup_console_dump(self):
# Console dump file
self.console_log = []
console = Console(self._cf)
console.receivedChar.add_callback(self._console_cb)
self.console_dump_enabled = True
def _console_cb(self, text):
# print(text)
self.console_log.append(text)
def end(self):
self._cf.close_link()
# Process task dumps
# TODO: add timestamps / ticks (like logging) to this
if self.console_dump_enabled:
self.process_taskdump(self.console_log)
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)
######################################################
### By lxrocks
### 'Skinny Progress Bar' tweak for Yosemite
### Tweak progress bar - artistic I am not - so pick your own colors !!!
### Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version,junk,machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10,10,0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info( "Found Yosemite:")
tcss = """
QProgressBar {
border: 2px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: #05B8CC;
}
"""
self.setStyleSheet(tcss)
else:
logger.info( "Pre-Yosemite")
######################################################
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.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()
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(self._display_input_device_error)
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._connect)
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.connectButton.clicked.connect(self._connect)
self.quickConnectButton.clicked.connect(self._quick_connect)
self.menuItemQuickConnect.triggered.connect(self._quick_connect)
self.menuItemConfInputDevice.triggered.connect(self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_vbatt)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
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._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self._update_ui_state(UIState.DISCONNECTED,
linkURI))
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self._update_ui_state(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._update_ui_state(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# 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._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(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)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_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 Config().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 [{}]".format(e))
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_mux, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m,
self._menu_mux,
checkable=True,
enabled=True)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_mux.addAction(node)
# TODO: Temporary
self._input_dev_stack = []
self._menu_mux.actions()[0].setChecked(True)
if Config().get("enable_input_muxing"):
self._menu_mux.setEnabled(True)
else:
logger.info("Input device muxing disabled in config")
self._mapping_support = True
def _update_ui_state(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._menu_cf2_config.setEnabled(False)
self._menu_cf1_config.setEnabled(True)
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)
# Find out if there's an I2C EEPROM, otherwise don't show the
# dialog.
if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
self._menu_cf2_config.setEnabled(True)
self._menu_cf1_config.setEnabled(False)
if newState == UIState.CONNECTING:
s = "Connecting to {} ...".format(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.available_devices()
if (len(devs) > 0):
self.device_discovery(devs)
def _show_input_device_config_dialog(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
Config().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: {}".format(checked))
def _show_connect_dialog(self):
self.logConfigDialogue.show()
def _update_vbatt(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
def _connected(self, linkURI):
self._update_ui_state(UIState.CONNECTED, linkURI)
Config().set("link_uri", str(linkURI))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error", "Error when starting log config"
" [{}]: {}".format(log_conf.name, msg))
def _connection_lost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if self.isActiveWindow():
warningCaption = "Communication failure"
error = "Connection lost to {}: {}".format(linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self._update_ui_state(UIState.DISCONNECTED, linkURI)
else:
self._quick_connect()
def _connection_failed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on {}: {}".format(linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self._update_ui_state(UIState.DISCONNECTED, linkURI)
else:
self._quick_connect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
Config().save_file()
def _connect(self):
if self.uiState == UIState.CONNECTED:
self.cf.close_link()
elif self.uiState == UIState.CONNECTING:
self.cf.close_link()
self._update_ui_state(UIState.DISCONNECTED)
else:
self.connectDialogue.show()
def _display_input_device_error(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._active_config = str(config)
self._active_device = str(device)
Config().set("input_device", str(self._active_device))
# 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 [{}] - "
"No input config selected".format
(self._active_device))
else:
self._statusbar_label.setText("Using [{}] with config [{}]".format
(self._active_device,
self._active_config))
def _mux_selected(self, checked):
if not checked:
return
selected_mux_name = str(self.sender().text())
self.joystickReader.set_mux(name=selected_mux_name)
logger.debug("Selected mux supports {} devices".format(self.joystickReader.get_mux_supported_dev_count()))
self._adjust_nbr_of_selected_devices()
def _get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in device_config_mapping.keys():
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def _update_input_device_footer(self, device_name=None, mapping_name=None):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
if not device_name and not mapping_name:
self._statusbar_label.setText("No input device selected")
elif self._mapping_support and not mapping_name:
self._statusbar_label.setText("Using [{}] - "
"No input config selected".format(
device_name))
elif not self._mapping_support:
self._statusbar_label.setText("Using [{}]".format(device_name))
else:
self._statusbar_label.setText("Using [{}] with config [{}]".format(
device_name, mapping_name))
def _adjust_nbr_of_selected_devices(self):
nbr_of_selected = len(self._input_dev_stack)
nbr_of_supported = self.joystickReader.get_mux_supported_dev_count()
while len(self._input_dev_stack) > nbr_of_supported:
to_close = self._input_dev_stack.pop(0)
# Close and de-select it in the UI
self.joystickReader.stop_input(to_close)
for c in self._menu_devices.actions():
if c.text() == to_close:
c.setChecked(False)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu"""
if not checked:
return
self._input_dev_stack.append(self.sender().text())
selected_device_name = str(self.sender().text())
self._active_device = selected_device_name
# Save the device as "last used device"
Config().set("input_device", str(selected_device_name))
# Read preferred config used for this controller from config,
# if found then select this config in the menu
self._mapping_support = self.joystickReader.start_input(selected_device_name)
self._adjust_nbr_of_selected_devices()
if self.joystickReader.get_mux_supported_dev_count() == 1:
preferred_config = self.joystickReader.get_saved_device_mapping(selected_device_name)
if preferred_config:
for c in self._menu_mappings.actions():
if c.text() == preferred_config:
c.setChecked(True)
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu"""
if not checked:
return
selected_mapping = str(self.sender().text())
self.joystickReader.set_input_map(self._active_device, selected_mapping)
self._update_input_device_footer(self._active_device, selected_mapping)
def device_discovery(self, devs):
group = QActionGroup(self._menu_devices, exclusive=False)
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._active_device = d.name
if len(self._active_device) == 0:
self._active_device = str(self._menu_devices.actions()[0].text())
device_config_mapping = Config().get("device_config_mapping")
if device_config_mapping:
if self._active_device in device_config_mapping.keys():
self._current_input_config = device_config_mapping[
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_devices.actions():
if c.text() == self._active_device:
c.setChecked(True)
for c in self._menu_mappings.actions():
c.setEnabled(True)
if c.text() == self._current_input_config:
c.setChecked(True)
def _quick_connect(self):
try:
self.cf.open_link(Config().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 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)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
######################################################
### By lxrocks
### 'Skinny Progress Bar' tweak for Yosemite
### Tweak progress bar - artistic I am not - so pick your own colors !!!
### Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version,junk,machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10,10,0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info( "Found Yosemite:")
tcss = """
QProgressBar {
border: 2px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: #05B8CC;
}
"""
self.setStyleSheet(tcss)
else:
logger.info( "Pre-Yosemite")
######################################################
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.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()
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(self._display_input_device_error)
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._connect)
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.connectButton.clicked.connect(self._connect)
self.quickConnectButton.clicked.connect(self._quick_connect)
self.menuItemQuickConnect.triggered.connect(self._quick_connect)
self.menuItemConfInputDevice.triggered.connect(self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_vbatt)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
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._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self._update_ui_state(UIState.DISCONNECTED,
linkURI))
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self._update_ui_state(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._update_ui_state(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# 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._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(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)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_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 Config().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 [{}]".format(e))
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_mux, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m,
self._menu_mux,
checkable=True,
enabled=True)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_mux.addAction(node)
# TODO: Temporary
self._input_dev_stack = []
self._menu_mux.actions()[0].setChecked(True)
if Config().get("enable_input_muxing"):
self._menu_mux.setEnabled(True)
else:
logger.info("Input device muxing disabled in config")
self._mapping_support = True
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 __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)
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)
self._devs = []
for d in self._jr.available_devices():
self._devs.append(d.name)
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.available_devices()
print("Will use [%s] for input" % self._devs[input_device])
self._jr.start_input(self._devs[input_device])
self._jr.set_input_map(self._devs[input_device], input_config)
def controller_connected(self):
""" Return True if a controller is connected"""
return True if (len(self._jr.available_devices()) > 0) else False
def list_controllers(self):
"""List the available controllers and input mapping"""
print("\nAvailable controllers:")
for i, dev in enumerate(self._devs):
print(" - Controller #{}: {}".format(i, dev))
print("\nAvailable input mapping:")
for map in os.listdir(sys.path[1] + '/input'):
print(" - " + map.split(".json")[0])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connection_failed.add_callback(self._connection_failed)
# 2014-11-25 chad: Add a callback for when we have a good connection.
self._cf.connected.add_callback(self._connected)
self._cf.param.add_update_callback(
group="imu_sensors",
name="HMC5883L",
cb=(lambda name, found: self._jr.set_alt_hold_available(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 _connected(self, link):
"""Callback for a successful Crazyflie connection."""
print("Connected to {}".format(link))
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print("Connection failed on {}: {}".format(link, message))
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 __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)
class CrazyflieController():
def __init__(self, link_uri, radio_port, serialport, xmode=True):
"""Initialize the headless client and libraries"""
from cfclient.utils.input import JoystickReader
self._cf = Crazyflie()
self._serial = serialport
self._cf.commander.set_client_xmode(xmode)
self._cf.open_link(link_uri)
self._port = radio_port
self.is_connected = True
self.running = True
self._console = ""
# Callbacks
self._cf.connected.add_callback(
lambda msg: print("Connected to " + msg))
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.console.receivedChar.add_callback(self._console_char)
self._cf.packet_received.add_callback(self._data_received)
# Search for controllers
self._jr = JoystickReader(do_device_discovery=False)
available_devices = self._jr.available_devices()
if len(available_devices):
used_device = available_devices[0]
print("Using input device: " + str(used_device))
self._jr.device_error.add_callback(
lambda msg: print("Input device error: " + str(msg)))
self._jr.start_input(used_device.name)
self._jr.set_input_map(used_device.name, "xbox360_mode1")
# Controller callback
self._jr.input_updated.add_callback(
self._cf.commander.send_setpoint)
else:
print("No input device detected")
self.serialthread = threading.Thread(target=self._serial_listener)
self.serialthread.start()
def stop(self):
self.running = False
self.serialthread.join()
self._cf.close_link()
def _console_char(self, pk):
self._console = self._console + str(pk)
if "\n" in self._console:
print("Console: " + str(self._console), end="")
self._console = ""
def _disconnected(self, link_uri):
print("Disconnected...")
if self.running:
self.stop()
def _connection_failed(self, link_uri, msg):
print("Conection failed...")
if self.running:
self.stop()
def _serial_listener(self):
while self.running and self.is_connected:
data = self._serial.read_until(size=30)
if (data):
pk = CRTPPacket()
pk.port = self._port
pk.data = data
pk.size = len(data)
self._cf.send_packet(pk)
#print("Got data from agent: len {}, data {}".format(pk.size, pk.data))
def _data_received(self, pk):
if pk and pk.port == self._port:
self._serial.write(pk.data)
self._serial.flush()
