joystick.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2014, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 import rospy
00031 
00032 from sensor_msgs.msg import Joy
00033 
00034 
00035 class ButtonTransition(object):
00036     """
00037     Monitor button transitions.
00038 
00039     The transition is measured when read.
00040     """
00041     def __init__(self, val_func, down_val=True, up_val=False):
00042         self._raw_value = val_func
00043         self._down_val = down_val
00044         self._up_val = up_val
00045         self._up_checked = True
00046         self._down_checked = False
00047 
00048     def down(self):
00049         val = self._raw_value()
00050         if val == self._down_val:
00051             if not self._down_checked:
00052                 self._down_checked = True
00053                 return True
00054         else:
00055             self._down_checked = False
00056         return False
00057 
00058     def up(self):
00059         val = self._raw_value()
00060         if val == self._up_val:
00061             if not self._up_checked:
00062                 self._up_checked = True
00063                 return True
00064         else:
00065             self._up_checked = False
00066         return False
00067 
00068 
00069 class StickTransition(object):
00070     """
00071     Monitor transitions in stick movement.
00072 
00073     The transition is measured when read.
00074     """
00075     def __init__(self, val_func, epsilon=0.05):
00076         self._raw_value = val_func
00077         self._epsilon = epsilon
00078         self._value = 0.0
00079 
00080     def value(self):
00081         self.changed()
00082         return self._value
00083 
00084     def changed(self):
00085         value = self._raw_value()
00086         if abs(value - self._value) > self._epsilon:
00087             self._value = value
00088             return True
00089         return False
00090 
00091     def increased(self):
00092         value = self._raw_value()
00093         if (value - self._value) > self._epsilon:
00094             self._value = value
00095             return True
00096         return False
00097 
00098     def decreased(self):
00099         value = self._raw_value()
00100         if (self._value - value) > self._epsilon:
00101             self._value = value
00102             return True
00103         return False
00104 
00105 
00106 class Joystick(object):
00107     """
00108     Abstract base class to handle joystick input.
00109     """
00110 
00111     def __init__(self, scale=1.0, offset=0.0, deadband=0.1):
00112         """
00113         Maps joystick input to robot control.
00114 
00115         @type scale: float
00116         @param scale: scaling applied to joystick values [1.0]
00117         @type offset: float
00118         @param offset: joystick offset values, post-scaling [0.0]
00119         @type deadband: float
00120         @param deadband: deadband post scaling and offset [0.1]
00121 
00122         Raw joystick valuess are in [1.0...-1.0].
00123         """
00124         sub = rospy.Subscriber("/joy", Joy, self._on_joy)
00125         self._scale = scale
00126         self._offset = offset
00127         self._deadband = deadband
00128         self._controls = {}
00129 
00130         self._buttons = {}
00131         self._sticks = {}
00132         button_names = (
00133             'btnLeft', 'btnUp', 'btnDown', 'btnRight',
00134             'dPadUp', 'dPadDown', 'dPadLeft', 'dPadRight',
00135             'leftBumper', 'rightBumper',
00136             'leftTrigger', 'rightTrigger',
00137             'function1', 'function2')
00138         stick_names = (
00139             'leftStickHorz', 'leftStickVert',
00140             'rightStickHorz', 'rightStickVert')
00141 
00142         #doing this with lambda won't work
00143         def gen_val_func(name, type_name):
00144             def val_func():
00145                 return type_name(
00146                     name in self._controls and self._controls[name])
00147             return val_func
00148 
00149         for name in button_names:
00150             self._buttons[name] = ButtonTransition(gen_val_func(name, bool))
00151         for name in stick_names:
00152             self._sticks[name] = StickTransition(gen_val_func(name, float))
00153 
00154     def _on_joy(self, msg):
00155         """ callback for messages from joystick input
00156         Args:
00157               msg(Joy): a joystick input message
00158         """
00159         raise NotImplementedError()
00160 
00161     def button_up(self, name):
00162         return self._buttons[name].up()
00163 
00164     def button_down(self, name):
00165         return self._buttons[name].down()
00166 
00167     def stick_changed(self, name):
00168         return self._sticks[name].changed()
00169 
00170     def stick_inc(self, name):
00171         return self._sticks[name].increased()
00172 
00173     def stick_dec(self, name):
00174         return self._sticks[name].decreased()
00175 
00176     def stick_value(self, name):
00177         """
00178         Returns:
00179             the deadbanded, scaled and offset value of the axis
00180         """
00181         value = self._sticks[name].value()
00182         if value > self._deadband or value < -self._deadband:
00183             return (value * self._scale) + self._offset
00184         return 0
00185 
00186 
00187 class XboxController(Joystick):
00188     """
00189     Xbox specialization of Joystick.
00190     """
00191     def __init__(self, scale=1.0, offset=0.0, deadband=0.5):
00192         super(XboxController, self).__init__(scale, offset, deadband)
00193 
00194     def _on_joy(self, msg):
00195         """ callback for messages from joystick input
00196         Args:
00197               msg(Joy): a joystick input message
00198         """
00199 
00200         self._controls['btnLeft'] = (msg.buttons[2] == 1)
00201         self._controls['btnUp'] = (msg.buttons[3] == 1)
00202         self._controls['btnDown'] = (msg.buttons[0] == 1)
00203         self._controls['btnRight'] = (msg.buttons[1] == 1)
00204 
00205         self._controls['dPadUp'] = (msg.axes[7] > 0.5)
00206         self._controls['dPadDown'] = (msg.axes[7] < -0.5)
00207         self._controls['dPadLeft'] = (msg.axes[6] > 0.5)
00208         self._controls['dPadRight'] = (msg.axes[6] < -0.5)
00209 
00210         self._controls['leftStickHorz'] = msg.axes[0]
00211         self._controls['leftStickVert'] = msg.axes[1]
00212         self._controls['rightStickHorz'] = msg.axes[3]
00213         self._controls['rightStickVert'] = msg.axes[4]
00214 
00215         self._controls['leftBumper'] = (msg.buttons[4] == 1)
00216         self._controls['rightBumper'] = (msg.buttons[5] == 1)
00217         self._controls['leftTrigger'] = (msg.axes[2] < 0.0)
00218         self._controls['rightTrigger'] = (msg.axes[5] < 0.0)
00219 
00220         self._controls['function1'] = (msg.buttons[6] == 1)
00221         self._controls['function2'] = (msg.buttons[10] == 1)
00222 
00223 
00224 class LogitechController(Joystick):
00225     """
00226     Logitech specialization of Joystick.
00227     """
00228     def __init__(self, scale=1.0, offset=0.0, deadband=0.1):
00229         super(LogitechController, self).__init__(scale, offset, deadband)
00230 
00231     def _on_joy(self, msg):
00232         """ callback for messages from joystick input
00233         Args:
00234               msg(Joy): a joystick input message
00235         """
00236 
00237         self._controls['btnLeft'] = (msg.buttons[0] == 1)
00238         self._controls['btnUp'] = (msg.buttons[3] == 1)
00239         self._controls['btnDown'] = (msg.buttons[1] == 1)
00240         self._controls['btnRight'] = (msg.buttons[2] == 1)
00241 
00242         self._controls['dPadUp'] = (msg.axes[5] > 0.5)
00243         self._controls['dPadDown'] = (msg.axes[5] < -0.5)
00244         self._controls['dPadLeft'] = (msg.axes[4] > 0.5)
00245         self._controls['dPadRight'] = (msg.axes[4] < -0.5)
00246 
00247         self._controls['leftStickHorz'] = msg.axes[0]
00248         self._controls['leftStickVert'] = msg.axes[1]
00249         self._controls['rightStickHorz'] = msg.axes[2]
00250         self._controls['rightStickVert'] = msg.axes[3]
00251 
00252         self._controls['leftBumper'] = (msg.buttons[4] == 1)
00253         self._controls['rightBumper'] = (msg.buttons[5] == 1)
00254         self._controls['leftTrigger'] = (msg.buttons[6] == 1)
00255         self._controls['rightTrigger'] = (msg.buttons[7] == 1)
00256 
00257         self._controls['function1'] = (msg.buttons[8] == 1)
00258         self._controls['function2'] = (msg.buttons[9] == 1)
00259 
00260 
00261 class PS3Controller(Joystick):
00262     """
00263     PS3 specialization of Joystick.
00264     """
00265     def __init__(self, scale=1.0, offset=0.0, deadband=0.1):
00266         super(PS3Controller, self).__init__(scale, offset, deadband)
00267 
00268     def _on_joy(self, msg):
00269         """ callback for messages from joystick input
00270         Args:
00271               msg(Joy): a joystick input message
00272         """
00273 
00274         self._controls['btnLeft'] = (msg.buttons[15] == 1)
00275         self._controls['btnUp'] = (msg.buttons[12] == 1)
00276         self._controls['btnDown'] = (msg.buttons[14] == 1)
00277         self._controls['btnRight'] = (msg.buttons[13] == 1)
00278 
00279         self._controls['dPadUp'] = (msg.buttons[4] == 1)
00280         self._controls['dPadDown'] = (msg.buttons[6] == 1)
00281         self._controls['dPadLeft'] = (msg.buttons[7] == 1)
00282         self._controls['dPadRight'] = (msg.buttons[5] == 1)
00283 
00284         self._controls['leftStickHorz'] = msg.axes[0]
00285         self._controls['leftStickVert'] = msg.axes[1]
00286         self._controls['rightStickHorz'] = msg.axes[2]
00287         self._controls['rightStickVert'] = msg.axes[3]
00288 
00289         self._controls['leftBumper'] = (msg.buttons[10] == 1)
00290         self._controls['rightBumper'] = (msg.buttons[11] == 1)
00291         self._controls['leftTrigger'] = (msg.buttons[8] == 1)
00292         self._controls['rightTrigger'] = (msg.buttons[9] == 1)
00293 
00294         self._controls['function1'] = (msg.buttons[0] == 1)
00295         self._controls['function2'] = (msg.buttons[3] == 1)


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Fri Oct 3 2014 16:37:39