$search
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 ################################################################################ 00030 #!/usr/bin/env python 00031 00032 """The wiimote_node broadcasts four topics, and listens to messages that control 00033 the Wiimote stick's rumble (vibration) and LEDs. Transmitted topics (@100Hz): 00034 00035 o wiijoy Messages contain the accelerometer and gyro axis data, and all button states. 00036 o imu/data Messages contain the accelerometer and gyro axis data, and covariance matrices 00037 o wiimote/state the wiijoy and /imu messages, the button states, the LED states, 00038 rumble (i.e. vibrator) state, IR light sensor readings, time since last zeroed, 00039 and battery state. See State.msg 00040 o imu/is_calibrated Latched message 00041 o nunchuk Joy messages using the nunchuk as a joystick 00042 o classic Joy messages using the nunchuck as a joystic 00043 00044 The node listens to the following messages: 00045 00046 o wiimote/rumble 00047 Instruct this node to turn on/off the rumble (i.e. vibrator). Rather 00048 than just switching rumble, the message can instead contain 00049 an array of on/off durations. This node will pulse the rumbler 00050 accordingly without the message sender's additional cooperation. 00051 See RumbleControl.mg and TimedSwitch.msg 00052 o wiimote/leds 00053 Turn each LED on the Wiimote on/off. The message can instead 00054 contain an array of TimedSwitch structures. Each such structure 00055 turns a respective LED on and off according to time intervals that 00056 are stored in the structure. See LEDControl.msg and TimedSwitch.msg 00057 o imu/calibrate 00058 Request to calibrate the device. 00059 00060 No command line parameters. 00061 """ 00062 00063 # Code structure: The main thread spawns one thread each for the 00064 # four message senders, and one thread each for the message listeners. 00065 # The respective classes are IMUSender, JoySender, NunSender and WiiSender for 00066 # topic sending, and WiimoteListeners for the two message listeners. 00067 # 00068 # The Wiimote driver is encapsulated in class WIIMote (see WIIMote.py). 00069 # That class' singleton instance runs in its own thread, and uses 00070 # the third-party cwiid access software. 00071 00072 00073 # TODO: Removal of gyro is noticed (covar[0,0]<--1). But software does not notice plugging in. 00074 # TODO: Command line option: --no-zeroing 00075 00076 # -------- Python Standard Modules: 00077 import sys 00078 import threading 00079 import traceback 00080 import time 00081 00082 # -------- ROS-Related Modules: 00083 import roslib; roslib.load_manifest('wiimote') 00084 import rospy 00085 from geometry_msgs.msg import Vector3 00086 from sensor_msgs.msg import Imu 00087 from std_srvs.srv import Empty 00088 from std_srvs.srv import EmptyResponse 00089 from std_msgs.msg import Bool 00090 from sensor_msgs.msg import Joy 00091 from wiimote.msg import IrSourceInfo 00092 from wiimote.msg import State 00093 from wiimote.msg import TimedSwitch 00094 from wiimote.msg import LEDControl 00095 from wiimote.msg import RumbleControl 00096 00097 # -------- WIIMote Modules: 00098 from wiimote.wiimoteExceptions import * 00099 from wiimote.wiimoteConstants import * 00100 import wiimote.WIIMote 00101 import wiimote.wiiutils 00102 00103 GATHER_CALIBRATION_STATS = True 00104 00105 class WiimoteNode(): 00106 00107 00108 def runWiimoteNode(self): 00109 """Initialize the wiimote_node, establishing its name for communication with the Master""" 00110 00111 # All exceptions will end up in the __main__ section 00112 # and are handled there: 00113 00114 rospy.init_node('wiimote', anonymous=True, log_level=rospy.ERROR) # log_level=rospy.DEBUG 00115 wiimoteDevice = wiimote.WIIMote.WIIMote() 00116 wiimoteDevice.zeroDevice() 00117 00118 try: 00119 IMUSender(wiimoteDevice, freq=100).start() 00120 JoySender(wiimoteDevice, freq=100).start() 00121 WiiSender(wiimoteDevice, freq=100).start() 00122 NunSender(wiimoteDevice, freq=100).start() 00123 ClasSender(wiimoteDevice, freq=100).start() 00124 WiimoteListeners(wiimoteDevice).start() 00125 00126 rospy.spin() 00127 00128 except: 00129 rospy.loginfo("Error in startup") 00130 rospy.loginfo(sys.exc_info()[0]) 00131 finally: 00132 try: 00133 wiimoteDevice.setRumble(False) 00134 wiimoteDevice.setLEDs([False, False, False, False]) 00135 wiimoteDevice.shutdown() 00136 except: 00137 pass 00138 00139 def shutdown(self): 00140 try: 00141 IMUSender.stop 00142 JoySender.stop 00143 WiiSender.stop 00144 NunSender.stop 00145 WiimoteListener.stop 00146 except: 00147 pass 00148 00149 class WiimoteDataSender(threading.Thread): 00150 00151 def __init__(self, wiiMote, freq=100): 00152 00153 threading.Thread.__init__(self) 00154 self.wiiMote = wiiMote 00155 self.freq = freq 00156 self.sleepDuration = 1.0 / freq 00157 00158 varianceAccelerator = self.wiiMote.getVarianceAccelerator(); 00159 self.linear_acceleration_covariance = [varianceAccelerator[X], 0., 0., 00160 0., varianceAccelerator[Y], 0., 00161 0., 0., varianceAccelerator[Z]] 00162 00163 varianceGyro = self.wiiMote.getVarianceGyro(); 00164 self.angular_velocity_covariance = [varianceGyro[X], 0., 0., 00165 0., varianceGyro[Y], 0., 00166 0., 0., varianceGyro[Z]] 00167 00168 # If no gyro is attached to the Wiimote then we signal 00169 # the invalidity of angular rate w/ a covariance matrix 00170 # whose first element is -1: 00171 self.gyroAbsence_covariance = [-1., 0., 0., 00172 0., 0., 0., 00173 0., 0., 0.] 00174 00175 def obtainWiimoteData(self): 00176 """Retrieve one set of Wiimote measurements from the Wiimote instance. Return scaled accelerator and gyro readings. 00177 00178 We canonicalize both accelerator and gyro data through 00179 scaling them by constants that turn them into m/sec^2, and 00180 radians/sec, respectively. 00181 00182 Return: list of canonicalized accelerator and gyro readings. 00183 """ 00184 00185 while not rospy.is_shutdown(): 00186 self.wiistate = self.wiiMote.getWiimoteState() 00187 if self.wiistate is not None and self.wiistate.acc is not None: 00188 break 00189 else: 00190 rospy.sleep(0.1) 00191 00192 return self.canonicalizeWiistate() 00193 00194 def canonicalizeWiistate(self): 00195 """Scale accelerator, nunchuk accelerator, and gyro readings to be m/sec^2, m/sec^2 and radians/sec, respectively.""" 00196 00197 try: 00198 # Convert acceleration, which is in g's into m/sec^2: 00199 canonicalAccel = self.wiistate.acc.scale(EARTH_GRAVITY) 00200 00201 # If nunchuk is connected, then 00202 # convert nunchuk acceleration into m/sec^2 00203 if self.wiistate.nunchukPresent: 00204 canonicalNunchukAccel = self.wiistate.nunchukAcc.scale(EARTH_GRAVITY) 00205 else: 00206 canonicalNunchukAccel = None 00207 00208 # If the gyro is connected, then 00209 # Convert gyro reading to radians/sec (see wiimoteConstants.py 00210 # for origin of this scale factor): 00211 if self.wiistate.motionPlusPresent: 00212 canonicalAngleRate = self.wiistate.angleRate.scale(GYRO_SCALE_FACTOR) 00213 else: 00214 canonicalAngleRate = None 00215 00216 return [canonicalAccel, canonicalNunchukAccel, canonicalAngleRate] 00217 except AttributeError: 00218 # An attribute error here occurs when user shuts 00219 # off the Wiimote before stopping the wiimote_node: 00220 rospy.loginfo(self.threadName + " shutting down.") 00221 exit(0) 00222 00223 00224 class IMUSender(WiimoteDataSender): 00225 """Broadcasting Wiimote accelerator and gyro readings as IMU messages to Topic sensor_data/Imu""" 00226 00227 def __init__(self, wiiMote, freq=100): 00228 """Initializes the Wiimote IMU publisher. 00229 00230 Parameters: 00231 wiiMote: a bluetooth-connected, calibrated WIIMote instance 00232 freq: the message sending frequency in messages/sec. Max is 100, because 00233 the Wiimote only samples the sensors at 100Hz. 00234 """ 00235 00236 WiimoteDataSender.__init__(self, wiiMote, freq) 00237 00238 self.pub = rospy.Publisher('imu/data', Imu) 00239 00240 def run(self): 00241 """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps. 00242 00243 The IMU message, if fully filled in, contains information on orientation, 00244 acceleration (in m/s^2), and angular rate (in radians/sec). For each of 00245 these quantities, the IMU message format also wants the corresponding 00246 covariance matrix. 00247 00248 Wiimote only gives us acceleration and angular rate. So we ensure that the orientation 00249 data entry is marked invalid. We do this by setting the first 00250 entry of its associated covariance matrix to -1. The covariance 00251 matrices are the 3x3 matrix with the axes' variance in the 00252 diagonal. We obtain the variance from the Wiimote instance. 00253 """ 00254 00255 rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).") 00256 self.threadName = "IMU topic Publisher" 00257 try: 00258 while not rospy.is_shutdown(): 00259 (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData() 00260 00261 msg = Imu(header=None, 00262 orientation=None, # will default to [0.,0.,0.,0], 00263 orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.], # -1 indicates that orientation is unknown 00264 angular_velocity=None, 00265 angular_velocity_covariance=self.angular_velocity_covariance, 00266 linear_acceleration=None, 00267 linear_acceleration_covariance=self.linear_acceleration_covariance) 00268 00269 00270 # If a gyro is plugged into the Wiimote, then note the 00271 # angular velocity in the message, else indicate with 00272 # the special gyroAbsence_covariance matrix that angular 00273 # velocity is unavailable: 00274 if self.wiistate.motionPlusPresent: 00275 msg.angular_velocity.x = canonicalAngleRate[PHI] 00276 msg.angular_velocity.y = canonicalAngleRate[THETA] 00277 msg.angular_velocity.z = canonicalAngleRate[PSI] 00278 else: 00279 msg.angular_velocity_covariance = self.gyroAbsence_covariance 00280 00281 msg.linear_acceleration.x = canonicalAccel[X] 00282 msg.linear_acceleration.y = canonicalAccel[Y] 00283 msg.linear_acceleration.z = canonicalAccel[Z] 00284 00285 measureTime = self.wiistate.time 00286 timeSecs = int(measureTime) 00287 timeNSecs = int(abs(timeSecs - measureTime) * 10**9) 00288 msg.header.stamp.secs = timeSecs 00289 msg.header.stamp.nsecs = timeNSecs 00290 00291 try: 00292 self.pub.publish(msg) 00293 except rospy.ROSException: 00294 rospy.loginfo("Topic imu/data closed. Shutting down Imu sender.") 00295 exit(0) 00296 00297 #rospy.logdebug("IMU state:") 00298 #rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate)) 00299 rospy.sleep(self.sleepDuration) 00300 except rospy.ROSInterruptException: 00301 rospy.loginfo("Shutdown request. Shutting down Imu sender.") 00302 exit(0) 00303 00304 00305 class JoySender(WiimoteDataSender): 00306 00307 """Broadcasting Wiimote accelerator and gyro readings as Joy(stick) messages to Topic joy""" 00308 00309 def __init__(self, wiiMote, freq=100): 00310 """Initializes the Wiimote Joy(stick) publisher. 00311 00312 Parameters: 00313 wiiMote: a bluetooth-connected, calibrated WIIMote instance 00314 freq: the message sending frequency in messages/sec. Max is 100, because 00315 the Wiimote only samples the sensors at 100Hz. 00316 """ 00317 00318 WiimoteDataSender.__init__(self, wiiMote, freq) 00319 00320 00321 self.pub = rospy.Publisher('joy', Joy) 00322 00323 def run(self): 00324 """Loop that obtains the latest wiimote state, publishes the joystick data, and sleeps. 00325 00326 The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons. 00327 """ 00328 00329 rospy.loginfo("Wiimote joystick publisher starting (topic wiijoy).") 00330 self.threadName = "Joy topic Publisher" 00331 try: 00332 while not rospy.is_shutdown(): 00333 (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData() 00334 00335 msg = Joy(header=None, 00336 axes=[canonicalAccel[X], canonicalAccel[Y], canonicalAccel[Z]], 00337 buttons=None) 00338 00339 # If a gyro is attached to the Wiimote, we add the 00340 # gyro information: 00341 if self.wiistate.motionPlusPresent: 00342 msg.axes.extend([canonicalAngleRate[PHI], canonicalAngleRate[THETA], canonicalAngleRate[PSI]]) 00343 00344 # Fill in the ROS message's buttons field (there *must* be 00345 # a better way in python to declare an array of 11 zeroes...] 00346 00347 theButtons = [False,False,False,False,False,False,False,False,False,False,False] 00348 theButtons[State.MSG_BTN_1] = self.wiistate.buttons[BTN_1] 00349 theButtons[State.MSG_BTN_2] = self.wiistate.buttons[BTN_2] 00350 theButtons[State.MSG_BTN_A] = self.wiistate.buttons[BTN_A] 00351 theButtons[State.MSG_BTN_B] = self.wiistate.buttons[BTN_B] 00352 theButtons[State.MSG_BTN_PLUS] = self.wiistate.buttons[BTN_PLUS] 00353 theButtons[State.MSG_BTN_MINUS] = self.wiistate.buttons[BTN_MINUS] 00354 theButtons[State.MSG_BTN_LEFT] = self.wiistate.buttons[BTN_LEFT] 00355 theButtons[State.MSG_BTN_RIGHT] = self.wiistate.buttons[BTN_RIGHT] 00356 theButtons[State.MSG_BTN_UP] = self.wiistate.buttons[BTN_UP] 00357 theButtons[State.MSG_BTN_DOWN] = self.wiistate.buttons[BTN_DOWN] 00358 theButtons[State.MSG_BTN_HOME] = self.wiistate.buttons[BTN_HOME] 00359 00360 msg.buttons = theButtons 00361 00362 measureTime = self.wiistate.time 00363 timeSecs = int(measureTime) 00364 timeNSecs = int(abs(timeSecs - measureTime) * 10**9) 00365 # Add the timestamp 00366 msg.header.stamp.secs = timeSecs 00367 msg.header.stamp.nsecs = timeNSecs 00368 00369 try: 00370 self.pub.publish(msg) 00371 except rospy.ROSException: 00372 rospy.loginfo("Topic wiijoy closed. Shutting down Joy sender.") 00373 exit(0) 00374 00375 #rospy.logdebug("Joystick state:") 00376 #rospy.logdebug(" Joy buttons: " + str(theButtons) + "\n Joy accel: " + str(canonicalAccel) + "\n Joy angular rate: " + str(canonicalAngleRate)) 00377 rospy.sleep(self.sleepDuration) 00378 except rospy.ROSInterruptException: 00379 rospy.loginfo("Shutdown request. Shutting down Joy sender.") 00380 exit(0) 00381 00382 class NunSender(WiimoteDataSender): 00383 00384 """Broadcasting nunchuk accelerator and joystick readings as Joy(stick) messages to Topic joy""" 00385 00386 def __init__(self, wiiMote, freq=100): 00387 """Initializes the nunchuk Joy(stick) publisher. 00388 00389 Parameters: 00390 wiiMote: a bluetooth-connected, calibrated WIIMote instance 00391 freq: the message sending frequency in messages/sec. Max is 100, because 00392 the Wiimote only samples the sensors at 100Hz. 00393 """ 00394 00395 WiimoteDataSender.__init__(self, wiiMote, freq) 00396 00397 00398 00399 # Set 'pub' to none here, and check for none-ness in the 00400 # loop below so as not to start this publisher unnecessarily. 00401 self.pub = None 00402 00403 def run(self): 00404 """Loop that obtains the latest nunchuk state, publishes the joystick data, and sleeps. 00405 00406 The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons. 00407 """ 00408 00409 self.threadName = "nunchuk Joy topic Publisher" 00410 try: 00411 while not rospy.is_shutdown(): 00412 rospy.sleep(self.sleepDuration) 00413 (canonicalAccel, scaledAcc, canonicalAngleRate) = self.obtainWiimoteData() 00414 if not self.wiistate.nunchukPresent: 00415 continue 00416 if self.pub is None: 00417 self.pub = rospy.Publisher('/wiimote/nunchuk', Joy) 00418 rospy.loginfo("Wiimote Nunchuk joystick publisher starting (topic nunchuk).") 00419 00420 (joyx, joyy) = self.wiistate.nunchukStick 00421 00422 msg = Joy(header=None, 00423 axes=[joyx, joyy, 00424 scaledAcc[X], scaledAcc[Y], scaledAcc[Z]], 00425 buttons=None) 00426 00427 theButtons = [False,False] 00428 theButtons[State.MSG_BTN_Z] = self.wiistate.nunchukButtons[BTN_Z] 00429 theButtons[State.MSG_BTN_C] = self.wiistate.nunchukButtons[BTN_C] 00430 00431 msg.buttons = theButtons 00432 00433 measureTime = self.wiistate.time 00434 timeSecs = int(measureTime) 00435 timeNSecs = int(abs(timeSecs - measureTime) * 10**9) 00436 msg.header.stamp.secs = timeSecs 00437 msg.header.stamp.nsecs = timeNSecs 00438 00439 try: 00440 self.pub.publish(msg) 00441 except rospy.ROSException: 00442 rospy.loginfo("Topic /wiimote/nunchuk closed. Shutting down Nun sender.") 00443 exit(0) 00444 00445 #rospy.logdebug("nunchuk state:") 00446 #rospy.logdebug(" nunchuk buttons: " + str(theButtons) + "\n Nuchuck axes: " + str(msg.axes) + "\n") 00447 00448 except rospy.ROSInterruptException: 00449 rospy.loginfo("Shutdown request. Shutting down Nun sender.") 00450 exit(0) 00451 00452 class ClasSender(WiimoteDataSender): 00453 00454 """Broadcasting Classic Controller joystick readings as Joy(stick) messages to Topic joy""" 00455 00456 def __init__(self, wiiMote, freq=100): 00457 """Initializes the Classic Controller Joy(stick) publisher. 00458 00459 Parameters: 00460 wiiMote: a bluetooth-connected, calibrated WIIMote instance 00461 freq: the message sending frequency in messages/sec. Max is 100, because 00462 the Wiimote only samples the sensors at 100Hz. 00463 """ 00464 00465 WiimoteDataSender.__init__(self, wiiMote, freq) 00466 00467 # Set 'pub' to none here, and check for none-ness in the 00468 # loop below so as not to start this publisher unnecessarily. 00469 self.pub = None 00470 00471 def run(self): 00472 """Loop that obtains the latest classic controller state, publishes the joystick data, and sleeps. 00473 00474 The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons. 00475 """ 00476 00477 self.threadName = "Classic Controller Joy topic Publisher" 00478 try: 00479 while not rospy.is_shutdown(): 00480 rospy.sleep(self.sleepDuration) 00481 self.obtainWiimoteData() 00482 00483 if not self.wiistate.classicPresent: 00484 continue 00485 if self.pub is None: 00486 rospy.Publisher('/wiimote/classic', Joy) 00487 rospy.loginfo("Wiimote Classic Controller joystick publisher starting (topic /wiimote/classic).") 00488 00489 (l_joyx, l_joyy) = self.wiistate.classicStickLeft 00490 (r_joyx, r_joyy) = self.wiistate.classicStickRight 00491 # scale the joystick to [-1, 1] 00492 l_joyx = -(l_joyx-33)/27. 00493 l_joyy = (l_joyy-33)/27. 00494 r_joyx = -(r_joyx-15)/13. 00495 r_joyy = (r_joyy-15)/13. 00496 # create a deadzone in the middle 00497 if abs(l_joyx) < .05: 00498 l_joyx = 0 00499 if abs(l_joyy) < .05: 00500 l_joyy = 0 00501 if abs(r_joyx) < .05: 00502 r_joyx = 0 00503 if abs(r_joyy) < .05: 00504 r_joyy = 0 00505 00506 msg = Joy(header=None, 00507 axes=[l_joyx, l_joyy,r_joyx, r_joyy], 00508 buttons=None) 00509 00510 theButtons = [False,False,False,False,False,False,False,False,False,False,False,False,False,False,False] 00511 theButtons[State.MSG_CLASSIC_BTN_X] = self.wiistate.classicButtons[CLASSIC_BTN_X] 00512 theButtons[State.MSG_CLASSIC_BTN_Y] = self.wiistate.classicButtons[CLASSIC_BTN_Y] 00513 theButtons[State.MSG_CLASSIC_BTN_A] = self.wiistate.classicButtons[CLASSIC_BTN_A] 00514 theButtons[State.MSG_CLASSIC_BTN_B] = self.wiistate.classicButtons[CLASSIC_BTN_B] 00515 theButtons[State.MSG_CLASSIC_BTN_PLUS] = self.wiistate.classicButtons[CLASSIC_BTN_PLUS] 00516 theButtons[State.MSG_CLASSIC_BTN_MINUS] = self.wiistate.classicButtons[CLASSIC_BTN_MINUS] 00517 theButtons[State.MSG_CLASSIC_BTN_LEFT] = self.wiistate.classicButtons[CLASSIC_BTN_LEFT] 00518 theButtons[State.MSG_CLASSIC_BTN_RIGHT] = self.wiistate.classicButtons[CLASSIC_BTN_RIGHT] 00519 theButtons[State.MSG_CLASSIC_BTN_UP] = self.wiistate.classicButtons[CLASSIC_BTN_UP] 00520 theButtons[State.MSG_CLASSIC_BTN_DOWN] = self.wiistate.classicButtons[CLASSIC_BTN_DOWN] 00521 theButtons[State.MSG_CLASSIC_BTN_HOME] = self.wiistate.classicButtons[CLASSIC_BTN_HOME] 00522 theButtons[State.MSG_CLASSIC_BTN_L] = self.wiistate.classicButtons[CLASSIC_BTN_L] 00523 theButtons[State.MSG_CLASSIC_BTN_R] = self.wiistate.classicButtons[CLASSIC_BTN_R] 00524 theButtons[State.MSG_CLASSIC_BTN_ZL] = self.wiistate.classicButtons[CLASSIC_BTN_ZL] 00525 theButtons[State.MSG_CLASSIC_BTN_ZR] = self.wiistate.classicButtons[CLASSIC_BTN_ZR] 00526 00527 msg.buttons = theButtons 00528 00529 measureTime = self.wiistate.time 00530 timeSecs = int(measureTime) 00531 timeNSecs = int(abs(timeSecs - measureTime) * 10**9) 00532 msg.header.stamp.secs = timeSecs 00533 msg.header.stamp.nsecs = timeNSecs 00534 00535 try: 00536 self.pub.publish(msg) 00537 except rospy.ROSException: 00538 rospy.loginfo("Topic /wiimote/classic closed. Shutting down Clas sender.") 00539 exit(0) 00540 00541 #rospy.logdebug("Classic Controller state:") 00542 #rospy.logdebug(" Classic Controller buttons: " + str(theButtons) + "\n Classic Controller axes: " + str(msg.axes) + "\n") 00543 00544 except rospy.ROSInterruptException: 00545 rospy.loginfo("Shutdown request. Shutting down Clas sender.") 00546 exit(0) 00547 00548 00549 class WiiSender(WiimoteDataSender): 00550 """Broadcasting complete Wiimote messages to Topic wiimote""" 00551 00552 def __init__(self, wiiMote, freq=100): 00553 """Initializes the full-Wiimote publisher. 00554 00555 Parameters: 00556 wiiMote: a bluetooth-connected, calibrated WIIMote instance 00557 freq: the message sending frequency in messages/sec. Max is 100, because 00558 the Wiimote only samples the sensors at 100Hz. 00559 """ 00560 00561 WiimoteDataSender.__init__(self, wiiMote, freq) 00562 00563 self.pub = rospy.Publisher('/wiimote/state', State) 00564 00565 def run(self): 00566 """Loop that obtains the latest wiimote state, publishes the data, and sleeps. 00567 00568 The wiimote message, if fully filled in, contains information in common with Imu.msg: 00569 acceleration (in m/s^2), and angular rate (in radians/sec). For each of 00570 these quantities, the IMU message format also wants the corresponding 00571 covariance matrix. 00572 00573 The covariance matrices are the 3x3 matrix with the axes' variance in the 00574 diagonal. We obtain the variance from the Wiimote instance. 00575 """ 00576 00577 rospy.loginfo("Wiimote state publisher starting (topic /wiimote/state).") 00578 self.threadName = "Wiimote topic Publisher" 00579 try: 00580 while not rospy.is_shutdown(): 00581 rospy.sleep(self.sleepDuration) 00582 (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData() 00583 00584 zeroingTimeSecs = int(self.wiiMote.lastZeroingTime) 00585 zeroingTimeNSecs = int((self.wiiMote.lastZeroingTime - zeroingTimeSecs) * 10**9) 00586 msg = State(header=None, 00587 angular_velocity_zeroed=None, 00588 angular_velocity_raw=None, 00589 angular_velocity_covariance=self.angular_velocity_covariance, 00590 linear_acceleration_zeroed=None, 00591 linear_acceleration_raw=None, 00592 linear_acceleration_covariance=self.linear_acceleration_covariance, 00593 nunchuk_acceleration_zeroed=None, 00594 nunchuk_acceleration_raw=None, 00595 nunchuk_joystick_zeroed=None, 00596 nunchuk_joystick_raw=None, 00597 buttons=[False,False,False,False,False,False,False,False,False,False], 00598 nunchuk_buttons=[False,False], 00599 rumble=False, 00600 LEDs=None, 00601 ir_tracking = None, 00602 raw_battery=None, 00603 percent_battery=None, 00604 zeroing_time=rospy.Time(zeroingTimeSecs, zeroingTimeNSecs), 00605 errors=0) 00606 00607 # If a gyro is plugged into the Wiimote, then note the 00608 # angular velocity in the message, else indicate with 00609 # the special gyroAbsence_covariance matrix that angular 00610 # velocity is unavailable: 00611 if self.wiistate.motionPlusPresent: 00612 msg.angular_velocity_zeroed.x = canonicalAngleRate[PHI] 00613 msg.angular_velocity_zeroed.y = canonicalAngleRate[THETA] 00614 msg.angular_velocity_zeroed.z = canonicalAngleRate[PSI] 00615 00616 msg.angular_velocity_raw.x = self.wiistate.angleRateRaw[PHI] 00617 msg.angular_velocity_raw.y = self.wiistate.angleRateRaw[THETA] 00618 msg.angular_velocity_raw.z = self.wiistate.angleRateRaw[PSI] 00619 00620 else: 00621 msg.angular_velocity_covariance = self.gyroAbsence_covariance 00622 00623 msg.linear_acceleration_zeroed.x = canonicalAccel[X] 00624 msg.linear_acceleration_zeroed.y = canonicalAccel[Y] 00625 msg.linear_acceleration_zeroed.z = canonicalAccel[Z] 00626 00627 msg.linear_acceleration_raw.x = self.wiistate.accRaw[X] 00628 msg.linear_acceleration_raw.y = self.wiistate.accRaw[Y] 00629 msg.linear_acceleration_raw.z = self.wiistate.accRaw[Z] 00630 00631 if self.wiistate.nunchukPresent: 00632 msg.nunchuk_acceleration_zeroed.x = canonicalNunchukAccel[X] 00633 msg.nunchuk_acceleration_zeroed.y = canonicalNunchukAccel[Y] 00634 msg.nunchuk_acceleration_zeroed.z = canonicalNunchukAccel[Z] 00635 00636 msg.nunchuk_acceleration_raw.x = self.wiistate.nunchukAccRaw[X] 00637 msg.nunchuk_acceleration_raw.y = self.wiistate.nunchukAccRaw[Y] 00638 msg.nunchuk_acceleration_raw.z = self.wiistate.nunchukAccRaw[Z] 00639 00640 msg.nunchuk_joystick_zeroed = self.wiistate.nunchukStick 00641 msg.nunchuk_joystick_raw = self.wiistate.nunchukStickRaw 00642 moreButtons = [] 00643 moreButtons.append(self.wiistate.nunchukButtons[BTN_Z]) 00644 moreButtons.append(self.wiistate.nunchukButtons[BTN_C]) 00645 msg.nunchuk_buttons = moreButtons 00646 00647 theButtons = [] 00648 theButtons.append(self.wiistate.buttons[BTN_1]) 00649 theButtons.append(self.wiistate.buttons[BTN_2]) 00650 theButtons.append(self.wiistate.buttons[BTN_PLUS]) 00651 theButtons.append(self.wiistate.buttons[BTN_MINUS]) 00652 theButtons.append(self.wiistate.buttons[BTN_A]) 00653 theButtons.append(self.wiistate.buttons[BTN_B]) 00654 theButtons.append(self.wiistate.buttons[BTN_UP]) 00655 theButtons.append(self.wiistate.buttons[BTN_DOWN]) 00656 theButtons.append(self.wiistate.buttons[BTN_LEFT]) 00657 theButtons.append(self.wiistate.buttons[BTN_RIGHT]) 00658 theButtons.append(self.wiistate.buttons[BTN_HOME]) 00659 00660 ledStates = self.wiiMote.getLEDs() 00661 for indx in range(len(msg.LEDs)): 00662 if ledStates[indx]: 00663 msg.LEDs[indx] = True 00664 else: 00665 msg.LEDs[indx] = False 00666 00667 msg.buttons = theButtons 00668 00669 msg.raw_battery = self.wiiMote.getBattery() 00670 msg.percent_battery = msg.raw_battery * 100./self.wiiMote.BATTERY_MAX 00671 00672 irSources = self.wiistate.IRSources 00673 00674 for irSensorIndx in range(NUM_IR_SENSORS): 00675 if irSources[irSensorIndx] is not None: 00676 # Did hardware deliver IR source position for this IR sensor? 00677 try: 00678 pos = irSources[irSensorIndx]['pos'] 00679 except KeyError: 00680 # If no position information from this IR sensor, use INVALID for the dimensions: 00681 msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID)) 00682 # The following else is unusual: its statements are bypassed is except clause had control: 00683 else: 00684 # Have IR position info from this IR sensor. We use the IR_source_info 00685 # message type. Get size (intensity?): 00686 try: 00687 size = irSources[irSensorIndx]['size'] 00688 except KeyError: 00689 # If the driver did not deliver size information, indicate by using INVALID: 00690 size = State.INVALID 00691 lightInfo = IrSourceInfo(pos[0], pos[1], size) 00692 msg.ir_tracking.append(lightInfo) 00693 else: 00694 msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID)) 00695 00696 measureTime = self.wiistate.time 00697 timeSecs = int(measureTime) 00698 timeNSecs = int(abs(timeSecs - measureTime) * 10**9) 00699 msg.header.stamp.secs = timeSecs 00700 msg.header.stamp.nsecs = timeNSecs 00701 00702 try: 00703 self.pub.publish(msg) 00704 except rospy.ROSException: 00705 rospy.loginfo("Topic /wiimote/state closed. Shutting down Wiimote sender.") 00706 exit(0) 00707 00708 #rospy.logdebug("Wiimote state:") 00709 #rospy.logdebug(" Accel: " + str(canonicalAccel) + "\n Angular rate: " + str(canonicalAngleRate)) 00710 #rospy.logdebug(" Rumble: " + str(msg.rumble) + "\n Battery: [" + str(msg.raw_battery) + "," + str(msg.percent_battery)) 00711 #rospy.logdebug(" IR positions: " + str(msg.ir_tracking)) 00712 00713 00714 except rospy.ROSInterruptException: 00715 rospy.loginfo("Shutdown request. Shutting down Wiimote sender.") 00716 exit(0) 00717 00718 class WiimoteListeners(threading.Thread): 00719 """Listen for request to rumble and LED blinking. 00720 00721 Subscribes to topics /rumble and /leds, listening for RumbleControl 00722 and LEDControl messages. 00723 00724 Parameters: The switch_mode field is either 00725 -1.0 to turn rumble on, zero to turn it off, or a 00726 positive float. If switch_mode is such a positive number, 00727 it is taken to be the repeat count for an on/off rumble 00728 pattern (see next parameter) 00729 00730 The pulse_pattern is a float32[MAX_RUMBLE_PATTERN_LENGTH], 00731 which contains fractional seconds that rumble is to be 00732 on or off. 00733 """ 00734 00735 def __init__(self, wiiMote): 00736 00737 threading.Thread.__init__(self) 00738 self.wiiMote = wiiMote 00739 self.pulserThread = None 00740 00741 # Even though this thread mostly listens, 00742 # we do publish the is_calibrated() message 00743 # here, because this msg is so closely related 00744 # to the calibrate() service: 00745 self.is_calibratedPublisher = rospy.Publisher('/imu/is_calibrated', Bool, latch=True) 00746 # We'll always just reuse this msg object: 00747 self.is_CalibratedResponseMsg = Bool(); 00748 00749 # Initialize the latched is_calibrated state. We use 00750 # the result of the initial zeroing, when the services 00751 # were first started and the the user was asked to 00752 # push the two buttons for pairing: 00753 00754 self.is_CalibratedResponseMsg.data = self.wiiMote.latestCalibrationSuccessful; 00755 self.is_calibratedPublisher.publish(self.is_CalibratedResponseMsg) 00756 00757 def run(self): 00758 00759 def rumbleSwitchCallback(msg): 00760 """Callback for turning rumble on/off, and to initiate pulse rumble.""" 00761 00762 #rospy.logdebug("From: " + rospy.get_caller_id() + ". Rumble request \n" + str(msg)) 00763 00764 # If a rumble pulser thread is running, stop it: 00765 if self.pulserThread is not None: 00766 self.pulserThread.stop = True 00767 # Wait for the thread to finish what it's doing 00768 self.pulserThread.join() 00769 self.pulserThread = None 00770 00771 if msg.rumble.switch_mode == TimedSwitch.ON: 00772 self.wiiMote.setRumble(True) 00773 return 00774 elif msg.rumble.switch_mode == TimedSwitch.OFF: 00775 self.wiiMote.setRumble(False) 00776 return 00777 elif msg.rumble.switch_mode != SWITCH_PULSE_PATTERN: 00778 rospy.loginfo("Illegal switch_mode value in rumble request from " + \ 00779 rospy.get_caller_id() + \ 00780 ": \n" + \ 00781 str(msg)) 00782 return 00783 00784 # Client wants to start a rumble pulse sequence: 00785 if msg.rumble.num_cycles == 0: 00786 return 00787 00788 # Pulser takes an *array* of OutputPattern. For rumble that array is 00789 # always of length 1. But for other feedback indicators, like LEDs, 00790 # there are more: 00791 self.pulserThread = SwitchPulser([OutputPattern(msg.rumble.pulse_pattern, msg.rumble.num_cycles)], RUMBLE, self.wiiMote) 00792 self.pulserThread.start() 00793 self.pulserThread.join() 00794 00795 return # end rumbleSwitchCallback 00796 00797 00798 def ledControlCallback(msg): 00799 """Callback for incoming LEDCOntrol requests.""" 00800 00801 #rospy.logdebug(rospy.get_caller_id() + "LED Control request " + str(msg)) 00802 00803 # Each LED has a TimedSwitch associated with it. Unpack 00804 # the data structure (an array of TimedSwitch) for passage 00805 # to the SwitchPulser thread. We need to pull out the 00806 # number of requested cycles for each LED's on/off pattern, 00807 # and the pattern arrays themselves: 00808 00809 patterns = [] 00810 ledCommands = [None, None, None, None] 00811 individualLED_simple_on_or_off = False 00812 00813 # Go through each switch. The array contains one TimedSwitch for 00814 # each of the LEDs. In each case determine whether the switch simply 00815 # calls for the LED to be turned on or off (as opposed to blinking in 00816 # a pattern). If so, set the ledCommands array to ON or OFF in the 00817 # respective position. Recall that None for an LED means 'leave as is.' 00818 for timedSwitch, switchIndex in zip(msg.timed_switch_array, range(len(msg.timed_switch_array))): 00819 # Is this a simple on/off request? 00820 if timedSwitch.switch_mode == TimedSwitch.ON: 00821 # Yes: simple ON: 00822 individualLED_simple_on_or_off = True 00823 ledCommands[switchIndex] = TimedSwitch.ON 00824 # Ensure that the pulse pattern engine blinks the 00825 # correct LEDs: indicate 'no blink action' for this LED: 00826 patterns.append(None) 00827 continue 00828 elif timedSwitch.switch_mode == TimedSwitch.OFF: 00829 # Yes: simple OFF: 00830 individualLED_simple_on_or_off = True 00831 ledCommands[switchIndex] = TimedSwitch.OFF 00832 # Ensure that the pulse pattern engine blinks the 00833 # correct LEDs: indicate 'no blink action' for this LED: 00834 patterns.append(None) 00835 continue 00836 elif timedSwitch.switch_mode == TimedSwitch.NO_CHANGE: 00837 patterns.append(None) 00838 continue 00839 # This LED is to blink by pattern: 00840 patterns.append(OutputPattern(timedSwitch.pulse_pattern, timedSwitch.num_cycles)) 00841 00842 # The ledCommands array may now have a mix of None, ON, or OFF. 00843 # If any of the LEDs are to be statically turned ON or OFF, do that now: 00844 if individualLED_simple_on_or_off: 00845 self.wiiMote.setLEDs(ledCommands) 00846 00847 # Start pulsing all other LEDs: 00848 self.pulserThread = SwitchPulser(patterns, LED, self.wiiMote) 00849 self.pulserThread.start() 00850 self.pulserThread.join() 00851 00852 return # end ledControlCallback() 00853 00854 def calibrateCallback(req): 00855 """The imu/calibrate service handler.""" 00856 00857 rospy.loginfo("Calibration request") 00858 00859 calibrationSuccess = self.wiiMote.zeroDevice() 00860 00861 # Update the latched is_calibrated state: 00862 00863 self.is_CalibratedResponseMsg.data = calibrationSuccess 00864 self.is_calibratedPublisher.publish(self.is_CalibratedResponseMsg) 00865 00866 return EmptyResponse() 00867 00868 # Done with embedded function definitions. Back at the top 00869 # level of WiimoteListeners' run() function. 00870 00871 # Subscribe to rumble and LED control messages and sit: 00872 rospy.loginfo("Wiimote rumble listener starting (topic /wiimote/rumble).") 00873 rospy.Subscriber("/wiimote/rumble", RumbleControl, rumbleSwitchCallback) 00874 rospy.loginfo("Wiimote LED control listener starting (topic /wiimote/leds).") 00875 rospy.Subscriber("/wiimote/leds", LEDControl, ledControlCallback) 00876 rospy.loginfo("Wiimote calibration service starting (topic /imu/calibrate).") 00877 rospy.Service("imu/calibrate", Empty, calibrateCallback) 00878 rospy.loginfo("Wiimote latched is_calibrated publisher starting (topic /imu/is_calibrated).") 00879 00880 try: 00881 rospy.spin() 00882 except rospy.ROSInterruptException: 00883 rospy.loginfo("Shutdown request. Shutting down Wiimote listeners.") 00884 exit(0) 00885 00886 00887 class SwitchPulser(threading.Thread): 00888 """Thread for executing rumble and LED pulse patterns.""" 00889 00890 def __init__(self, patternArray, outputIndicator, wiimoteDevice): 00891 """Parameters: 00892 o patternArray: For each pattern: an OutputPattern object 00893 There will only be one such object for Rumble output. 00894 For LEDs there will be one for each LED on the Wiimote. 00895 If one of the elements is None, that output indicator 00896 is left unchanged. For example, if the 2nd element 00897 in an LED pattern object array is None, the 2nd LED on the 00898 Wiimote will be left in its current state. 00899 00900 Note that the patterns may be of different lengths. 00901 So, one LED might have a 3-state pattern, while another 00902 LED's pattern is 5 states long. 00903 o RUMBLE or LED to indicate what is to be pulsed 00904 o A Wiimote device object 00905 00906 Note: We always start the affected indicators as if they were 00907 in the OFF state, and we always leave them in the off state. 00908 """ 00909 00910 threading.Thread.__init__(self) 00911 self.patternArray = patternArray 00912 self.wiimoteDevice = wiimoteDevice 00913 # Whether to pulse rumble or LEDs 00914 self.outputIndicator = outputIndicator 00915 # Allow this thread to be stopped by setting 00916 # instance variable 'stop' to True: 00917 self.stop = False 00918 00919 def run(self): 00920 00921 #rospy.logdebug("In pulser thread. patternArray: " + str(self.patternArray) + ". Len: " + str(len(self.patternArray))) 00922 00923 # First state is always ON: 00924 self.turnIndicatorOn(self.outputIndicator) 00925 00926 numPatterns = len(self.patternArray) 00927 try: 00928 while not rospy.is_shutdown() and not self.stop: 00929 00930 # Initialize nextDuration for sleeping to infinity: 00931 nextDuration = float('inf') 00932 00933 # Get the next sleep duration, which is the 00934 # minimum of all nextDurations times: 00935 00936 for pattern in self.patternArray: 00937 00938 if pattern is None: continue 00939 patternHeadTime = pattern.timeRemaining() 00940 if patternHeadTime is None: continue 00941 nextDuration = min(nextDuration, patternHeadTime) 00942 00943 # All patterns done? 00944 if nextDuration == float('inf'): 00945 #rospy.logdebug("End of pattern.") 00946 exit(0) 00947 00948 rospy.sleep(nextDuration) 00949 00950 # Time for a state change in at least one of the 00951 # patterns. We: 00952 # o Flip the state of the respective output indicator(s) 00953 # o Obtain the next duration entry in the pattern(s) with 00954 # the timeout we just finished. 00955 # o We subtract the amout of sleep that we just 00956 # awoke from in all other nextDurations 00957 # o We find the new minimum next delay and sleep 00958 00959 durationJustFinished = nextDuration 00960 for pattern, patternIndex in zip(self.patternArray, range(numPatterns)): 00961 00962 if pattern is None: 00963 continue 00964 00965 # reduceTimer() returns pattern header minus given time duration, 00966 # or None if the pattern is spent. As side effect 00967 # this operation also takes care of the repeats: 00968 00969 reducedTime = pattern.reduceTimer(durationJustFinished) 00970 # If this call started the pattern over, we 00971 # need to turn the indicator(s) on during this 00972 # coming new duration: 00973 00974 if pattern.startOfRepeat: 00975 self.turnIndicatorOn(self.outputIndicator) 00976 00977 if reducedTime is None: continue 00978 if reducedTime == 0.: 00979 # This pattern had a timeout: 00980 self.flipRumbleOrLED(patternIndex) 00981 00982 # continue while not rospy.is_shutdown() and not self.stop 00983 00984 except rospy.ROSInterruptException: 00985 rospy.loginfo("Shutdown request. Shutting down pulse switcher.") 00986 finally: 00987 # Make sure that the indicators that we manipulated 00988 # get turned off: 00989 00990 if self.outputIndicator == RUMBLE: 00991 self.wiimoteDevice.setRumble(False) 00992 exit(0) 00993 elif self.outputIndicator == LED: 00994 for oneLED, LEDIndex in zip(self.LEDMask, range(len(self.LEDMask))): 00995 if oneLED is not None: 00996 self.LEDMask[LEDIndex] = False 00997 self.wiimoteDevice.setLEDs(self.LEDMask) 00998 00999 01000 01001 def turnIndicatorOn(self, theIndicator): 01002 """Turns indicator(s) of interest ON. 01003 01004 Parameter: RUMBLE or LED 01005 """ 01006 01007 # Recall: patterns are None, we must leave 01008 # the respective output Indicator alone: 01009 01010 if theIndicator == RUMBLE and self.patternArray[0] is not None: 01011 # Start to rumble: 01012 self.wiimoteDevice.setRumble(True) 01013 01014 # Is this LED action? 01015 elif theIndicator == LED: 01016 # Get a clean 4-tuple with True/None depending on 01017 # whether we have a TimedSwitch for the respective 01018 # LED. For LEDs for which we don't have 01019 # a TimedSwitch: leave those alone: 01020 01021 self.LEDMask = [] 01022 for i in range(min(NUM_LEDS, len(self.patternArray))): 01023 try: 01024 if self.patternArray[i] is None: 01025 self.LEDMask.append(None) 01026 else: 01027 self.LEDMask.append(True) 01028 except IndexError: 01029 pass 01030 01031 self.wiimoteDevice.setLEDs(self.LEDMask) 01032 else: 01033 raise ValueError("Only RUMBLE and LED are legal values here.") 01034 01035 01036 def flipRumbleOrLED(self, index=0): 01037 01038 if self.outputIndicator == RUMBLE: 01039 self.flipRumble() 01040 else: 01041 self.flipLED(index) 01042 01043 01044 def flipRumble(self): 01045 self.wiimoteDevice.setRumble(not self.wiimoteDevice.getRumble()) 01046 01047 01048 def flipLED(self, index): 01049 01050 LEDStatus = self.wiimoteDevice.getLEDs() 01051 01052 # None's leave the respective LED unchanged: 01053 newLEDStatus = [None, None, None, None] 01054 01055 newLEDStatus[index] = not LEDStatus[index] 01056 self.wiimoteDevice.setLEDs(newLEDStatus) 01057 01058 class OutputPattern(object): 01059 """Instances encapsulate rumble or LED on/off time patterns as received from related ROS messages. 01060 01061 This class provides convenient encapsulation for the pattern arrays themselves, 01062 for associated pointers into the arrays, and for status change and inquiry requests. 01063 Terminology: 'Pattern Head' is the currently used time duration. A pattern is 'Spent' 01064 if all the time sequences have run, and no repeats are left. 01065 01066 Public instance variables: 01067 o startOfRepeat ; indicates whether pattern just starts to repeat. (see method reduceTimer()) 01068 """ 01069 01070 01071 def __init__(self, rosMsgPattern, numReps): 01072 """Takes a TimedSwitch type ROS message (pattern and number of repeats), and initializes the pointers.""" 01073 01074 # Copy the rosMsgPattern, which is a tuple, and therefore immutable, 01075 # to an array that we'll be able to modify: 01076 01077 self.origTimePattern = [] 01078 for timeDuration in rosMsgPattern: 01079 self.origTimePattern.append(timeDuration) 01080 01081 # Make a working copy of the time series, so that we can subtract from the 01082 # elements and still have access to the original pattern for repeats: 01083 01084 self.timePattern = self.origTimePattern[:] 01085 self.numReps = numReps 01086 # Make first element the pattern head: 01087 self.patternPt = 0 01088 self.patternSpent = False 01089 self.startOfRepeat = False 01090 01091 def timeRemaining(self): 01092 """Return the time at the pattern head. If pattern is spent, return None instead.""" 01093 01094 if self.patternSpent: 01095 return None 01096 return self.timePattern[self.patternPt] 01097 01098 def resetForRepeat(self): 01099 """Get ready to repeat the pattern. Returns True if another repeat is allowed, else returns False""" 01100 01101 if self.patternSpent: 01102 return False 01103 01104 self.numReps -= 1 01105 if self.numReps <= 0: 01106 return False 01107 01108 # Have at least one repetion of the pattern left: 01109 self.patternPt = 0 01110 01111 # Need to start with a fresh copy of the pattern, b/c we 01112 # may subtracted time from the elements as we went 01113 # through the patters in the previous cycle: 01114 01115 self.timePattern = self.origTimePattern[:] 01116 01117 return True 01118 01119 def reduceTimer(self, time): 01120 """Given a float fractional number of seconds, subtract the time from the pattern head 01121 01122 Returns the remaining time, rounded to 100th of a second, or None. If the 01123 remaining time after subtraction is <= 0, we check whether any repeats 01124 are left. If so, we get ready for another repeat, and return the time of the 01125 first pattern time. Else we return None, indicating that this pattern 01126 is spent. 01127 01128 After this method returns, this instance's public startOfRepeat variable 01129 will hold True or False, depending on whether the pattern is 01130 just starting over. 01131 """ 01132 01133 if self.patternSpent: 01134 return None 01135 01136 res = self.timePattern[self.patternPt] - time 01137 # Guard against weird rounding errors: 01138 if res < 0: 01139 res = 0.0 01140 # Update the head of the pattern: 01141 self.timePattern[self.patternPt] = res 01142 res = round(res,2) 01143 01144 self.startOfRepeat = False 01145 01146 if res <= 0: 01147 01148 self.patternPt += 1 01149 if self.patternPt < len(self.timePattern): 01150 return res 01151 01152 # Repeat the pattern if there's a rep left: 01153 canRepeat = self.resetForRepeat() 01154 01155 if canRepeat: 01156 self.startOfRepeat = True 01157 # Return as next timeout the first delay 01158 # of the pattern: 01159 return self.timePattern[self.patternPt] 01160 else: 01161 # Pattern has finished for good, including all repeats: 01162 self.patternSpent = True 01163 return None 01164 return res 01165 01166 def __repr__(self): 01167 res = "<OutputPattern. Reps:" + str(self.numReps) + ". Pattern: [" 01168 for i in range(min(3, len(self.timePattern))): 01169 res += str(self.timePattern[i]) + ", " 01170 01171 return res + "...]>" 01172 01173 01174 if __name__ == '__main__': 01175 wiimoteNode = WiimoteNode() 01176 try: 01177 wiimoteNode.runWiimoteNode() 01178 01179 except rospy.ROSInterruptException: 01180 rospy.loginfo("ROS Shutdown Request.") 01181 except KeyboardInterrupt, e: 01182 rospy.loginfo("Received keyboard interrupt.") 01183 except WiimoteNotFoundError, e: 01184 rospy.logfatal(str(e)) 01185 except WiimoteEnableError, e: 01186 rospy.logfatal(str(e)) 01187 except CallbackStackMultInstError, e: 01188 rospy.logfatal(str(e)) 01189 except CallbackStackEmptyError, e: 01190 rospy.logfatal(str(e)) 01191 except ResumeNonPausedError, e: 01192 rospy.logfatal(str(e)) 01193 except CallbackStackEmptyError, e: 01194 rospy.logfatal(str(e)) 01195 01196 except: 01197 excType, excValue, excTraceback = sys.exc_info()[:3] 01198 traceback.print_exception(excType, excValue, excTraceback) 01199 finally: 01200 if (wiimoteNode is not None): 01201 wiimoteNode.shutdown() 01202 rospy.loginfo("Exiting Wiimote node.") 01203 sys.exit(0)