def connect_ros(self):
'''
Connect ROS nodes, services, and alarms to variables and methods
within this class.
'''
# Attempts to read the battery voltage parameters (sets them to defaults if they have not been set)
self.battery_low_voltage = rospy.get_param(
"/battery_monitor/battery_low_voltage", 22.1)
self.battery_critical_voltage = rospy.get_param(
"/battery_monitor/battery_critical_voltage", 20.6)
rospy.Subscriber("/battery_monitor", Float32,
self.cache_battery_voltage)
rospy.Subscriber("/clock", Clock, self.cache_system_time)
self.wrench_changer = rospy.ServiceProxy('/change_wrench',
WrenchSelect)
self.kill_listener = AlarmListener('kill', self.update_kill_status)
alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = alarm_broadcaster.add_alarm(name='kill',
action_required=True,
severity=0)
self.station_hold_alarm = alarm_broadcaster.add_alarm(
name='station_hold', action_required=True, severity=3)
def __init__(self, controller_name, wrench_pub=None):
self.name = controller_name
self.wrench_choices = itertools.cycle(["rc", "keyboard", "autonomous"])
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name="kill",
action_required=True,
severity=0
)
self.station_hold_alarm = self.alarm_broadcaster.add_alarm(
name="station_hold",
action_required=False,
severity=3
)
# rospy.wait_for_service("/change_wrench")
self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)
self.kill_listener = AlarmListener("kill", self._update_kill_status)
if (wrench_pub is None):
self.wrench_pub = wrench_pub
else:
self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1)
self.shooter_load_client = actionlib.SimpleActionClient("/shooter/load", ShooterDoAction)
self.shooter_fire_client = actionlib.SimpleActionClient("/shooter/fire", ShooterDoAction)
self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger)
self.shooter_manual_client = rospy.ServiceProxy("/shooter/manual", ShooterManual)
self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger)
self.is_killed = False
self.is_timed_out = False
self.clear_wrench()
def connect_ros(self):
'''
Connect ROS nodes, services, and alarms to variables and methods
within this class.
'''
# Attempts to read the battery voltage parameters (sets them to defaults if they have not been set)
self.battery_low_voltage = rospy.get_param("/battery_monitor/battery_low_voltage", 22.1)
self.battery_critical_voltage = rospy.get_param("/battery_monitor/battery_critical_voltage", 20.6)
rospy.Subscriber("/battery_monitor", Float32, self.cache_battery_voltage)
rospy.Subscriber("/clock", Clock, self.cache_system_time)
self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect)
self.kill_listener = AlarmListener('kill', self.update_kill_status)
alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = alarm_broadcaster.add_alarm(
name='kill',
action_required=True,
severity=0
)
self.station_hold_alarm = alarm_broadcaster.add_alarm(
name='station_hold',
action_required=True,
severity=3
)
def __init__(self, port="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0", baud=9600):
self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.25)
self.ser.flush()
self.killstatus_pub = rospy.Publisher("/killstatus", KillStatus, queue_size=1)
ab = AlarmBroadcaster()
self.kill_alarm = ab.add_alarm("hw_kill", problem_description="Hardware kill from a kill switch.")
self.disconnect = ab.add_alarm("kill_system_disconnect")
self.killed = False
# Initial check of kill status
self.get_status()
self.current_wrencher = ''
_set_wrencher = lambda msg: setattr(self, 'current_wrencher', msg.data)
rospy.Subscriber("/wrench/current", String, _set_wrencher)
al = AlarmListener("kill", self.alarm_kill_cb)
# Dict of op-codes to functions that need to be run with each code for aysnc responses.
self.update_cbs = {'\x10': self.set_kill, '\x11': self.set_unkill,
'\x12': lambda: True, '\x13': lambda: True, '\x14': lambda: True, '\x15': lambda: True,
'\x16': lambda: True, '\x17': lambda: True, '\x18': lambda: True, '\x19': lambda: True,
'\x1A': lambda: True, '\x1B': lambda: True,
'\x1C': lambda: True, '\x1D': lambda: True,
'\x1E': self.disconnect.clear_alarm, '\x1F': self.disconnect.raise_alarm,}
while not rospy.is_shutdown():
rospy.sleep(.1)
while self.ser.inWaiting() > 0:
self.check_buffer()
self.control_check()
self.get_status()
def test_add_alarm(self):
'''Ensure that adding an alarm succeeds without errors'''
broadcaster = AlarmBroadcaster()
alarm = broadcaster.add_alarm(
name='wake-up',
action_required=True,
severity=1,
problem_description='This is a problem',
parameters={"concern": ["a", "b", "c"]}
)
self.assertIsNotNone(alarm)
def __init__(self):
self.force_scale = rospy.get_param("~force_scale", 600)
self.torque_scale = rospy.get_param("~torque_scale", 500)
self.wrench_choices = itertools.cycle(['rc', 'autonomous'])
self.current_pose = Odometry()
self.active = False
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name='kill',
action_required=True,
severity=0
)
self.station_hold = self.alarm_broadcaster.add_alarm(
name='station_hold',
action_required=False,
severity=3
)
# self.docking_alarm = self.alarm_broadcaster.add_alarm(
# name='docking',
# action_required=True,
# severity=0
# )
self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1)
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
self.reset()
def __init__(self, controller_name, wrench_pub=None):
self.name = controller_name
self.wrench_choices = itertools.cycle(["rc", "emergency", "keyboard", "autonomous"])
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name="rc_kill",
action_required=True,
severity=0
)
self.station_hold_alarm = self.alarm_broadcaster.add_alarm(
name="station_hold",
action_required=False,
severity=3
)
# rospy.wait_for_service("/change_wrench")
self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)
self.kill_listener = AlarmListener("rc_kill", self._update_kill_status)
if (wrench_pub is None):
self.wrench_pub = wrench_pub
else:
self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1)
self.shooter_load_client = actionlib.SimpleActionClient("/shooter/load", ShooterDoAction)
self.shooter_fire_client = actionlib.SimpleActionClient("/shooter/fire", ShooterDoAction)
self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger)
self.shooter_manual_client = rospy.ServiceProxy("/shooter/manual", ShooterManual)
self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger)
self.is_killed = False
self.is_timed_out = False
self.clear_wrench()
class HandlerBase(object):
'''
We will listen for an alarm with this `alarm_name`.
When that alarm is raised, self.handle will be called.
When that alarm is cleared, self.cancel will be called.
'''
alarm_name = 'generic_name'
alarm_broadcaster = AlarmBroadcaster()
def handle(self, alarm, time_sent, parameters):
rospy.logwarn("No handle function defined for '{}'.".format(
alarm.alarm_name))
return
def cancel(self, alarm, time_sent, parameters):
rospy.logwarn("No cancel function defined for '{}'.".format(
alarm.alarm_name))
return
def __init__(self):
# Initialize the average voltage to none for the case of no feedback
self.voltage = None
# Initialize a list to hold the 1000 most recent supply voltage readings
# Holding 1000 values ensures that changes in the average are gradual rather than sharp
self.supply_voltages = []
# The publisher for the averaged voltage
self.pub_voltage = rospy.Publisher("/battery_monitor", Float32, queue_size=1)
# Subscribes to the feedback from each of the four thrusters
rospy.Subscriber("/FL_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/FR_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/BL_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/BR_motor/feedback", Feedback, self.add_voltage)
# Attempts to read the battery voltage parameters (sets them to defaults if they have not been set)
self.battery_low_voltage = rospy.get_param("~battery_low_voltage", 26)
self.battery_critical_voltage = rospy.get_param("~battery_critical_voltage", 24)
self.battery_kill_voltage = rospy.get_param("~battery_kill_voltage", 20)
# Sets up the battery voltage alarms
alarm_broadcaster = AlarmBroadcaster()
self.battery_status_unknown_alarm = alarm_broadcaster.add_alarm(
name="battery_status_unknown",
action_required=True,
severity=2
)
self.battery_low_alarm = alarm_broadcaster.add_alarm(
name="battery_low",
action_required=False,
severity=2
)
self.battery_critical_alarm = alarm_broadcaster.add_alarm(
name="battery_critical",
action_required=True,
severity=1
)
self.battery_kill_alarm = alarm_broadcaster.add_alarm(
name="kill",
action_required=True,
severity=0
)
def test_instantiate_alarm_broadcaster(self):
'''Ensure that the alarm broadcaster instantiates without errors'''
broadcaster = AlarmBroadcaster()
self.assertIsNotNone(broadcaster)
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.wrench_choices = itertools.cycle(['rc', 'autonomous'])
self.current_pose = Odometry()
self.active = False
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name='kill',
action_required=True,
severity=0
)
self.station_hold = self.alarm_broadcaster.add_alarm(
name='station_hold',
action_required=False,
severity=3
)
# self.docking_alarm = self.alarm_broadcaster.add_alarm(
# name='docking',
# action_required=True,
# severity=0
# )
self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1)
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
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 wack.
'''
self.last_change_mode = False
self.last_station_hold_state = False
self.last_kill = False
self.last_rc = False
self.last_auto = False
self.start_count = 0
self.last_joy = None
self.active = False
self.killed = False
# self.docking = False
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.wrench.force.x = 0
wrench.wrench.force.y = 0
wrench.wrench.torque.z = 0
self.wrench_pub.publish(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_cb(self, joy):
self.check_for_timeout(joy)
# Handle Button presses
start = joy.buttons[7]
change_mode = bool(joy.buttons[3]) # Y
kill = bool(joy.buttons[2]) # X
station_hold = bool(joy.buttons[0]) # A
# docking = bool(joy.buttons[1]) # B
rc_control = bool(joy.buttons[11]) # d-pad left
auto_control = bool(joy.buttons[12]) # d-pad right
# Reset controller state if only start is pressed down for awhile
self.start_count += start
if self.start_count > 10: # About 3 seconds
rospy.loginfo("Resetting controller state.")
self.reset()
self.active = True
self.kill_alarm.clear_alarm()
self.wrench_changer("rc")
if not self.active:
return
# Change vehicle mode
if change_mode == 1 and change_mode != self.last_change_mode:
mode = next(self.wrench_choices)
rospy.loginfo("Changing Control Mode: {}".format(mode))
self.wrench_changer(mode)
if rc_control == 1 and rc_control != self.last_rc:
rospy.loginfo("Changing Control to RC")
self.wrench_changer("rc")
if auto_control == 1 and auto_control != self.last_auto:
rospy.loginfo("Changing Control to Autonomous")
self.wrench_changer("autonomous")
# Station hold
if station_hold == 1 and station_hold != self.last_station_hold_state:
rospy.loginfo("Station Holding")
self.station_hold.raise_alarm()
self.wrench_changer("autonomous")
# Turn on full system kill
if kill == 1 and kill != self.last_kill:
rospy.loginfo("Toggling Kill")
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(
problem_description='System kill from location: joystick'
)
self.killed = not self.killed
# # Turn on docking mode
# if docking == 1 and docking != self.last_docking_state:
# rospy.loginfo("Toggling Docking")
# if self.docking:
# self.docking_alarm.clear_alarm()
# else:
# self.docking_alarm.raise_alarm(
# problem_description='Docking kill from location: joystick'
# )
# self.docking = not self.docking
self.last_start = start
self.last_change_mode = change_mode
self.last_kill = kill
self.last_station_hold_state = station_hold
# self.last_docking_state = docking
self.last_auto_control = auto_control
self.last_rc = rc_control
self.last_auto = auto_control
# Handle joystick commands
left_stick_x = joy.axes[1]
left_stick_y = joy.axes[0]
right_stick_y = joy.axes[3]
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.header.stamp = joy.header.stamp
wrench.wrench.force.x = self.force_scale * left_stick_x
wrench.wrench.force.y = self.force_scale * left_stick_y
wrench.wrench.torque.z = self.torque_scale * right_stick_y
self.wrench_pub.publish(wrench)
class RemoteControl(object):
def __init__(self, controller_name, wrench_pub=None):
self.name = controller_name
self.wrench_choices = itertools.cycle(["rc", "emergency", "keyboard", "autonomous"])
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name="rc_kill",
action_required=True,
severity=0
)
self.station_hold_alarm = self.alarm_broadcaster.add_alarm(
name="station_hold",
action_required=False,
severity=3
)
# rospy.wait_for_service("/change_wrench")
self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)
self.kill_listener = AlarmListener("rc_kill", self._update_kill_status)
if (wrench_pub is None):
self.wrench_pub = wrench_pub
else:
self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1)
self.shooter_load_client = actionlib.SimpleActionClient("/shooter/load", ShooterDoAction)
self.shooter_fire_client = actionlib.SimpleActionClient("/shooter/fire", ShooterDoAction)
self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger)
self.shooter_manual_client = rospy.ServiceProxy("/shooter/manual", ShooterManual)
self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger)
self.is_killed = False
self.is_timed_out = False
self.clear_wrench()
def _timeout_check(function):
'''
Simple decorator to check whether or not the remote control device is
timed out before running the function that was called.
'''
@functools.wraps(function)
def wrapper(self, *args, **kwargs):
if (not self.is_timed_out):
return function(self, *args, **kwargs)
return wrapper
def _update_kill_status(self, alarm):
'''
Updates the kill status display when there is an update on the kill
alarm.
'''
self.is_killed = not alarm.clear
@_timeout_check
def kill(self, *args, **kwargs):
'''
Kills the system regardless of what state it is in.
'''
rospy.loginfo("Killing")
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(
problem_description="System kill from location: {}".format(self.name)
)
@_timeout_check
def clear_kill(self, *args, **kwargs):
'''
Clears the system kill regardless of what state it is in.
'''
rospy.loginfo("Reviving")
self.wrench_changer("rc")
self.kill_alarm.clear_alarm()
@_timeout_check
def toggle_kill(self, *args, **kwargs):
'''
Toggles the kill status when the toggle_kill_button is pressed.
'''
rospy.loginfo("Toggling Kill")
# Responds to the kill broadcaster and checks the status of the kill alarm
if self.is_killed:
self.kill_alarm.clear_alarm()
else:
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(
problem_description="System kill from location: {}".format(self.name)
)
@_timeout_check
def station_hold(self, *args, **kwargs):
'''
Sets the goal point to the current location and switches to autonomous
mode in order to stay at that point.
'''
rospy.loginfo("Station Holding")
# Trigger station holding at the current pose
self.station_hold_alarm.raise_alarm(
problem_description="Request to station hold from: {}".format(self.name)
)
rospy.sleep(0.2)
self.station_hold_alarm.clear_alarm()
@_timeout_check
def select_autonomous_control(self, *args, **kwargs):
'''
Selects the autonomously generated trajectory as the active controller.
'''
rospy.loginfo("Changing Control to Autonomous")
self.wrench_changer("autonomous")
@_timeout_check
def select_rc_control(self, *args, **kwargs):
'''
Selects the XBox remote joystick as the active controller.
'''
rospy.loginfo("Changing Control to RC")
self.wrench_changer("rc")
def select_emergency_control(self, *args, **kwargs):
'''
Selects the emergency controller as the active controller.
'''
rospy.loginfo("Changing Control to Emergency Controller")
self.wrench_changer("emergency")
@_timeout_check
def select_keyboard_control(self, *args, **kwargs):
'''
Selects the keyboard teleoperation service as the active controller.
'''
rospy.loginfo("Changing Control to Keyboard")
self.wrench_changer("keyboard")
@_timeout_check
def select_next_control(self, *args, **kwargs):
'''
Selects the autonomously generated trajectory as the active controller.
'''
mode = next(self.wrench_choices)
rospy.loginfo("Changing Control Mode: {}".format(mode))
self.wrench_changer(mode)
def _shooter_load_feedback(self, status, result):
'''
Prints the feedback that is returned by the shooter load action client
'''
rospy.loginfo("Shooter Load Status={} Success={} Error={}".format(status, result.success, result.error))
@_timeout_check
def shooter_load(self, *args, **kwargs):
'''
Loads the shooter by using the action client to retract the linear actuator
'''
self.shooter_load_client.send_goal(goal=ShooterDoActionGoal(), done_cb=self._shooter_load_feedback)
rospy.loginfo("Kip, do not throw away your shot.")
def _shooter_fire_feedback(self, status, result):
'''
Prints the feedback that is returned by the shooter fire action client
'''
rospy.loginfo("Shooter Fire Status={} Success={} Error={}".format(status, result.success, result.error))
@_timeout_check
def shooter_fire(self, *args, **kwargs):
'''
Fires the shooter by using the action client to spin up the
acceleration discs and extend the linear actuator.
'''
self.shooter_fire_client.send_goal(goal=ShooterDoActionGoal(), done_cb=self._shooter_fire_feedback)
rospy.loginfo("One, two, three, four, five, six, seven, eight, nine. Number... TEN PACES! FIRE!")
@_timeout_check
def shooter_cancel(self, *args, **kwargs):
'''
Cancels the process that the shooter action client is currently
running.
'''
rospy.loginfo("Canceling shooter requests")
self.shooter_cancel_client(TriggerRequest())
rospy.loginfo("I imaging death so much it feels more like a memory.")
rospy.loginfo("When's it gonna get me? While I'm blocked? Seven clocks ahead of me?")
def _shooter_reset_helper(self, event):
'''
Used to actually call the shooter's reset service.
'''
rospy.loginfo("Reseting the shooter service")
self.shooter_reset_client(TriggerRequest())
rospy.loginfo("In New York you can be a new man! In New York you can be a new man!")
@_timeout_check
def shooter_reset(self, *args, **kwargs):
'''
Ensures that the shooter is fully retracted by initiating a retract and
using a ~6s delay before calling the actual reset service.
'''
self.shooter_linear_retract()
rospy.Timer(rospy.Duration(6), self._shooter_reset_helper, oneshot=True)
@_timeout_check
def shooter_linear_extend(self, *args, **kwargs):
'''
Extends the shooter's linear actuator by setting it's speed to full
forward
'''
rospy.loginfo("Extending the shooter's linear actuator")
self.shooter_manual_client(1, 0)
@_timeout_check
def shooter_linear_retract(self, *args, **kwargs):
'''
Retracts the shooter's linear actuator by setting it's speed to full
reverse
'''
rospy.loginfo("Retracting the shooter's linear actuator")
self.shooter_manual_client(-1, 0)
@_timeout_check
def set_disc_speed(self, speed, *args, **kwargs):
'''
Sets the shooters disc speed to the speed value passed in. The value is
a percentage from -100 to 100, which is scaled down to a number from -1
to 1.
'''
rospy.loginfo("Setting the shooter's accelerator disc speed to {}".format(speed))
self.shooter_manual_client(0, float(speed) / -100)
@_timeout_check
def publish_wrench(self, x, y, rotation, stamp=None, *args, **kwargs):
'''
Publishes a wrench to the specified node based on force inputs from the
controller.
'''
if (stamp is None):
stamp = rospy.Time.now()
if (self.wrench_pub is not None):
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.header.stamp = stamp
wrench.wrench.force.x = x
wrench.wrench.force.y = y
wrench.wrench.torque.z = rotation
self.wrench_pub.publish(wrench)
@_timeout_check
def clear_wrench(self, *args, **kwargs):
'''
Publishes a wrench to the specified node based on force inputs from the
controller.
'''
if (self.wrench_pub is not None):
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.header.stamp = rospy.Time.now()
wrench.wrench.force.x = 0
wrench.wrench.force.y = 0
wrench.wrench.torque.z = 0
self.wrench_pub.publish(wrench)
def __init__(
self,
port="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0",
baud=9600):
self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.25)
self.ser.flush()
self.timeout = rospy.Duration(1)
self.network_msg = None
update_network = lambda msg: setattr(self, "network_msg", msg)
self.network_listener = rospy.Subscriber("/keep_alive", Header,
update_network)
self.killstatus_pub = rospy.Publisher("/killstatus",
KillStatus,
queue_size=1)
ab = AlarmBroadcaster()
self.kill_alarm = ab.add_alarm(
"hw_kill", problem_description="Hardware kill from a kill switch.")
self.disconnect = ab.add_alarm("kill_system_disconnect")
self.killed = False
self.kill_status = {
'overall': False,
'PF': False,
'PA': False,
'SF': False,
'SA': False,
'computer': False,
'remote': False
}
# Dict of op-codes to functions that need to be run with each code for aysnc responses
self.update_cbs = {
'\x10': lambda: self.update('overall', True),
'\x11': lambda: self.update('overall', False),
'\x12': lambda: self.update('PF', True),
'\x13': lambda: self.update('PF', False),
'\x14': lambda: self.update('PA', True),
'\x15': lambda: self.update('PA', False),
'\x16': lambda: self.update('SF', True),
'\x17': lambda: self.update('SF', False),
'\x18': lambda: self.update('SA', True),
'\x19': lambda: self.update('SA', False),
'\x1A': lambda: self.update('remote', True),
'\x1B': lambda: self.update('remote', False),
'\x1C': lambda: self.update('computer', True),
'\x1D': lambda: self.update('computer', False),
'\x1E': self.disconnect.clear_alarm,
'\x1F': self.disconnect.raise_alarm
}
# Initial check of kill status
self.get_status()
self.current_wrencher = ''
_set_wrencher = lambda msg: setattr(self, 'current_wrencher', msg.data)
rospy.Subscriber("/wrench/current", String, _set_wrencher)
al = AlarmListener("kill", self.alarm_kill_cb)
rospy.Timer(rospy.Duration(.5), self.get_status)
while not rospy.is_shutdown():
self.control_check()
while self.ser.inWaiting() > 0 and not rospy.is_shutdown():
self.check_buffer()
if not self.network_kill():
self.ping()
else:
rospy.logerr("KILLBOARD: Network Kill!")
class RemoteControl(object):
def __init__(self, controller_name, wrench_pub=None):
self.name = controller_name
self.wrench_choices = itertools.cycle(["rc", "keyboard", "autonomous"])
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name="kill",
action_required=True,
severity=0
)
self.station_hold_alarm = self.alarm_broadcaster.add_alarm(
name="station_hold",
action_required=False,
severity=3
)
# rospy.wait_for_service("/change_wrench")
self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)
self.kill_listener = AlarmListener("kill", self._update_kill_status)
if (wrench_pub is None):
self.wrench_pub = wrench_pub
else:
self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1)
self.shooter_load_client = actionlib.SimpleActionClient("/shooter/load", ShooterDoAction)
self.shooter_fire_client = actionlib.SimpleActionClient("/shooter/fire", ShooterDoAction)
self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger)
self.shooter_manual_client = rospy.ServiceProxy("/shooter/manual", ShooterManual)
self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger)
self.is_killed = False
self.is_timed_out = False
self.clear_wrench()
def _timeout_check(function):
'''
Simple decorator to check whether or not the remote control device is
timed out before running the function that was called.
'''
@functools.wraps(function)
def wrapper(self, *args, **kwargs):
if (not self.is_timed_out):
return function(self, *args, **kwargs)
return wrapper
def _update_kill_status(self, alarm):
'''
Updates the kill status display when there is an update on the kill
alarm.
'''
self.is_killed = not alarm.clear
@_timeout_check
def kill(self, *args, **kwargs):
'''
Kills the system regardless of what state it is in.
'''
rospy.loginfo("Killing")
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(
problem_description="System kill from location: {}".format(self.name)
)
@_timeout_check
def clear_kill(self, *args, **kwargs):
'''
Clears the system kill regardless of what state it is in.
'''
rospy.loginfo("Reviving")
self.wrench_changer("rc")
self.kill_alarm.clear_alarm()
@_timeout_check
def toggle_kill(self, *args, **kwargs):
'''
Toggles the kill status when the toggle_kill_button is pressed.
'''
rospy.loginfo("Toggling Kill")
# Responds to the kill broadcaster and checks the status of the kill alarm
if self.is_killed:
self.kill_alarm.clear_alarm()
else:
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(
problem_description="System kill from location: {}".format(self.name)
)
@_timeout_check
def station_hold(self, *args, **kwargs):
'''
Sets the goal point to the current location and switches to autonomous
mode in order to stay at that point.
'''
rospy.loginfo("Station Holding")
# Trigger station holding at the current pose
self.station_hold_alarm.raise_alarm(
problem_description="Request to station hold from: {}".format(self.name)
)
rospy.sleep(0.2)
self.station_hold_alarm.clear_alarm()
@_timeout_check
def select_autonomous_control(self, *args, **kwargs):
'''
Selects the autonomously generated trajectory as the active controller.
'''
rospy.loginfo("Changing Control to Autonomous")
self.wrench_changer("autonomous")
@_timeout_check
def select_rc_control(self, *args, **kwargs):
'''
Selects the XBox remote joystick as the active controller.
'''
rospy.loginfo("Changing Control to RC")
self.wrench_changer("rc")
@_timeout_check
def select_keyboard_control(self, *args, **kwargs):
'''
Selects the keyboard teleoperation service as the active controller.
'''
rospy.loginfo("Changing Control to Keyboard")
self.wrench_changer("keyboard")
@_timeout_check
def select_next_control(self, *args, **kwargs):
'''
Selects the autonomously generated trajectory as the active controller.
'''
mode = next(self.wrench_choices)
rospy.loginfo("Changing Control Mode: {}".format(mode))
self.wrench_changer(mode)
def _shooter_load_feedback(self, status, result):
'''
Prints the feedback that is returned by the shooter load action client
'''
rospy.loginfo("Shooter Load Status={} Success={} Error={}".format(status, result.success, result.error))
@_timeout_check
def shooter_load(self, *args, **kwargs):
'''
Loads the shooter by using the action client to retract the linear actuator
'''
self.shooter_load_client.send_goal(goal=ShooterDoActionGoal(), done_cb=self._shooter_load_feedback)
rospy.loginfo("Kip, do not throw away your shot.")
def _shooter_fire_feedback(self, status, result):
'''
Prints the feedback that is returned by the shooter fire action client
'''
rospy.loginfo("Shooter Fire Status={} Success={} Error={}".format(status, result.success, result.error))
@_timeout_check
def shooter_fire(self, *args, **kwargs):
'''
Fires the shooter by using the action client to spin up the
acceleration discs and extend the linear actuator.
'''
self.shooter_fire_client.send_goal(goal=ShooterDoActionGoal(), done_cb=self._shooter_fire_feedback)
rospy.loginfo("One, two, three, four, five, six, seven, eight, nine. Number... TEN PACES! FIRE!")
@_timeout_check
def shooter_cancel(self, *args, **kwargs):
'''
Cancels the process that the shooter action client is currently
running.
'''
rospy.loginfo("Canceling shooter requests")
self.shooter_cancel_client(TriggerRequest())
rospy.loginfo("I imaging death so much it feels more like a memory.")
rospy.loginfo("When's it gonna get me? While I'm blocked? Seven clocks ahead of me?")
def _shooter_reset_helper(self, event):
'''
Used to actually call the shooter's reset service.
'''
rospy.loginfo("Reseting the shooter service")
self.shooter_reset_client(TriggerRequest())
rospy.loginfo("In New York you can be a new man! In New York you can be a new man!")
@_timeout_check
def shooter_reset(self, *args, **kwargs):
'''
Ensures that the shooter is fully retracted by initiating a retract and
using a ~6s delay before calling the actual reset service.
'''
self.shooter_linear_retract()
rospy.Timer(rospy.Duration(6), self._shooter_reset_helper, oneshot=True)
@_timeout_check
def shooter_linear_extend(self, *args, **kwargs):
'''
Extends the shooter's linear actuator by setting it's speed to full
forward
'''
rospy.loginfo("Extending the shooter's linear actuator")
self.shooter_manual_client(1, 0)
@_timeout_check
def shooter_linear_retract(self, *args, **kwargs):
'''
Retracts the shooter's linear actuator by setting it's speed to full
reverse
'''
rospy.loginfo("Retracting the shooter's linear actuator")
self.shooter_manual_client(-1, 0)
@_timeout_check
def set_disc_speed(self, speed, *args, **kwargs):
'''
Sets the shooters disc speed to the speed value passed in. The value is
a percentage from -100 to 100, which is scaled down to a number from -1
to 1.
'''
rospy.loginfo("Setting the shooter's accelerator disc speed to {}".format(speed))
self.shooter_manual_client(0, float(speed) / -100)
@_timeout_check
def publish_wrench(self, x, y, rotation, stamp=None, *args, **kwargs):
'''
Publishes a wrench to the specified node based on force inputs from the
controller.
'''
if (stamp is None):
stamp = rospy.Time.now()
if (self.wrench_pub is not None):
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.header.stamp = stamp
wrench.wrench.force.x = x
wrench.wrench.force.y = y
wrench.wrench.torque.z = rotation
self.wrench_pub.publish(wrench)
@_timeout_check
def clear_wrench(self, *args, **kwargs):
'''
Publishes a wrench to the specified node based on force inputs from the
controller.
'''
if (self.wrench_pub is not None):
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.header.stamp = rospy.Time.now()
wrench.wrench.force.x = 0
wrench.wrench.force.y = 0
wrench.wrench.torque.z = 0
self.wrench_pub.publish(wrench)
