def test_choosingSpecies(self):
# Increment
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 1, 0, 0]),
self.module)
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 0, 0, 0]),
self.module)
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 1, 0, 0]),
self.module)
# Press the choose button
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 0, 0, 1]),
self.module)
# Make sure the correct data is sent to the UI
d = self.module.GetOutputDict()
self.assertEquals(d['state'], State.SPECIES_INFO_DISPLAYING)
self.assertEquals(d['name'], 'SpeciesSelection')
self.assertTrue(d['user_chose'])
self.checkSpeciesInfo(self.speciesInfo[2], d['species'])
# Make sure that the choice is logged properly
self.pubMock.publish.assert_called_with(
species_id=2,
image_id=3,
image_path=self.pathGen.GetOSImagePath(3))
def callback(self, inmsg):
global extract_warn
outmsg = Joy()
outmsg.buttons = remap(inmsg.buttons, self.button_mapping)
outmsg.axes = remap(inmsg.axes, self.axis_mapping)
self.pub.publish(outmsg)
if not self.warned and extract_warn:
self.warned = True
rospy.logwarn("Out of range remapping. Setting to zero. Joystick has %i buttons and %i axes.")%(len(inmsg.buttons), len(inmsg.axes))
def callback(self, inmsg):
global extract_warn
outmsg = Joy()
outmsg.buttons = remap(inmsg.buttons, self.button_mapping)
outmsg.axes = remap(inmsg.axes, self.axis_mapping)
self.pub.publish(outmsg)
if not self.warned and extract_warn:
self.warned = True
rospy.logwarn(
"Out of range remapping. Setting to zero. Joystick has %i buttons and %i axes."
) % (len(inmsg.buttons), len(inmsg.axes))
def run(self):
"""Loop that obtains the latest nunchuk state, publishes the joystick data, and sleeps.
The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
"""
self.threadName = "nunchuk Joy topic Publisher"
try:
while not rospy.is_shutdown():
(canonicalAccel, scaledAcc,
canonicalAngleRate) = self.obtainWiimoteData()
if not self.wiistate.nunchukPresent:
continue
if self.pub is None:
self.pub = rospy.Publisher('/wiimote/nunchuk', Joy)
rospy.loginfo(
"Wiimote Nunchuk joystick publisher starting (topic nunchuk)."
)
(joyx, joyy) = self.wiistate.nunchukStick
msg = Joy( # the Joy msg does not have a header :-( header=None,
axes=[
joyx, joyy, scaledAcc[X], scaledAcc[Y], scaledAcc[Z]
],
buttons=None)
theButtons = [False, False]
theButtons[
State.MSG_BTN_Z] = self.wiistate.nunchukButtons[BTN_Z]
theButtons[
State.MSG_BTN_C] = self.wiistate.nunchukButtons[BTN_C]
msg.buttons = theButtons
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
# the Joy msg does not have a header :-(
# msg.header.stamp.secs = timeSecs
# msg.header.stamp.nsecs = timeNSecs
self.pub.publish(msg)
rospy.logdebug("nunchuk state:")
rospy.logdebug(" nunchuk buttons: " + str(theButtons) +
"\n Nuchuck axes: " + str(msg.axes) + "\n")
rospy.sleep(self.sleepDuration)
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Nun sender.")
exit(0)
def test_decAroundHorn(self):
# There are three entries and then UNKNOWN, so go around the horn
for i in range(7):
# Decrement button press
SpeciesSelectionModule.buttonCallback(Joy([], [0, 1, 0, 0, 0]),
self.module)
# Decrement button unpress
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 0, 0, 0]),
self.module)
d = self.module.GetOutputDict()
self.assertEquals(d['state'], State.USER_SELECTING)
self.assertEquals(d['name'], 'SpeciesSelection')
self.assertEquals(d['idx'], 1)
self.assertFalse(d['user_chose'])
def sendJoy(self, w, v):
joy_msg = Joy()
joy_msg.axes.append(w)
joy_msg.axes.append(v)
self.joy_pub.publish(joy_msg)
def test_twoIncrMessagesOnePress(self):
# Increment
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 1, 0, 0]),
self.module)
self.checkSignalUpdate(3)
d = self.module.GetOutputDict()
self.assertEquals(d['state'], State.USER_SELECTING)
self.assertEquals(d['name'], 'SpeciesSelection')
self.assertEquals(d['idx'], 1)
self.assertFalse(d['user_chose'])
# Shouldn't trigger another message to the UI
self.signalMock.reset_mock()
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 1, 0, 0]),
self.module)
self.assertFalse(self.signalMock.called)
def run(self):
"""Loop that obtains the latest nunchuk state, publishes the joystick data, and sleeps.
The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
"""
self.threadName = "nunchuk Joy topic Publisher"
try:
while not rospy.is_shutdown():
(canonicalAccel, scaledAcc, canonicalAngleRate) = self.obtainWiimoteData()
if not self.wiistate.nunchukPresent:
continue
if self.pub is None:
self.pub = rospy.Publisher('/wiimote/nunchuk', Joy)
rospy.loginfo("Wiimote Nunchuk joystick publisher starting (topic nunchuk).")
(joyx, joyy) = self.wiistate.nunchukStick
msg = Joy(# the Joy msg does not have a header :-( header=None,
axes=[joyx, joyy,
scaledAcc[X], scaledAcc[Y], scaledAcc[Z]],
buttons=None)
theButtons = [False,False]
theButtons[State.MSG_BTN_Z] = self.wiistate.nunchukButtons[BTN_Z]
theButtons[State.MSG_BTN_C] = self.wiistate.nunchukButtons[BTN_C]
msg.buttons = theButtons
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
# the Joy msg does not have a header :-(
# msg.header.stamp.secs = timeSecs
# msg.header.stamp.nsecs = timeNSecs
self.pub.publish(msg)
rospy.logdebug("nunchuk state:")
rospy.logdebug(" nunchuk buttons: " + str(theButtons) + "\n Nuchuck axes: " + str(msg.axes) + "\n")
rospy.sleep(self.sleepDuration)
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Nun sender.")
exit(0)
def main(screen):
screen.nodelay(1) # make getch() a non-blocking call
pub = rospy.Publisher(PUBLISH_TOPIC, Joy)
rospy.init_node(NODE_NAME)
rate = rospy.Rate(LOOP_FREQ)
while not rospy.is_shutdown():
keypress = screen.getch()
if keypress > -1:
keyval = chr(keypress)
if keyval == "q": # quit
rospy.signal_shutdown("User requested shutdown.")
else:
button = int(keyval)
if button < 10:
msg = Joy()
msg.buttons = [0 for x in range(10)]
msg.buttons[button] = 1
pub.publish(msg)
rate.sleep()
def main(screen):
screen.nodelay(1) # make getch() a non-blocking call
pub = rospy.Publisher(PUBLISH_TOPIC, Joy)
rospy.init_node(NODE_NAME)
rate = rospy.Rate(LOOP_FREQ)
while not rospy.is_shutdown():
keypress = screen.getch()
if keypress > -1:
keyval = chr(keypress)
if keyval == "q": # quit
rospy.signal_shutdown("User requested shutdown.")
else:
button = int(keyval)
if button < 10:
msg = Joy()
msg.buttons = [0 for x in range(10)]
msg.buttons[button] = 1
pub.publish(msg)
rate.sleep()
def processTwist(self, data):
axis0 = data.angular.z / self.wmax
if (axis0 > 1.0):
axis0 = 1.0
elif (axis0 < -1.0):
axis0 = -1.0
axis1 = data.linear.x / self.vmax
if (axis1 > 1.0):
axis1 = 1.0
elif (axis1 < -1.0):
axis1 = -1.0
joy_msg = Joy()
joy_msg.axes.append(axis0)
joy_msg.axes.append(axis1)
self.joy_pub.publish(joy_msg)
def handle(self):
newmsg = self.rfile.readline().rstrip()
print "Client %s said ``%s''" % (self.client_address[0], newmsg)
self.wfile.write(self.server.oldmsg)
self.server.oldmsg = newmsg
args = newmsg.split(',')
x = -float(args[2])
y = float(args[3])
z = float(args[4])
d = math.sin(math.radians(5))
if abs(x) < d:
x = 0
else:
x -= x/math.fabs(x)*d
if abs(z) < d:
z = 0
else:
z -= z/math.fabs(z)*d
k = 1 / math.cos(math.radians(60))
pub.publish(Joy([1.5*k*z, 0.0, 0.0, k*x], [1] ))
#!/usr/bin/env python
import roslib; roslib.load_manifest('reefbot-controller')
import rospy
import copy
import thread
import threading
from joy.msg import Joy
from reefbot_msgs.msg import RobotStatus
from reefbot_msgs.msg import CameraPower
from robot_api import *
from CommandAdapter import CommandAdapter
from SpinTracker import SpinTracker
from ButtonMapper import ButtonMapper
EMPTY_JOY = Joy(axes=[0.0, 0.0, 0.0, 0.0, 0.0],
buttons=[1,2,3,4,5,6,7,8,9,10,11])
def joystick_handler(data, video_ray_api, command_adapter, pub_signal,
thruster_limit, robot_status, thread_signal,
spin_tracker, button_mapper, last_joy_msg, min_depth,
max_depth, max_spin_time):
thread_signal.acquire()
# Record the Joystick message
if len(last_joy_msg) > 0:
last_joy_msg.pop()
last_joy_msg.append(data)
try:
vertical_thrust = button_mapper.GetDiveAxis(data)
video_ray_api.depth_keeping(abs(vertical_thrust) < 0.1)
def on_new_joy(msg,publisher):
out = Joy()
out.axes= msg.axes;
out.buttons = msg.buttons;
publisher[0].publish(out)
def __init__(self):
self.chat_sub = rospy.Subscriber('chatter', String, self.chatter_cb)
self.joy_pub = rospy.Publisher('joy', Joy)
self.joy_msg = Joy()
self.joy_msg.axes.extend([0.0, 0.0, 1.0])
def test_initialChoice(self):
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 0, 0, 0]),
self.module)
self.assertFalse(self.signalMock.called)
def test_buttonPressBeforeSpeciesId(self):
SpeciesSelectionModule.buttonCallback(Joy([], [0, 0, 0, 0, 4]),
self.module)
self.assertFalse(self.signalMock.called)
def run(self):
"""Loop that obtains the latest wiimote state, publishes the joystick data, and sleeps.
The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
"""
rospy.loginfo("Wiimote joystick publisher starting (topic wiijoy).")
self.threadName = "Joy topic Publisher"
try:
while not rospy.is_shutdown():
(canonicalAccel, canonicalNunchukAccel,
canonicalAngleRate) = self.obtainWiimoteData()
axes = [
canonicalAccel[X], canonicalAccel[Y], canonicalAccel[Z]
]
# If a gyro is attached to the Wiimote, we add the
# gyro information:
if self.wiistate.motionPlusPresent:
axes.extend([
canonicalAngleRate[PHI], canonicalAngleRate[THETA],
canonicalAngleRate[PSI]
])
# Fill in the ROS message's buttons field (there *must* be
# a better way in python to declare an array of 11 zeroes...]
buttons = [
False, False, False, False, False, False, False, False,
False, False, False
]
buttons[State.MSG_BTN_1] = self.wiistate.buttons[BTN_1]
buttons[State.MSG_BTN_2] = self.wiistate.buttons[BTN_2]
buttons[State.MSG_BTN_A] = self.wiistate.buttons[BTN_A]
buttons[State.MSG_BTN_B] = self.wiistate.buttons[BTN_B]
buttons[State.MSG_BTN_PLUS] = self.wiistate.buttons[BTN_PLUS]
buttons[State.MSG_BTN_MINUS] = self.wiistate.buttons[BTN_MINUS]
buttons[State.MSG_BTN_LEFT] = self.wiistate.buttons[BTN_LEFT]
buttons[State.MSG_BTN_RIGHT] = self.wiistate.buttons[BTN_RIGHT]
buttons[State.MSG_BTN_UP] = self.wiistate.buttons[BTN_UP]
buttons[State.MSG_BTN_DOWN] = self.wiistate.buttons[BTN_DOWN]
buttons[State.MSG_BTN_HOME] = self.wiistate.buttons[BTN_HOME]
msg = Joy(axes, buttons)
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
# the Joy msg does not have a header :-(
# msg.header.stamp.secs = timeSecs
# msg.header.stamp.nsecs = timeNSecs
self.pub.publish(msg)
#rospy.logdebug("Joystick state:")
#rospy.logdebug(" Joy buttons: " + str(theButtons) + "\n Joy accel: " + str(canonicalAccel) + "\n Joy angular rate: " + str(canonicalAngleRate))
rospy.sleep(self.sleepDuration)
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Joy sender.")
exit(0)
