class Joystick(object):
def __init__(self):
self.force_scale = rospy.get_param("~force_scale", 600)
self.torque_scale = rospy.get_param("~torque_scale", 500)
self.remote = RemoteControl("joystick", "/wrench/rc")
self.reset()
rospy.Subscriber("joy", Joy, self.joy_recieved)
self.active = False
def reset(self):
'''
Used to reset the state of the controller. Sometimes when it
disconnects then comes back online, the settings are all out of whack.
'''
self.last_kill = False
self.last_station_hold_state = False
self.last_rc_control = False
self.last_emergency_control = False
self.last_keyboard_control = False
self.last_auto_control = False
self.last_change_mode = False
# self.last_shooter_load = False
# self.last_shooter_fire = False
self.last_shooter_cancel = False
self.start_count = 0
self.last_joy = None
self.active = False
self.remote.clear_wrench()
def check_for_timeout(self, joy):
if self.last_joy is None:
self.last_joy = joy
return
if joy.axes == self.last_joy.axes and joy.buttons == self.last_joy.buttons:
# No change in state
if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60):
# The controller times out after 15 minutes
if self.active:
rospy.logwarn("Controller Timed out. Hold start to resume.")
self.reset()
else:
joy.header.stamp = rospy.Time.now() # In the sim, stamps weren't working right
self.last_joy = joy
def joy_recieved(self, joy):
self.check_for_timeout(joy)
# Assigns readable names to the buttons that are used
start = joy.buttons[7]
kill = bool(joy.buttons[2]) # X
station_hold = bool(joy.buttons[0]) # A
rc_control = bool(joy.buttons[11]) # d-pad left
emergency_control = bool(joy.buttons[13]) # d-pad up
keyboard_control = bool(joy.buttons[14]) # d-pad down
auto_control = bool(joy.buttons[12]) # d-pad right
change_mode = bool(joy.buttons[3]) # Y
# shooter_load = bool(joy.buttons[4])
# shooter_fire = bool(joy.axes[5] < -0.9)
shooter_cancel = bool(joy.buttons[5])
# Reset controller state if only start is pressed down about 3 seconds
self.start_count += start
if self.start_count > 10:
rospy.loginfo("Resetting controller state")
self.reset()
self.active = True
self.remote.clear_kill()
if not self.active:
return
if kill and not self.last_kill:
self.remote.toggle_kill()
if station_hold and not self.last_station_hold_state:
self.remote.station_hold()
if rc_control and not self.last_rc_control:
self.remote.select_rc_control()
if emergency_control and not self.last_emergency_control:
self.remote.select_emergency_control()
if keyboard_control and not self.last_keyboard_control:
self.remote.select_keyboard_control()
if auto_control and not self.last_auto_control:
self.remote.select_autonomous_control()
if change_mode and not self.last_change_mode:
self.remote.select_next_control()
# if shooter_load and not self.last_shooter_load:
# self.remote.shooter_load()
# if shooter_fire and not self.last_shooter_fire:
# self.remote.shooter_fire()
if shooter_cancel and not self.last_shooter_cancel:
self.remote.shooter_cancel()
self.last_kill = kill
self.last_station_hold_state = station_hold
self.last_rc_control = rc_control
self.last_emergency_control = emergency_control
self.last_keyboard_control = keyboard_control
self.last_auto_control = auto_control
self.last_change_mode = change_mode
# self.last_shooter_load = shooter_load
# self.last_shooter_fire = shooter_fire
self.last_shooter_cancel = shooter_cancel
# Scale joystick input to force and publish a wrench
x = joy.axes[1] * self.force_scale
y = joy.axes[0] * self.force_scale
rotation = joy.axes[3] * self.torque_scale
self.remote.publish_wrench(x, y, rotation, joy.header.stamp)
class Joystick(object):
def __init__(self):
self.force_scale = rospy.get_param("~force_scale", 600)
self.torque_scale = rospy.get_param("~torque_scale", 500)
self.remote = RemoteControl("joystick", "/wrench/rc")
self.reset()
rospy.Subscriber("joy", Joy, self.joy_recieved)
def reset(self):
'''
Used to reset the state of the controller. Sometimes when it
disconnects then comes back online, the settings are all out of whack.
'''
self.last_raise_kill = False
self.last_clear_kill = False
self.last_station_hold_state = False
self.last_rc_control = False
self.last_emergency_control = False
self.last_keyboard_control = False
self.last_auto_control = False
self.start_count = 0
self.last_joy = None
self.active = False
self.remote.clear_wrench()
def check_for_timeout(self, joy):
if self.last_joy is None:
self.last_joy = joy
return
if joy.axes == self.last_joy.axes and joy.buttons == self.last_joy.buttons:
# No change in state
if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60):
# The controller times out after 15 minutes
if self.active:
rospy.logwarn("Controller Timed out. Hold start to resume.")
self.reset()
else:
joy.header.stamp = rospy.Time.now() # In the sim, stamps weren't working right
self.last_joy = joy
def joy_recieved(self, joy):
self.check_for_timeout(joy)
# Assigns readable names to the buttons that are used
start = joy.buttons[7]
back = joy.buttons[6]
raise_kill = bool(joy.buttons[1]) # B
clear_kill = bool(joy.buttons[2]) # X
station_hold = bool(joy.buttons[0]) # A
rc_control = bool(joy.buttons[11]) # d-pad left
emergency_control = bool(joy.buttons[13]) # d-pad up
keyboard_control = bool(joy.buttons[14]) # d-pad down
auto_control = bool(joy.buttons[12]) # d-pad right
if back and not self.last_back:
rospy.loginfo('Back pressed. Going inactive')
self.reset()
return
# Reset controller state if only start is pressed down about 1 second
self.start_count += start
if self.start_count > 5:
rospy.loginfo("Resetting controller state")
self.reset()
self.active = True
if not self.active:
self.remote.clear_wrench()
return
if raise_kill and not self.last_raise_kill:
self.remote.kill()
if clear_kill and not self.last_clear_kill:
self.remote.clear_kill()
if station_hold and not self.last_station_hold_state:
self.remote.station_hold()
if rc_control and not self.last_rc_control:
self.remote.select_rc_control()
if emergency_control and not self.last_emergency_control:
self.remote.select_emergency_control()
if keyboard_control and not self.last_keyboard_control:
self.remote.select_keyboard_control()
if auto_control and not self.last_auto_control:
self.remote.select_autonomous_control()
self.last_back = back
self.last_raise_kill = raise_kill
self.last_clear_kill = clear_kill
self.last_station_hold_state = station_hold
self.last_rc_control = rc_control
self.last_emergency_control = emergency_control
self.last_keyboard_control = keyboard_control
self.last_auto_control = auto_control
# Scale joystick input to force and publish a wrench
x = joy.axes[1] * self.force_scale
y = joy.axes[0] * self.force_scale
rotation = joy.axes[3] * self.torque_scale
self.remote.publish_wrench(x, y, rotation, rospy.Time.now())
class KeyboardServer(object):
def __init__(self):
self.force_scale = rospy.get_param("/joystick_wrench/force_scale", 600)
self.torque_scale = rospy.get_param("/joystick_wrench/torque_scale", 500)
self.remote = RemoteControl("keyboard", "/wrench/keyboard")
rospy.Service("/keyboard_control", KeyboardControl, self.key_recieved)
# Initialize this to a random UUID so that a client without a UUID cannot authenticate
self.locked_uuid = uuid.uuid4().hex
# This maps each key to a remote control function
self.key_mappings = {ord('k'): lambda: self.remote.toggle_kill(),
ord('K'): lambda: self.remote.kill(),
ord('h'): lambda: self.remote.station_hold(),
ord('j'): lambda: self.remote.select_rc_control(),
ord('e'): lambda: self.remote.select_emergency_control(),
ord('b'): lambda: self.remote.select_keyboard_control(),
ord('u'): lambda: self.remote.select_autonomous_control(),
ord('c'): lambda: self.remote.select_next_control(),
ord('r'): lambda: self.remote.shooter_load(),
ord('f'): lambda: self.remote.shooter_fire(),
ord('t'): lambda: self.remote.shooter_cancel(),
ord('w'): lambda: self.remote.publish_wrench(self.force_scale, 0, 0),
ord('s'): lambda: self.remote.publish_wrench(-self.force_scale, 0, 0),
ord('a'): lambda: self.remote.publish_wrench(0, self.force_scale, 0),
ord('d'): lambda: self.remote.publish_wrench(0, -self.force_scale, 0),
curses.KEY_LEFT: lambda: self.remote.publish_wrench(0, 0, self.torque_scale),
curses.KEY_RIGHT: lambda: self.remote.publish_wrench(0, 0, -self.torque_scale)
}
self.movement_keys = [ord('w'), ord('s'), ord('a'), ord('d'), curses.KEY_LEFT, curses.KEY_RIGHT]
def key_recieved(self, req):
'''
This function handles the process of locking control of the service to
one client. If an 'L' is received, a UUID is generated for the client
and control of the service is locked to it.
'''
# If the key pressed was L, locks control of the service to the clinet's UUID
if (req.keycode == 76):
# Generates a new UUID for the client if it does not already have one
if (req.uuid is ''):
self.locked_uuid = uuid.uuid4().hex
else:
self.locked_uuid = req.uuid
return {"generated_uuid": self.locked_uuid, "is_locked": True}
# If the key was from the client with locked control, pass it to execution
elif (req.uuid == self.locked_uuid):
self.execute_key(req.keycode)
return {"generated_uuid": '', "is_locked": True}
# Ignore keys sent by a client that has not locked control of the service
else:
return {"generated_uuid": '', "is_locked": False}
def execute_key(self, key):
'''
Executes a remote control action based on the key that was received.
'''
if (key in self.key_mappings):
self.key_mappings[key]()
# If no motion key was received, clear the wrench
if (key not in self.movement_keys):
self.remote.clear_wrench()
class KeyboardServer(object):
def __init__(self):
self.force_scale = rospy.get_param("/joystick_wrench/force_scale", 600)
self.torque_scale = rospy.get_param("/joystick_wrench/torque_scale",
500)
self.remote = RemoteControl("keyboard", "/wrench/keyboard")
rospy.Service("/keyboard_control", KeyboardControl, self.key_recieved)
# Initialize this to a random UUID so that a client without a UUID cannot authenticate
self.locked_uuid = uuid.uuid4().hex
# This maps each key to a remote control function
self.key_mappings = {
ord('k'):
lambda: self.remote.toggle_kill(),
ord('K'):
lambda: self.remote.kill(),
ord('h'):
lambda: self.remote.station_hold(),
ord('u'):
lambda: self.remote.select_autonomous_control(),
ord('j'):
lambda: self.remote.select_rc_control(),
ord('b'):
lambda: self.remote.select_keyboard_control(),
ord('c'):
lambda: self.remote.select_next_control(),
ord('r'):
lambda: self.remote.shooter_load(),
ord('f'):
lambda: self.remote.shooter_fire(),
ord('t'):
lambda: self.remote.shooter_cancel(),
ord('w'):
lambda: self.remote.publish_wrench(self.force_scale, 0, 0),
ord('s'):
lambda: self.remote.publish_wrench(-self.force_scale, 0, 0),
ord('a'):
lambda: self.remote.publish_wrench(0, self.force_scale, 0),
ord('d'):
lambda: self.remote.publish_wrench(0, -self.force_scale, 0),
curses.KEY_LEFT:
lambda: self.remote.publish_wrench(0, 0, self.torque_scale),
curses.KEY_RIGHT:
lambda: self.remote.publish_wrench(0, 0, -self.torque_scale)
}
self.movement_keys = [
ord('w'),
ord('s'),
ord('a'),
ord('d'), curses.KEY_LEFT, curses.KEY_RIGHT
]
def key_recieved(self, req):
'''
This function handles the process of locking control of the service to
one client. If an 'L' is received, a UUID is generated for the client
and control of the service is locked to it.
'''
# If the key pressed was L, locks control of the service to the clinet's UUID
if (req.keycode == 76):
# Generates a new UUID for the client if it does not already have one
if (req.uuid is ''):
self.locked_uuid = uuid.uuid4().hex
else:
self.locked_uuid = req.uuid
return {"generated_uuid": self.locked_uuid, "is_locked": True}
# If the key was from the client with locked control, pass it to execution
elif (req.uuid == self.locked_uuid):
self.execute_key(req.keycode)
return {"generated_uuid": '', "is_locked": True}
# Ignore keys sent by a client that has not locked control of the service
else:
return {"generated_uuid": '', "is_locked": False}
def execute_key(self, key):
'''
Executes a remote control action based on the key that was received.
'''
if (key in self.key_mappings):
self.key_mappings[key]()
# If no motion key was received, clear the wrench
if (key not in self.movement_keys):
self.remote.clear_wrench()
