Python JoystickReader.start_input方法代码示例

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

#.........这里部分代码省略.........

    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()):

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

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

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

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

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

        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)

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