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 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)