Python JoystickReader.setAltHoldAvailable方法代码示例

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