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_back = False self.last_auto_control = False self.thruster_deploy_count = 0 self.thruster_retract_count = 0 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 thruster_retract = bool(joy.buttons[4]) # LB thruster_deploy = bool(joy.buttons[5]) # RB 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 thruster_retract: self.thruster_retract_count += 1 else: self.thruster_retract_count = 0 if thruster_deploy: self.thruster_deploy_count += 1 else: self.thruster_deploy_count = 0 if self.thruster_retract_count > 10: self.remote.retract_thrusters() self.thruster_retract_count = 0 elif self.thruster_deploy_count > 10: self.remote.deploy_thrusters() self.thruster_deploy_count = 0 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 Joystick(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("emergency", "/wrench/emergency") rospy.Subscriber("joy_emergency", Joy, self.joy_recieved) self.active = False self.reset() 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_auto_control = False self.last_emergency_control = False self.last_shooter_cancel = False self.last_change_mode = 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.last_time = rospy.Time.now() self.check_for_timeout(joy) # Assigns readable names to the buttons that are used start = joy.buttons[7] kill = bool(joy.buttons[2]) station_hold = bool(joy.buttons[0]) emergency_control = bool(joy.buttons[11]) auto_control = bool(joy.buttons[12]) shooter_cancel = bool(joy.buttons[1]) change_mode = bool(joy.buttons[3]) # 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 auto_control and not self.last_auto_control: self.remote.select_autonomous_control() if emergency_control and not self.last_emergency_control: self.remote.select_emergency_control() if shooter_cancel and not self.last_shooter_cancel: self.remote.shooter_cancel() if change_mode and not self.last_change_mode: self.remote.select_next_control() self.last_kill = kill self.last_station_hold_state = station_hold self.last_auto_control = auto_control self.last_emergency_control = emergency_control self.last_shooter_cancel = shooter_cancel self.last_change_mode = change_mode # 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) def die_check(self, event): ''' Publishes zeros after 2 seconds of no update in case node navigator_emergency_xbee dies ''' if self.active: # No new instructions after 2 seconds if rospy.Time.now() - self.last_time > rospy.Duration(2): # Zero the wrench, reset self.reset()
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") rospy.Subscriber("joy", Joy, self.joy_recieved) self.active = False self.reset() 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_change_mode = False self.last_auto_control = False self.last_rc_control = False self.last_keyboard_control = 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] change_mode = bool(joy.buttons[3]) # Y kill = bool(joy.buttons[2]) # X station_hold = bool(joy.buttons[0]) # A rc_control = bool(joy.buttons[11]) # d-pad left auto_control = bool(joy.buttons[12]) # d-pad right keyboard_control = bool(joy.buttons[14]) # d-pad down 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 change_mode and not self.last_change_mode: self.remote.select_next_control() if auto_control and not self.last_auto_control: self.remote.select_autonomous_control() if rc_control and not self.last_rc_control: self.remote.select_rc_control() if keyboard_control and not self.last_keyboard_control: self.remote.select_keyboard_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_change_mode = change_mode self.last_auto_control = auto_control self.last_rc_control = rc_control self.last_keyboard_control = keyboard_control 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("/joystick_wrench/force_scale", 600) self.torque_scale = rospy.get_param("/joystick_wrench/torque_scale", 500) self.remote = RemoteControl("emergency", "/wrench/emergency") rospy.Subscriber("joy_emergency", Joy, self.joy_recieved) self.active = False self.reset() 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_emergency_control = False self.last_go_inactive = False self.thruster_deploy_count = 0 self.thruster_retract_count = 0 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.last_time = rospy.Time.now() self.check_for_timeout(joy) # Assigns readable names to the buttons that are used start = joy.buttons[7] go_inactive = joy.buttons[6] raise_kill = bool(joy.buttons[1]) clear_kill = bool(joy.buttons[2]) station_hold = bool(joy.buttons[0]) emergency_control = bool(joy.buttons[13]) thruster_retract = bool(joy.buttons[4]) thruster_deploy = bool(joy.buttons[5]) if go_inactive and not self.last_go_inactive: rospy.loginfo('Go inactive pressed. Going inactive') self.reset() return # Reset controller state if only start is pressed down about 1 seconds self.start_count += start if self.start_count > 5: rospy.loginfo("Resetting controller state") self.reset() self.active = True if not self.active: return if thruster_retract: self.thruster_retract_count += 1 else: self.thruster_retract_count = 0 if thruster_deploy: self.thruster_deploy_count += 1 else: self.thruster_deploy_count = 0 if self.thruster_retract_count > 10: self.remote.retract_thrusters() self.thruster_retract_count = 0 elif self.thruster_deploy_count > 10: self.remote.deploy_thrusters() self.thruster_deploy_count = 0 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 emergency_control and not self.last_emergency_control: self.remote.select_emergency_control() self.last_raise_go_inactive = go_inactive self.last_raise_kill = raise_kill self.last_clear_kill = clear_kill self.last_station_hold_state = station_hold self.last_emergency_control = emergency_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, joy.header.stamp) def die_check(self, event): ''' Publishes zeros after 2 seconds of no update in case node navigator_emergency_xbee dies ''' if self.active: # No new instructions after 2 seconds if rospy.Time.now() - self.last_time > rospy.Duration(2): # Zero the wrench, reset self.reset()