def __init__(self, events_pub, routes_pub, feedback_pub, imu_calibrate, stale_handler, mvp):
self.events_pub = events_pub
self.routes_pub = routes_pub
self.feedback_pub = feedback_pub
self.imu_calibrate = imu_calibrate
self.stale_handler = stale_handler
self.mvp = mvp
self.last_state = State()
self.last_vp = ''
self.stale_count = 0
self.z = 0
self.x = 0
self.y = 0
self.vz = 0
self.vx = 0
self.vy = 0
self.was_clicking = False
self._lock = Lock()
def run(self):
"""Loop that obtains the latest wiimote state, publishes the data, and sleeps.
The wiimote message, if fully filled in, contains information in common with Imu.msg:
acceleration (in m/s^2), and angular rate (in radians/sec). For each of
these quantities, the IMU message format also wants the corresponding
covariance matrix.
The covariance matrices are the 3x3 matrix with the axes' variance in the
diagonal. We obtain the variance from the Wiimote instance.
"""
rospy.loginfo(
"Wiimote state publisher starting (topic /wiimote/state).")
self.threadName = "Wiimote topic Publisher"
try:
while not rospy.is_shutdown():
rospy.sleep(self.sleepDuration)
(canonicalAccel, canonicalNunchukAccel,
canonicalAngleRate) = self.obtainWiimoteData()
zeroingTimeSecs = int(self.wiiMote.lastZeroingTime)
zeroingTimeNSecs = int(
(self.wiiMote.lastZeroingTime - zeroingTimeSecs) * 10**9)
msg = State(header=None,
angular_velocity_zeroed=None,
angular_velocity_raw=None,
angular_velocity_covariance=self.
angular_velocity_covariance,
linear_acceleration_zeroed=None,
linear_acceleration_raw=None,
linear_acceleration_covariance=self.
linear_acceleration_covariance,
nunchuk_acceleration_zeroed=None,
nunchuk_acceleration_raw=None,
nunchuk_joystick_zeroed=None,
nunchuk_joystick_raw=None,
buttons=[
False, False, False, False, False, False,
False, False, False, False
],
nunchuk_buttons=[False, False],
rumble=False,
LEDs=None,
ir_tracking=None,
raw_battery=None,
percent_battery=None,
zeroing_time=rospy.Time(zeroingTimeSecs,
zeroingTimeNSecs),
errors=0)
# If a gyro is plugged into the Wiimote, then note the
# angular velocity in the message, else indicate with
# the special gyroAbsence_covariance matrix that angular
# velocity is unavailable:
if self.wiistate.motionPlusPresent:
msg.angular_velocity_zeroed.x = canonicalAngleRate[PHI]
msg.angular_velocity_zeroed.y = canonicalAngleRate[THETA]
msg.angular_velocity_zeroed.z = canonicalAngleRate[PSI]
msg.angular_velocity_raw.x = self.wiistate.angleRateRaw[
PHI]
msg.angular_velocity_raw.y = self.wiistate.angleRateRaw[
THETA]
msg.angular_velocity_raw.z = self.wiistate.angleRateRaw[
PSI]
else:
msg.angular_velocity_covariance = self.gyroAbsence_covariance
msg.linear_acceleration_zeroed.x = canonicalAccel[X]
msg.linear_acceleration_zeroed.y = canonicalAccel[Y]
msg.linear_acceleration_zeroed.z = canonicalAccel[Z]
msg.linear_acceleration_raw.x = self.wiistate.accRaw[X]
msg.linear_acceleration_raw.y = self.wiistate.accRaw[Y]
msg.linear_acceleration_raw.z = self.wiistate.accRaw[Z]
if self.wiistate.nunchukPresent:
msg.nunchuk_acceleration_zeroed.x = canonicalNunchukAccel[
X]
msg.nunchuk_acceleration_zeroed.y = canonicalNunchukAccel[
Y]
msg.nunchuk_acceleration_zeroed.z = canonicalNunchukAccel[
Z]
msg.nunchuk_acceleration_raw.x = self.wiistate.nunchukAccRaw[
X]
msg.nunchuk_acceleration_raw.y = self.wiistate.nunchukAccRaw[
Y]
msg.nunchuk_acceleration_raw.z = self.wiistate.nunchukAccRaw[
Z]
msg.nunchuk_joystick_zeroed = self.wiistate.nunchukStick
msg.nunchuk_joystick_raw = self.wiistate.nunchukStickRaw
moreButtons = []
moreButtons.append(self.wiistate.nunchukButtons[BTN_Z])
moreButtons.append(self.wiistate.nunchukButtons[BTN_C])
msg.nunchuk_buttons = moreButtons
theButtons = []
theButtons.append(self.wiistate.buttons[BTN_1])
theButtons.append(self.wiistate.buttons[BTN_2])
theButtons.append(self.wiistate.buttons[BTN_PLUS])
theButtons.append(self.wiistate.buttons[BTN_MINUS])
theButtons.append(self.wiistate.buttons[BTN_A])
theButtons.append(self.wiistate.buttons[BTN_B])
theButtons.append(self.wiistate.buttons[BTN_UP])
theButtons.append(self.wiistate.buttons[BTN_DOWN])
theButtons.append(self.wiistate.buttons[BTN_LEFT])
theButtons.append(self.wiistate.buttons[BTN_RIGHT])
theButtons.append(self.wiistate.buttons[BTN_HOME])
ledStates = self.wiiMote.getLEDs()
for indx in range(len(msg.LEDs)):
if ledStates[indx]:
msg.LEDs[indx] = True
else:
msg.LEDs[indx] = False
msg.buttons = theButtons
msg.raw_battery = self.wiiMote.getBattery()
msg.percent_battery = msg.raw_battery * 100. / self.wiiMote.BATTERY_MAX
irSources = self.wiistate.IRSources
for irSensorIndx in range(NUM_IR_SENSORS):
if irSources[irSensorIndx] is not None:
# Did hardware deliver IR source position for this IR sensor?
try:
pos = irSources[irSensorIndx]['pos']
except KeyError:
# If no position information from this IR sensor, use INVALID for the dimensions:
msg.ir_tracking.append(
IrSourceInfo(State.INVALID_FLOAT,
State.INVALID_FLOAT,
State.INVALID))
# The following else is unusual: its statements are bypassed is except clause had control:
else:
# Have IR position info from this IR sensor. We use the IR_source_info
# message type. Get size (intensity?):
try:
size = irSources[irSensorIndx]['size']
except KeyError:
# If the driver did not deliver size information, indicate by using INVALID:
size = State.INVALID
lightInfo = IrSourceInfo(pos[0], pos[1], size)
msg.ir_tracking.append(lightInfo)
else:
msg.ir_tracking.append(
IrSourceInfo(State.INVALID_FLOAT,
State.INVALID_FLOAT, State.INVALID))
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
try:
self.pub.publish(msg)
except rospy.ROSException:
rospy.loginfo(
"Topic /wiimote/state closed. Shutting down Wiimote sender."
)
exit(0)
#rospy.logdebug("Wiimote state:")
#rospy.logdebug(" Accel: " + str(canonicalAccel) + "\n Angular rate: " + str(canonicalAngleRate))
#rospy.logdebug(" Rumble: " + str(msg.rumble) + "\n Battery: [" + str(msg.raw_battery) + "," + str(msg.percent_battery))
#rospy.logdebug(" IR positions: " + str(msg.ir_tracking))
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Wiimote sender.")
exit(0)
def run(self):
"""Loop that obtains the latest wiimote state, publishes the data, and sleeps.
The wiimote message, if fully filled in, contains information in common with Imu.msg:
acceleration (in m/s^2), and angular rate (in radians/sec). For each of
these quantities, the IMU message format also wants the corresponding
covariance matrix.
The covariance matrices are the 3x3 matrix with the axes' variance in the
diagonal. We obtain the variance from the Wiimote instance.
"""
rospy.loginfo("Wiimote state publisher starting (topic /wiimote/state).")
self.threadName = "Wiimote topic Publisher"
try:
while not rospy.is_shutdown():
rospy.sleep(self.sleepDuration)
(canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
zeroingTimeSecs = int(self.wiiMote.lastZeroingTime)
zeroingTimeNSecs = int((self.wiiMote.lastZeroingTime - zeroingTimeSecs) * 10**9)
msg = State(header=None,
angular_velocity_zeroed=None,
angular_velocity_raw=None,
angular_velocity_covariance=self.angular_velocity_covariance,
linear_acceleration_zeroed=None,
linear_acceleration_raw=None,
linear_acceleration_covariance=self.linear_acceleration_covariance,
nunchuk_acceleration_zeroed=None,
nunchuk_acceleration_raw=None,
nunchuk_joystick_zeroed=None,
nunchuk_joystick_raw=None,
buttons=[False,False,False,False,False,False,False,False,False,False],
nunchuk_buttons=[False,False],
rumble=False,
LEDs=None,
ir_tracking = None,
raw_battery=None,
percent_battery=None,
zeroing_time=rospy.Time(zeroingTimeSecs, zeroingTimeNSecs),
errors=0)
# If a gyro is plugged into the Wiimote, then note the
# angular velocity in the message, else indicate with
# the special gyroAbsence_covariance matrix that angular
# velocity is unavailable:
if self.wiistate.motionPlusPresent:
msg.angular_velocity_zeroed.x = canonicalAngleRate[PHI]
msg.angular_velocity_zeroed.y = canonicalAngleRate[THETA]
msg.angular_velocity_zeroed.z = canonicalAngleRate[PSI]
msg.angular_velocity_raw.x = self.wiistate.angleRateRaw[PHI]
msg.angular_velocity_raw.y = self.wiistate.angleRateRaw[THETA]
msg.angular_velocity_raw.z = self.wiistate.angleRateRaw[PSI]
else:
msg.angular_velocity_covariance = self.gyroAbsence_covariance
msg.linear_acceleration_zeroed.x = canonicalAccel[X]
msg.linear_acceleration_zeroed.y = canonicalAccel[Y]
msg.linear_acceleration_zeroed.z = canonicalAccel[Z]
msg.linear_acceleration_raw.x = self.wiistate.accRaw[X]
msg.linear_acceleration_raw.y = self.wiistate.accRaw[Y]
msg.linear_acceleration_raw.z = self.wiistate.accRaw[Z]
if self.wiistate.nunchukPresent:
msg.nunchuk_acceleration_zeroed.x = canonicalNunchukAccel[X]
msg.nunchuk_acceleration_zeroed.y = canonicalNunchukAccel[Y]
msg.nunchuk_acceleration_zeroed.z = canonicalNunchukAccel[Z]
msg.nunchuk_acceleration_raw.x = self.wiistate.nunchukAccRaw[X]
msg.nunchuk_acceleration_raw.y = self.wiistate.nunchukAccRaw[Y]
msg.nunchuk_acceleration_raw.z = self.wiistate.nunchukAccRaw[Z]
msg.nunchuk_joystick_zeroed = self.wiistate.nunchukStick
msg.nunchuk_joystick_raw = self.wiistate.nunchukStickRaw
moreButtons = []
moreButtons.append(self.wiistate.nunchukButtons[BTN_Z])
moreButtons.append(self.wiistate.nunchukButtons[BTN_C])
msg.nunchuk_buttons = moreButtons
theButtons = []
theButtons.append(self.wiistate.buttons[BTN_1])
theButtons.append(self.wiistate.buttons[BTN_2])
theButtons.append(self.wiistate.buttons[BTN_PLUS])
theButtons.append(self.wiistate.buttons[BTN_MINUS])
theButtons.append(self.wiistate.buttons[BTN_A])
theButtons.append(self.wiistate.buttons[BTN_B])
theButtons.append(self.wiistate.buttons[BTN_UP])
theButtons.append(self.wiistate.buttons[BTN_DOWN])
theButtons.append(self.wiistate.buttons[BTN_LEFT])
theButtons.append(self.wiistate.buttons[BTN_RIGHT])
theButtons.append(self.wiistate.buttons[BTN_HOME])
ledStates = self.wiiMote.getLEDs()
for indx in range(len(msg.LEDs)):
if ledStates[indx]:
msg.LEDs[indx] = True
else:
msg.LEDs[indx] = False
msg.buttons = theButtons
msg.raw_battery = self.wiiMote.getBattery()
msg.percent_battery = msg.raw_battery * 100./self.wiiMote.BATTERY_MAX
irSources = self.wiistate.IRSources
for irSensorIndx in range(NUM_IR_SENSORS):
if irSources[irSensorIndx] is not None:
# Did hardware deliver IR source position for this IR sensor?
try:
pos = irSources[irSensorIndx]['pos']
except KeyError:
# If no position information from this IR sensor, use INVALID for the dimensions:
msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID))
# The following else is unusual: its statements are bypassed is except clause had control:
else:
# Have IR position info from this IR sensor. We use the IR_source_info
# message type. Get size (intensity?):
try:
size = irSources[irSensorIndx]['size']
except KeyError:
# If the driver did not deliver size information, indicate by using INVALID:
size = State.INVALID
lightInfo = IrSourceInfo(pos[0], pos[1], size)
msg.ir_tracking.append(lightInfo)
else:
msg.ir_tracking.append(IrSourceInfo(State.INVALID_FLOAT, State.INVALID_FLOAT, State.INVALID))
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
try:
self.pub.publish(msg)
except rospy.ROSException:
rospy.loginfo("Topic /wiimote/state closed. Shutting down Wiimote sender.")
exit(0)
#rospy.logdebug("Wiimote state:")
#rospy.logdebug(" Accel: " + str(canonicalAccel) + "\n Angular rate: " + str(canonicalAngleRate))
#rospy.logdebug(" Rumble: " + str(msg.rumble) + "\n Battery: [" + str(msg.raw_battery) + "," + str(msg.percent_battery))
#rospy.logdebug(" IR positions: " + str(msg.ir_tracking))
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Wiimote sender.")
exit(0)
#!/usr/bin/env python
import roslib; roslib.load_manifest('wiimote')
import rospy,math
from sensor_msgs.msg import JoyFeedbackArray
from sensor_msgs.msg import JoyFeedback
from wiimote.msg import State
siita=444
sp=2
past=State()
def callback(data):
global past,led0,led1,led2,led3,rum,siita,sp
if not (data.linear_acceleration_zeroed == past.linear_acceleration_zeroed):
x=data.linear_acceleration_zeroed.x/math.sqrt(pow((data.linear_acceleration_zeroed.x),2)+pow((data.linear_acceleration_zeroed.y),2))
y=data.linear_acceleration_zeroed.y/math.sqrt(pow((data.linear_acceleration_zeroed.x),2)+pow((data.linear_acceleration_zeroed.y),2))
siita=180+math.degrees(math.atan2(y,x))
siita=int(siita)
if(10>siita or siita>350):
rum.intensity=1
else:
rum.intensity=0
if data.buttons[1]==True and past.buttons[1]==False:
#accel
pass
elif data.buttons[0]==True and past.buttons[0]==False:
#Brake
pass
elif data.buttons[6]==True and past.buttons[6]==False: