wiimote_node.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 ################################################################################
3 #
4 # File: wiimode_node.py
5 # RCS: $Header: $
6 # Description: Top level ROS node that publishes Wiimote data
7 # and allows Wiimote rumble/LED setting.
8 # Author: Andreas Paepcke
9 # Created: Thu Sep 10 10:31:44 2009
10 # Modified: Fri Jan 14 10:51:11 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com
11 # Language: Python
12 # Package: N/A
13 # Status: Experimental (Do Not Distribute)
14 #
15 # (c) Copyright 2009, Willow Garage, all rights reserved.
16 #
17 ################################################################################
18 #
19 # Revisions:
20 #
21 # Thu Mar 18 10:56:09 2010 (David Lu) davidlu@wustl.edu
22 # Enabled nunchuk message publishing
23 # Fri Oct 29 08:58:21 2010 (Miguel Angel Julian Aguilar, QBO Project)
24 # miguel.angel@thecorpora.com
25 # Enabled classic controller message publishing
26 # Mon Nov 08 11:46:58 2010 (David Lu) davidlu@wustl.edu
27 # Added calibrated nunchuk information, changed /joy to /wiijoy
28 # Only publish on /wiimote/nunchuk if attached
29 # Tue Jul 05, 2011 (Chad Rockey) chadrockey@gmail.com
30 # Removed LED and Rumble Feedback
31 # Added support for sensor_msgs/JoyFeedbackArray
32 ################################################################################
33 #!/usr/bin/env python
34 
35 """The wiimote_node broadcasts four topics, and listens to messages that control
36 the Wiimote stick's rumble (vibration) and LEDs. Transmitted topics (@100Hz):
37 
38  o wiijoy Messages contain the accelerometer and gyro axis data, and all button states.
39  o imu/data Messages contain the accelerometer and gyro axis data, and covariance matrices
40  o wiimote/state the wiijoy and /imu messages, the button states, the LED states,
41  rumble (i.e. vibrator) state, IR light sensor readings, time since last zeroed,
42  and battery state. See State.msg
43  o imu/is_calibrated Latched message
44  o nunchuk Joy messages using the nunchuk as a joystick
45  o classic Joy messages using the nunchuck as a joystic
46 
47 The node listens to the following messages:
48 
49  o joy/set_feedback
50  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.
51  o imu/calibrate
52  Request to calibrate the device.
53 
54 No command line parameters.
55 """
56 
57 # Code structure: The main thread spawns one thread each for the
58 # four message senders, and one thread each for the message listeners.
59 # The respective classes are IMUSender, JoySender, NunSender and WiiSender for
60 # topic sending, and WiimoteListeners for the two message listeners.
61 #
62 # The Wiimote driver is encapsulated in class WIIMote (see WIIMote.py).
63 # That class' singleton instance runs in its own thread, and uses
64 # the third-party cwiid access software.
65 
66 
67 # TODO: Removal of gyro is noticed (covar[0,0]<--1). But software does not notice plugging in.
68 # TODO: Command line option: --no-zeroing
69 
70 # -------- Python Standard Modules:
71 import sys
72 import threading
73 import traceback
74 import time
75 
76 # -------- ROS-Related Modules:
77 #import roslib; roslib.load_manifest('wiimote') # old fuerte code
78 import rospy
79 from geometry_msgs.msg import Vector3
80 from sensor_msgs.msg import Imu
81 from std_srvs.srv import Empty
82 from std_srvs.srv import EmptyResponse
83 from std_msgs.msg import Bool
84 from sensor_msgs.msg import Joy
85 from sensor_msgs.msg import JoyFeedback
86 from sensor_msgs.msg import JoyFeedbackArray
87 from wiimote.msg import IrSourceInfo
88 from wiimote.msg import State
89 
90 # -------- WIIMote Modules:
91 from wiimote.wiimoteExceptions import *
92 from wiimote.wiimoteConstants import *
93 import wiimote.WIIMote
94 import wiimote.wiiutils
95 
96 GATHER_CALIBRATION_STATS = True
97 
98 class WiimoteNode():
99 
100 
101  def runWiimoteNode(self):
102  """Initialize the wiimote_node, establishing its name for communication with the Master"""
103 
104  # All exceptions will end up in the __main__ section
105  # and are handled there:
106 
107  rospy.init_node('wiimote', anonymous=True, log_level=rospy.ERROR) # log_level=rospy.DEBUG
108  wiimoteDevice = wiimote.WIIMote.WIIMote()
109  wiimoteDevice.zeroDevice()
110 
111  try:
112  IMUSender(wiimoteDevice, freq=100).start()
113  JoySender(wiimoteDevice, freq=100).start()
114  WiiSender(wiimoteDevice, freq=100).start()
115  NunSender(wiimoteDevice, freq=100).start()
116  ClasSender(wiimoteDevice, freq=100).start()
117  WiimoteListeners(wiimoteDevice).start()
118 
119  rospy.spin()
120 
121  except:
122  rospy.loginfo("Error in startup")
123  rospy.loginfo(sys.exc_info()[0])
124  finally:
125  try:
126  wiimoteDevice.setRumble(False)
127  wiimoteDevice.setLEDs([False, False, False, False])
128  wiimoteDevice.shutdown()
129  except:
130  pass
131 
132  def shutdown(self):
133  try:
134  IMUSender.stop
135  JoySender.stop
136  WiiSender.stop
137  NunSender.stop
138  WiimoteListener.stop
139  except:
140  pass
141 
142 class WiimoteDataSender(threading.Thread):
143 
144  def __init__(self, wiiMote, freq=100):
145 
146  threading.Thread.__init__(self)
147  self.wiiMote = wiiMote
148  self.freq = freq
149  self.sleepDuration = 1.0 / freq
150 
151  varianceAccelerator = self.wiiMote.getVarianceAccelerator();
152  self.linear_acceleration_covariance = [varianceAccelerator[X], 0., 0.,
153  0., varianceAccelerator[Y], 0.,
154  0., 0., varianceAccelerator[Z]]
155 
156  varianceGyro = self.wiiMote.getVarianceGyro();
157  self.angular_velocity_covariance = [varianceGyro[X], 0., 0.,
158  0., varianceGyro[Y], 0.,
159  0., 0., varianceGyro[Z]]
160 
161  # If no gyro is attached to the Wiimote then we signal
162  # the invalidity of angular rate w/ a covariance matrix
163  # whose first element is -1:
164  self.gyroAbsence_covariance = [-1., 0., 0.,
165  0., 0., 0.,
166  0., 0., 0.]
167 
168  def obtainWiimoteData(self):
169  """Retrieve one set of Wiimote measurements from the Wiimote instance. Return scaled accelerator and gyro readings.
170 
171  We canonicalize both accelerator and gyro data through
172  scaling them by constants that turn them into m/sec^2, and
173  radians/sec, respectively.
174 
175  Return: list of canonicalized accelerator and gyro readings.
176  """
177 
178  while not rospy.is_shutdown():
179  self.wiistate = self.wiiMote.getWiimoteState()
180  if self.wiistate is not None and self.wiistate.acc is not None:
181  break
182  else:
183  rospy.sleep(0.1)
184 
185  return self.canonicalizeWiistate()
186 
187  def canonicalizeWiistate(self):
188  """Scale accelerator, nunchuk accelerator, and gyro readings to be m/sec^2, m/sec^2 and radians/sec, respectively."""
189 
190  try:
191  # Convert acceleration, which is in g's into m/sec^2:
192  canonicalAccel = self.wiistate.acc.scale(EARTH_GRAVITY)
193 
194  # If nunchuk is connected, then
195  # convert nunchuk acceleration into m/sec^2
196  if self.wiistate.nunchukPresent:
197  canonicalNunchukAccel = self.wiistate.nunchukAcc.scale(EARTH_GRAVITY)
198  else:
199  canonicalNunchukAccel = None
200 
201  # If the gyro is connected, then
202  # Convert gyro reading to radians/sec (see wiimoteConstants.py
203  # for origin of this scale factor):
204  if self.wiistate.motionPlusPresent:
205  canonicalAngleRate = self.wiistate.angleRate.scale(GYRO_SCALE_FACTOR)
206  else:
207  canonicalAngleRate = None
208 
209  return [canonicalAccel, canonicalNunchukAccel, canonicalAngleRate]
210  except AttributeError:
211  # An attribute error here occurs when user shuts
212  # off the Wiimote before stopping the wiimote_node:
213  rospy.loginfo(self.threadName + " shutting down.")
214  exit(0)
215 
216 
217 class IMUSender(WiimoteDataSender):
218  """Broadcasting Wiimote accelerator and gyro readings as IMU messages to Topic sensor_data/Imu"""
219 
220  def __init__(self, wiiMote, freq=100):
221  """Initializes the Wiimote IMU publisher.
222 
223  Parameters:
224  wiiMote: a bluetooth-connected, calibrated WIIMote instance
225  freq: the message sending frequency in messages/sec. Max is 100, because
226  the Wiimote only samples the sensors at 100Hz.
227  """
228 
229  WiimoteDataSender.__init__(self, wiiMote, freq)
230 
231  self.pub = rospy.Publisher('imu/data', Imu, queue_size=1)
232 
233  def run(self):
234  """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps.
235 
236  The IMU message, if fully filled in, contains information on orientation,
237  acceleration (in m/s^2), and angular rate (in radians/sec). For each of
238  these quantities, the IMU message format also wants the corresponding
239  covariance matrix.
240 
241  Wiimote only gives us acceleration and angular rate. So we ensure that the orientation
242  data entry is marked invalid. We do this by setting the first
243  entry of its associated covariance matrix to -1. The covariance
244  matrices are the 3x3 matrix with the axes' variance in the
245  diagonal. We obtain the variance from the Wiimote instance.
246  """
247 
248  rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).")
249  self.threadName = "IMU topic Publisher"
250  try:
251  while not rospy.is_shutdown():
252  (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
253 
254  msg = Imu(header=None,
255  orientation=None, # will default to [0.,0.,0.,0],
256  orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.], # -1 indicates that orientation is unknown
257  angular_velocity=None,
258  angular_velocity_covariance=self.angular_velocity_covariance,
259  linear_acceleration=None,
260  linear_acceleration_covariance=self.linear_acceleration_covariance)
261 
262 
263  # If a gyro is plugged into the Wiimote, then note the
264  # angular velocity in the message, else indicate with
265  # the special gyroAbsence_covariance matrix that angular
266  # velocity is unavailable:
267  if self.wiistate.motionPlusPresent:
268  msg.angular_velocity.x = canonicalAngleRate[PHI]
269  msg.angular_velocity.y = canonicalAngleRate[THETA]
270  msg.angular_velocity.z = canonicalAngleRate[PSI]
271  else:
272  msg.angular_velocity_covariance = self.gyroAbsence_covariance
273 
274  msg.linear_acceleration.x = canonicalAccel[X]
275  msg.linear_acceleration.y = canonicalAccel[Y]
276  msg.linear_acceleration.z = canonicalAccel[Z]
277 
278  measureTime = self.wiistate.time
279  timeSecs = int(measureTime)
280  timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
281  msg.header.stamp.secs = timeSecs
282  msg.header.stamp.nsecs = timeNSecs
283 
284  try:
285  self.pub.publish(msg)
286  except rospy.ROSException:
287  rospy.loginfo("Topic imu/data closed. Shutting down Imu sender.")
288  exit(0)
289 
290  #rospy.logdebug("IMU state:")
291  #rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate))
292  rospy.sleep(self.sleepDuration)
293  except rospy.ROSInterruptException:
294  rospy.loginfo("Shutdown request. Shutting down Imu sender.")
295  exit(0)
296 
297 
298 class JoySender(WiimoteDataSender):
299 
300  """Broadcasting Wiimote accelerator and gyro readings as Joy(stick) messages to Topic joy"""
301 
302  def __init__(self, wiiMote, freq=100):
303  """Initializes the Wiimote Joy(stick) publisher.
304 
305  Parameters:
306  wiiMote: a bluetooth-connected, calibrated WIIMote instance
307  freq: the message sending frequency in messages/sec. Max is 100, because
308  the Wiimote only samples the sensors at 100Hz.
309  """
310 
311  WiimoteDataSender.__init__(self, wiiMote, freq)
312 
313 
314  self.pub = rospy.Publisher('joy', Joy, queue_size=1)
315 
316  def run(self):
317  """Loop that obtains the latest wiimote state, publishes the joystick data, and sleeps.
318 
319  The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
320  """
321 
322  rospy.loginfo("Wiimote joystick publisher starting (topic wiijoy).")
323  self.threadName = "Joy topic Publisher"
324  try:
325  while not rospy.is_shutdown():
326  (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
327 
328  msg = Joy(header=None,
329  axes=[canonicalAccel[X], canonicalAccel[Y], canonicalAccel[Z]],
330  buttons=None)
331 
332  # If a gyro is attached to the Wiimote, we add the
333  # gyro information:
334  if self.wiistate.motionPlusPresent:
335  msg.axes.extend([canonicalAngleRate[PHI], canonicalAngleRate[THETA], canonicalAngleRate[PSI]])
336 
337  # Fill in the ROS message's buttons field (there *must* be
338  # a better way in python to declare an array of 11 zeroes...]
339 
340  theButtons = [False,False,False,False,False,False,False,False,False,False,False]
341  theButtons[State.MSG_BTN_1] = self.wiistate.buttons[BTN_1]
342  theButtons[State.MSG_BTN_2] = self.wiistate.buttons[BTN_2]
343  theButtons[State.MSG_BTN_A] = self.wiistate.buttons[BTN_A]
344  theButtons[State.MSG_BTN_B] = self.wiistate.buttons[BTN_B]
345  theButtons[State.MSG_BTN_PLUS] = self.wiistate.buttons[BTN_PLUS]
346  theButtons[State.MSG_BTN_MINUS] = self.wiistate.buttons[BTN_MINUS]
347  theButtons[State.MSG_BTN_LEFT] = self.wiistate.buttons[BTN_LEFT]
348  theButtons[State.MSG_BTN_RIGHT] = self.wiistate.buttons[BTN_RIGHT]
349  theButtons[State.MSG_BTN_UP] = self.wiistate.buttons[BTN_UP]
350  theButtons[State.MSG_BTN_DOWN] = self.wiistate.buttons[BTN_DOWN]
351  theButtons[State.MSG_BTN_HOME] = self.wiistate.buttons[BTN_HOME]
352 
353  msg.buttons = theButtons
354 
355  measureTime = self.wiistate.time
356  timeSecs = int(measureTime)
357  timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
358  # Add the timestamp
359  msg.header.stamp.secs = timeSecs
360  msg.header.stamp.nsecs = timeNSecs
361 
362  try:
363  self.pub.publish(msg)
364  except rospy.ROSException:
365  rospy.loginfo("Topic wiijoy closed. Shutting down Joy sender.")
366  exit(0)
367 
368  #rospy.logdebug("Joystick state:")
369  #rospy.logdebug(" Joy buttons: " + str(theButtons) + "\n Joy accel: " + str(canonicalAccel) + "\n Joy angular rate: " + str(canonicalAngleRate))
370  rospy.sleep(self.sleepDuration)
371  except rospy.ROSInterruptException:
372  rospy.loginfo("Shutdown request. Shutting down Joy sender.")
373  exit(0)
374 
375 class NunSender(WiimoteDataSender):
376 
377  """Broadcasting nunchuk accelerator and joystick readings as Joy(stick) messages to Topic joy"""
378 
379  def __init__(self, wiiMote, freq=100):
380  """Initializes the nunchuk Joy(stick) publisher.
381 
382  Parameters:
383  wiiMote: a bluetooth-connected, calibrated WIIMote instance
384  freq: the message sending frequency in messages/sec. Max is 100, because
385  the Wiimote only samples the sensors at 100Hz.
386  """
387 
388  WiimoteDataSender.__init__(self, wiiMote, freq)
389 
390 
391 
392  # Set 'pub' to none here, and check for none-ness in the
393  # loop below so as not to start this publisher unnecessarily.
394  self.pub = None
395 
396  def run(self):
397  """Loop that obtains the latest nunchuk state, publishes the joystick data, and sleeps.
398 
399  The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
400  """
401 
402  self.threadName = "nunchuk Joy topic Publisher"
403  try:
404  while not rospy.is_shutdown():
405  rospy.sleep(self.sleepDuration)
406  (canonicalAccel, scaledAcc, canonicalAngleRate) = self.obtainWiimoteData()
407  if not self.wiistate.nunchukPresent:
408  continue
409  if self.pub is None:
410  self.pub = rospy.Publisher('/wiimote/nunchuk', Joy, queue_size=1)
411  rospy.loginfo("Wiimote Nunchuk joystick publisher starting (topic nunchuk).")
412 
413  (joyx, joyy) = self.wiistate.nunchukStick
414 
415  msg = Joy(header=None,
416  axes=[joyx, joyy,
417  scaledAcc[X], scaledAcc[Y], scaledAcc[Z]],
418  buttons=None)
419 
420  theButtons = [False,False]
421  theButtons[State.MSG_BTN_Z] = self.wiistate.nunchukButtons[BTN_Z]
422  theButtons[State.MSG_BTN_C] = self.wiistate.nunchukButtons[BTN_C]
423 
424  msg.buttons = theButtons
425 
426  measureTime = self.wiistate.time
427  timeSecs = int(measureTime)
428  timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
429  msg.header.stamp.secs = timeSecs
430  msg.header.stamp.nsecs = timeNSecs
431 
432  try:
433  self.pub.publish(msg)
434  except rospy.ROSException:
435  rospy.loginfo("Topic /wiimote/nunchuk closed. Shutting down Nun sender.")
436  exit(0)
437 
438  #rospy.logdebug("nunchuk state:")
439  #rospy.logdebug(" nunchuk buttons: " + str(theButtons) + "\n Nuchuck axes: " + str(msg.axes) + "\n")
440 
441  except rospy.ROSInterruptException:
442  rospy.loginfo("Shutdown request. Shutting down Nun sender.")
443  exit(0)
444 
445 class ClasSender(WiimoteDataSender):
446 
447  """Broadcasting Classic Controller joystick readings as Joy(stick) messages to Topic joy"""
448 
449  def __init__(self, wiiMote, freq=100):
450  """Initializes the Classic Controller Joy(stick) publisher.
451 
452  Parameters:
453  wiiMote: a bluetooth-connected, calibrated WIIMote instance
454  freq: the message sending frequency in messages/sec. Max is 100, because
455  the Wiimote only samples the sensors at 100Hz.
456  """
457 
458  WiimoteDataSender.__init__(self, wiiMote, freq)
459 
460  # Set 'pub' to none here, and check for none-ness in the
461  # loop below so as not to start this publisher unnecessarily.
462  self.pub = None
463 
464  def run(self):
465  """Loop that obtains the latest classic controller state, publishes the joystick data, and sleeps.
466 
467  The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
468  """
469 
470  self.threadName = "Classic Controller Joy topic Publisher"
471  try:
472  while not rospy.is_shutdown():
473  rospy.sleep(self.sleepDuration)
474  self.obtainWiimoteData()
475 
476  if not self.wiistate.classicPresent:
477  continue
478  if self.pub is None:
479  self.pub = rospy.Publisher('/wiimote/classic', Joy)
480  rospy.loginfo("Wiimote Classic Controller joystick publisher starting (topic /wiimote/classic).")
481 
482  (l_joyx, l_joyy) = self.wiistate.classicStickLeft
483  (r_joyx, r_joyy) = self.wiistate.classicStickRight
484  # scale the joystick to [-1, 1]
485  l_joyx = -(l_joyx-33)/27.
486  l_joyy = (l_joyy-33)/27.
487  r_joyx = -(r_joyx-15)/13.
488  r_joyy = (r_joyy-15)/13.
489  # create a deadzone in the middle
490  if abs(l_joyx) < .05:
491  l_joyx = 0
492  if abs(l_joyy) < .05:
493  l_joyy = 0
494  if abs(r_joyx) < .05:
495  r_joyx = 0
496  if abs(r_joyy) < .05:
497  r_joyy = 0
498 
499  msg = Joy(header=None,
500  axes=[l_joyx, l_joyy,r_joyx, r_joyy],
501  buttons=None)
502 
503  theButtons = [False,False,False,False,False,False,False,False,False,False,False,False,False,False,False]
504  theButtons[State.MSG_CLASSIC_BTN_X] = self.wiistate.classicButtons[CLASSIC_BTN_X]
505  theButtons[State.MSG_CLASSIC_BTN_Y] = self.wiistate.classicButtons[CLASSIC_BTN_Y]
506  theButtons[State.MSG_CLASSIC_BTN_A] = self.wiistate.classicButtons[CLASSIC_BTN_A]
507  theButtons[State.MSG_CLASSIC_BTN_B] = self.wiistate.classicButtons[CLASSIC_BTN_B]
508  theButtons[State.MSG_CLASSIC_BTN_PLUS] = self.wiistate.classicButtons[CLASSIC_BTN_PLUS]
509  theButtons[State.MSG_CLASSIC_BTN_MINUS] = self.wiistate.classicButtons[CLASSIC_BTN_MINUS]
510  theButtons[State.MSG_CLASSIC_BTN_LEFT] = self.wiistate.classicButtons[CLASSIC_BTN_LEFT]
511  theButtons[State.MSG_CLASSIC_BTN_RIGHT] = self.wiistate.classicButtons[CLASSIC_BTN_RIGHT]
512  theButtons[State.MSG_CLASSIC_BTN_UP] = self.wiistate.classicButtons[CLASSIC_BTN_UP]
513  theButtons[State.MSG_CLASSIC_BTN_DOWN] = self.wiistate.classicButtons[CLASSIC_BTN_DOWN]
514  theButtons[State.MSG_CLASSIC_BTN_HOME] = self.wiistate.classicButtons[CLASSIC_BTN_HOME]
515  theButtons[State.MSG_CLASSIC_BTN_L] = self.wiistate.classicButtons[CLASSIC_BTN_L]
516  theButtons[State.MSG_CLASSIC_BTN_R] = self.wiistate.classicButtons[CLASSIC_BTN_R]
517  theButtons[State.MSG_CLASSIC_BTN_ZL] = self.wiistate.classicButtons[CLASSIC_BTN_ZL]
518  theButtons[State.MSG_CLASSIC_BTN_ZR] = self.wiistate.classicButtons[CLASSIC_BTN_ZR]
519 
520  msg.buttons = theButtons
521 
522  measureTime = self.wiistate.time
523  timeSecs = int(measureTime)
524  timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
525  msg.header.stamp.secs = timeSecs
526  msg.header.stamp.nsecs = timeNSecs
527 
528  try:
529  self.pub.publish(msg)
530  except rospy.ROSException:
531  rospy.loginfo("Topic /wiimote/classic closed. Shutting down Clas sender.")
532  exit(0)
533 
534  #rospy.logdebug("Classic Controller state:")
535  #rospy.logdebug(" Classic Controller buttons: " + str(theButtons) + "\n Classic Controller axes: " + str(msg.axes) + "\n")
536 
537  except rospy.ROSInterruptException:
538  rospy.loginfo("Shutdown request. Shutting down Clas sender.")
539  exit(0)
540 
541 
542 class WiiSender(WiimoteDataSender):
543  """Broadcasting complete Wiimote messages to Topic wiimote"""
544 
545  def __init__(self, wiiMote, freq=100):
546  """Initializes the full-Wiimote publisher.
547 
548  Parameters:
549  wiiMote: a bluetooth-connected, calibrated WIIMote instance
550  freq: the message sending frequency in messages/sec. Max is 100, because
551  the Wiimote only samples the sensors at 100Hz.
552  """
553 
554  WiimoteDataSender.__init__(self, wiiMote, freq)
555 
556  self.pub = rospy.Publisher('/wiimote/state', State, queue_size=1)
557 
558  def run(self):
559  """Loop that obtains the latest wiimote state, publishes the data, and sleeps.
560 
561  The wiimote message, if fully filled in, contains information in common with Imu.msg:
562  acceleration (in m/s^2), and angular rate (in radians/sec). For each of
563  these quantities, the IMU message format also wants the corresponding
564  covariance matrix.
565 
566  The covariance matrices are the 3x3 matrix with the axes' variance in the
567  diagonal. We obtain the variance from the Wiimote instance.
568  """
569 
570  rospy.loginfo("Wiimote state publisher starting (topic /wiimote/state).")
571  self.threadName = "Wiimote topic Publisher"
572  try:
573  while not rospy.is_shutdown():
574  rospy.sleep(self.sleepDuration)
575  (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
576 
577  zeroingTimeSecs = int(self.wiiMote.lastZeroingTime)
578  zeroingTimeNSecs = int((self.wiiMote.lastZeroingTime - zeroingTimeSecs) * 10**9)
579  msg = State(header=None,
580  angular_velocity_zeroed=None,
581  angular_velocity_raw=None,
582  angular_velocity_covariance=self.angular_velocity_covariance,
583  linear_acceleration_zeroed=None,
584  linear_acceleration_raw=None,
585  linear_acceleration_covariance=self.linear_acceleration_covariance,
586  nunchuk_acceleration_zeroed=None,
587  nunchuk_acceleration_raw=None,
588  nunchuk_joystick_zeroed=None,
589  nunchuk_joystick_raw=None,
590  buttons=[False,False,False,False,False,False,False,False,False,False],
591  nunchuk_buttons=[False,False],
592  rumble=False,
593  LEDs=None,
594  ir_tracking = None,
595  raw_battery=None,
596  percent_battery=None,
597  zeroing_time=rospy.Time(zeroingTimeSecs, zeroingTimeNSecs),
598  errors=0)
599 
600  # If a gyro is plugged into the Wiimote, then note the
601  # angular velocity in the message, else indicate with
602  # the special gyroAbsence_covariance matrix that angular
603  # velocity is unavailable:
604  if self.wiistate.motionPlusPresent:
605  msg.angular_velocity_zeroed.x = canonicalAngleRate[PHI]
606  msg.angular_velocity_zeroed.y = canonicalAngleRate[THETA]
607  msg.angular_velocity_zeroed.z = canonicalAngleRate[PSI]
608 
609  msg.angular_velocity_raw.x = self.wiistate.angleRateRaw[PHI]
610  msg.angular_velocity_raw.y = self.wiistate.angleRateRaw[THETA]
611  msg.angular_velocity_raw.z = self.wiistate.angleRateRaw[PSI]
612 
613  else:
614  msg.angular_velocity_covariance = self.gyroAbsence_covariance
615 
616  msg.linear_acceleration_zeroed.x = canonicalAccel[X]
617  msg.linear_acceleration_zeroed.y = canonicalAccel[Y]
618  msg.linear_acceleration_zeroed.z = canonicalAccel[Z]
619 
620  msg.linear_acceleration_raw.x = self.wiistate.accRaw[X]
621  msg.linear_acceleration_raw.y = self.wiistate.accRaw[Y]
622  msg.linear_acceleration_raw.z = self.wiistate.accRaw[Z]
623 
624  if self.wiistate.nunchukPresent:
625  msg.nunchuk_acceleration_zeroed.x = canonicalNunchukAccel[X]
626  msg.nunchuk_acceleration_zeroed.y = canonicalNunchukAccel[Y]
627  msg.nunchuk_acceleration_zeroed.z = canonicalNunchukAccel[Z]
628 
629  msg.nunchuk_acceleration_raw.x = self.wiistate.nunchukAccRaw[X]
630  msg.nunchuk_acceleration_raw.y = self.wiistate.nunchukAccRaw[Y]
631  msg.nunchuk_acceleration_raw.z = self.wiistate.nunchukAccRaw[Z]
632 
633  msg.nunchuk_joystick_zeroed = self.wiistate.nunchukStick
634  msg.nunchuk_joystick_raw = self.wiistate.nunchukStickRaw
635  moreButtons = []
636  moreButtons.append(self.wiistate.nunchukButtons[BTN_Z])
637  moreButtons.append(self.wiistate.nunchukButtons[BTN_C])
638  msg.nunchuk_buttons = moreButtons
639 
640  theButtons = []
641  theButtons.append(self.wiistate.buttons[BTN_1])
642  theButtons.append(self.wiistate.buttons[BTN_2])
643  theButtons.append(self.wiistate.buttons[BTN_PLUS])
644  theButtons.append(self.wiistate.buttons[BTN_MINUS])
645  theButtons.append(self.wiistate.buttons[BTN_A])
646  theButtons.append(self.wiistate.buttons[BTN_B])
647  theButtons.append(self.wiistate.buttons[BTN_UP])
648  theButtons.append(self.wiistate.buttons[BTN_DOWN])
649  theButtons.append(self.wiistate.buttons[BTN_LEFT])
650  theButtons.append(self.wiistate.buttons[BTN_RIGHT])
651  theButtons.append(self.wiistate.buttons[BTN_HOME])
652 
653  ledStates = self.wiiMote.getLEDs()
654  for indx in range(len(msg.LEDs)):
655  if ledStates[indx]:
656  msg.LEDs[indx] = True
657  else:
658  msg.LEDs[indx] = False
659 
660  msg.buttons = theButtons
661 
662  msg.raw_battery = self.wiiMote.getBattery()
663  msg.percent_battery = msg.raw_battery * 100./self.wiiMote.BATTERY_MAX
664 
665  irSources = self.wiistate.IRSources
666 
667  for irSensorIndx in range(NUM_IR_SENSORS):
668  if irSources[irSensorIndx] is not None:
669  # Did hardware deliver IR source position for this IR sensor?
670  try:
671  pos = irSources[irSensorIndx]['pos']
672  except KeyError:
673  # If no position information from this IR sensor, use INVALID for the dimensions:
674  msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID))
675  # The following else is unusual: its statements are bypassed is except clause had control:
676  else:
677  # Have IR position info from this IR sensor. We use the IR_source_info
678  # message type. Get size (intensity?):
679  try:
680  size = irSources[irSensorIndx]['size']
681  except KeyError:
682  # If the driver did not deliver size information, indicate by using INVALID:
683  size = State.INVALID
684  lightInfo = IrSourceInfo(pos[0], pos[1], size)
685  msg.ir_tracking.append(lightInfo)
686  else:
687  msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID))
688 
689  measureTime = self.wiistate.time
690  timeSecs = int(measureTime)
691  timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
692  msg.header.stamp.secs = timeSecs
693  msg.header.stamp.nsecs = timeNSecs
694 
695  try:
696  self.pub.publish(msg)
697  except rospy.ROSException:
698  rospy.loginfo("Topic /wiimote/state closed. Shutting down Wiimote sender.")
699  exit(0)
700 
701  #rospy.logdebug("Wiimote state:")
702  #rospy.logdebug(" Accel: " + str(canonicalAccel) + "\n Angular rate: " + str(canonicalAngleRate))
703  #rospy.logdebug(" Rumble: " + str(msg.rumble) + "\n Battery: [" + str(msg.raw_battery) + "," + str(msg.percent_battery))
704  #rospy.logdebug(" IR positions: " + str(msg.ir_tracking))
705 
706 
707  except rospy.ROSInterruptException:
708  rospy.loginfo("Shutdown request. Shutting down Wiimote sender.")
709  exit(0)
710 
711 class WiimoteListeners(threading.Thread):
712  """Listen for request to rumble and LED blinking.
713  """
714 
715  def __init__(self, wiiMote):
716 
717  threading.Thread.__init__(self)
718  self.wiiMote = wiiMote
719 
720  self.ledCommands = [False, False, False, False]
721  self.rumbleCommand = False
722 
723  # Even though this thread mostly listens,
724  # we do publish the is_calibrated() message
725  # here, because this msg is so closely related
726  # to the calibrate() service:
727  self.is_calibratedPublisher = rospy.Publisher('/imu/is_calibrated', Bool, latch=True, queue_size=1)
728  # We'll always just reuse this msg object:
729  self.is_CalibratedResponseMsg = Bool();
730 
731  # Initialize the latched is_calibrated state. We use
732  # the result of the initial zeroing, when the services
733  # were first started and the the user was asked to
734  # push the two buttons for pairing:
735 
736  self.is_CalibratedResponseMsg.data = self.wiiMote.latestCalibrationSuccessful;
737  self.is_calibratedPublisher.publish(self.is_CalibratedResponseMsg)
738 
739  def run(self):
740 
741  def feedbackCallback(msg):
742  """The callback for handle the feedback array messages and sending that to the Wiimote"""
743  for fb in msg.array:
744  if fb.type == JoyFeedback.TYPE_LED:
745  try:
746  if fb.intensity >= 0.5:
747  self.ledCommands[fb.id] = True
748  else:
749  self.ledCommands[fb.id] = False
750  except:
751  rospy.logwarn("LED ID out of bounds, ignoring!")
752  elif fb.type == JoyFeedback.TYPE_RUMBLE:
753  if fb.id == 0:
754  if fb.intensity >= 0.5:
755  self.rumbleCommand = True
756  else:
757  self.rumbleCommand = False
758  else:
759  rospy.logwarn("RUMBLE ID out of bounds, ignoring!")
760 
761  self.wiiMote.setLEDs(self.ledCommands)
762  self.wiiMote.setRumble(self.rumbleCommand)
763 
764 
765  return
766 
767 
768  def calibrateCallback(req):
769  """The imu/calibrate service handler."""
770 
771  rospy.loginfo("Calibration request")
772 
773  calibrationSuccess = self.wiiMote.zeroDevice()
774 
775  # Update the latched is_calibrated state:
776 
777  self.is_CalibratedResponseMsg.data = calibrationSuccess
778  self.is_calibratedPublisher.publish(self.is_CalibratedResponseMsg)
779 
780  return EmptyResponse()
781 
782  # Done with embedded function definitions. Back at the top
783  # level of WiimoteListeners' run() function.
784 
785  # Subscribe to rumble and LED control messages and sit:
786  rospy.loginfo("Wiimote feedback listener starting (topic /joy/set_feedback).")
787  rospy.Subscriber("joy/set_feedback", JoyFeedbackArray, feedbackCallback)
788  rospy.loginfo("Wiimote calibration service starting (topic /imu/calibrate).")
789  rospy.Service("imu/calibrate", Empty, calibrateCallback)
790  rospy.loginfo("Wiimote latched is_calibrated publisher starting (topic /imu/is_calibrated).")
791 
792  try:
793  rospy.spin()
794  except rospy.ROSInterruptException:
795  rospy.loginfo("Shutdown request. Shutting down Wiimote listeners.")
796  exit(0)
797 
798 
799 if __name__ == '__main__':
800  wiimoteNode = WiimoteNode()
801  try:
802  wiimoteNode.runWiimoteNode()
803 
804  except rospy.ROSInterruptException:
805  rospy.loginfo("ROS Shutdown Request.")
806  except KeyboardInterrupt, e:
807  rospy.loginfo("Received keyboard interrupt.")
808  except WiimoteNotFoundError, e:
809  rospy.logfatal(str(e))
810  except WiimoteEnableError, e:
811  rospy.logfatal(str(e))
812  except CallbackStackMultInstError, e:
813  rospy.logfatal(str(e))
814  except CallbackStackEmptyError, e:
815  rospy.logfatal(str(e))
816  except ResumeNonPausedError, e:
817  rospy.logfatal(str(e))
818  except CallbackStackEmptyError, e:
819  rospy.logfatal(str(e))
820 
821  except:
822  excType, excValue, excTraceback = sys.exc_info()[:3]
823  traceback.print_exception(excType, excValue, excTraceback)
824  finally:
825  if (wiimoteNode is not None):
826  wiimoteNode.shutdown()
827  rospy.loginfo("Exiting Wiimote node.")
828  sys.exit(0)
wiimote_node.IMUSender.__init__
def __init__(self, wiiMote, freq=100)
Definition: wiimote_node.py:220
wiimote_node.WiimoteListeners.run
def run(self)
Definition: wiimote_node.py:739
wiimote_node.JoySender.run
def run(self)
Definition: wiimote_node.py:316
wiimote_node.WiimoteNode.shutdown
def shutdown(self)
Definition: wiimote_node.py:132
wiimote_node.ClasSender
Definition: wiimote_node.py:445
wiimote_node.WiimoteDataSender
Definition: wiimote_node.py:142
wiimote_node.JoySender
Definition: wiimote_node.py:298
wiimote_node.ClasSender.run
def run(self)
Definition: wiimote_node.py:464
wiimote_node.IMUSender.threadName
threadName
Definition: wiimote_node.py:249
wiimote_node.ClasSender.__init__
def __init__(self, wiiMote, freq=100)
Definition: wiimote_node.py:449
wiimote_node.WiimoteListeners.ledCommands
ledCommands
Definition: wiimote_node.py:720
wiimote_node.ClasSender.pub
pub
Definition: wiimote_node.py:462
wiimote_node.IMUSender.pub
pub
Definition: wiimote_node.py:231
wiimote_node.WiimoteNode
Definition: wiimote_node.py:98
wiimote_node.WiimoteDataSender.__init__
def __init__(self, wiiMote, freq=100)
Definition: wiimote_node.py:144
wiimote_node.WiimoteDataSender.gyroAbsence_covariance
gyroAbsence_covariance
Definition: wiimote_node.py:164
wiimote_node.JoySender.threadName
threadName
Definition: wiimote_node.py:323
wiimote_node.WiiSender.__init__
def __init__(self, wiiMote, freq=100)
Definition: wiimote_node.py:545
wiimote_node.WiimoteDataSender.obtainWiimoteData
def obtainWiimoteData(self)
Definition: wiimote_node.py:168
wiimote_node.WiiSender.threadName
threadName
Definition: wiimote_node.py:571
wiimote_node.NunSender.pub
pub
Definition: wiimote_node.py:394
wiimote_node.WiimoteListeners.__init__
def __init__(self, wiiMote)
Definition: wiimote_node.py:715
wiimote_node.IMUSender
Definition: wiimote_node.py:217
wiimote_node.WiimoteDataSender.sleepDuration
sleepDuration
Definition: wiimote_node.py:149
wiimote_node.WiimoteListeners.is_CalibratedResponseMsg
is_CalibratedResponseMsg
Definition: wiimote_node.py:729
wiimote_node.WiimoteListeners
Definition: wiimote_node.py:711
wiimote_node.WiimoteDataSender.wiiMote
wiiMote
Definition: wiimote_node.py:147
wiimote_node.WiimoteNode.runWiimoteNode
def runWiimoteNode(self)
Definition: wiimote_node.py:101
wiimote_node.WiimoteDataSender.linear_acceleration_covariance
linear_acceleration_covariance
Definition: wiimote_node.py:152
wiimote_node.WiiSender.run
def run(self)
Definition: wiimote_node.py:558
wiimote_node.NunSender.threadName
threadName
Definition: wiimote_node.py:402
wiimote_node.WiimoteDataSender.angular_velocity_covariance
angular_velocity_covariance
Definition: wiimote_node.py:157
wiimote_node.NunSender.__init__
def __init__(self, wiiMote, freq=100)
Definition: wiimote_node.py:379
wiimote_node.WiimoteListeners.wiiMote
wiiMote
Definition: wiimote_node.py:718
wiimote_node.WiimoteDataSender.wiistate
wiistate
Definition: wiimote_node.py:179
wiimote_node.IMUSender.run
def run(self)
Definition: wiimote_node.py:233
wiimote_node.WiimoteDataSender.canonicalizeWiistate
def canonicalizeWiistate(self)
Definition: wiimote_node.py:187
wiimote_node.NunSender
Definition: wiimote_node.py:375
wiimote_node.JoySender.__init__
def __init__(self, wiiMote, freq=100)
Definition: wiimote_node.py:302
wiimote_node.JoySender.pub
pub
Definition: wiimote_node.py:314
wiimote_node.NunSender.run
def run(self)
Definition: wiimote_node.py:396
wiimote_node.WiiSender.pub
pub
Definition: wiimote_node.py:556
wiimote_node.WiimoteDataSender.freq
freq
Definition: wiimote_node.py:148
wiimote_node.WiiSender
Definition: wiimote_node.py:542
wiimote_node.WiimoteListeners.is_calibratedPublisher
is_calibratedPublisher
Definition: wiimote_node.py:727
wiimote_node.WiimoteListeners.rumbleCommand
rumbleCommand
Definition: wiimote_node.py:721

wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Fri Jun 7 2019 22:01:33