wiimote_node.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 ################################################################################
00003 #
00004 # File:         wiimode_node.py
00005 # RCS:          $Header: $
00006 # Description:  Top level ROS node that publishes Wiimote data
00007 #               and allows Wiimote rumble/LED setting.
00008 # Author:       Andreas Paepcke
00009 # Created:      Thu Sep 10 10:31:44 2009
00010 # Modified:     Fri Jan 14 10:51:11 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com
00011 # Language:     Python
00012 # Package:      N/A
00013 # Status:       Experimental (Do Not Distribute)
00014 #
00015 # (c) Copyright 2009, Willow Garage, all rights reserved.
00016 #
00017 ################################################################################
00018 #
00019 # Revisions:
00020 #
00021 # Thu Mar 18 10:56:09 2010 (David Lu) davidlu@wustl.edu
00022 #  Enabled nunchuk message publishing
00023 # Fri Oct 29 08:58:21 2010 (Miguel Angel Julian Aguilar, QBO Project)
00024 #    miguel.angel@thecorpora.com
00025 #    Enabled classic controller message publishing
00026 # Mon Nov 08 11:46:58 2010 (David Lu) davidlu@wustl.edu
00027 #  Added calibrated nunchuk information, changed /joy to /wiijoy
00028 #  Only publish on /wiimote/nunchuk if attached
00029 # Tue Jul 05, 2011 (Chad Rockey) chadrockey@gmail.com
00030 #  Removed LED and Rumble Feedback
00031 #  Added support for sensor_msgs/JoyFeedbackArray
00032 ################################################################################
00033 #!/usr/bin/env python
00034 
00035 """The wiimote_node broadcasts four topics, and listens to messages that control
00036 the Wiimote stick's rumble (vibration) and LEDs. Transmitted topics (@100Hz):
00037 
00038    o wiijoy            Messages contain the accelerometer and gyro axis data, and all button states.
00039    o imu/data          Messages contain the accelerometer and gyro axis data, and covariance matrices
00040    o wiimote/state     the wiijoy and /imu messages, the button states, the LED states,
00041                        rumble (i.e. vibrator) state, IR light sensor readings, time since last zeroed, 
00042                        and battery state. See State.msg
00043    o imu/is_calibrated Latched message
00044    o nunchuk           Joy messages using the nunchuk as a joystick
00045    o classic           Joy messages using the nunchuck as a joystic
00046                  
00047 The node listens to the following messages:
00048 
00049    o joy/set_feedback
00050                  Topic that listens to sensor_msgs/JoyFeedbackArray messages.  This controls the LEDs and the Rumble.  There are 4 LEDs with ids of 0 through 3.  The "Player 1" LED is id 0; the "Player 4" LED is id3.  The Wiimote only has one rumble, so it is id 0.
00051    o imu/calibrate
00052                  Request to calibrate the device.
00053                  
00054 No command line parameters.
00055 """
00056 
00057 # Code structure: The main thread spawns one thread each for the 
00058 # four message senders, and one thread each for the message listeners.
00059 # The respective classes are IMUSender, JoySender, NunSender and WiiSender for
00060 # topic sending, and WiimoteListeners for the two message listeners.
00061 #
00062 # The Wiimote driver is encapsulated in class WIIMote (see WIIMote.py).
00063 # That class' singleton instance runs in its own thread, and uses 
00064 # the third-party cwiid access software.
00065 
00066 
00067 # TODO: Removal of gyro is noticed (covar[0,0]<--1). But software does not notice plugging in.
00068 # TODO: Command line option: --no-zeroing
00069 
00070 # -------- Python Standard Modules:
00071 import sys
00072 import threading
00073 import traceback
00074 import time
00075 
00076 # -------- ROS-Related Modules:
00077 #import roslib; roslib.load_manifest('wiimote') # old fuerte code
00078 import rospy
00079 from geometry_msgs.msg import Vector3
00080 from sensor_msgs.msg import Imu
00081 from std_srvs.srv import Empty
00082 from std_srvs.srv import EmptyResponse
00083 from std_msgs.msg import Bool
00084 from sensor_msgs.msg import Joy
00085 from sensor_msgs.msg import JoyFeedback
00086 from sensor_msgs.msg import JoyFeedbackArray
00087 from wiimote.msg import IrSourceInfo
00088 from wiimote.msg import State
00089 
00090 # -------- WIIMote Modules:
00091 from wiimote.wiimoteExceptions import *
00092 from wiimote.wiimoteConstants import *
00093 import wiimote.WIIMote
00094 import wiimote.wiiutils
00095 
00096 GATHER_CALIBRATION_STATS = True
00097 
00098 class WiimoteNode():
00099     
00100 
00101     def runWiimoteNode(self):
00102         """Initialize the wiimote_node, establishing its name for communication with the Master"""
00103     
00104         # All exceptions will end up in the __main__ section
00105         # and are handled there:
00106         
00107         rospy.init_node('wiimote', anonymous=True, log_level=rospy.ERROR) # log_level=rospy.DEBUG
00108         wiimoteDevice = wiimote.WIIMote.WIIMote()
00109         wiimoteDevice.zeroDevice()
00110         
00111         try:
00112             IMUSender(wiimoteDevice, freq=100).start()
00113             JoySender(wiimoteDevice, freq=100).start()
00114             WiiSender(wiimoteDevice, freq=100).start()
00115             NunSender(wiimoteDevice, freq=100).start()
00116             ClasSender(wiimoteDevice, freq=100).start()
00117             WiimoteListeners(wiimoteDevice).start()
00118             
00119             rospy.spin()
00120         
00121         except:    
00122             rospy.loginfo("Error in startup")
00123             rospy.loginfo(sys.exc_info()[0])
00124         finally:
00125             try:
00126                 wiimoteDevice.setRumble(False)
00127                 wiimoteDevice.setLEDs([False, False, False, False])
00128                 wiimoteDevice.shutdown()
00129             except:
00130                 pass
00131     
00132     def shutdown(self):
00133         try:
00134             IMUSender.stop
00135             JoySender.stop
00136             WiiSender.stop
00137             NunSender.stop
00138             WiimoteListener.stop
00139         except:
00140             pass
00141         
00142 class WiimoteDataSender(threading.Thread):
00143     
00144     def __init__(self, wiiMote, freq=100):
00145         
00146         threading.Thread.__init__(self)
00147         self.wiiMote = wiiMote
00148         self.freq = freq
00149         self.sleepDuration = 1.0 / freq
00150         
00151         varianceAccelerator = self.wiiMote.getVarianceAccelerator();
00152         self.linear_acceleration_covariance = [varianceAccelerator[X], 0., 0.,
00153                                                0., varianceAccelerator[Y], 0.,
00154                                                0., 0., varianceAccelerator[Z]]
00155 
00156         varianceGyro = self.wiiMote.getVarianceGyro();
00157         self.angular_velocity_covariance = [varianceGyro[X], 0., 0.,
00158                                             0., varianceGyro[Y], 0.,
00159                                             0., 0., varianceGyro[Z]]
00160         
00161         # If no gyro is attached to the Wiimote then we signal
00162         # the invalidity of angular rate w/ a covariance matrix
00163         # whose first element is -1:
00164         self.gyroAbsence_covariance = [-1., 0., 0.,
00165                                        0., 0., 0.,
00166                                        0., 0., 0.]
00167     
00168     def obtainWiimoteData(self):
00169         """Retrieve one set of Wiimote measurements from the Wiimote instance. Return scaled accelerator and gyro readings.
00170         
00171         We canonicalize both accelerator and gyro data through
00172         scaling them by constants that turn them into m/sec^2, and 
00173         radians/sec, respectively.
00174         
00175         Return: list of canonicalized accelerator and gyro readings. 
00176         """
00177         
00178         while not rospy.is_shutdown():
00179             self.wiistate = self.wiiMote.getWiimoteState()
00180             if self.wiistate is not None and self.wiistate.acc is not None:
00181                 break
00182             else:
00183                 rospy.sleep(0.1)
00184 
00185         return self.canonicalizeWiistate()
00186         
00187     def canonicalizeWiistate(self):
00188         """Scale accelerator, nunchuk accelerator, and gyro readings to be m/sec^2, m/sec^2 and radians/sec, respectively."""
00189         
00190         try: 
00191             # Convert acceleration, which is in g's into m/sec^2:
00192             canonicalAccel = self.wiistate.acc.scale(EARTH_GRAVITY)
00193 
00194             # If nunchuk is connected, then 
00195             # convert nunchuk acceleration into m/sec^2
00196             if self.wiistate.nunchukPresent:
00197                 canonicalNunchukAccel = self.wiistate.nunchukAcc.scale(EARTH_GRAVITY)
00198             else:
00199                 canonicalNunchukAccel = None
00200                 
00201             # If the gyro is connected, then 
00202             # Convert gyro reading to radians/sec (see wiimoteConstants.py
00203             # for origin of this scale factor):
00204             if self.wiistate.motionPlusPresent:
00205                 canonicalAngleRate = self.wiistate.angleRate.scale(GYRO_SCALE_FACTOR)
00206             else:
00207                  canonicalAngleRate = None
00208             
00209             return [canonicalAccel, canonicalNunchukAccel, canonicalAngleRate]
00210         except AttributeError:
00211             # An attribute error here occurs when user shuts
00212             # off the Wiimote before stopping the wiimote_node:
00213             rospy.loginfo(self.threadName + " shutting down.")
00214             exit(0)
00215 
00216             
00217 class IMUSender(WiimoteDataSender):
00218     """Broadcasting Wiimote accelerator and gyro readings as IMU messages to Topic sensor_data/Imu"""
00219     
00220     def __init__(self, wiiMote, freq=100):
00221         """Initializes the Wiimote IMU publisher.
00222     
00223         Parameters:
00224             wiiMote: a bluetooth-connected, calibrated WIIMote instance
00225             freq:    the message sending frequency in messages/sec. Max is 100, because
00226                      the Wiimote only samples the sensors at 100Hz.
00227         """
00228         
00229         WiimoteDataSender.__init__(self, wiiMote, freq)
00230         
00231         self.pub = rospy.Publisher('imu/data', Imu, queue_size=1)
00232         
00233     def run(self):
00234         """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps.
00235         
00236         The IMU message, if fully filled in, contains information on orientation,
00237         acceleration (in m/s^2), and angular rate (in radians/sec). For each of
00238         these quantities, the IMU message format also wants the corresponding
00239         covariance matrix.
00240         
00241         Wiimote only gives us acceleration and angular rate. So we ensure that the orientation
00242         data entry is marked invalid. We do this by setting the first
00243         entry of its associated covariance matrix to -1. The covariance
00244         matrices are the 3x3 matrix with the axes' variance in the 
00245         diagonal. We obtain the variance from the Wiimote instance.  
00246         """
00247         
00248         rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).")
00249         self.threadName = "IMU topic Publisher"
00250         try:
00251             while not rospy.is_shutdown():
00252                 (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
00253                 
00254                 msg = Imu(header=None,
00255                           orientation=None,                                         # will default to [0.,0.,0.,0],
00256                           orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.],     # -1 indicates that orientation is unknown
00257                           angular_velocity=None,
00258                           angular_velocity_covariance=self.angular_velocity_covariance,
00259                           linear_acceleration=None,
00260                           linear_acceleration_covariance=self.linear_acceleration_covariance)
00261                           
00262                     
00263                 # If a gyro is plugged into the Wiimote, then note the 
00264                 # angular velocity in the message, else indicate with
00265                 # the special gyroAbsence_covariance matrix that angular
00266                 # velocity is unavailable:      
00267                 if self.wiistate.motionPlusPresent:
00268                     msg.angular_velocity.x = canonicalAngleRate[PHI]
00269                     msg.angular_velocity.y = canonicalAngleRate[THETA]
00270                     msg.angular_velocity.z = canonicalAngleRate[PSI]
00271                 else:
00272                     msg.angular_velocity_covariance = self.gyroAbsence_covariance
00273                 
00274                 msg.linear_acceleration.x = canonicalAccel[X]
00275                 msg.linear_acceleration.y = canonicalAccel[Y]
00276                 msg.linear_acceleration.z = canonicalAccel[Z]
00277                 
00278                 measureTime = self.wiistate.time
00279                 timeSecs = int(measureTime)
00280                 timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
00281                 msg.header.stamp.secs = timeSecs
00282                 msg.header.stamp.nsecs = timeNSecs
00283                 
00284                 try:
00285                   self.pub.publish(msg)
00286                 except rospy.ROSException:
00287                   rospy.loginfo("Topic imu/data closed. Shutting down Imu sender.")
00288                   exit(0)
00289                 
00290                 #rospy.logdebug("IMU state:")
00291                 #rospy.logdebug("    IMU accel: " + str(canonicalAccel) + "\n    IMU angular rate: " + str(canonicalAngleRate))
00292                 rospy.sleep(self.sleepDuration)
00293         except rospy.ROSInterruptException:
00294             rospy.loginfo("Shutdown request. Shutting down Imu sender.")
00295             exit(0)
00296             
00297             
00298 class JoySender(WiimoteDataSender):
00299     
00300     """Broadcasting Wiimote accelerator and gyro readings as Joy(stick) messages to Topic joy"""
00301     
00302     def __init__(self, wiiMote, freq=100):
00303         """Initializes the Wiimote Joy(stick) publisher.
00304     
00305         Parameters:
00306             wiiMote: a bluetooth-connected, calibrated WIIMote instance
00307             freq:    the message sending frequency in messages/sec. Max is 100, because
00308                      the Wiimote only samples the sensors at 100Hz.
00309         """
00310         
00311         WiimoteDataSender.__init__(self, wiiMote, freq)
00312 
00313         
00314         self.pub = rospy.Publisher('joy', Joy, queue_size=1)
00315         
00316     def run(self):
00317         """Loop that obtains the latest wiimote state, publishes the joystick data, and sleeps.
00318         
00319         The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
00320         """
00321         
00322         rospy.loginfo("Wiimote joystick publisher starting (topic wiijoy).")
00323         self.threadName = "Joy topic Publisher"
00324         try:
00325             while not rospy.is_shutdown():
00326                 (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
00327                 
00328                 msg = Joy(header=None,
00329                           axes=[canonicalAccel[X], canonicalAccel[Y], canonicalAccel[Z]],
00330                           buttons=None)
00331                 
00332                 # If a gyro is attached to the Wiimote, we add the
00333                 # gyro information:
00334                 if self.wiistate.motionPlusPresent:
00335                     msg.axes.extend([canonicalAngleRate[PHI], canonicalAngleRate[THETA], canonicalAngleRate[PSI]])
00336                           
00337                 # Fill in the ROS message's buttons field (there *must* be
00338                 #     a better way in python to declare an array of 11 zeroes...]
00339 
00340                 theButtons = [False,False,False,False,False,False,False,False,False,False,False]
00341                 theButtons[State.MSG_BTN_1]     = self.wiistate.buttons[BTN_1]
00342                 theButtons[State.MSG_BTN_2]     = self.wiistate.buttons[BTN_2]
00343                 theButtons[State.MSG_BTN_A]     = self.wiistate.buttons[BTN_A]
00344                 theButtons[State.MSG_BTN_B]     = self.wiistate.buttons[BTN_B]
00345                 theButtons[State.MSG_BTN_PLUS]  = self.wiistate.buttons[BTN_PLUS]
00346                 theButtons[State.MSG_BTN_MINUS] = self.wiistate.buttons[BTN_MINUS]
00347                 theButtons[State.MSG_BTN_LEFT]  = self.wiistate.buttons[BTN_LEFT]
00348                 theButtons[State.MSG_BTN_RIGHT] = self.wiistate.buttons[BTN_RIGHT]
00349                 theButtons[State.MSG_BTN_UP]    = self.wiistate.buttons[BTN_UP]
00350                 theButtons[State.MSG_BTN_DOWN]  = self.wiistate.buttons[BTN_DOWN]
00351                 theButtons[State.MSG_BTN_HOME]  = self.wiistate.buttons[BTN_HOME]
00352 
00353                 msg.buttons = theButtons
00354                 
00355                 measureTime = self.wiistate.time
00356                 timeSecs = int(measureTime)
00357                 timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
00358                 # Add the timestamp
00359                 msg.header.stamp.secs = timeSecs
00360                 msg.header.stamp.nsecs = timeNSecs
00361                 
00362                 try:
00363                   self.pub.publish(msg)
00364                 except rospy.ROSException:
00365                   rospy.loginfo("Topic wiijoy closed. Shutting down Joy sender.")
00366                   exit(0)
00367 
00368                 #rospy.logdebug("Joystick state:")
00369                 #rospy.logdebug("    Joy buttons: " + str(theButtons) + "\n    Joy accel: " + str(canonicalAccel) + "\n    Joy angular rate: " + str(canonicalAngleRate))
00370                 rospy.sleep(self.sleepDuration)
00371         except rospy.ROSInterruptException:
00372             rospy.loginfo("Shutdown request. Shutting down Joy sender.")
00373             exit(0)
00374 
00375 class NunSender(WiimoteDataSender):
00376     
00377     """Broadcasting nunchuk accelerator and joystick readings as Joy(stick) messages to Topic joy"""
00378     
00379     def __init__(self, wiiMote, freq=100):
00380         """Initializes the nunchuk Joy(stick) publisher.
00381     
00382         Parameters:
00383             wiiMote: a bluetooth-connected, calibrated WIIMote instance
00384             freq:    the message sending frequency in messages/sec. Max is 100, because
00385                      the Wiimote only samples the sensors at 100Hz.
00386         """
00387         
00388         WiimoteDataSender.__init__(self, wiiMote, freq)
00389 
00390         
00391         
00392         # Set 'pub' to none here, and check for none-ness in the
00393         # loop below so as not to start this publisher unnecessarily.
00394         self.pub = None
00395         
00396     def run(self):
00397         """Loop that obtains the latest nunchuk state, publishes the joystick data, and sleeps.
00398         
00399         The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
00400         """
00401         
00402         self.threadName = "nunchuk Joy topic Publisher"
00403         try:
00404             while not rospy.is_shutdown():
00405                 rospy.sleep(self.sleepDuration)
00406                 (canonicalAccel, scaledAcc, canonicalAngleRate) = self.obtainWiimoteData()
00407                 if not self.wiistate.nunchukPresent:
00408                     continue
00409                 if self.pub is None:
00410                     self.pub = rospy.Publisher('/wiimote/nunchuk', Joy, queue_size=1)
00411                     rospy.loginfo("Wiimote Nunchuk joystick publisher starting (topic nunchuk).")
00412                 
00413                 (joyx, joyy) = self.wiistate.nunchukStick
00414                                 
00415                 msg = Joy(header=None,
00416                           axes=[joyx, joyy,
00417                                 scaledAcc[X], scaledAcc[Y], scaledAcc[Z]],
00418                           buttons=None)
00419 
00420                 theButtons = [False,False]
00421                 theButtons[State.MSG_BTN_Z]     = self.wiistate.nunchukButtons[BTN_Z]
00422                 theButtons[State.MSG_BTN_C]     = self.wiistate.nunchukButtons[BTN_C]
00423 
00424                 msg.buttons = theButtons
00425                 
00426                 measureTime = self.wiistate.time
00427                 timeSecs = int(measureTime)
00428                 timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
00429                 msg.header.stamp.secs = timeSecs
00430                 msg.header.stamp.nsecs = timeNSecs
00431                 
00432                 try:
00433                   self.pub.publish(msg)
00434                 except rospy.ROSException:
00435                   rospy.loginfo("Topic /wiimote/nunchuk closed. Shutting down Nun sender.")
00436                   exit(0)
00437                 
00438                 #rospy.logdebug("nunchuk state:")
00439                 #rospy.logdebug("    nunchuk buttons: " + str(theButtons) + "\n    Nuchuck axes: " + str(msg.axes) + "\n")
00440 
00441         except rospy.ROSInterruptException:
00442             rospy.loginfo("Shutdown request. Shutting down Nun sender.")
00443             exit(0)
00444 
00445 class ClasSender(WiimoteDataSender):
00446     
00447     """Broadcasting Classic Controller joystick readings as Joy(stick) messages to Topic joy"""
00448     
00449     def __init__(self, wiiMote, freq=100):
00450         """Initializes the Classic Controller Joy(stick) publisher.
00451     
00452         Parameters:
00453             wiiMote: a bluetooth-connected, calibrated WIIMote instance
00454             freq:    the message sending frequency in messages/sec. Max is 100, because
00455                      the Wiimote only samples the sensors at 100Hz.
00456         """
00457         
00458         WiimoteDataSender.__init__(self, wiiMote, freq)
00459 
00460         # Set 'pub' to none here, and check for none-ness in the
00461         # loop below so as not to start this publisher unnecessarily.
00462         self.pub = None
00463         
00464     def run(self):
00465         """Loop that obtains the latest classic controller state, publishes the joystick data, and sleeps.
00466         
00467         The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
00468         """
00469 
00470         self.threadName = "Classic Controller Joy topic Publisher"
00471         try:
00472             while not rospy.is_shutdown():
00473                 rospy.sleep(self.sleepDuration)
00474                 self.obtainWiimoteData()
00475                 
00476                 if not self.wiistate.classicPresent:
00477                     continue
00478                 if self.pub is None:
00479                     self.pub = rospy.Publisher('/wiimote/classic', Joy)
00480                     rospy.loginfo("Wiimote Classic Controller joystick publisher starting (topic /wiimote/classic).")
00481           
00482                 (l_joyx, l_joyy) = self.wiistate.classicStickLeft
00483                 (r_joyx, r_joyy) = self.wiistate.classicStickRight
00484                 # scale the joystick to [-1, 1]
00485                 l_joyx = -(l_joyx-33)/27.
00486                 l_joyy = (l_joyy-33)/27.
00487                 r_joyx = -(r_joyx-15)/13.
00488                 r_joyy = (r_joyy-15)/13.
00489                 # create a deadzone in the middle
00490                 if abs(l_joyx) < .05:
00491                     l_joyx = 0
00492                 if abs(l_joyy) < .05:
00493                     l_joyy = 0
00494                 if abs(r_joyx) < .05:
00495                     r_joyx = 0
00496                 if abs(r_joyy) < .05:
00497                     r_joyy = 0
00498                 
00499                 msg = Joy(header=None,
00500                           axes=[l_joyx, l_joyy,r_joyx, r_joyy],
00501                           buttons=None)
00502 
00503                 theButtons = [False,False,False,False,False,False,False,False,False,False,False,False,False,False,False]
00504                 theButtons[State.MSG_CLASSIC_BTN_X]     = self.wiistate.classicButtons[CLASSIC_BTN_X]
00505                 theButtons[State.MSG_CLASSIC_BTN_Y]     = self.wiistate.classicButtons[CLASSIC_BTN_Y]
00506                 theButtons[State.MSG_CLASSIC_BTN_A]     = self.wiistate.classicButtons[CLASSIC_BTN_A]
00507                 theButtons[State.MSG_CLASSIC_BTN_B]     = self.wiistate.classicButtons[CLASSIC_BTN_B]
00508                 theButtons[State.MSG_CLASSIC_BTN_PLUS]     = self.wiistate.classicButtons[CLASSIC_BTN_PLUS]
00509                 theButtons[State.MSG_CLASSIC_BTN_MINUS]     = self.wiistate.classicButtons[CLASSIC_BTN_MINUS]
00510                 theButtons[State.MSG_CLASSIC_BTN_LEFT]     = self.wiistate.classicButtons[CLASSIC_BTN_LEFT]
00511                 theButtons[State.MSG_CLASSIC_BTN_RIGHT]     = self.wiistate.classicButtons[CLASSIC_BTN_RIGHT]
00512                 theButtons[State.MSG_CLASSIC_BTN_UP]     = self.wiistate.classicButtons[CLASSIC_BTN_UP]
00513                 theButtons[State.MSG_CLASSIC_BTN_DOWN]     = self.wiistate.classicButtons[CLASSIC_BTN_DOWN]
00514                 theButtons[State.MSG_CLASSIC_BTN_HOME]     = self.wiistate.classicButtons[CLASSIC_BTN_HOME]
00515                 theButtons[State.MSG_CLASSIC_BTN_L]     = self.wiistate.classicButtons[CLASSIC_BTN_L]
00516                 theButtons[State.MSG_CLASSIC_BTN_R]     = self.wiistate.classicButtons[CLASSIC_BTN_R]
00517                 theButtons[State.MSG_CLASSIC_BTN_ZL]     = self.wiistate.classicButtons[CLASSIC_BTN_ZL]
00518                 theButtons[State.MSG_CLASSIC_BTN_ZR]     = self.wiistate.classicButtons[CLASSIC_BTN_ZR]
00519 
00520                 msg.buttons = theButtons
00521                 
00522                 measureTime = self.wiistate.time
00523                 timeSecs = int(measureTime)
00524                 timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
00525                 msg.header.stamp.secs = timeSecs
00526                 msg.header.stamp.nsecs = timeNSecs
00527                 
00528                 try:
00529                   self.pub.publish(msg)
00530                 except rospy.ROSException:
00531                   rospy.loginfo("Topic /wiimote/classic closed. Shutting down Clas sender.")
00532                   exit(0)
00533 
00534                 #rospy.logdebug("Classic Controller state:")
00535                 #rospy.logdebug("    Classic Controller buttons: " + str(theButtons) + "\n    Classic Controller axes: " + str(msg.axes) + "\n")
00536 
00537         except rospy.ROSInterruptException:
00538             rospy.loginfo("Shutdown request. Shutting down Clas sender.")
00539             exit(0)
00540             
00541 
00542 class WiiSender(WiimoteDataSender):
00543     """Broadcasting complete Wiimote messages to Topic wiimote"""
00544     
00545     def __init__(self, wiiMote, freq=100):
00546         """Initializes the full-Wiimote publisher.
00547     
00548         Parameters:
00549             wiiMote: a bluetooth-connected, calibrated WIIMote instance
00550             freq:    the message sending frequency in messages/sec. Max is 100, because
00551                      the Wiimote only samples the sensors at 100Hz.
00552         """
00553         
00554         WiimoteDataSender.__init__(self, wiiMote, freq)
00555         
00556         self.pub = rospy.Publisher('/wiimote/state', State, queue_size=1)
00557         
00558     def run(self):
00559         """Loop that obtains the latest wiimote state, publishes the data, and sleeps.
00560         
00561         The wiimote message, if fully filled in, contains information in common with Imu.msg:
00562         acceleration (in m/s^2), and angular rate (in radians/sec). For each of
00563         these quantities, the IMU message format also wants the corresponding
00564         covariance matrix.
00565         
00566         The covariance matrices are the 3x3 matrix with the axes' variance in the 
00567         diagonal. We obtain the variance from the Wiimote instance.  
00568         """
00569         
00570         rospy.loginfo("Wiimote state publisher starting (topic /wiimote/state).")
00571         self.threadName = "Wiimote topic Publisher"
00572         try:
00573             while not rospy.is_shutdown():
00574                 rospy.sleep(self.sleepDuration)
00575                 (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
00576                 
00577                 zeroingTimeSecs = int(self.wiiMote.lastZeroingTime)
00578                 zeroingTimeNSecs = int((self.wiiMote.lastZeroingTime - zeroingTimeSecs) * 10**9)
00579                 msg = State(header=None,
00580                             angular_velocity_zeroed=None,
00581                             angular_velocity_raw=None,
00582                             angular_velocity_covariance=self.angular_velocity_covariance,
00583                             linear_acceleration_zeroed=None,
00584                             linear_acceleration_raw=None,
00585                             linear_acceleration_covariance=self.linear_acceleration_covariance,
00586                             nunchuk_acceleration_zeroed=None,
00587                             nunchuk_acceleration_raw=None,
00588                             nunchuk_joystick_zeroed=None,
00589                             nunchuk_joystick_raw=None,
00590                             buttons=[False,False,False,False,False,False,False,False,False,False],
00591                             nunchuk_buttons=[False,False],
00592                             rumble=False,
00593                             LEDs=None,
00594                             ir_tracking = None,
00595                             raw_battery=None,
00596                             percent_battery=None,
00597                             zeroing_time=rospy.Time(zeroingTimeSecs, zeroingTimeNSecs),
00598                             errors=0)
00599                     
00600                 # If a gyro is plugged into the Wiimote, then note the 
00601                 # angular velocity in the message, else indicate with
00602                 # the special gyroAbsence_covariance matrix that angular
00603                 # velocity is unavailable:      
00604                 if self.wiistate.motionPlusPresent:
00605                     msg.angular_velocity_zeroed.x = canonicalAngleRate[PHI]
00606                     msg.angular_velocity_zeroed.y = canonicalAngleRate[THETA]
00607                     msg.angular_velocity_zeroed.z = canonicalAngleRate[PSI]
00608                     
00609                     msg.angular_velocity_raw.x = self.wiistate.angleRateRaw[PHI]
00610                     msg.angular_velocity_raw.y = self.wiistate.angleRateRaw[THETA]
00611                     msg.angular_velocity_raw.z = self.wiistate.angleRateRaw[PSI]
00612                     
00613                 else:
00614                     msg.angular_velocity_covariance = self.gyroAbsence_covariance
00615                 
00616                 msg.linear_acceleration_zeroed.x = canonicalAccel[X]
00617                 msg.linear_acceleration_zeroed.y = canonicalAccel[Y]
00618                 msg.linear_acceleration_zeroed.z = canonicalAccel[Z]
00619                 
00620                 msg.linear_acceleration_raw.x = self.wiistate.accRaw[X]
00621                 msg.linear_acceleration_raw.y = self.wiistate.accRaw[Y]
00622                 msg.linear_acceleration_raw.z = self.wiistate.accRaw[Z]
00623 
00624                 if self.wiistate.nunchukPresent:
00625                     msg.nunchuk_acceleration_zeroed.x = canonicalNunchukAccel[X]
00626                     msg.nunchuk_acceleration_zeroed.y = canonicalNunchukAccel[Y]
00627                     msg.nunchuk_acceleration_zeroed.z = canonicalNunchukAccel[Z]
00628                     
00629                     msg.nunchuk_acceleration_raw.x = self.wiistate.nunchukAccRaw[X]
00630                     msg.nunchuk_acceleration_raw.y = self.wiistate.nunchukAccRaw[Y]
00631                     msg.nunchuk_acceleration_raw.z = self.wiistate.nunchukAccRaw[Z]
00632 
00633                     msg.nunchuk_joystick_zeroed = self.wiistate.nunchukStick
00634                     msg.nunchuk_joystick_raw    = self.wiistate.nunchukStickRaw
00635                     moreButtons = []
00636                     moreButtons.append(self.wiistate.nunchukButtons[BTN_Z])
00637                     moreButtons.append(self.wiistate.nunchukButtons[BTN_C])
00638                     msg.nunchuk_buttons = moreButtons
00639 
00640                 theButtons = []
00641                 theButtons.append(self.wiistate.buttons[BTN_1])
00642                 theButtons.append(self.wiistate.buttons[BTN_2])
00643                 theButtons.append(self.wiistate.buttons[BTN_PLUS])
00644                 theButtons.append(self.wiistate.buttons[BTN_MINUS])
00645                 theButtons.append(self.wiistate.buttons[BTN_A])
00646                 theButtons.append(self.wiistate.buttons[BTN_B])
00647                 theButtons.append(self.wiistate.buttons[BTN_UP])
00648                 theButtons.append(self.wiistate.buttons[BTN_DOWN])
00649                 theButtons.append(self.wiistate.buttons[BTN_LEFT])
00650                 theButtons.append(self.wiistate.buttons[BTN_RIGHT])
00651                 theButtons.append(self.wiistate.buttons[BTN_HOME])
00652                 
00653                 ledStates = self.wiiMote.getLEDs()
00654                 for indx in range(len(msg.LEDs)):
00655                     if ledStates[indx]: 
00656                         msg.LEDs[indx] = True
00657                     else:
00658                         msg.LEDs[indx] = False
00659 
00660                 msg.buttons = theButtons
00661 
00662                 msg.raw_battery = self.wiiMote.getBattery()
00663                 msg.percent_battery = msg.raw_battery * 100./self.wiiMote.BATTERY_MAX
00664     
00665                 irSources = self.wiistate.IRSources
00666                 
00667                 for irSensorIndx in range(NUM_IR_SENSORS):
00668                     if irSources[irSensorIndx] is not None:
00669                         # Did hardware deliver IR source position for this IR sensor?
00670                         try:
00671                           pos  = irSources[irSensorIndx]['pos']
00672                         except KeyError:
00673                             # If no position information from this IR sensor, use INVALID for the dimensions:
00674                             msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID))
00675                         # The following else is unusual: its statements are bypassed is except clause had control:
00676                         else:
00677                             # Have IR position info from this IR sensor. We use the IR_source_info
00678                             # message type. Get size (intensity?):
00679                             try: 
00680                                 size = irSources[irSensorIndx]['size']
00681                             except KeyError:
00682                                 # If the driver did not deliver size information, indicate by using INVALID:
00683                                 size = State.INVALID
00684                             lightInfo = IrSourceInfo(pos[0], pos[1], size)
00685                             msg.ir_tracking.append(lightInfo)
00686                     else:
00687                         msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID))
00688 
00689                 measureTime = self.wiistate.time
00690                 timeSecs = int(measureTime)
00691                 timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
00692                 msg.header.stamp.secs = timeSecs
00693                 msg.header.stamp.nsecs = timeNSecs
00694                 
00695                 try:
00696                   self.pub.publish(msg)
00697                 except rospy.ROSException:
00698                   rospy.loginfo("Topic /wiimote/state closed. Shutting down Wiimote sender.")
00699                   exit(0)
00700 
00701                 #rospy.logdebug("Wiimote state:")
00702                 #rospy.logdebug("    Accel: " + str(canonicalAccel) + "\n    Angular rate: " + str(canonicalAngleRate))
00703                 #rospy.logdebug("    Rumble: " + str(msg.rumble) + "\n    Battery: [" + str(msg.raw_battery) + "," + str(msg.percent_battery))
00704                 #rospy.logdebug("    IR positions: " + str(msg.ir_tracking))
00705                                 
00706 
00707         except rospy.ROSInterruptException:
00708             rospy.loginfo("Shutdown request. Shutting down Wiimote sender.")
00709             exit(0)
00710         
00711 class WiimoteListeners(threading.Thread):
00712     """Listen for request to rumble and LED blinking.
00713     """
00714     
00715     def __init__(self, wiiMote):
00716         
00717         threading.Thread.__init__(self)
00718         self.wiiMote = wiiMote    
00719         
00720         self.ledCommands = [False, False, False, False]
00721         self.rumbleCommand = False
00722         
00723         # Even though this thread mostly listens,
00724         # we do publish the is_calibrated() message
00725         # here, because this msg is so closely related
00726         # to the calibrate() service:
00727         self.is_calibratedPublisher = rospy.Publisher('/imu/is_calibrated', Bool, latch=True, queue_size=1)
00728         # We'll always just reuse this msg object:        
00729         self.is_CalibratedResponseMsg = Bool();
00730 
00731         # Initialize the latched is_calibrated state. We use
00732         # the result of the initial zeroing, when the services
00733         # were first started and the the user was asked to
00734         # push the two buttons for pairing:
00735 
00736         self.is_CalibratedResponseMsg.data = self.wiiMote.latestCalibrationSuccessful;
00737         self.is_calibratedPublisher.publish(self.is_CalibratedResponseMsg)
00738         
00739     def run(self):
00740         
00741       def feedbackCallback(msg):
00742         """The callback for handle the feedback array messages and sending that to the Wiimote"""
00743         for fb in msg.array:
00744           if fb.type == JoyFeedback.TYPE_LED:
00745             try:
00746               if fb.intensity >= 0.5:
00747                 self.ledCommands[fb.id] = True
00748               else:
00749                 self.ledCommands[fb.id] = False
00750             except:
00751               rospy.logwarn("LED ID out of bounds, ignoring!")
00752           elif fb.type == JoyFeedback.TYPE_RUMBLE:
00753             if fb.id == 0:
00754               if fb.intensity >= 0.5:
00755                 self.rumbleCommand = True
00756               else:
00757                 self.rumbleCommand = False
00758             else:
00759               rospy.logwarn("RUMBLE ID out of bounds, ignoring!")
00760 
00761         self.wiiMote.setLEDs(self.ledCommands)        
00762         self.wiiMote.setRumble(self.rumbleCommand)
00763 
00764         
00765         return
00766 
00767 
00768       def calibrateCallback(req):
00769         """The imu/calibrate service handler."""
00770           
00771         rospy.loginfo("Calibration request")
00772         
00773         calibrationSuccess = self.wiiMote.zeroDevice()
00774         
00775         # Update the latched is_calibrated state:
00776 
00777         self.is_CalibratedResponseMsg.data = calibrationSuccess
00778         self.is_calibratedPublisher.publish(self.is_CalibratedResponseMsg)
00779         
00780         return EmptyResponse()
00781 
00782       # Done with embedded function definitions. Back at the top
00783       # level of WiimoteListeners' run() function.
00784        
00785       # Subscribe to rumble and LED control messages and sit:
00786       rospy.loginfo("Wiimote feedback listener starting (topic /joy/set_feedback).")
00787       rospy.Subscriber("joy/set_feedback", JoyFeedbackArray, feedbackCallback)
00788       rospy.loginfo("Wiimote calibration service starting (topic /imu/calibrate).")
00789       rospy.Service("imu/calibrate", Empty, calibrateCallback)
00790       rospy.loginfo("Wiimote latched is_calibrated publisher starting (topic /imu/is_calibrated).")
00791       
00792       try:
00793           rospy.spin()
00794       except rospy.ROSInterruptException:
00795         rospy.loginfo("Shutdown request. Shutting down Wiimote listeners.")
00796         exit(0)
00797         
00798 
00799 if __name__ == '__main__':
00800     wiimoteNode = WiimoteNode()
00801     try:
00802         wiimoteNode.runWiimoteNode()
00803         
00804     except rospy.ROSInterruptException:
00805         rospy.loginfo("ROS Shutdown Request.")
00806     except KeyboardInterrupt, e:
00807         rospy.loginfo("Received keyboard interrupt.")
00808     except WiimoteNotFoundError, e:
00809         rospy.logfatal(str(e))
00810     except WiimoteEnableError, e:
00811         rospy.logfatal(str(e))
00812     except CallbackStackMultInstError, e:
00813         rospy.logfatal(str(e))
00814     except CallbackStackEmptyError, e:
00815         rospy.logfatal(str(e))
00816     except ResumeNonPausedError, e:
00817         rospy.logfatal(str(e))
00818     except CallbackStackEmptyError, e:
00819         rospy.logfatal(str(e))
00820     
00821     except:
00822         excType, excValue, excTraceback = sys.exc_info()[:3]
00823         traceback.print_exception(excType, excValue, excTraceback)
00824     finally:
00825         if (wiimoteNode is not None):
00826             wiimoteNode.shutdown()
00827         rospy.loginfo("Exiting Wiimote node.")
00828         sys.exit(0)


wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Sun Jul 9 2017 02:34:58