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

wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Mon Feb 28 2022 22:37:04