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

wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Thu Dec 5 2024 03:18:13