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):
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, 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("/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()
