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 __init__(self, context): super(Shooter, self).__init__(context) # Create the widget and name it self._widget = QtGui.QWidget() self._widget.setObjectName("Shooter") self.setObjectName("Shooter") # Extend the widget with all attributes and children in the UI file ui_file = os.path.join(rospkg.RosPack().get_path("navigator_gui"), "resource", "shooter.ui") loadUi(ui_file, self._widget) self.remote = RemoteControl("shooter gui") self.remote.is_timed_out = True self.shooter_status = { "received": "Unknown", "stamp": rospy.Time.now(), "cached": "Unknown" } self.disc_speed_setting = 0 self.connect_ui() rospy.Subscriber("/shooter/status", String, self.cache_shooter_status) # Show _widget.windowTitle on left-top of each plugin (when it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (" (%d)" % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget)
def __init__(self, context): super(Shooter, self).__init__(context) # Create the widget and name it self._widget = QtWidgets.QWidget() self._widget.setObjectName("Shooter") self.setObjectName("Shooter") # Extend the widget with all attributes and children in the UI file ui_file = os.path.join(rospkg.RosPack().get_path("navigator_gui"), "resource", "shooter.ui") loadUi(ui_file, self._widget) self.remote = RemoteControl("shooter gui") self.remote.is_timed_out = True self.shooter_status = { "received": "Unknown", "stamp": rospy.Time.now(), "cached": "Unknown" } self.disc_speed_setting = 0 self.connect_ui() rospy.Subscriber("/shooter/status", String, self.cache_shooter_status) # Deals with problem of multiple instances of same plugin if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (" (%d)" % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget)
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 __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 __init__(self, context): super(Dashboard, self).__init__(context) # Create the widget and name it self._widget = QtGui.QWidget() self._widget.setObjectName("Dashboard") self.setObjectName("Dashboard") # Extend the widget with all attributes and children in the UI file ui_file = os.path.join(rospkg.RosPack().get_path("navigator_gui"), "resource", "dashboard.ui") loadUi(ui_file, self._widget) self.is_killed = False self.remote = RemoteControl("dashboard") # Creates dictionaries that are used by the monitor functions to keep track of their node or service service_monitor_template = {"received": "Unknown", "cached": "Unknown"} node_monitor_template = service_monitor_template.copy() node_monitor_template["timeout_count"] = 0 node_monitor_template["is_timed_out"] = False self.operating_mode = service_monitor_template.copy() self.battery_voltage = node_monitor_template.copy() self.battery_voltage["cached_warning_color"] = "red" self.system_time = node_monitor_template.copy() self.system_time["is_timed_out"] = True # Build an ordered list of host dictionaries that resolve to devices on navigator host_list = [ "mil-nav-ubnt-wamv", "mil-nav-ubnt-shore", "mil-nav-wamv", "mil-com-velodyne-vlp16", "mil-com-sick-lms111" ] host_template = {"hostname": "", "ip": "Unknown", "status": "Unknown"} self.hosts = [] for host in host_list: host_entry = host_template.copy() host_entry["hostname"] = host self.hosts.append(host_entry) self.connect_ui() self.connect_ros() # Show _widget.windowTitle on left-top of each plugin (when it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (" (%d)" % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) # Creates monitors that update data on the GUI periodically self.monitor_battery_voltage() self.monitor_system_time() self.monitor_hosts()
def __init__(self, context): super(Dashboard, self).__init__(context) # Create the widget and name it self._widget = QtGui.QWidget() self._widget.setObjectName("Dashboard") self.setObjectName("Dashboard") # Extend the widget with all attributes and children in the UI file ui_file = os.path.join(rospkg.RosPack().get_path("navigator_gui"), "resource", "dashboard.ui") loadUi(ui_file, self._widget) self.is_killed = False self.remote = RemoteControl("dashboard") self.remote.is_timed_out = True # Creates dictionaries that are used by the monitor functions to keep track of their node or service node_monitor_template = { "received": "Unknown", "stamp": rospy.Time.now(), "cached": "Unknown", } self.operating_mode = node_monitor_template.copy() self.battery_voltage = node_monitor_template.copy() self.battery_voltage["cached_warning_color"] = "red" self.system_time = node_monitor_template.copy() del self.system_time["stamp"] self.system_time["timeout_count"] = 0 self.hosts = node_monitor_template.copy() self.clear_hosts() self.hosts["cached"] = self.hosts["received"] self.connect_ui() self.connect_ros() # Show _widget.windowTitle on left-top of each plugin (when it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (" (%d)" % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) # Creates monitors that update data on the GUI periodically self.monitor_operating_mode() self.monitor_battery_voltage() self.monitor_system_time() self.monitor_hosts()
def __init__(self, context): super(Dashboard, self).__init__(context) # Create the widget and name it self._widget = QtWidgets.QWidget() self._widget.setObjectName("Dashboard") self.setObjectName("Dashboard") # Extend the widget with all attributes and children in the UI file ui_file = os.path.join(rospkg.RosPack().get_path("navigator_gui"), "resource", "dashboard.ui") loadUi(ui_file, self._widget) self.remote = RemoteControl("dashboard") # Creates dictionaries that are used by the monitor functions to keep track of their node or service node_monitor_template = { "current": "Unknown", "displayed": "Unknown", } self.operating_mode = copy(node_monitor_template) self.battery_voltage = copy(node_monitor_template) self.kill_status = copy(node_monitor_template) self.kill_status["current"] = True self.system_time = copy(node_monitor_template) self.hosts = node_monitor_template.copy() self.clear_hosts() self.connect_ui() self.connect_ros() # Deals with problem when they're multiple instances of Dashboard plugin if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (" (%d)" % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) # Creates monitors that update data on the GUI periodically self.update_gui()
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") 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("~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 Shooter(Plugin): def __init__(self, context): super(Shooter, self).__init__(context) # Create the widget and name it self._widget = QtGui.QWidget() self._widget.setObjectName("Shooter") self.setObjectName("Shooter") # Extend the widget with all attributes and children in the UI file ui_file = os.path.join(rospkg.RosPack().get_path("navigator_gui"), "resource", "shooter.ui") loadUi(ui_file, self._widget) self.remote = RemoteControl("shooter gui") self.remote.is_timed_out = True self.shooter_status = { "received": "Unknown", "stamp": rospy.Time.now(), "cached": "Unknown" } self.disc_speed_setting = 0 self.connect_ui() rospy.Subscriber("/shooter/status", String, self.cache_shooter_status) # Show _widget.windowTitle on left-top of each plugin (when it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (" (%d)" % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) def connect_ui(self): ''' Links objects in the shooter GUI to variables in the backend shooter object. ''' # Shooter status indicator self.shooter_status_frame = self._widget.findChild(QtGui.QFrame, "shooter_status_frame") self.shooter_status_message = self._widget.findChild(QtGui.QLabel, "shooter_status_message") # Control panel buttons load_button = self._widget.findChild(QtGui.QPushButton, "load_button") load_button.clicked.connect(self.remote.shooter_load) fire_button = self._widget.findChild(QtGui.QPushButton, "fire_button") fire_button.clicked.connect(self.remote.shooter_fire) cancel_button = self._widget.findChild(QtGui.QPushButton, "cancel_button") cancel_button.clicked.connect(self.remote.shooter_cancel) reset_button = self._widget.findChild(QtGui.QPushButton, "reset_button") reset_button.clicked.connect(self.remote.shooter_reset) linear_extend_button = self._widget.findChild(QtGui.QPushButton, "linear_extend_button") linear_extend_button.clicked.connect(self.remote.shooter_linear_extend) linear_retract_button = self._widget.findChild(QtGui.QPushButton, "linear_retract_button") linear_retract_button.clicked.connect(self.remote.shooter_linear_retract) disc_speed_slider = self._widget.findChild(QtGui.QSlider, "disc_speed_slider") disc_speed_slider.valueChanged[int].connect(self.cache_disc_speed_setting) set_disc_speed_button = self._widget.findChild(QtGui.QPushButton, "set_disc_speed_button") set_disc_speed_button.clicked.connect(self.set_disc_speed) # Defines the color scheme as QT style sheets self.colors = { "red": "QWidget {background-color:#FF432E;}", "green": "QWidget {background-color:#B1EB00;}", "yellow": "QWidget {background-color:#FDEF14;}" } self.status_colors = {"green": ["Standby", "Loaded"], "yellow": ["Loading", "Firing", "Canceled", "Manual"], "red": ["Unknown"] } # Creates a monitor that update the shooter status on the GUI periodically self.monitor_shooter_status() def cache_disc_speed_setting(self, speed): ''' Caches the shooter acceleration disc speed whenever it is updated on the GUI. ''' self.disc_speed_setting = speed def set_disc_speed(self): ''' Sets the shooter's accelerator disc speed to the cached value. ''' self.remote.set_disc_speed(self.disc_speed_setting) def cache_shooter_status(self, msg): ''' Stores the shooter status when it is published. ''' self.shooter_status["received"] = msg.data self.shooter_status["stamp"] = rospy.Time.now() def monitor_shooter_status(self): ''' Monitors the shooter status on a 1s interval. Only updates the display when the received shooter status has changed. The connection to the status will time out if no message has been received in 1s. ''' if ((rospy.Time.now() - self.shooter_status["stamp"]) < rospy.Duration(1)): self.remote.is_timed_out = False # Sets the remote control to timed out and the shooter status to 'Unknown' if no message has been received in 1s else: self.remote.is_timed_out = True self.shooter_status["received"] = "Unknown" if (self.shooter_status["received"] != self.shooter_status["cached"]): self.update_shooter_status() # Schedules the next instance of this method with a QT timer QtCore.QTimer.singleShot(200, self.monitor_shooter_status) def update_shooter_status(self): ''' Updates the displayed shooter status text and color. Sets the color of the status frame based on the text's relationship to it in the status_colors dictionary. ''' self.shooter_status_message.setText(self.shooter_status["received"]) # Update the status display color based on the message received for color, messages in self.status_colors.iteritems(): if (self.shooter_status["received"] in messages): self.shooter_status_frame.setStyleSheet(self.colors[color]) # Set the cached shooter status to the value that was just displayed self.shooter_status["cached"] = self.shooter_status["received"]
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 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()