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 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):
# 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
)
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 test_instantiate_alarm_broadcaster(self):
'''Ensure that the alarm broadcaster instantiates without errors'''
broadcaster = AlarmBroadcaster()
self.assertIsNotNone(broadcaster)
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!")
