Python JoystickReader.getAvailableDevices方法代码示例

# 需要导入模块: from cfclient.utils.input import JoystickReader [as 别名]
# 或者: from cfclient.utils.input.JoystickReader import getAvailableDevices [as 别名]
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))

# 需要导入模块: from cfclient.utils.input import JoystickReader [as 别名]
# 或者: from cfclient.utils.input.JoystickReader import getAvailableDevices [as 别名]
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)

# 需要导入模块: from cfclient.utils.input import JoystickReader [as 别名]
# 或者: from cfclient.utils.input.JoystickReader import getAvailableDevices [as 别名]

#.........这里部分代码省略.........
            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()

# 需要导入模块: from cfclient.utils.input import JoystickReader [as 别名]
# 或者: from cfclient.utils.input.JoystickReader import getAvailableDevices [as 别名]

#.........这里部分代码省略.........
            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):

# 需要导入模块: from cfclient.utils.input import JoystickReader [as 别名]
# 或者: from cfclient.utils.input.JoystickReader import getAvailableDevices [as 别名]
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)