def __init__(self, *args):
self._current_alarm_state = None
rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
super(killtest, self).__init__(*args)
self.AlarmListener = AlarmListener('hw-kill')
self.AlarmBroadcaster = AlarmBroadcaster('kill')
self.AlarmBroadcaster.clear_alarm()
def __init__(self):
self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
self.connected = False
self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
while not self.connected and not rospy.is_shutdown():
try:
self.connect()
self.connected = True
except serial.SerialException as e:
rospy.logerr('Cannot connect to kill board. {}'.format(e))
self.publish_diagnostics(e)
rospy.sleep(1)
rospy.loginfo('Board connected!')
self.board_status = {}
for kill in constants['KILLS']:
self.board_status[kill] = False
self.kills = self.board_status.keys()
self.expected_responses = []
self.network_msg = None
self.wrench = ''
self._hw_killed = False
self._last_hw_kill_paramaters = self.board_status
self.last_request = None
self.request_index = -1
self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
AlarmListener('hw-kill', self.hw_kill_alarm_cb)
AlarmListener('kill', self.kill_alarm_cb)
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber("/network", Header, self.network_cb) # Passes along network hearbeat to kill board
def __init__(self, controller_name, wrench_pub=None):
self.name = controller_name
self.wrench_choices = itertools.cycle(
["rc", "emergency", "keyboard", "autonomous"])
self.kill_broadcaster = AlarmBroadcaster('kill')
self.station_hold_broadcaster = AlarmBroadcaster('station-hold')
self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect)
self.kill_listener = AlarmListener(
'kill', callback_funct=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 __init__(self):
self.broadcaster = AlarmBroadcaster(self.alarm_name)
# Thruster topics to listen to
motor_topics = ['/FL_motor', '/FR_motor', '/BL_motor', '/BR_motor']
# Dictionary for the faults as defined in roboteq_msgs/Status
self.fault_codes = {
1: 'OVERHEAT',
2: 'OVERVOLTAGE',
4: 'UNDERVOLTAGE',
8: 'SHORT_CIRCUIT',
16: 'EMERGENCY_STOP',
32: 'SEPEX_EXCITATION_FAULT',
64: 'MOSFET_FAILURE',
128: 'STARTUP_CONFIG_FAULT'
}
self._raised = False
self._raised_alarms = {}
# Subscribe to the status message for all thruster topics
[
rospy.Subscriber(topic + '/status', Status, self._check_faults,
topic) for topic in motor_topics
]
class StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.move_client = SimpleActionClient('/move_to', MoveAction)
self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
self.broadcaster = AlarmBroadcaster(self.alarm_name)
def _client_cb(self, terminal_state, result):
if terminal_state != 3:
rospy.logwarn(
'Station hold goal failed (Status={}, Result={})'.format(
TerminalState.to_string(terminal_state), result))
return
try:
self.change_wrench('autonomous')
rospy.loginfo('Now station holding')
self.broadcaster.clear_alarm()
except rospy.ServiceException as e:
rospy.logwarn('Station hold changing wrench failed: {}'.format(e))
def raised(self, alarm):
rospy.loginfo("Attempting to station hold")
station_hold_goal = MoveGoal()
station_hold_goal.move_type = 'hold'
self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass
def __init__(self, port, baud=9600):
self.load_yaml()
rospy.init_node("actuator_driver")
self.disconnection_alarm = AlarmBroadcaster('actuator-board-disconnect')
self.packet_error_alarm = AlarmBroadcaster('actuator-board-packet-error')
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
# Reset all valves
rospy.loginfo("Resetting valves.")
for actuator_key in self.actuators:
actuator = self.actuators[actuator_key]
for valve_ports in actuator['ports']:
valve_port = actuator['ports'][valve_ports]
self.send_data(valve_port['id'], valve_port['default'])
rospy.loginfo("Valves ready to go.")
rospy.Service('~actuate', SetValve, self.got_service_request)
rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)
r = rospy.Rate(.2) # hz
while not rospy.is_shutdown():
# Heartbeat to make sure the board is still connected.
r.sleep()
self.ping()
class BatteryVoltage(HandlerBase):
alarm_name = 'battery-voltage'
def __init__(self):
self.broadcaster = AlarmBroadcaster(self.alarm_name)
self.low_threshold = rospy.get_param('~battery-voltage/low')
self.critical_threshold = rospy.get_param('~battery-voltage/critical')
self.voltage_sub = rospy.Subscriber('/battery_monitor',
Float32,
self._check_voltage,
queue_size=3)
self._raised = False
self._severity = 0
def _check_voltage(self, msg):
voltage = msg.data
do_raise = voltage < self.low_threshold
if do_raise:
severity = 2 if voltage < self.critical_threshold else 1
if not self._raised or self._severity != severity:
self.broadcaster.raise_alarm(
severity=severity,
problem_description='battery critcaly low'
if severity == 2 else 'battery low',
parameters={'voltage': voltage})
def raised(self, alarm):
self._raised = True
self._severity = alarm.severity
def cleared(self, alarm):
self._raised = False
self._severity = alarm.severity
def __init__(self, *args, **kwargs):
super(ThrusterAndKillBoard, self).__init__(*args, **kwargs)
# Initialize thruster mapping from params
self.thrusters = make_thruster_dictionary(
rospy.get_param('/thruster_layout/thrusters'))
# Tracks last hw-kill alarm update
self._last_hw_kill = None
# Tracks last soft kill status received from board
self._last_soft_kill = None
# Tracks last hard kill status received from board
self._last_hard_kill = None
# Tracks last go status broadcasted
self._last_go = None
# Used to raise/clear hw-kill when board updates
self._kill_broadcaster = AlarmBroadcaster('hw-kill')
# Alarm broadaster for GO command
self._go_alarm_broadcaster = AlarmBroadcaster('go')
# Listens to hw-kill updates to ensure another nodes doesn't manipulate it
self._hw_kill_listener = AlarmListener('hw-kill',
callback_funct=self.on_hw_kill)
# Provide service for alarm handler to set/clear the motherboard kill
self._unkill_service = rospy.Service('/set_mobo_kill', SetBool,
self.set_mobo_kill)
# Sends hearbeat to board
self._hearbeat_timer = rospy.Timer(rospy.Duration(0.4),
self.send_heartbeat)
# Create a subscribe for thruster commands
self._sub = rospy.Subscriber('/thrusters/thrust',
Thrust,
self.on_command,
queue_size=10)
class BatteryVoltage(HandlerBase):
alarm_name = 'battery-voltage'
def __init__(self):
self.broadcaster = AlarmBroadcaster(self.alarm_name)
self.low_threshold = rospy.get_param('~battery-voltage/low')
self.critical_threshold = rospy.get_param('~battery-voltage/critical')
self.voltage_sub = rospy.Subscriber('/battery_monitor', Float32, self._check_voltage, queue_size=3)
self._raised = False
self._severity = 0
def _check_voltage(self, msg):
voltage = msg.data
do_raise = voltage < self.low_threshold
if do_raise:
severity = 2 if voltage < self.critical_threshold else 1
if not self._raised or self._severity != severity:
self.broadcaster.raise_alarm(
severity=severity,
problem_description='battery critcaly low' if severity == 2 else 'battery low',
parameters={'voltage': voltage}
)
def raised(self, alarm):
self._raised = True
self._severity = alarm.severity
def cleared(self, alarm):
self._raised = False
self._severity = alarm.severity
def __init__(self, *args, **kwargs):
super(ThrusterAndKillBoard, self).__init__(*args, **kwargs)
# Initialize thruster mapping from params
self.thrusters = make_thruster_dictionary(
rospy.get_param('/thruster_layout/thrusters'))
# Tracks last hw-kill alarm update
self._last_hw_kill = None
# Tracks last soft kill status received from board
self._last_soft_kill = None
# Tracks last hard kill status received from board
self._last_hard_kill = None
# Tracks last go status broadcasted
self._last_go = None
# Used to raise/clear hw-kill when board updates
self._kill_broadcaster = AlarmBroadcaster('hw-kill')
# Alarm broadaster for GO command
self._go_alarm_broadcaster = AlarmBroadcaster('go')
# Listens to hw-kill updates to ensure another nodes doesn't manipulate it
self._hw_kill_listener = AlarmListener(
'hw-kill', callback_funct=self.on_hw_kill)
# Provide service for alarm handler to set/clear the motherboard kill
self._unkill_service = rospy.Service(
'/set_mobo_kill', SetBool, self.set_mobo_kill)
# Sends hearbeat to board
self._hearbeat_timer = rospy.Timer(
rospy.Duration(0.4), self.send_heartbeat)
# Create a subscribe for thruster commands
self._sub = rospy.Subscriber(
'/thrusters/thrust', Thrust, self.on_command, queue_size=10)
def __init__(self, port, baud=9600):
self.load_yaml()
rospy.init_node("actuator_driver")
self.disconnection_alarm = AlarmBroadcaster(
'actuator-board-disconnect')
self.packet_error_alarm = AlarmBroadcaster(
'actuator-board-packet-error')
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
# Reset all valves
rospy.loginfo("Resetting valves.")
for actuator_key in self.actuators:
actuator = self.actuators[actuator_key]
for valve_ports in actuator['ports']:
valve_port = actuator['ports'][valve_ports]
self.send_data(valve_port['id'], valve_port['default'])
rospy.loginfo("Valves ready to go.")
rospy.Service('~actuate', SetValve, self.got_service_request)
rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)
r = rospy.Rate(.2) # hz
while not rospy.is_shutdown():
# Heartbeat to make sure the board is still connected.
r.sleep()
self.ping()
class StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.move_client = SimpleActionClient('/move_to', MoveAction)
self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
self.broadcaster = AlarmBroadcaster(self.alarm_name)
def _client_cb(self, terminal_state, result):
if terminal_state != 3:
rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format(
TerminalState.to_string(terminal_state), result))
return
try:
self.change_wrench('autonomous')
rospy.loginfo('Now station holding')
self.broadcaster.clear_alarm()
except rospy.ServiceException as e:
rospy.logwarn('Station hold changing wrench failed: {}'.format(e))
def raised(self, alarm):
rospy.loginfo("Attempting to station hold")
station_hold_goal = MoveGoal()
station_hold_goal.move_type = 'hold'
self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass
def __init__(self, config_path, ports, thruster_definitions):
'''Thruster driver, an object for commanding all of the sub's thrusters
- Gather configuration data and make it available to other nodes
- Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters
- Track a thrust_dict, which maps thruster names to the appropriate port
- Given a command message, route that command to the appropriate port/thruster
- Send a thruster status message describing the status of the particular thruster
'''
self.thruster_heartbeats = {}
self.failed_thrusters = []
# Bus voltage
self.bus_voltage_estimator = BusVoltageEstimator(self._window_duration)
self.bus_voltage_pub = rospy.Publisher('bus_voltage',
Float64,
queue_size=1)
self.bus_timer = rospy.Timer(rospy.Duration(0.1),
self.publish_bus_voltage)
self.warn_voltage = rospy.get_param("/battery/warn_voltage", 44.5)
self.kill_voltage = rospy.get_param("/battery/kill_voltage", 44.0)
self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn(
"Running fake thrusters for simulation, based on parameter '/simulate'"
)
# Individual thruster configuration data
newtons, thruster_input = self.load_effort_to_thrust_map(config_path)
self.interpolate = scipy.interpolate.interp1d(newtons, thruster_input)
self.thrust_service = rospy.Service('thrusters/thruster_range',
ThrusterInfo,
self.get_thruster_info)
self.status_pub = rospy.Publisher('thrusters/thruster_status',
ThrusterStatus,
queue_size=8)
# Port and thruster layout
self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
AlarmListener("thruster-out",
self.check_alarm_status,
call_when_raised=False)
self.port_dict = self.load_thruster_layout(ports, thruster_definitions)
self.drop_check = rospy.Timer(rospy.Duration(0.5),
self.check_for_drops)
# The bread and bones
self.thrust_sub = rospy.Subscriber('thrusters/thrust',
Thrust,
self.thrust_cb,
queue_size=1)
# This is essentially only for testing
self.fail_thruster_server = rospy.Service('fail_thruster',
FailThruster,
self.fail_thruster)
def __init__(self):
self.port = rospy.get_param(
'~port',
"/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
self.connected = False
self.diagnostics_pub = rospy.Publisher("/diagnostics",
DiagnosticArray,
queue_size=3)
while not self.connected and not rospy.is_shutdown():
try:
self.connect()
self.connected = True
except serial.SerialException as e:
rospy.logerr('Cannot connect to kill board. {}'.format(e))
self.publish_diagnostics(e)
rospy.sleep(1)
rospy.loginfo('Board connected!')
self.board_status = {}
for kill in constants['KILLS']:
self.board_status[kill] = False
self.kills = self.board_status.keys()
self.expected_responses = []
self.network_msg = None
self.wrench = ''
self._hw_killed = False
self._last_hw_kill_paramaters = self.board_status
self.last_request = None
self.request_index = -1
self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
self.hw_kill_broadcaster.wait_for_server()
self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
self.ctrl_msg_received = False
self.ctrl_msg_count = 0
self.ctrl_msg_timeout = 0
self.sticks = {}
for stick in constants[
'CTRL_STICKS']: # These are 3 signed 16-bit values for stick positions
self.sticks[stick] = 0x0000
self.sticks_temp = 0x0000
self.buttons = {}
for button in constants[
'CTRL_BUTTONS']: # These are the button on/off states (16 possible inputs)
self.buttons[button] = False
self.buttons_temp = 0x0000
self._hw_kill_listener = AlarmListener('hw-kill',
self.hw_kill_alarm_cb)
self._kill_listener = AlarmListener('kill', self.kill_alarm_cb)
self._hw_kill_listener.wait_for_server()
self._kill_listener.wait_for_server()
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber(
"/network", Header,
self.network_cb) # Passes along network hearbeat to kill board
def __init__(self):
self.broadcaster = AlarmBroadcaster(self.alarm_name)
self.low_threshold = rospy.get_param('~battery-voltage/low')
self.critical_threshold = rospy.get_param('~battery-voltage/critical')
self.voltage_sub = rospy.Subscriber('/battery_monitor',
Float32,
self._check_voltage,
queue_size=3)
self._raised = False
self._severity = 0
def __init__(self):
gpio = rospy.get_param("/pause-kill/gpio", 507)
# Use sysfs to monitor gpio
self.gpio_file = '/sys/class/gpio/gpio{}/value'.format(gpio)
self.ab = AlarmBroadcaster(self.alarm_name, node_name='pause-kill')
self._killed = False
# The correct way would be use 'poll' syscall but meh
rospy.Timer(rospy.Duration(0.01), self._check)
def __init__(self, timeout=0.5):
self.timeout = rospy.Duration(timeout)
self.last_time = rospy.Time.now()
self.last_position = None
self.check_count = 0
self.max_jump = 1.0
self.odom_discontinuity = False
self._killed = False
self.odom_listener = rospy.Subscriber('/odom', Odometry, self.got_odom_msg, queue_size=1)
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Timer(rospy.Duration(0.1), self.check)
def __init__(self, ports_layout, thruster_definitions):
'''Thruster driver, an object for commanding all of the sub's thrusters
- Gather configuration data and make it available to other nodes
- Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters
- Track a thrust_dict, which maps thruster names to the appropriate port
- Given a command message, route that command to the appropriate port/thruster
- Send a thruster status message describing the status of the particular thruster
'''
self.failed_thrusters = set() # This is only determined by comms
self.deactivated_thrusters = set(
) # These will not come back online even if comms are good (user managed)
# Alarms
self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
AlarmListener("thruster-out",
self.check_alarm_status,
call_when_raised=False) # Prevent outside interference
# Create ThrusterPort objects in a dict indexed by port name
self.load_thruster_ports(ports_layout, thruster_definitions)
# Feedback on thrusters (thruster mapper blocks until it can use this service)
self.thruster_info_service = rospy.Service('thrusters/thruster_info',
ThrusterInfo,
self.get_thruster_info)
self.status_publishers = {
name: rospy.Publisher('thrusters/status/' + name,
ThrusterStatus,
queue_size=10)
for name in self.thruster_to_port_map.keys()
}
# These alarms require this service to be available before things will work
rospy.wait_for_service("update_thruster_layout")
self.update_thruster_out_alarm()
# Bus voltage
self.bus_voltage_monitor = BusVoltageMonitor(self._window_duration)
# Command thrusters
self.thrust_sub = rospy.Subscriber('thrusters/thrust',
Thrust,
self.thrust_cb,
queue_size=1)
# To programmatically deactivate thrusters
self.fail_thruster_server = rospy.Service('fail_thruster',
FailThruster,
self.fail_thruster)
self.unfail_thruster_server = rospy.Service('unfail_thruster',
UnfailThruster,
self.unfail_thruster)
class OdomKill(HandlerBase):
'''
Alarm raised when either Odometry has not been published in a while or the euclidean distance
between the positions in the last two messages is too large, indicating a sensor error or other
state estimation issue that will cause unsafe behavior
'''
alarm_name = 'odom-kill'
TIMEOUT_SECONDS = 0.1 # Max time delta between Odometry messages before alarm is raised
MAX_DISTANCE_METERS = 0.5 # Max distance in position between Odometry messages before alarm is raised
def __init__(self):
self.hm = HeartbeatMonitor(self.alarm_name,
"/odom",
Odometry,
node_name="alarm_server",
prd=self.TIMEOUT_SECONDS)
self.MAX_JUMP = 0.5
self.launch_time = rospy.Time.now()
self.last_time = self.launch_time
self.last_position = None
self._raised = False
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Subscriber('/odom',
Odometry,
self.check_continuity,
queue_size=5)
def check_continuity(self, odom):
'''
On new odom message, find distance in position between this message and the last one.
If the distance is > MAX_JUMP, raise the alarm
'''
position = rosmsg_to_numpy(odom.pose.pose.position)
if self.last_position is not None:
jump = np.linalg.norm(position - self.last_position)
if jump > self.MAX_JUMP and not self._killed:
self._raised = True # Avoid raising multiple times
rospy.logwarn('ODOM DISCONTINUITY DETECTED')
self.ab.raise_alarm(
problem_description=
'ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format(
jump),
severity=5)
self.last_position = position
self.last_time = odom.header.stamp
def raised(self, alarm):
self._raised = True
def cleared(self, alarm):
self._raised = False
def __init__(self, window_duration):
'''
window_duration - float (amount of seconds for which to keep a reading in the buffer)
'''
self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")
self.bus_voltage_pub = rospy.Publisher('bus_voltage',
Float64,
queue_size=1)
self.warn_voltage = rospy.get_param("/battery/warn_voltage", 44.5)
self.kill_voltage = rospy.get_param("/battery/kill_voltage", 44.0)
self.last_estimate_time = rospy.Time.now()
self.WINDOW_DURATION = rospy.Duration(window_duration)
self.ESTIMATION_PERIOD = rospy.Duration(0.2)
self.cached_severity = 0
self.buffer = []
def __init__(self):
self.hm = HeartbeatMonitor(self.alarm_name,
"/odom",
Odometry,
node_name="alarm_server",
prd=self.TIMEOUT_SECONDS)
self.MAX_JUMP = 0.5
self.launch_time = rospy.Time.now()
self.last_time = self.launch_time
self.last_position = None
self._raised = False
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Subscriber('/odom',
Odometry,
self.check_continuity,
queue_size=5)
def __init__(self):
self.broadcaster = AlarmBroadcaster(self.alarm_name)
self.low_threshold = rospy.get_param('~battery-voltage/low')
self.critical_threshold = rospy.get_param('~battery-voltage/critical')
self.voltage_sub = rospy.Subscriber('/battery_monitor', Float32, self._check_voltage, queue_size=3)
self._raised = False
self._severity = 0
def __init__(self, *args):
self._current_alarm_state = None
rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
super(killtest, self).__init__(*args)
self.AlarmListener = AlarmListener('hw-kill')
self.AlarmBroadcaster = AlarmBroadcaster('kill')
self.AlarmBroadcaster.clear_alarm()
def __init__(self):
self._killed = False
self._update_height()
self.ab = AlarmBroadcaster(self.alarm_name,
node_name="height_over_bottom_kill")
# Keep track of the current height
self._last_height = 100
set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)
# Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
rospy.Timer(rospy.Duration(5), self._update_height)
# This should smooth out random dips below the limit
rospy.Timer(rospy.Duration(0.5), self._do_check)
def __init__(self, timeout=0.5):
self.GRACE_PERIOD = rospy.Duration(
5.0) # Alarms won't be raised within grace period
self.TIMEOUT = rospy.Duration(timeout)
self.MAX_JUMP = 0.5
self.LAUNCH_TIME = rospy.Time.now()
self.last_time = self.LAUNCH_TIME
self.last_position = None
self.check_count = 0
self.odom_discontinuity = False
self._killed = False
self.odom_listener = rospy.Subscriber('/odom',
Odometry,
self.got_odom_msg,
queue_size=1)
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Timer(rospy.Duration(0.1), self.check)
class PauseKill(HandlerBase):
alarm_name = "pause-kill"
def __init__(self):
gpio = rospy.get_param("/pause-kill/gpio", 507)
# Use sysfs to monitor gpio
self.gpio_file = '/sys/class/gpio/gpio{}/value'.format(gpio)
self.ab = AlarmBroadcaster(self.alarm_name, node_name='pause-kill')
self._killed = False
# The correct way would be use 'poll' syscall but meh
rospy.Timer(rospy.Duration(0.01), self._check)
def _check(self, *args):
try:
'''
The following must be completed to enable GPIO sysfs
echo "507" > /sys/class/gpio/export
echo "in" > /sys/class/gpio/gpio507/direction
chmod 777 /sys/class/gpio/gpio507/value
'''
file_open = open(self.gpio_file, 'r')
except IOError:
rospy.logwarn_throttle(60, 'Is Kill Plug GPIO enabled via sysfs?')
return
except Exception:
rospy.logwarn_throttle(
60, 'There was an error in opening GPIO for Kill Plug')
return
res = file_open.read(1)
file_open.close()
if not self._killed and not int(res):
self._killed = True
self.ab.raise_alarm(problem_description='KILL PULLED', severity=5)
elif self._killed and int(res):
self._killed = False
self.ab.clear_alarm()
def raised(self, alarm):
self._killed = True
def cleared(self, alarm):
self._killed = False
def __init__(self):
self.hm = HeartbeatMonitor(self.alarm_name, "/odom", Odometry,
node_name="alarm_server", prd=self.TIMEOUT_SECONDS)
self.MAX_JUMP = 0.5
self.launch_time = rospy.Time.now()
self.last_time = self.launch_time
self.last_position = None
self._raised = False
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Subscriber('/odom', Odometry, self.check_continuity, queue_size=5)
class OdomKill(HandlerBase):
'''
Alarm raised when either Odometry has not been published in a while or the euclidean distance
between the positions in the last two messages is too large, indicating a sensor error or other
state estimation issue that will cause unsafe behavior
'''
alarm_name = 'odom-kill'
TIMEOUT_SECONDS = 0.1 # Max time delta between Odometry messages before alarm is raised
MAX_DISTANCE_METERS = 0.5 # Max distance in position between Odometry messages before alarm is raised
def __init__(self):
self.hm = HeartbeatMonitor(self.alarm_name, "/odom", Odometry,
node_name="alarm_server", prd=self.TIMEOUT_SECONDS)
self.MAX_JUMP = 0.5
self.launch_time = rospy.Time.now()
self.last_time = self.launch_time
self.last_position = None
self._raised = False
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Subscriber('/odom', Odometry, self.check_continuity, queue_size=5)
def check_continuity(self, odom):
'''
On new odom message, find distance in position between this message and the last one.
If the distance is > MAX_JUMP, raise the alarm
'''
position = rosmsg_to_numpy(odom.pose.pose.position)
if self.last_position is not None:
jump = np.linalg.norm(position - self.last_position)
if jump > self.MAX_JUMP and not self._killed:
self._raised = True # Avoid raising multiple times
rospy.logwarn('ODOM DISCONTINUITY DETECTED')
self.ab.raise_alarm(problem_description='ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format(jump),
severity=5)
self.last_position = position
self.last_time = odom.header.stamp
def raised(self, alarm):
self._raised = True
def cleared(self, alarm):
self._raised = False
class HeightOverBottom(HandlerBase):
alarm_name = "height-over-bottom"
def __init__(self):
self._killed = False
self._update_height()
self.ab = AlarmBroadcaster(self.alarm_name,
node_name="height_over_bottom_kill")
# Keep track of the current height
self._last_height = 100
set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)
# Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
rospy.Timer(rospy.Duration(5), self._update_height)
# This should smooth out random dips below the limit
rospy.Timer(rospy.Duration(0.5), self._do_check)
def _do_check(self, *args):
if self._last_height <= self._height_to_kill and not self._killed:
rospy.logwarn("SUB TOO LOW!")
self.ab.raise_alarm(
problem_description="The sub was too low: {}".format(
self._last_height),
parameters={"height": self._last_height},
severity=5)
elif self._last_height >= self._height_to_kill and self._killed:
rospy.logwarn("REVIVING")
self.ab.clear_alarm()
def _update_height(self, *args):
self._height_to_kill = rospy.get_param("/height_over_bottom", 0.4)
def raised(self, alarm):
self._killed = True
def cleared(self, alarm):
self._killed = False
def test_callbacks(self):
al_a = AlarmListener("alarm_d")
ab_a = AlarmBroadcaster("alarm_d")
self.raised = False
self.cleared = False
self.both = False
ab_a.clear_alarm()
al_a.add_callback(self.raised_cb, call_when_cleared=False)
al_a.add_callback(self.cleared_cb, call_when_raised=False)
al_a.add_callback(self.both_cb)
rospy.sleep(0.5)
# Make sure callbacks are called on creation
self.assertFalse(self.raised)
self.assertTrue(self.cleared)
self.assertTrue(self.both)
self.raised = False
self.cleared = False
self.both = False
# Make sure callbacks run when they're suppsed to
ab_a.raise_alarm()
rospy.sleep(0.5)
self.assertTrue(self.raised)
self.assertFalse(self.cleared)
self.assertTrue(self.both)
self.raised = False
self.cleared = False
self.both = False
class HeightOverBottom(HandlerBase):
alarm_name = "height-over-bottom"
def __init__(self):
self._killed = False
self._update_height()
self.ab = AlarmBroadcaster(self.alarm_name, node_name="height_over_bottom_kill")
# Keep track of the current height
self._last_height = 100
set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)
# Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
rospy.Timer(rospy.Duration(5), self._update_height)
# This should smooth out random dips below the limit
rospy.Timer(rospy.Duration(0.5), self._do_check)
def _do_check(self, *args):
if self._last_height <= self._height_to_kill and not self._killed:
rospy.logwarn("SUB TOO LOW!")
self.ab.raise_alarm(problem_description="The sub was too low: {}".format(self._last_height),
parameters={"height": self._last_height},
severity=5
)
elif self._last_height >= self._height_to_kill and self._killed:
rospy.logwarn("REVIVING")
self.ab.clear_alarm()
def _update_height(self, *args):
self._height_to_kill = rospy.get_param("/height_over_bottom", 0.4)
def raised(self, alarm):
self._killed = True
def cleared(self, alarm):
self._killed = False
class StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.task_client = TaskClient()
self.broadcaster = AlarmBroadcaster(self.alarm_name)
def _client_cb(self, terminal_state, result):
if terminal_state != 3:
rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format(
TerminalState.to_string(terminal_state), result.result))
return
rospy.loginfo('Station holding!')
self.broadcaster.clear_alarm()
def raised(self, alarm):
rospy.loginfo("Attempting to station hold")
self.task_client.run_task('StationHold', done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass
def __init__(self, timeout=0.5):
self.GRACE_PERIOD = rospy.Duration(5.0) # Alarms won't be raised within grace period
self.TIMEOUT = rospy.Duration(timeout)
self.MAX_JUMP = 0.5
self.LAUNCH_TIME = rospy.Time.now()
self.last_time = self.LAUNCH_TIME
self.last_position = None
self.check_count = 0
self.odom_discontinuity = False
self._killed = False
self.odom_listener = rospy.Subscriber('/odom', Odometry, self.got_odom_msg, queue_size=1)
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Timer(rospy.Duration(0.1), self.check)
def __init__(self, window_duration):
'''
window_duration - float (amount of seconds for which to keep a reading in the buffer)
'''
self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")
self.bus_voltage_pub = rospy.Publisher('bus_voltage', Float64, queue_size=1)
self.warn_voltage = rospy.get_param("/battery/warn_voltage", 44.5)
self.kill_voltage = rospy.get_param("/battery/kill_voltage", 44.0)
self.last_estimate_time = rospy.Time.now()
self.WINDOW_DURATION = rospy.Duration(window_duration)
self.ESTIMATION_PERIOD = rospy.Duration(0.2)
self.cached_severity = 0
self.buffer = []
class StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.task_client = TaskClient()
self.broadcaster = AlarmBroadcaster(self.alarm_name)
def _client_cb(self, terminal_state, result):
if terminal_state != 3:
rospy.logwarn(
'Station hold goal failed (Status={}, Result={})'.format(
TerminalState.to_string(terminal_state), result.result))
return
rospy.loginfo('Station holding!')
self.broadcaster.clear_alarm()
def raised(self, alarm):
rospy.loginfo("Attempting to station hold")
self.task_client.run_task('StationHold', done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass
def __init__(self, controller_name, wrench_pub=None):
self.name = controller_name
self.wrench_choices = itertools.cycle(["rc", "emergency", "keyboard", "autonomous"])
self.kill_broadcaster = AlarmBroadcaster('kill')
self.station_hold_broadcaster = AlarmBroadcaster('station-hold')
self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect)
self.kill_listener = AlarmListener('kill', callback_funct=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 __init__(self):
self._killed = False
self._update_height()
self.ab = AlarmBroadcaster(self.alarm_name, node_name="height_over_bottom_kill")
# Keep track of the current height
self._last_height = 100
set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)
# Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
rospy.Timer(rospy.Duration(5), self._update_height)
# This should smooth out random dips below the limit
rospy.Timer(rospy.Duration(0.5), self._do_check)
def __init__(self):
self.broadcaster = AlarmBroadcaster(self.alarm_name)
# Thruster topics to listen to
motor_topics = ['/FL_motor', '/FR_motor', '/BL_motor', '/BR_motor']
# Subscribe to the status message for all thruster topics
[rospy.Subscriber(
topic + '/status', Status, self._check_faults, topic) for topic in motor_topics]
# Dictionary for the faults as defined in roboteq_msgs/Status
self.fault_codes = {1: 'OVERHEAT', 2: 'OVERVOLTAGE', 4: 'UNDERVOLTAGE', 8: 'SHORT_CIRCUIT',
16: 'EMERGENCY_STOP', 32: 'SEPEX_EXCITATION_FAULT', 64: 'MOSFET_FAILURE',
128: 'STARTUP_CONFIG_FAULT'}
self._raised = False
self._raised_alarms = {}
def __init__(self):
self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
self.connected = False
self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
while not self.connected and not rospy.is_shutdown():
try:
self.connect()
self.connected = True
except serial.SerialException as e:
rospy.logerr('Cannot connect to kill board. {}'.format(e))
self.publish_diagnostics(e)
rospy.sleep(1)
rospy.loginfo('Board connected!')
self.board_status = {}
for kill in constants['KILLS']:
self.board_status[kill] = False
self.kills = self.board_status.keys()
self.expected_responses = []
self.network_msg = None
self.wrench = ''
self._hw_killed = False
self._last_hw_kill_paramaters = self.board_status
self.last_request = None
self.request_index = -1
self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
self.ctrl_msg_received = False
self.ctrl_msg_count = 0
self.ctrl_msg_timeout = 0
self.sticks = {}
for stick in constants['CTRL_STICKS']: # These are 3 signed 16-bit values for stick positions
self.sticks[stick] = 0x0000
self.sticks_temp = 0x0000
self.buttons = {}
for button in constants['CTRL_BUTTONS']: # These are the button on/off states (16 possible inputs)
self.buttons[button] = False
self.buttons_temp = 0x0000
AlarmListener('hw-kill', self.hw_kill_alarm_cb)
AlarmListener('kill', self.kill_alarm_cb)
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber("/network", Header, self.network_cb) # Passes along network hearbeat to kill board
def __init__(self, ports_layout, thruster_definitions):
'''Thruster driver, an object for commanding all of the sub's thrusters
- Gather configuration data and make it available to other nodes
- Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters
- Track a thrust_dict, which maps thruster names to the appropriate port
- Given a command message, route that command to the appropriate port/thruster
- Send a thruster status message describing the status of the particular thruster
'''
self.failed_thrusters = set() # This is only determined by comms
self.deactivated_thrusters = set() # These will not come back online even if comms are good (user managed)
# Alarms
self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
AlarmListener("thruster-out", self.check_alarm_status, call_when_raised=False) # Prevent outside interference
# Create ThrusterPort objects in a dict indexed by port name
self.load_thruster_ports(ports_layout, thruster_definitions)
# Feedback on thrusters (thruster mapper blocks until it can use this service)
self.thruster_info_service = rospy.Service('thrusters/thruster_info', ThrusterInfo, self.get_thruster_info)
self.status_publishers = {name: rospy.Publisher('thrusters/status/' + name, ThrusterStatus, queue_size=10)
for name in self.thruster_to_port_map.keys()}
# These alarms require this service to be available before things will work
rospy.wait_for_service("update_thruster_layout")
self.update_thruster_out_alarm()
# Bus voltage
self.bus_voltage_monitor = BusVoltageMonitor(self._window_duration)
# Command thrusters
self.thrust_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=1)
# To programmatically deactivate thrusters
self.fail_thruster_server = rospy.Service('fail_thruster', FailThruster, self.fail_thruster)
self.unfail_thruster_server = rospy.Service('unfail_thruster', UnfailThruster, self.unfail_thruster)
class KillInterface(object):
"""
Driver to interface with NaviGator's kill handeling board, which disconnects power to actuators
if any of 4 emergency buttons is pressed, a software kill command is sent, or the network hearbeat
stops. This driver enables the software kill option via ros_alarms and outputs diagnostics
data about the board to ROS. The driver can handle the board's asyncronous updates of kill statuses
and will also periodicly request updates in case the async is not working (it often doesn't).
"""
ALARM = 'hw-kill' # Alarm to raise when hardware kill is detected
YELLOW_WRENCHES = ['rc', '/wrench/rc', 'keyboard', '/wrench/keyboard'
] # Wrenches which activate YELLOW LED
GREEN_WRENCHES = ['autonomous', '/wrench/autonomous'
] # Wrenches which activate GREEN LED
def __init__(self):
self.port = rospy.get_param(
'~port',
"/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
self.connected = False
self.diagnostics_pub = rospy.Publisher("/diagnostics",
DiagnosticArray,
queue_size=3)
while not self.connected and not rospy.is_shutdown():
try:
self.connect()
self.connected = True
except serial.SerialException as e:
rospy.logerr('Cannot connect to kill board. {}'.format(e))
self.publish_diagnostics(e)
rospy.sleep(1)
rospy.loginfo('Board connected!')
self.board_status = {}
for kill in constants['KILLS']:
self.board_status[kill] = False
self.kills = self.board_status.keys()
self.expected_responses = []
self.network_msg = None
self.wrench = ''
self._hw_killed = False
self._last_hw_kill_paramaters = self.board_status
self.last_request = None
self.request_index = -1
self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
self.hw_kill_broadcaster.wait_for_server()
self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
self.ctrl_msg_received = False
self.ctrl_msg_count = 0
self.ctrl_msg_timeout = 0
self.sticks = {}
for stick in constants[
'CTRL_STICKS']: # These are 3 signed 16-bit values for stick positions
self.sticks[stick] = 0x0000
self.sticks_temp = 0x0000
self.buttons = {}
for button in constants[
'CTRL_BUTTONS']: # These are the button on/off states (16 possible inputs)
self.buttons[button] = False
self.buttons_temp = 0x0000
self._hw_kill_listener = AlarmListener('hw-kill',
self.hw_kill_alarm_cb)
self._kill_listener = AlarmListener('kill', self.kill_alarm_cb)
self._hw_kill_listener.wait_for_server()
self._kill_listener.wait_for_server()
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber(
"/network", Header,
self.network_cb) # Passes along network hearbeat to kill board
def connect(self):
if rospy.get_param(
'/is_simulation', False
): # If in Gazebo, run fake serial class following board's protocol
from navigator_kill_board import SimulatedKillBoard
self.ser = SimulatedKillBoard()
else:
baud = rospy.get_param('~baud', 9600)
self.ser = serial.Serial(port=self.port, baudrate=baud)
def run(self):
'''
Main loop for driver, at a fixed rate updates alarms and diagnostics output with new
kill board output.
'''
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self.last_request is None:
self.request_next()
self.receive()
self.update_ros()
rate.sleep()
def update_ros(self):
self.update_hw_kill()
self.publish_diagnostics()
if self.ctrl_msg_received is True:
self.publish_joy()
self.ctrl_msg_received = False
def handle_byte(self, msg):
'''
React to a byte recieved from the board. This could by an async update of a kill status or
a known response to a recent request
'''
# If the controller message start byte is received, next 8 bytes are the controller data
if msg == constants['CONTROLLER']:
self.ctrl_msg_count = 8
self.ctrl_msg_timeout = rospy.Time.now()
return
# If receiving the controller message, record the byte as stick/button data
if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 8):
# If 1 second has passed since the message began, timeout and report warning
if (rospy.Time.now() - self.ctrl_msg_timeout) >= rospy.Duration(1):
self.ctrl_msg_received = False
self.ctrl_msg_count = 0
rospy.logwarn(
'Timeout receiving controller message. Please disconnect controller.'
)
if self.ctrl_msg_count > 2: # The first 6 bytes in the message are stick data bytes
if (self.ctrl_msg_count %
2) == 0: # Even number byte: first byte in data word
self.sticks_temp = (int(msg.encode("hex"), 16) << 8)
else: # Odd number byte: combine two bytes into a stick's data word
self.sticks_temp += int(msg.encode("hex"), 16)
if (self.ctrl_msg_count > 6):
self.sticks['UD'] = self.sticks_temp
elif (self.ctrl_msg_count > 4):
self.sticks['LR'] = self.sticks_temp
else:
self.sticks['TQ'] = self.sticks_temp
self.sticks_temp = 0x0000
else: # The last 2 bytes are button data bytes
if (self.ctrl_msg_count % 2) == 0:
self.buttons_temp = (int(msg.encode("hex"), 16) << 8)
else: # Combine two bytes into the button data word
self.buttons_temp += int(msg.encode("hex"), 16)
for button in self.buttons: # Each of the 16 bits represents a button on/off state
button_check = int(
constants['CTRL_BUTTONS_VALUES'][button].encode(
"hex"), 16)
self.buttons[button] = ((
self.buttons_temp & button_check) == button_check)
self.buttons_temp = 0x0000
self.ctrl_msg_received = True # After receiving last byte, trigger joy update
self.ctrl_msg_count -= 1
return
# If a response has been recieved to a requested status (button, remove, etc), update internal state
if self.last_request is not None:
if msg == constants['RESPONSE_FALSE']:
if self.board_status[self.last_request] is True:
rospy.logdebug('SYNC FALSE for {}'.format(
self.last_request))
self.board_status[self.last_request] = False
self.last_request = None
return
if msg == constants['RESPONSE_TRUE']:
if self.board_status[self.last_request] is False:
rospy.logdebug('SYNC TRUE for {}'.format(
self.last_request))
self.board_status[self.last_request] = True
self.last_request = None
return
# If an async update was recieved, update internal state
for kill in self.board_status:
if msg == constants[kill]['FALSE']:
if self.board_status[kill] is True:
rospy.logdebug('ASYNC FALSE for {}'.format(
self.last_request))
self.board_status[kill] = False
return
if msg == constants[kill]['TRUE']:
if self.board_status[kill] is False:
rospy.logdebug('ASYNC TRUE FOR {}'.format(kill))
self.board_status[kill] = True
return
# If a response to another request, like ping or computer kill/clear is recieved
for index, byte in enumerate(self.expected_responses):
if msg == byte:
del self.expected_responses[index]
return
# Log a warning if an unexpected byte was recieved
rospy.logwarn(
'Recieved an unexpected byte {}, remaining expected_responses={}'.
format(hex(ord(msg)), len(self.expected_responses)))
@thread_lock(lock)
def receive(self):
'''
Recieve update bytes sent from the board without requests being sent, updating internal
state, raising alarms, etc in response to board updates. Clears the in line buffer.
'''
while self.ser.in_waiting > 0 and not rospy.is_shutdown():
msg = self.ser.read(1)
self.handle_byte(msg)
def request_next(self):
'''
Manually request status of the next kill, looping back to the first
once all have been responsded to.
'''
self.request_index += 1
if self.request_index == len(self.kills):
self.request_index = 0
self.last_request = self.kills[self.request_index]
self.request(constants[self.last_request]['REQUEST'])
def wrench_cb(self, msg):
'''
Updates wrench (autnomous vs teleop) diagnostic light if nessesary
on wrench changes
'''
wrench = msg.data
if wrench != self.wrench:
if wrench in self.YELLOW_WRENCHES:
self.request(constants['LIGHTS']['YELLOW_REQUEST'],
constants['LIGHTS']['YELLOW_RESPONSE'])
elif wrench in self.GREEN_WRENCHES:
self.request(constants['LIGHTS']['GREEN_REQUEST'],
constants['LIGHTS']['GREEN_RESPONSE'])
else:
self.request(constants['LIGHTS']['OFF_REQUEST'],
constants['LIGHTS']['OFF_RESPONSE'])
self.wrench = wrench
def network_cb(self, msg):
'''
Pings kill board on every network hearbeat message. Pretends to be the rf-based hearbeat because
real one does not work :(
'''
self.request(constants['PING']['REQUEST'],
constants['PING']['RESPONSE'])
def publish_diagnostics(self, err=None):
'''
Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
'''
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'kill_board'
status.hardware_id = self.port
if not self.connected:
status.level = DiagnosticStatus.ERROR
status.message = 'Cannot connect to kill board. Retrying every second.'
status.values.append(KeyValue("Exception", str(err)))
else:
status.level = DiagnosticStatus.OK
for key, value in self.board_status.items():
status.values.append(KeyValue(key, str(value)))
msg.status.append(status)
self.diagnostics_pub.publish(msg)
def publish_joy(self):
'''
Publishes current stick/button state as a Joy object, to be handled by navigator_emergency.py node
'''
current_joy = Joy()
current_joy.axes.extend([0] * 4)
current_joy.buttons.extend([0] * 16)
for stick in self.sticks:
if self.sticks[
stick] >= 0x8000: # Convert 2's complement hex to signed decimal if negative
self.sticks[stick] -= 0x10000
current_joy.axes[0] = np.float32(self.sticks['LR']) / 2048
current_joy.axes[1] = np.float32(self.sticks['UD']) / 2048
current_joy.axes[3] = np.float32(self.sticks['TQ']) / 2048
current_joy.buttons[0] = np.int32(self.buttons['STATION_HOLD'])
current_joy.buttons[1] = np.int32(self.buttons['RAISE_KILL'])
current_joy.buttons[2] = np.int32(self.buttons['CLEAR_KILL'])
current_joy.buttons[4] = np.int32(self.buttons['THRUSTER_RETRACT'])
current_joy.buttons[5] = np.int32(self.buttons['THRUSTER_DEPLOY'])
current_joy.buttons[6] = np.int32(self.buttons['GO_INACTIVE'])
current_joy.buttons[7] = np.int32(self.buttons['START'])
current_joy.buttons[13] = np.int32(self.buttons['EMERGENCY_CONTROL'])
current_joy.header.frame_id = "/base_link"
current_joy.header.stamp = rospy.Time.now()
self.joy_pub.publish(current_joy)
def update_hw_kill(self):
'''
Raise/Clear hw-kill ROS Alarm is nessesary (any kills on board are engaged)
'''
killed = self.board_status['OVERALL']
if (killed and not self._hw_killed) or (
killed and self.board_status != self._last_hw_kill_paramaters):
self._hw_killed = True
self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status)
self._last_hw_kill_paramaters = copy.copy(self.board_status)
if not killed and self._hw_killed:
self._hw_killed = False
self.hw_kill_broadcaster.clear_alarm()
@thread_lock(lock)
def request(self, write_str, expected_response=None):
"""
Deals with requesting data and checking if the response matches some `recv_str`.
Returns True or False depending on the response.
With no `recv_str` passed in the raw result will be returned.
"""
self.ser.write(write_str)
if expected_response is not None:
for byte in expected_response:
self.expected_responses.append(byte)
def kill_alarm_cb(self, alarm):
'''
Informs kill board about software kills through ROS Alarms
'''
if alarm.raised:
self.request(constants['COMPUTER']['KILL']['REQUEST'],
constants['COMPUTER']['KILL']['RESPONSE'])
else:
self.request(constants['COMPUTER']['CLEAR']['REQUEST'],
constants['COMPUTER']['CLEAR']['RESPONSE'])
def hw_kill_alarm_cb(self, alarm):
self._hw_killed = alarm.raised
class KillInterface(object):
"""
Driver to interface with NaviGator's kill handeling board, which disconnects power to actuators
if any of 4 emergency buttons is pressed, a software kill command is sent, or the network hearbeat
stops. This driver enables the software kill option via ros_alarms and outputs diagnostics
data about the board to ROS. The driver can handle the board's asyncronous updates of kill statuses
and will also periodicly request updates in case the async is not working (it often doesn't).
"""
ALARM = 'hw-kill' # Alarm to raise when hardware kill is detected
YELLOW_WRENCHES = ['rc', '/wrench/rc', 'keyboard', '/wrench/keyboard'] # Wrenches which activate YELLOW LED
GREEN_WRENCHES = ['autonomous', '/wrench/autonomous'] # Wrenches which activate GREEN LED
def __init__(self):
self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
self.connected = False
self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
while not self.connected and not rospy.is_shutdown():
try:
self.connect()
self.connected = True
except serial.SerialException as e:
rospy.logerr('Cannot connect to kill board. {}'.format(e))
self.publish_diagnostics(e)
rospy.sleep(1)
rospy.loginfo('Board connected!')
self.board_status = {}
for kill in constants['KILLS']:
self.board_status[kill] = False
self.kills = self.board_status.keys()
self.expected_responses = []
self.network_msg = None
self.wrench = ''
self._hw_killed = False
self._last_hw_kill_paramaters = self.board_status
self.last_request = None
self.request_index = -1
self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
AlarmListener('hw-kill', self.hw_kill_alarm_cb)
AlarmListener('kill', self.kill_alarm_cb)
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber("/network", Header, self.network_cb) # Passes along network hearbeat to kill board
def connect(self):
if rospy.get_param('/is_simulation', False): # If in Gazebo, run fake serial class following board's protocol
from navigator_kill_board import SimulatedKillBoard
self.ser = SimulatedKillBoard()
else:
baud = rospy.get_param('~baud', 9600)
self.ser = serial.Serial(port=self.port, baudrate=baud)
def run(self):
'''
Main loop for driver, at a fixed rate updates alarms and diagnostics output with new
kill board output.
'''
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self.last_request is None:
self.request_next()
self.receive()
self.update_ros()
rate.sleep()
def update_ros(self):
self.update_hw_kill()
self.publish_diagnostics()
def handle_byte(self, msg):
'''
React to a byte recieved from the board. This could by an async update of a kill status or
a known response to a recent request
'''
# If a response has been recieved to a requested status (button, remove, etc), update internal state
if self.last_request is not None:
if msg == constants['RESPONSE_FALSE']:
self.board_status[self.last_request] = False
self.last_request = None
return
if msg == constants['RESPONSE_TRUE']:
rospy.logdebug('RESPONSE TRUE for {}'.format(self.last_request))
self.board_status[self.last_request] = True
self.last_request = None
return
# If an async update was recieved, update internal state
for kill in self.board_status:
if msg == constants[kill]['FALSE']:
rospy.logdebug('ASYNC FALSE FOR {}'.format(kill))
self.board_status[kill] = False
return
if msg == constants[kill]['TRUE']:
rospy.logdebug('ASYNC TRUE FOR {}'.format(kill))
self.board_status[kill] = True
return
# If a response to another request, like ping or computer kill/clear is recieved
for index, byte in enumerate(self.expected_responses):
if msg == byte:
del self.expected_responses[index]
return
# Log a warning if an unexpected byte was recieved
rospy.logwarn('Recieved an unexpected byte {}, remaining expected_responses={}'.format(
hex(ord(msg)), len(self.expected_responses)))
@thread_lock(lock)
def receive(self):
'''
Recieve update bytes sent from the board without requests being sent, updating internal
state, raising alarms, etc in response to board updates. Clears the in line buffer.
'''
while self.ser.in_waiting > 0 and not rospy.is_shutdown():
msg = self.ser.read(1)
self.handle_byte(msg)
def request_next(self):
'''
Manually request status of the next kill, looping back to the first
once all have been responsded to.
'''
self.request_index += 1
if self.request_index == len(self.kills):
self.request_index = 0
self.last_request = self.kills[self.request_index]
self.request(constants[self.last_request]['REQUEST'])
def wrench_cb(self, msg):
'''
Updates wrench (autnomous vs teleop) diagnostic light if nessesary
on wrench changes
'''
wrench = msg.data
if wrench != self.wrench:
if wrench in self.YELLOW_WRENCHES:
self.request(constants['LIGHTS']['YELLOW_REQUEST'], constants['LIGHTS']['YELLOW_RESPONSE'])
elif wrench in self.GREEN_WRENCHES:
self.request(constants['LIGHTS']['GREEN_REQUEST'], constants['LIGHTS']['GREEN_RESPONSE'])
else:
self.request(constants['LIGHTS']['OFF_REQUEST'], constants['LIGHTS']['OFF_RESPONSE'])
self.wrench = wrench
def network_cb(self, msg):
'''
Pings kill board on every network hearbeat message. Pretends to be the rf-based hearbeat because
real one does not work :(
'''
self.request(constants['PING']['REQUEST'], constants['PING']['RESPONSE'])
def publish_diagnostics(self, err=None):
'''
Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
'''
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'kill_board'
status.hardware_id = self.port
if not self.connected:
status.level = DiagnosticStatus.ERROR
status.message = 'Cannot connect to kill board. Retrying every second.'
status.values.append(KeyValue("Exception", str(err)))
else:
status.level = DiagnosticStatus.OK
for key, value in self.board_status.items():
status.values.append(KeyValue(key, str(value)))
msg.status.append(status)
self.diagnostics_pub.publish(msg)
def update_hw_kill(self):
'''
Raise/Clear hw-kill ROS Alarm is nessesary (any kills on board are engaged)
'''
killed = self.board_status['OVERALL']
if (killed and not self._hw_killed) or (killed and self.board_status != self._last_hw_kill_paramaters):
self._hw_killed = True
self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status)
self._last_hw_kill_paramaters = copy.copy(self.board_status)
if not killed and self._hw_killed:
self._hw_killed = False
self.hw_kill_broadcaster.clear_alarm()
@thread_lock(lock)
def request(self, write_str, expected_response=None):
"""
Deals with requesting data and checking if the response matches some `recv_str`.
Returns True or False depending on the response.
With no `recv_str` passed in the raw result will be returned.
"""
self.ser.write(write_str)
if expected_response is not None:
for byte in expected_response:
self.expected_responses.append(byte)
def kill_alarm_cb(self, alarm):
'''
Informs kill board about software kills through ROS Alarms
'''
if alarm.raised:
self.request(constants['COMPUTER']['KILL']['REQUEST'], constants['COMPUTER']['KILL']['RESPONSE'])
else:
self.request(constants['COMPUTER']['CLEAR']['REQUEST'], constants['COMPUTER']['CLEAR']['RESPONSE'])
def hw_kill_alarm_cb(self, alarm):
self._hw_killed = alarm.raised
class OdomKill(HandlerBase):
'''
Will kill the sub when it stops hearing odometry messages or when there is a large
discontinuity in state estimation. Only meant to run on the sub as a safety measure.
'''
def __init__(self, timeout=0.5):
self.GRACE_PERIOD = rospy.Duration(
5.0) # Alarms won't be raised within grace period
self.TIMEOUT = rospy.Duration(timeout)
self.MAX_JUMP = 0.5
self.LAUNCH_TIME = rospy.Time.now()
self.last_time = self.LAUNCH_TIME
self.last_position = None
self.check_count = 0
self.odom_discontinuity = False
self._killed = False
self.odom_listener = rospy.Subscriber('/odom',
Odometry,
self.got_odom_msg,
queue_size=1)
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Timer(rospy.Duration(0.1), self.check)
def check(self, *args):
self.check_count += 1
if not self._killed and self.need_kill():
if self.last_position is None:
if self.check_count < 10: # Probably just havent received odom yet
pass
else: # Odom is probably not publishing
self._killed = True
self.ab.raise_alarm(
problem_description=
'STATE ESTIMATION NOT AVAILABLE: KILLING SUB',
severity=5)
rospy.logerr('STATE ESTIMATION NOT AVAILABLE: KILLING SUB')
else:
self._killed = True
self.ab.raise_alarm(
problem_description='STATE ESTIMATION LOSS: KILLING SUB',
severity=5)
rospy.logerr("STATE ESTIMATION LOSS: KILLING SUB")
def got_odom_msg(self, msg):
if self.last_position is not None:
self.check_continuity(msg)
self.last_position = msg.pose.pose.position
self.last_time = rospy.Time.now()
def check_continuity(self, new_odom_msg): # True if 'continuous'
if self.odom_discontinuity:
return
this_p = new_odom_msg.pose.pose.position
last_p = self.last_position
jump = ((this_p.x - last_p.x)**2 + (this_p.y - last_p.y)**2 +
(this_p.z - last_p.z)**2)**0.5
if jump > self.MAX_JUMP:
rospy.logerr('ODOM DISCONTINUITY DETECTED')
self.ab.raise_alarm(
problem_description=
'ODOM DISCONTINUITY DETECTED JUMPED {} METERS'.format(jump),
severity=5)
self.odom_discontinuity = True
return False
def need_kill(self):
now = rospy.Time.now()
in_grace_period = now - self.LAUNCH_TIME < self.GRACE_PERIOD
odom_loss = now - self.last_time > self.TIMEOUT and not in_grace_period
if odom_loss:
rospy.logerr_throttle(
1, 'LOST ODOM FOR {} SECONDS'.format(
(rospy.Time.now() - self.last_time).to_sec()))
self.ab.raise_alarm(
problem_description='LOST ODOM FOR {} SECONDS'.format(
(rospy.Time.now() - self.last_time).to_sec()),
severity=5)
return odom_loss or self.odom_discontinuity
def clear_kill(self, alarm):
msg = ""
if alarm.clear:
if self._killed:
self._killed = False
self.odom_discontinuity = False
msg = "Odom kill successfully cleared!"
else:
msg = "Attempted to clear odom kill, but was already not killed."
rospy.logwarn(msg)
def raised(self, alarm):
self._killed = True
def cleared(self, alarm):
self._killed = False
class ActuatorDriver():
'''
Allows high level ros code to interface with Daniel's pneumatics board.
For dropper and grabber, call service with True or False to open or close.
For shooter, sending a True signal will pulse the valve.
TODO: Add a function to try and reconnect to the serial port if we lose connection.
'''
def __init__(self, port, baud=9600):
self.load_yaml()
rospy.init_node("actuator_driver")
self.disconnection_alarm = AlarmBroadcaster(
'actuator-board-disconnect')
self.packet_error_alarm = AlarmBroadcaster(
'actuator-board-packet-error')
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
# Reset all valves
rospy.loginfo("Resetting valves.")
for actuator_key in self.actuators:
actuator = self.actuators[actuator_key]
for valve_ports in actuator['ports']:
valve_port = actuator['ports'][valve_ports]
self.send_data(valve_port['id'], valve_port['default'])
rospy.loginfo("Valves ready to go.")
rospy.Service('~actuate', SetValve, self.got_service_request)
rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)
r = rospy.Rate(.2) # hz
while not rospy.is_shutdown():
# Heartbeat to make sure the board is still connected.
r.sleep()
self.ping()
@thread_lock(lock)
def set_raw_valve(self, srv):
'''
Set the valves manually so you don't have to have them defined in the YAML.
Service parameters:
actuator: PORT_ID
opened: OPENED
'''
self.send_data(int(srv.actuator), srv.opened)
return True
@thread_lock(lock)
def got_service_request(self, srv):
'''
Find out what actuator needs to be changed and how to change it with the valves.yaml file.
'''
try:
this_valve = self.actuators[srv.actuator]
except:
rospy.logerr(
"'%s' not found in valves.yaml so no configuration has been set for that actuator."
% srv.actuator)
return False
if this_valve['type'] == 'pulse':
# We want to pulse the port from the default value for the desired
# pulse_time then go back to the default value.
open_port = this_valve['ports']['open_port']
open_id = open_port['id']
open_default_value = open_port['default']
close_port = this_valve['ports']['close_port']
close_id = close_port['id']
close_default_value = close_port['default']
open_pulse_value = not open_default_value
close_pulse_value = not close_default_value
pulse_time = this_valve['pulse_time']
self.send_data(open_id, open_pulse_value)
self.send_data(close_id, close_pulse_value)
rospy.sleep(pulse_time)
self.send_data(open_id, open_default_value)
self.send_data(close_id, close_default_value)
elif this_valve['type'] == 'set':
# If the desired action is to open, set the open valve to true and the
# closed false (and visa versa for closing).
open_port = this_valve['ports']['open_port']
open_id = open_port['id']
close_port = this_valve['ports']['close_port']
close_id = close_port['id']
if srv.opened:
self.send_data(open_id, True)
self.send_data(close_id, False)
else:
self.send_data(open_id, False)
self.send_data(close_id, True)
return True
@thread_lock(lock)
def ping(self):
rospy.loginfo("ping")
ping = 0x10
chksum = ping ^ 0xFF
data = struct.pack("BB", ping, chksum)
self.ser.write(data)
if not self.parse_response(None):
rospy.logwarn(
"The board appears to be disconnected, trying again in 3 seconds."
)
self.disconnection_alarm.raise_alarm(
problem_description='The board appears to be disconnected.')
rospy.sleep(3)
def send_data(self, port, state):
'''
Infomation on communication protcol:
Sending bytes: send byte (command), then send byte XOR w/ 0xFF (checksum)
Receiving bytes: receive byte (response), then receive byte XOR w/ 0xFF (checksum)
Base opcodes:
- 0x10 ping
- 0x20 open valve (allow air flow)
- 0x30 close valve (prevent air flow)
- 0x40 read switch
- To 'ping' board (check if board is operational): send 0x10, if operational,
will reply with 0x11.
- To open valve (allow air flow): send 0x20 + valve number
(ex. 0x20 + 0x04 (valve #4) = 0x24 <-- byte to send), will reply with 0x01.
- To close valve (prevent air flow): send 0x30 + valve number
(ex. 0x30 + 0x0B (valve #11) = 0x3B <-- byte to send),will reply with 0x00.
- To read switch: send 0x40 + switch number
(ex. 0x40 + 0x09 (valve #9) = 0x49 <-- byte to send), will reply with 0x00
if switch is open (not pressed) or 0x01 if switch is closed (pressed).
'''
if port == -1:
return
# Calculate checksum and send data to board.
# A true state tells the pnuematics controller to allow air into the tube.
open_base_code = 0x20
closed_base_code = 0x30
op_code = port
op_code += open_base_code if state else closed_base_code
chksum = op_code ^ 0xFF
data = struct.pack("BB", op_code, chksum)
rospy.loginfo("Writing: %s. Chksum: %s." % (hex(op_code), hex(chksum)))
try:
self.ser.write(data)
except:
self.disconnection_alarm.raise_alarm(
problem_description=
"Actuator board serial connection has been terminated.")
return False
self.parse_response(state)
def parse_response(self, state):
'''
State can be True, False, or None for Open, Closed, or Ping signals respectively.
This will return True or False based on the correctness of the message.
TODO: Test with board and make sure all serial signals can be sent and received without need delays.
'''
try:
response = struct.unpack("BB", self.ser.read(2))
except:
self.disconnection_alarm.raise_alarm(
problem_description=
"Actuator board serial connection has been terminated.")
return False
data = response[0]
chksum = response[1]
rospy.loginfo("Received: %s. Chksum: %s." % (hex(data), hex(chksum)))
# Check if packet is intact.
if data != chksum ^ 0xFF:
rospy.logerr("CHKSUM NOT MATCHING.")
self.packet_error_alarm.raise_alarm(
problem_description="Actuator board checksum error.",
parameters={
'fault_info': {
'expected': data ^ 0xFF,
'got': response[1]
}
})
return False
# Check if packet data is correct.
if state is None:
if data == 0x11:
return True
return False
if data == state:
return True
self.packet_error_alarm.raise_alarm(
problem_description="Incorrect response to command.",
parameters={
'fault_info': {
'expected_state': state,
'got_state': data == 1
},
'note':
'True indicates an open port, false indicates a closed port.'
})
return False
def load_yaml(self):
with open(VALVES_FILE, 'r') as f:
self.actuators = yaml.load(f)
class killtest(unittest.TestCase):
def __init__(self, *args):
self._current_alarm_state = None
rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
super(killtest, self).__init__(*args)
self.AlarmListener = AlarmListener('hw-kill')
self.AlarmBroadcaster = AlarmBroadcaster('kill')
self.AlarmBroadcaster.clear_alarm()
def callback(self, msg):
self._current_alarm_state = msg
def test_AA_initial_state(self): # test the initial state of kill signal
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.1)
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
def test_AB_computer(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
self.AlarmBroadcaster.raise_alarm() # raise the computer alarm
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_raised(),
True,
msg='COMPUTER raise not worked')
self.AlarmBroadcaster.clear_alarm() # clear the computer alarm
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='COMPUTER shutdown not worked')
def test_AC_button_front_port(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
# call the service of button
rospy.wait_for_service('/kill_board_driver/BUTTON_FRONT_PORT')
try:
bfp = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_FRONT_PORT', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bfp(True) # set the button value to true
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTON_FRONT_PORT raise not worked')
bfp(False) # shut down the button
# when we shut down the button, we also need to clear the computer
# alarm
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_FRONT_PORT shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AD_button_aft_port(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_driver/BUTTON_AFT_PORT')
try:
bap = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_AFT_PORT', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bap(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTTON_AFT_PORT raise not worked')
bap(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTTON_AFT_PORT shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AE_button_front_starboard(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_driver/BUTTON_FRONT_STARBOARD')
try:
bfs = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_FRONT_STARBOARD', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bfs(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTON_FRONT_STARBOARD raise not worked')
bfs(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_FRONT_STARBOARD shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AF_button_aft_starboard(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_driver/BUTTON_AFT_STARBOARD')
try:
bas = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_AFT_STARBOARD', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bas(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTON_AFT_STARBOARD raise not worked')
bas(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_AFT_STARBOARD shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AG_remote(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
# publishing msg to network
pub = rospy.Publisher('/network', Header, queue_size=10)
rate = rospy.Rate(10)
t_end = rospy.Time.now() + rospy.Duration(3)
while rospy.Time.now() < t_end:
hello_header = Header()
hello_header.stamp = rospy.Time.now()
rospy.loginfo(hello_header)
pub.publish(hello_header)
rate.sleep()
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='REMOTE shutdown not worked. State = {}'.format(
self._current_alarm_state))
# stop sending the msg, the remote alarm will raise when stop recieving
# the msg for 8 secs
rospy.sleep(8.2)
self.assertEqual(
self.AlarmListener.is_raised(),
True,
msg='REMOTE raised not worked. State = {}'.format(
self._current_alarm_state))
class killtest(unittest.TestCase):
def __init__(self, *args):
self.hw_kill_alarm = None
self.updated = False
self.AlarmListener = AlarmListener('hw-kill', self._hw_kill_cb)
self.AlarmBroadcaster = AlarmBroadcaster('kill')
super(killtest, self).__init__(*args)
@thread_lock(lock)
def reset_update(self):
'''
Reset update state to False so we can notice changes to hw-kill
'''
self.updated = False
@thread_lock(lock)
def _hw_kill_cb(self, alarm):
'''
Called on change in hw-kill alarm.
If the raised status changed, set update flag to true so test an notice change
'''
if self.hw_kill_alarm is None or alarm.raised != self.hw_kill_alarm.raised:
self.updated = True
self.hw_kill_alarm = alarm
def wait_for_kill_update(self, timeout=rospy.Duration(0.5)):
'''
Wait up to timeout time to an hw-kill alarm change. Returns a copy of the new alarm or throws if times out
'''
timeout = rospy.Time.now() + timeout
while rospy.Time.now() < timeout:
lock.acquire()
updated = copy(self.updated)
alarm = deepcopy(self.hw_kill_alarm)
lock.release()
if updated:
return alarm
rospy.sleep(0.01)
raise Exception('timeout')
def assert_raised(self, timeout=rospy.Duration(0.5)):
'''
Waits for update and ensures it is now raised
'''
alarm = self.wait_for_kill_update(timeout)
self.assertEqual(alarm.raised, True)
def assert_cleared(self, timeout=rospy.Duration(0.5)):
'''
Wait for update and ensures is now cleared
'''
alarm = self.wait_for_kill_update(timeout)
self.assertEqual(alarm.raised, False)
def test_1_initial_state(self): # test the initial state of kill signal
'''
Tests initial state of system, which should have hw-kill raised beause kill is raised at startup.
Because hw-kill will be initialized to cleared then later raised when alarm server is fully started,
so we need to allow for pottentialy two updates before it is raised.
'''
alarm = self.wait_for_kill_update(timeout=rospy.Duration(10.0)) # Allow lots of time for initial alarm setup
if alarm.raised:
self.assertTrue(True)
return
self.reset_update()
self.assert_raised(timeout=rospy.Duration(10.0))
def test_2_computer_kill(self):
'''
Test raising/clearing kill alarm (user kill) will cause same change in hw-kill
'''
self.reset_update()
self.AlarmBroadcaster.clear_alarm()
self.assert_cleared()
self.reset_update()
self.AlarmBroadcaster.raise_alarm()
self.assert_raised()
self.reset_update()
self.AlarmBroadcaster.clear_alarm()
self.assert_cleared()
def _test_button(self, button):
'''
Tests that button kills work through simulated service.
'''
bfp = rospy.ServiceProxy('/kill_board_interface/BUTTON_{}'.format(button), SetBool)
bfp.wait_for_service(timeout=5.0)
self.reset_update()
bfp(True) # set the button value to true
self.assert_raised()
self.reset_update()
self.AlarmBroadcaster.clear_alarm()
bfp(False)
self.assert_cleared()
def test_3_buttons(self):
'''
Tests each of the four buttons
'''
for button in ['FRONT_PORT', 'AFT_PORT', 'FRONT_STARBOARD', 'AFT_STARBOARD']:
self._test_button('FRONT_PORT')
def test_4_remote(self):
'''
Tests remote kill by publishing hearbeat, stopping and checking alarm is raised, then
publishing hearbeat again to ensure alarm gets cleared.
'''
# publishing msg to network
pub = rospy.Publisher('/network', Header, queue_size=10)
rate = rospy.Rate(10)
t_end = rospy.Time.now() + rospy.Duration(1)
while rospy.Time.now() < t_end:
h = Header()
h.stamp = rospy.Time.now()
pub.publish(h)
rate.sleep()
self.reset_update()
rospy.sleep(8.5) # Wait slighly longer then the timeout on killboard
self.assert_raised()
self.reset_update()
t_end = rospy.Time.now() + rospy.Duration(1)
while rospy.Time.now() < t_end:
h = Header()
h.stamp = rospy.Time.now()
pub.publish(h)
rate.sleep()
self.AlarmBroadcaster.clear_alarm()
self.assert_cleared()
class OdomKill(HandlerBase):
'''
Will kill the sub when it stops hearing odometry messages or when there is a large
discontinuity in state estimation. Only meant to run on the sub as a safety measure.
'''
def __init__(self, timeout=0.5):
self.GRACE_PERIOD = rospy.Duration(5.0) # Alarms won't be raised within grace period
self.TIMEOUT = rospy.Duration(timeout)
self.MAX_JUMP = 0.5
self.LAUNCH_TIME = rospy.Time.now()
self.last_time = self.LAUNCH_TIME
self.last_position = None
self.check_count = 0
self.odom_discontinuity = False
self._killed = False
self.odom_listener = rospy.Subscriber('/odom', Odometry, self.got_odom_msg, queue_size=1)
self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
rospy.Timer(rospy.Duration(0.1), self.check)
def check(self, *args):
self.check_count += 1
if not self._killed and self.need_kill():
if self.last_position is None:
if self.check_count < 10: # Probably just havent received odom yet
pass
else: # Odom is probably not publishing
self._killed = True
self.ab.raise_alarm(problem_description='STATE ESTIMATION NOT AVAILABLE: KILLING SUB',
severity=5)
rospy.logerr('STATE ESTIMATION NOT AVAILABLE: KILLING SUB')
else:
self._killed = True
self.ab.raise_alarm(problem_description='STATE ESTIMATION LOSS: KILLING SUB', severity=5)
rospy.logerr("STATE ESTIMATION LOSS: KILLING SUB")
def got_odom_msg(self, msg):
if self.last_position is not None:
self.check_continuity(msg)
self.last_position = msg.pose.pose.position
self.last_time = rospy.Time.now()
def check_continuity(self, new_odom_msg): # True if 'continuous'
if self.odom_discontinuity:
return
this_p = new_odom_msg.pose.pose.position
last_p = self.last_position
jump = ((this_p.x - last_p.x) ** 2 + (this_p.y - last_p.y) ** 2 + (this_p.z - last_p.z) ** 2) ** 0.5
if jump > self.MAX_JUMP:
rospy.logerr('ODOM DISCONTINUITY DETECTED')
self.ab.raise_alarm(problem_description='ODOM DISCONTINUITY DETECTED JUMPED {} METERS'.format(jump),
severity=5)
self.odom_discontinuity = True
return False
def need_kill(self):
now = rospy.Time.now()
in_grace_period = now - self.LAUNCH_TIME < self.GRACE_PERIOD
odom_loss = now - self.last_time > self.TIMEOUT and not in_grace_period
if odom_loss:
rospy.logerr_throttle(1, 'LOST ODOM FOR {} SECONDS'.format((rospy.Time.now() - self.last_time).to_sec()))
self.ab.raise_alarm(
problem_description='LOST ODOM FOR {} SECONDS'.format((rospy.Time.now() - self.last_time).to_sec()),
severity=5
)
return odom_loss or self.odom_discontinuity
def clear_kill(self, alarm):
msg = ""
if alarm.clear:
if self._killed:
self._killed = False
self.odom_discontinuity = False
msg = "Odom kill successfully cleared!"
else:
msg = "Attempted to clear odom kill, but was already not killed."
rospy.logwarn(msg)
def raised(self, alarm):
self._killed = True
def cleared(self, alarm):
self._killed = False
def __init__(self):
self.move_client = SimpleActionClient('/move_to', MoveAction)
self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
self.broadcaster = AlarmBroadcaster(self.alarm_name)
class ThrusterAndKillBoard(CANDeviceHandle):
'''
Device handle for the thrust and kill board.
'''
def __init__(self, *args, **kwargs):
super(ThrusterAndKillBoard, self).__init__(*args, **kwargs)
# Initialize thruster mapping from params
self.thrusters = make_thruster_dictionary(
rospy.get_param('/thruster_layout/thrusters'))
# Tracks last hw-kill alarm update
self._last_hw_kill = None
# Tracks last soft kill status received from board
self._last_soft_kill = None
# Tracks last hard kill status received from board
self._last_hard_kill = None
# Tracks last go status broadcasted
self._last_go = None
# Used to raise/clear hw-kill when board updates
self._kill_broadcaster = AlarmBroadcaster('hw-kill')
# Alarm broadaster for GO command
self._go_alarm_broadcaster = AlarmBroadcaster('go')
# Listens to hw-kill updates to ensure another nodes doesn't manipulate it
self._hw_kill_listener = AlarmListener(
'hw-kill', callback_funct=self.on_hw_kill)
# Provide service for alarm handler to set/clear the motherboard kill
self._unkill_service = rospy.Service(
'/set_mobo_kill', SetBool, self.set_mobo_kill)
# Sends hearbeat to board
self._hearbeat_timer = rospy.Timer(
rospy.Duration(0.4), self.send_heartbeat)
# Create a subscribe for thruster commands
self._sub = rospy.Subscriber(
'/thrusters/thrust', Thrust, self.on_command, queue_size=10)
def set_mobo_kill(self, req):
'''
Called on service calls to /set_mobo_kill, sending the approrpriate packet to the board
to unassert or assert to motherboard-origin kill
'''
self.send_data(KillMessage.create_kill_message(command=True, hard=False, asserted=req.data).to_bytes(),
can_id=KILL_SEND_ID)
return {'success': True}
def send_heartbeat(self, timer):
'''
Send the special heartbeat packet when the timer triggers
'''
self.send_data(HeartbeatMessage.create().to_bytes(),
can_id=KILL_SEND_ID)
def on_hw_kill(self, alarm):
'''
Update the classe's hw-kill alarm to the latest update
'''
self._last_hw_kill = alarm
def on_command(self, msg):
'''
When a thrust command message is received from the Subscriber, send the appropriate packet
to the board
'''
for cmd in msg.thruster_commands:
# If we don't have a mapping for this thruster, ignore it
if cmd.name not in self.thrusters:
rospy.logwarn(
'Command received for {}, but this is not a thruster.'.format(cmd.name))
continue
# Map commanded thrust (in newetons) to effort value (-1 to 1)
effort = self.thrusters[cmd.name].effort_from_thrust(cmd.thrust)
# Send packet to command specified thruster the specified force
packet = ThrustPacket.create_thrust_packet(
ThrustPacket.ID_MAPPING[cmd.name], effort)
self.send_data(packet.to_bytes(), can_id=THRUST_SEND_ID)
def update_hw_kill(self, reason):
'''
If needed, update the hw-kill alarm so it reflects the latest status from the board
'''
if self._last_soft_kill is None and self._last_hard_kill is None:
return
# Set serverity / problem message appropriately
severity = 0
message = ""
if self._last_hard_kill is True:
severity = 2
message = "Hard kill"
elif self._last_soft_kill is True:
severity = 1
message = "Soft kill"
raised = severity != 0
message = message + ": " + reason
# If the current status differs from the alarm, update the alarm
if self._last_hw_kill is None or self._last_hw_kill.raised != raised or \
self._last_hw_kill.problem_description != message or self._last_hw_kill.severity != severity:
if raised:
self._kill_broadcaster.raise_alarm(
severity=severity, problem_description=message)
else:
self._kill_broadcaster.clear_alarm(
severity=severity, problem_description=message)
def on_data(self, data):
'''
Parse the two bytes and raise kills according to the specs:
First Byte => Kill trigger statuses (Asserted = 1, Unasserted = 0):
Bit 7: Hard kill status, changed by On/Off Hall effect switch. If this becomes 1, begin shutdown procedure
Bit 6: Overall soft kill status, 1 if any of the following flag bits are 1. This becomes 0 if all kills are cleared and the thrusters are done re-initializing
Bit 5: Hall effect soft kill flag
Bit 4: Mobo command soft kill flag
Bit 3: Heartbeat lost flag (times out after 1 s)
Bit 2-0: Reserved
Second Byte => Input statuses ("Pressed" = 1, "Unpressed" = 0) and thruster initializing status:
Bit 7: On (1) / Off (0) Hall effect switch status. If this becomes 0, the hard kill in the previous byte becomes 1.
Bit 6: Soft kill Hall effect switch status. If this is "pressed" = 1, bit 5 of previous byte is unkilled = 0. "Removing" the magnet will "unpress" the switch
Bit 5: Go Hall effect switch status. "Pressed" = 1, removing the magnet will "unpress" the switch
Bit 4: Thruster initializing status: This becomes 1 when the board is unkilling, and starts powering thrusters. After the "grace period" it becomes 0 and the overall soft kill status becomes 0. This flag also becomes 0 if killed in the middle of initializing thrusters.
Bit 3-0: Reserved
'''
#print(ord(data[0]), ord(data[1]))
# Hard Kill - bit 7
if (ord(data[0]) & 0x8):
self._last_hard_kill = True
else:
self._last_hard_kill = False
# Soft Kill - bit 6
if (ord(data[0]) & 0x40):
self._last_soft_kill = True
else:
self._last_soft_kill = False
reason = ""
# hall effect - bit 5
if (ord(data[0]) & 0x20):
reason = 'hall effect'
# mobo soft kill - bit 4
if (ord(data[0]) & 0x10):
reason = 'mobo soft kill'
# heartbeat loss - bit 3
if (ord(data[0]) & 0x08):
reason = 'Heart beat lost'
self.update_hw_kill(reason)
# Switch - bit 5
if (ord(data[1]) & 0x20):
asserted = True
else:
asserted = False
if self._last_go is None or asserted != self._last_go:
if asserted:
self._go_alarm_broadcaster.raise_alarm(
problem_description="Go plug pulled!")
else:
self._go_alarm_broadcaster.clear_alarm(
problem_description="Go plug returned")
self._last_go = asserted
def __init__(self):
self.task_client = TaskClient()
self.broadcaster = AlarmBroadcaster(self.alarm_name)
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.kill_broadcaster = AlarmBroadcaster('kill')
self.station_hold_broadcaster = AlarmBroadcaster('station-hold')
self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect)
self.kill_listener = AlarmListener('kill', callback_funct=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 = alarm.raised
@_timeout_check
def kill(self, *args, **kwargs):
'''
Kills the system regardless of what state it is in.
'''
rospy.loginfo("Killing")
self.kill_broadcaster.raise_alarm(
problem_description="System kill from user remote control",
parameters={'location': 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.kill_broadcaster.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_broadcaster.clear_alarm()
else:
self.kill_broadcaster.raise_alarm(
problem_description="System kill from user remote control",
parameters={'location': 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_broadcaster.raise_alarm(
problem_description="Request to station hold from remote control",
parameters={'location': self.name}
)
@_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, *args):
self.hw_kill_alarm = None
self.updated = False
self.AlarmListener = AlarmListener('hw-kill', self._hw_kill_cb)
self.AlarmBroadcaster = AlarmBroadcaster('kill')
super(killtest, self).__init__(*args)
class RvizVisualizer(object):
'''Cute tool for drawing both depth and height-from-bottom in RVIZ
'''
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def update_kill_button(self):
if self.killed:
self.kill_marker.controls[0].markers[0].text = "KILLED"
self.kill_marker.controls[0].markers[0].color.r = 1
self.kill_marker.controls[0].markers[0].color.g = 0
else:
self.kill_marker.controls[0].markers[0].text = "UNKILLED"
self.kill_marker.controls[0].markers[0].color.r = 0
self.kill_marker.controls[0].markers[0].color.g = 1
self.kill_server.insert(self.kill_marker)
self.kill_server.applyChanges()
def kill_alarm_callback(self, alarm):
self.need_kill_update = False
self.killed = alarm.raised
self.update_kill_button()
def kill_buttton_callback(self, feedback):
if not feedback.event_type == 3:
return
if self.need_kill_update:
return
self.need_kill_update = True
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.kill_alarm.raise_alarm()
def voltage_callback(self, voltage):
self.voltage_marker.text = str(round(voltage.data, 2)) + ' volts'
self.voltage_marker.header.stamp = rospy.Time()
if voltage.data < self.low_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 0
elif voltage.data < self.warn_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 1
else:
self.voltage_marker.color.r = 0
self.voltage_marker.color.g = 1
self.rviz_pub_utils.publish(self.voltage_marker)
def depth_callback(self, msg):
'''Handle depth data sent from depth sensor'''
frame = '/depth'
distance = msg.depth
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=(0.0, 1.0, 0.2, 0.7), # green,
frame=frame,
id=0 # Keep these guys from overwriting eachother
)
self.surface_marker.ns = 'sub'
self.surface_marker.header = mil_ros_tools.make_header(frame='/depth')
self.surface_marker.pose = marker.pose
self.surface_marker.text = str(round(distance, 3)) + 'm'
self.surface_marker.id = 0
self.rviz_pub.publish(marker)
self.rviz_pub_t.publish(self.depth_marker)
def range_callback(self, msg):
'''Handle range data grabbed from dvl'''
# future: should be /base_link/dvl, no?
frame = '/dvl'
distance = msg.range
# Color a sharper red if we're in danger
if distance < 1.0:
color = (1.0, 0.1, 0.0, 0.9)
else:
color = (0.2, 0.8, 0.0, 0.7)
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=color, # red,
frame=frame,
up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world
id=1 # Keep these guys from overwriting eachother
)
self.depth_marker.ns = 'sub'
self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl')
self.depth_marker.pose = marker.pose
self.depth_marker.text = str(round(distance, 3)) + 'm'
self.depth_marker.id = 1
self.rviz_pub_t.publish(self.depth_marker)
self.rviz_pub.publish(marker)
def make_cylinder_marker(self, base, length, color, frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]), **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(
position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs
)
return marker
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
class KillInterface(object):
"""
Driver to interface with NaviGator's kill handeling board, which disconnects power to actuators
if any of 4 emergency buttons is pressed, a software kill command is sent, or the network hearbeat
stops. This driver enables the software kill option via ros_alarms and outputs diagnostics
data about the board to ROS. The driver can handle the board's asyncronous updates of kill statuses
and will also periodicly request updates in case the async is not working (it often doesn't).
"""
ALARM = 'hw-kill' # Alarm to raise when hardware kill is detected
YELLOW_WRENCHES = ['rc', '/wrench/rc', 'keyboard', '/wrench/keyboard'] # Wrenches which activate YELLOW LED
GREEN_WRENCHES = ['autonomous', '/wrench/autonomous'] # Wrenches which activate GREEN LED
def __init__(self):
self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
self.connected = False
self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
while not self.connected and not rospy.is_shutdown():
try:
self.connect()
self.connected = True
except serial.SerialException as e:
rospy.logerr('Cannot connect to kill board. {}'.format(e))
self.publish_diagnostics(e)
rospy.sleep(1)
rospy.loginfo('Board connected!')
self.board_status = {}
for kill in constants['KILLS']:
self.board_status[kill] = False
self.kills = self.board_status.keys()
self.expected_responses = []
self.network_msg = None
self.wrench = ''
self._hw_killed = False
self._last_hw_kill_paramaters = self.board_status
self.last_request = None
self.request_index = -1
self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
self.ctrl_msg_received = False
self.ctrl_msg_count = 0
self.ctrl_msg_timeout = 0
self.sticks = {}
for stick in constants['CTRL_STICKS']: # These are 3 signed 16-bit values for stick positions
self.sticks[stick] = 0x0000
self.sticks_temp = 0x0000
self.buttons = {}
for button in constants['CTRL_BUTTONS']: # These are the button on/off states (16 possible inputs)
self.buttons[button] = False
self.buttons_temp = 0x0000
AlarmListener('hw-kill', self.hw_kill_alarm_cb)
AlarmListener('kill', self.kill_alarm_cb)
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber("/network", Header, self.network_cb) # Passes along network hearbeat to kill board
def connect(self):
if rospy.get_param('/is_simulation', False): # If in Gazebo, run fake serial class following board's protocol
from navigator_kill_board import SimulatedKillBoard
self.ser = SimulatedKillBoard()
else:
baud = rospy.get_param('~baud', 9600)
self.ser = serial.Serial(port=self.port, baudrate=baud)
def run(self):
'''
Main loop for driver, at a fixed rate updates alarms and diagnostics output with new
kill board output.
'''
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self.last_request is None:
self.request_next()
self.receive()
self.update_ros()
rate.sleep()
def update_ros(self):
self.update_hw_kill()
self.publish_diagnostics()
if self.ctrl_msg_received is True:
self.publish_joy()
self.ctrl_msg_received = False
def handle_byte(self, msg):
'''
React to a byte recieved from the board. This could by an async update of a kill status or
a known response to a recent request
'''
# If the controller message start byte is received, next 8 bytes are the controller data
if msg == constants['CONTROLLER']:
self.ctrl_msg_count = 8
self.ctrl_msg_timeout = rospy.Time.now()
return
# If receiving the controller message, record the byte as stick/button data
if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 8):
# If 1 second has passed since the message began, timeout and report warning
if (rospy.Time.now() - self.ctrl_msg_timeout) >= rospy.Duration(1):
self.ctrl_msg_received = False
self.ctrl_msg_count = 0
rospy.logwarn('Timeout receiving controller message. Please disconnect controller.')
if self.ctrl_msg_count > 2: # The first 6 bytes in the message are stick data bytes
if (self.ctrl_msg_count % 2) == 0: # Even number byte: first byte in data word
self.sticks_temp = (int(msg.encode("hex"), 16) << 8)
else: # Odd number byte: combine two bytes into a stick's data word
self.sticks_temp += int(msg.encode("hex"), 16)
if (self.ctrl_msg_count > 6):
self.sticks['UD'] = self.sticks_temp
elif (self.ctrl_msg_count > 4):
self.sticks['LR'] = self.sticks_temp
else:
self.sticks['TQ'] = self.sticks_temp
self.sticks_temp = 0x0000
else: # The last 2 bytes are button data bytes
if (self.ctrl_msg_count % 2) == 0:
self.buttons_temp = (int(msg.encode("hex"), 16) << 8)
else: # Combine two bytes into the button data word
self.buttons_temp += int(msg.encode("hex"), 16)
for button in self.buttons: # Each of the 16 bits represents a button on/off state
button_check = int(constants['CTRL_BUTTONS_VALUES'][button].encode("hex"), 16)
self.buttons[button] = ((self.buttons_temp & button_check) == button_check)
self.buttons_temp = 0x0000
self.ctrl_msg_received = True # After receiving last byte, trigger joy update
self.ctrl_msg_count -= 1
return
# If a response has been recieved to a requested status (button, remove, etc), update internal state
if self.last_request is not None:
if msg == constants['RESPONSE_FALSE']:
self.board_status[self.last_request] = False
self.last_request = None
return
if msg == constants['RESPONSE_TRUE']:
rospy.logdebug('RESPONSE TRUE for {}'.format(self.last_request))
self.board_status[self.last_request] = True
self.last_request = None
return
# If an async update was recieved, update internal state
for kill in self.board_status:
if msg == constants[kill]['FALSE']:
rospy.logdebug('ASYNC FALSE FOR {}'.format(kill))
self.board_status[kill] = False
return
if msg == constants[kill]['TRUE']:
rospy.logdebug('ASYNC TRUE FOR {}'.format(kill))
self.board_status[kill] = True
return
# If a response to another request, like ping or computer kill/clear is recieved
for index, byte in enumerate(self.expected_responses):
if msg == byte:
del self.expected_responses[index]
return
# Log a warning if an unexpected byte was recieved
rospy.logwarn('Recieved an unexpected byte {}, remaining expected_responses={}'.format(
hex(ord(msg)), len(self.expected_responses)))
@thread_lock(lock)
def receive(self):
'''
Recieve update bytes sent from the board without requests being sent, updating internal
state, raising alarms, etc in response to board updates. Clears the in line buffer.
'''
while self.ser.in_waiting > 0 and not rospy.is_shutdown():
msg = self.ser.read(1)
self.handle_byte(msg)
def request_next(self):
'''
Manually request status of the next kill, looping back to the first
once all have been responsded to.
'''
self.request_index += 1
if self.request_index == len(self.kills):
self.request_index = 0
self.last_request = self.kills[self.request_index]
self.request(constants[self.last_request]['REQUEST'])
def wrench_cb(self, msg):
'''
Updates wrench (autnomous vs teleop) diagnostic light if nessesary
on wrench changes
'''
wrench = msg.data
if wrench != self.wrench:
if wrench in self.YELLOW_WRENCHES:
self.request(constants['LIGHTS']['YELLOW_REQUEST'], constants['LIGHTS']['YELLOW_RESPONSE'])
elif wrench in self.GREEN_WRENCHES:
self.request(constants['LIGHTS']['GREEN_REQUEST'], constants['LIGHTS']['GREEN_RESPONSE'])
else:
self.request(constants['LIGHTS']['OFF_REQUEST'], constants['LIGHTS']['OFF_RESPONSE'])
self.wrench = wrench
def network_cb(self, msg):
'''
Pings kill board on every network hearbeat message. Pretends to be the rf-based hearbeat because
real one does not work :(
'''
self.request(constants['PING']['REQUEST'], constants['PING']['RESPONSE'])
def publish_diagnostics(self, err=None):
'''
Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
'''
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'kill_board'
status.hardware_id = self.port
if not self.connected:
status.level = DiagnosticStatus.ERROR
status.message = 'Cannot connect to kill board. Retrying every second.'
status.values.append(KeyValue("Exception", str(err)))
else:
status.level = DiagnosticStatus.OK
for key, value in self.board_status.items():
status.values.append(KeyValue(key, str(value)))
msg.status.append(status)
self.diagnostics_pub.publish(msg)
def publish_joy(self):
'''
Publishes current stick/button state as a Joy object, to be handled by navigator_emergency.py node
'''
current_joy = Joy()
current_joy.axes.extend([0] * 4)
current_joy.buttons.extend([0] * 16)
for stick in self.sticks:
if self.sticks[stick] >= 0x8000: # Convert 2's complement hex to signed decimal if negative
self.sticks[stick] -= 0x10000
current_joy.axes[0] = np.float32(self.sticks['UD']) / 2048
current_joy.axes[1] = np.float32(self.sticks['LR']) / 2048
current_joy.axes[3] = np.float32(self.sticks['TQ']) / 2048
current_joy.buttons[7] = np.int32(self.buttons['START'])
current_joy.buttons[3] = np.int32(self.buttons['Y'])
current_joy.buttons[2] = np.int32(self.buttons['X'])
current_joy.buttons[0] = np.int32(self.buttons['A'])
current_joy.buttons[1] = np.int32(self.buttons['B'])
current_joy.buttons[11] = np.int32(self.buttons['DL']) # Dpad Left
current_joy.buttons[12] = np.int32(self.buttons['DR']) # Dpad Right
current_joy.header.frame_id = "/base_link"
current_joy.header.stamp = rospy.Time.now()
self.joy_pub.publish(current_joy)
def update_hw_kill(self):
'''
Raise/Clear hw-kill ROS Alarm is nessesary (any kills on board are engaged)
'''
killed = self.board_status['OVERALL']
if (killed and not self._hw_killed) or (killed and self.board_status != self._last_hw_kill_paramaters):
self._hw_killed = True
self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status)
self._last_hw_kill_paramaters = copy.copy(self.board_status)
if not killed and self._hw_killed:
self._hw_killed = False
self.hw_kill_broadcaster.clear_alarm()
@thread_lock(lock)
def request(self, write_str, expected_response=None):
"""
Deals with requesting data and checking if the response matches some `recv_str`.
Returns True or False depending on the response.
With no `recv_str` passed in the raw result will be returned.
"""
self.ser.write(write_str)
if expected_response is not None:
for byte in expected_response:
self.expected_responses.append(byte)
def kill_alarm_cb(self, alarm):
'''
Informs kill board about software kills through ROS Alarms
'''
if alarm.raised:
self.request(constants['COMPUTER']['KILL']['REQUEST'], constants['COMPUTER']['KILL']['RESPONSE'])
else:
self.request(constants['COMPUTER']['CLEAR']['REQUEST'], constants['COMPUTER']['CLEAR']['RESPONSE'])
def hw_kill_alarm_cb(self, alarm):
self._hw_killed = alarm.raised
class ActuatorDriver():
'''
Allows high level ros code to interface with Daniel's pneumatics board.
For dropper and grabber, call service with True or False to open or close.
For shooter, sending a True signal will pulse the valve.
TODO: Add a function to try and reconnect to the serial port if we lose connection.
'''
def __init__(self, port, baud=9600):
self.load_yaml()
rospy.init_node("actuator_driver")
self.disconnection_alarm = AlarmBroadcaster('actuator-board-disconnect')
self.packet_error_alarm = AlarmBroadcaster('actuator-board-packet-error')
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
# Reset all valves
rospy.loginfo("Resetting valves.")
for actuator_key in self.actuators:
actuator = self.actuators[actuator_key]
for valve_ports in actuator['ports']:
valve_port = actuator['ports'][valve_ports]
self.send_data(valve_port['id'], valve_port['default'])
rospy.loginfo("Valves ready to go.")
rospy.Service('~actuate', SetValve, self.got_service_request)
rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)
r = rospy.Rate(.2) # hz
while not rospy.is_shutdown():
# Heartbeat to make sure the board is still connected.
r.sleep()
self.ping()
@thread_lock(lock)
def set_raw_valve(self, srv):
'''
Set the valves manually so you don't have to have them defined in the YAML.
Service parameters:
actuator: PORT_ID
opened: OPENED
'''
self.send_data(int(srv.actuator), srv.opened)
return True
@thread_lock(lock)
def got_service_request(self, srv):
'''
Find out what actuator needs to be changed and how to change it with the valves.yaml file.
'''
try:
this_valve = self.actuators[srv.actuator]
except:
rospy.logerr(
"'%s' not found in valves.yaml so no configuration has been set for that actuator." %
srv.actuator)
return False
if this_valve['type'] == 'pulse':
# We want to pulse the port from the default value for the desired
# pulse_time then go back to the default value.
open_port = this_valve['ports']['open_port']
open_id = open_port['id']
open_default_value = open_port['default']
close_port = this_valve['ports']['close_port']
close_id = close_port['id']
close_default_value = close_port['default']
open_pulse_value = not open_default_value
close_pulse_value = not close_default_value
pulse_time = this_valve['pulse_time']
self.send_data(open_id, open_pulse_value)
self.send_data(close_id, close_pulse_value)
rospy.sleep(pulse_time)
self.send_data(open_id, open_default_value)
self.send_data(close_id, close_default_value)
elif this_valve['type'] == 'set':
# If the desired action is to open, set the open valve to true and the
# closed false (and visa versa for closing).
open_port = this_valve['ports']['open_port']
open_id = open_port['id']
close_port = this_valve['ports']['close_port']
close_id = close_port['id']
if srv.opened:
self.send_data(open_id, True)
self.send_data(close_id, False)
else:
self.send_data(open_id, False)
self.send_data(close_id, True)
return True
@thread_lock(lock)
def ping(self):
rospy.loginfo("ping")
ping = 0x10
chksum = ping ^ 0xFF
data = struct.pack("BB", ping, chksum)
self.ser.write(data)
if not self.parse_response(None):
rospy.logwarn("The board appears to be disconnected, trying again in 3 seconds.")
self.disconnection_alarm.raise_alarm(
problem_description='The board appears to be disconnected.'
)
rospy.sleep(3)
def send_data(self, port, state):
'''
Infomation on communication protcol:
Sending bytes: send byte (command), then send byte XOR w/ 0xFF (checksum)
Receiving bytes: receive byte (response), then receive byte XOR w/ 0xFF (checksum)
Base opcodes:
- 0x10 ping
- 0x20 open valve (allow air flow)
- 0x30 close valve (prevent air flow)
- 0x40 read switch
- To 'ping' board (check if board is operational): send 0x10, if operational,
will reply with 0x11.
- To open valve (allow air flow): send 0x20 + valve number
(ex. 0x20 + 0x04 (valve #4) = 0x24 <-- byte to send), will reply with 0x01.
- To close valve (prevent air flow): send 0x30 + valve number
(ex. 0x30 + 0x0B (valve #11) = 0x3B <-- byte to send),will reply with 0x00.
- To read switch: send 0x40 + switch number
(ex. 0x40 + 0x09 (valve #9) = 0x49 <-- byte to send), will reply with 0x00
if switch is open (not pressed) or 0x01 if switch is closed (pressed).
'''
if port == -1:
return
# Calculate checksum and send data to board.
# A true state tells the pnuematics controller to allow air into the tube.
open_base_code = 0x20
closed_base_code = 0x30
op_code = port
op_code += open_base_code if state else closed_base_code
chksum = op_code ^ 0xFF
data = struct.pack("BB", op_code, chksum)
rospy.loginfo("Writing: %s. Chksum: %s." % (hex(op_code), hex(chksum)))
try:
self.ser.write(data)
except:
self.disconnection_alarm.raise_alarm(
problem_description="Actuator board serial connection has been terminated."
)
return False
self.parse_response(state)
def parse_response(self, state):
'''
State can be True, False, or None for Open, Closed, or Ping signals respectively.
This will return True or False based on the correctness of the message.
TODO: Test with board and make sure all serial signals can be sent and received without need delays.
'''
try:
response = struct.unpack("BB", self.ser.read(2))
except:
self.disconnection_alarm.raise_alarm(
problem_description="Actuator board serial connection has been terminated."
)
return False
data = response[0]
chksum = response[1]
rospy.loginfo("Received: %s. Chksum: %s." % (hex(data), hex(chksum)))
# Check if packet is intact.
if data != chksum ^ 0xFF:
rospy.logerr("CHKSUM NOT MATCHING.")
self.packet_error_alarm.raise_alarm(
problem_description="Actuator board checksum error.",
parameters={
'fault_info': {'expected': data ^ 0xFF, 'got': response[1]}
}
)
return False
# Check if packet data is correct.
if state is None:
if data == 0x11:
return True
return False
if data == state:
return True
self.packet_error_alarm.raise_alarm(
problem_description="Incorrect response to command.",
parameters={
'fault_info': {'expected_state': state, 'got_state': data == 1},
'note': 'True indicates an open port, false indicates a closed port.'
}
)
return False
def load_yaml(self):
with open(VALVES_FILE, 'r') as f:
self.actuators = yaml.load(f)
#!/usr/bin/env python
import rospy
from ros_alarms import AlarmListener, AlarmBroadcaster
if __name__ == "__main__":
rospy.init_node("broadcaster_listener_test")
ab = AlarmBroadcaster("test_alarm")
al = AlarmListener("test_alarm")
print "Inited"
assert al.is_cleared()
ab.raise_alarm()
assert al.is_raised()
rospy.sleep(0.5)
ab.clear_alarm()
assert al.is_cleared()
rospy.sleep(0.5)
ab.raise_alarm(parameters={
"int": 4,
"list": [1, 2, 3],
"str": "stringing"
})
print "All checks passed"
def __init__(self, *args):
self.hw_kill_alarm = None
self.updated = False
self.AlarmListener = AlarmListener('hw-kill', self._hw_kill_cb)
self.AlarmBroadcaster = AlarmBroadcaster('kill')
super(killtest, self).__init__(*args)