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 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
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 test_stress(self):
''' Stress test the server, lots of raises and clears '''
ab_a = AlarmBroadcaster("alarm_a")
al_a = AlarmListener("alarm_a")
ab_b = AlarmBroadcaster("alarm_b")
al_b = AlarmListener("alarm_b")
al_c = AlarmListener("alarm_c")
ab_c = AlarmBroadcaster("alarm_c")
actions = [ab_a.raise_alarm, ab_b.raise_alarm, ab_c.raise_alarm,
ab_a.clear_alarm, ab_b.clear_alarm, ab_c.clear_alarm]
for i in range(100):
random.choice(actions)()
rospy.sleep(0.01)
# Clear them all as a find state
ab_a.clear_alarm()
ab_b.raise_alarm()
ab_c.raise_alarm()
# Make sure all alarms are correct
self.assertTrue(al_a.is_cleared())
self.assertFalse(al_b.is_cleared())
self.assertFalse(al_c.is_cleared())
# Set everyhing cleared
ab_b.clear_alarm()
ab_c.clear_alarm()
# Make sure of that
self.assertTrue(al_b.is_cleared())
self.assertTrue(al_c.is_cleared())
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 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
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
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_broadcaster_and_listener(self):
''' Simple broadcaster and listener tests, with arguments '''
ab_a = AlarmBroadcaster("alarm_a")
al_a = AlarmListener("alarm_a")
al_b = AlarmListener("alarm_b")
ab_b = AlarmBroadcaster("alarm_b")
al_c = AlarmListener("alarm_c")
ab_c = AlarmBroadcaster("alarm_c")
# Make sure all the alarm start off clear
self.assertFalse(al_a.is_raised())
self.assertFalse(al_b.is_raised())
self.assertFalse(al_b.is_raised())
# Same as above
self.assertTrue(al_a.is_cleared())
self.assertTrue(al_b.is_cleared())
self.assertTrue(al_c.is_cleared())
# Some args to pass in
_severity = 3
_blank_params = ''
_full_params = {'test': 1, 'test2': 2}
_problem_desc = "This is a test"
# Raise them all, with some arguments
ab_a.raise_alarm(severity=_severity)
ab_b.raise_alarm(severity=_severity, parameters=_blank_params)
ab_c.raise_alarm(problem_description=_problem_desc, parameters=_full_params)
# Make sure all the alarm start off clear
self.assertTrue(al_a.is_raised())
self.assertTrue(al_b.is_raised())
self.assertTrue(al_c.is_raised())
# Make sure alarm values are correct
self.assertEqual(al_a.get_alarm().severity, _severity)
self.assertEqual(al_b.get_alarm().severity, _severity)
self.assertEqual(al_b.get_alarm().parameters, _blank_params)
self.assertEqual(al_c.get_alarm().problem_description, _problem_desc)
self.assertEqual(al_c.get_alarm().parameters, _full_params)
# Now clear the alarms, some again with arguments
ab_a.clear_alarm()
ab_b.clear_alarm(parameters=_full_params)
ab_c.clear_alarm(parameters=_blank_params)
# Make sure all alarms are cleared
self.assertTrue(al_a.is_cleared())
self.assertTrue(al_b.is_cleared())
self.assertTrue(al_c.is_cleared())
# Make sure arugments were passed correctly
self.assertEqual(al_b.get_alarm().parameters, _full_params)
self.assertEqual(al_c.get_alarm().parameters, _blank_params)
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
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)
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
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)
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_interface/BUTTON_FRONT_PORT')
try:
bfp = rospy.ServiceProxy('/kill_board_interface/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_interface/BUTTON_AFT_PORT')
try:
bap = rospy.ServiceProxy('/kill_board_interface/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_interface/BUTTON_FRONT_STARBOARD')
try:
bfs = rospy.ServiceProxy(
'/kill_board_interface/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_interface/BUTTON_AFT_STARBOARD')
try:
bas = rospy.ServiceProxy(
'/kill_board_interface/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 ThrusterFault(HandlerBase):
alarm_name = 'thruster-fault'
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
]
# Return a list that decodes the binary to strings
def _get_fault_codes(self, fault_id):
get_fault_codes = []
for key, value in self.fault_codes.iteritems():
# Fault message is the sum of binary strings
decode = fault_id & key
if decode != 0:
get_fault_codes.append(value)
return get_fault_codes
def _check_faults(self, msg, topic):
update = False
# Check if there is a change
if topic not in self._raised_alarms or self._raised_alarms[
topic] != msg.fault:
# Check if change is to no faults
if msg.fault == 0:
# If the topic is there, there delete it
if topic in self._raised_alarms:
del self._raised_alarms[topic]
update = True
# if not a no fault, then update
else:
self._raised_alarms[topic] = msg.fault
update = True
if update:
if len(self._raised_alarms) == 0:
self.broadcaster.clear_alarm()
return
self.broadcaster.raise_alarm(
severity=5,
problem_description='{} thrusters have faults'.format(
len(self._raised_alarms)),
parameters=dict([(t, self._get_fault_codes(k))
for t, k in self._raised_alarms.iteritems()]))
def raised(self, alarm):
pass
def cleared(self, alarm):
self._raised_alarms = {}
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 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 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.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 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):
'''
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
# 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):
if KillMessage.IDENTIFIER == ord(data[0]):
msg = KillMessage.from_bytes(data)
if not msg.is_response:
rospy.logwarn(
'Recieved kill message from board but was not response')
return
if msg.is_soft:
self._last_soft_kill = msg.is_asserted
elif msg.is_hard:
self._last_hard_kill = msg.is_asserted
self.update_hw_kill()
elif GoMessage.IDENTIFIER == ord(data[0]):
msg = GoMessage.from_bytes(data)
asserted = msg.asserted
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
else:
rospy.logwarn('UNEXPECTED MESSAGE with identifier {}'.format(
ord(data[0])))
return
rospy.loginfo(str(msg))
class ThrusterFault(HandlerBase):
alarm_name = 'thruster-fault'
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 = {}
# Return a list that decodes the binary to strings
def _get_fault_codes(self, fault_id):
get_fault_codes = []
for key, value in self.fault_codes.iteritems():
# Fault message is the sum of binary strings
decode = fault_id & key
if decode != 0:
get_fault_codes.append(value)
return get_fault_codes
def _check_faults(self, msg, topic):
update = False
# Check if there is a change
if topic not in self._raised_alarms or self._raised_alarms[topic] != msg.fault:
# Check if change is to no faults
if msg.fault == 0:
# If the topic is there, there delete it
if topic in self._raised_alarms:
del self._raised_alarms[topic]
update = True
# if not a no fault, then update
else:
self._raised_alarms[topic] = msg.fault
update = True
if update:
if len(self._raised_alarms) == 0:
self.broadcaster.clear_alarm()
return
self.broadcaster.raise_alarm(
severity=5,
problem_description='{} thrusters have faults'.format(
len(self._raised_alarms)),
parameters=dict([(t, self._get_fault_codes(k))
for t, k in self._raised_alarms.iteritems()])
)
def raised(self, alarm):
pass
def cleared(self, alarm):
self._raised_alarms = {}
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 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 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, status):
'''
If needed, update the hw-kill alarm so it reflects the latest status from the board
'''
# Set serverity / problem message appropriately
severity = 0
message = ""
if status.hard_killed:
severity = 2
message = "Hard kill"
elif status.soft_killed is True:
severity = 1
reasons = []
if status.switch_soft_kill:
reasons.append('switch')
if status.mobo_soft_kill:
reasons.append('mobo')
if status.heartbeat_lost:
reasons.append('hearbeat')
message = 'Soft kill from: ' + ','.join(reasons)
raised = severity != 0
# 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)
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
'''
status = StatusMessage.from_bytes(data)
self.update_hw_kill(status)
go = status.go_switch
if self._last_go is None or go != self._last_go:
if go:
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 = go
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 BusVoltageMonitor(object):
'''
Class that estimates sub8's thruster bus voltage.
As of May 2017, this is just a simple rolling average with a constant width sliding
window. However add_reading and get_estimate methods are left for when smarter
filtering is needed
'''
VMAX = 50 # volts
VMIN = 0 # volts
class VoltageReading(object):
def __init__(self, voltage, time):
self.v = voltage
self.t = time
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 add_reading(self, voltage, time):
''' Adds voltage readings to buffer '''
voltage = float(voltage)
# Only add if it makes sense (the M5's will give nonsense feedback at times)
if voltage >= self.VMIN and voltage <= self.VMAX:
self.buffer.append(self.VoltageReading(voltage, time))
self.prune_buffer()
# check bus voltage if enough time has passed
if rospy.Time.now() - self.last_estimate_time > self.ESTIMATION_PERIOD:
self.check_bus_voltage()
def prune_buffer(self):
''' Removes readings older than the window_duration from buffer '''
for reading in self.buffer:
age = rospy.Time.now() - reading.t
if age > self.WINDOW_DURATION:
self.buffer.remove(reading)
def get_voltage_estimate(self):
''' Returns average voltage in buffer '''
voltages = []
if len(self.buffer) == 0:
return None
for r in self.buffer:
voltages.append(r.v)
return np.mean(voltages)
def check_bus_voltage(self):
''' Publishes bus_voltage estimate and raises alarm if necessary '''
bus_voltage = self.get_voltage_estimate()
if bus_voltage is None:
return
self.bus_voltage_pub.publish(Float64(bus_voltage))
severity = None
if bus_voltage < self.warn_voltage:
severity = 3
if bus_voltage < self.kill_voltage:
severity = 5
if severity is not None and self.cached_severity != severity:
self.bus_voltage_alarm.raise_alarm(
problem_description='Bus voltage has fallen to {}'.format(bus_voltage),
parameters={'bus_voltage': bus_voltage},
severity=severity
)
self.cached_severity = severity
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 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
class ThrusterDriver(object):
_dropped_timeout = 1.0 # s
_window_duration = 30.0 # s
_NODE_NAME = rospy.get_name()
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)
@thread_lock(lock)
def load_thruster_ports(self, ports_layout, thruster_definitions):
''' Loads a dictionary ThrusterPort objects '''
self.ports = {} # ThrusterPort objects
self.thruster_to_port_map = {} # node_id to ThrusterPort
rospack = rospkg.RosPack()
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn(
"Running fake thrusters for simulation, based on parameter '/simulate'"
)
# Instantiate thruster comms port
for port_info in ports_layout:
port_name = port_info['port']
self.ports[port_name] = thruster_comm_factory(port_info,
thruster_definitions,
fake=self.make_fake)
# Add the thrusters to the thruster dict and configure if present
for thruster_name in port_info['thruster_names']:
self.thruster_to_port_map[thruster_name] = port_info['port']
if thruster_name not in self.ports[
port_name].online_thruster_names:
rospy.logerr(
"ThrusterDriver: {} IS MISSING!".format(thruster_name))
else:
rospy.loginfo(
"ThrusterDriver: {} registered".format(thruster_name))
# Set firmware settings
port = self.ports[port_name]
node_id = thruster_definitions[thruster_name]['node_id']
config_path = (
rospack.get_path('sub8_videoray_m5_thruster') +
'/config/firmware_settings/' + thruster_name + '.yaml')
rospy.loginfo(
'Configuring {} with settings specified in {}.'.format(
thruster_name, config_path))
port.set_registers_from_dict(
node_id=node_id,
reg_dict=rosparam.load_file(config_path)[0][0])
port.reboot_thruster(
node_id) # Necessary for some settings to take effect
def get_thruster_info(self, srv):
''' Get the thruster info for a particular thruster name '''
query_name = srv.thruster_name
info = self.ports[
self.thruster_to_port_map[query_name]].thruster_info[query_name]
thruster_info = ThrusterInfoResponse(
node_id=info.node_id,
min_force=info.thrust_bounds[0],
max_force=info.thrust_bounds[1],
position=numpy_to_point(info.position),
direction=Vector3(*info.direction))
return thruster_info
def check_alarm_status(self, alarm):
# If someone else cleared this alarm, we need to make sure to raise it again
if not alarm.raised and alarm.node_name != self._NODE_NAME:
self.update_thruster_out_alarm()
def update_thruster_out_alarm(self):
'''
Raises or clears the thruster out alarm
Updates the 'offline_thruster_names' parameter accordingly
Sets the severity to the number of failed thrusters (clipped at 5)
'''
offline_names = list(self.failed_thrusters)
if len(self.failed_thrusters) > 0:
self.thruster_out_alarm.raise_alarm(
node_name=self._NODE_NAME,
parameters={'offline_thruster_names': offline_names},
severity=int(np.clip(len(self.failed_thrusters), 1, 5)))
else:
self.thruster_out_alarm.clear_alarm(
node_name=self._NODE_NAME,
parameters={'offline_thruster_names': offline_names})
@thread_lock(lock)
def command_thruster(self, name, thrust):
'''
Issue a a force command (in Newtons) to a named thruster
Example names are BLR, FLH, etc.
Raises RuntimeError if a thrust value outside of the configured thrust bounds is commanded
Raises UnavailableThrusterException if a thruster that is offline is commanded a non-zero thrust
'''
port_name = self.thruster_to_port_map[name]
target_port = self.ports[port_name]
thruster_model = target_port.thruster_info[name]
if thrust < thruster_model.thrust_bounds[
0] or thrust > thruster_model.thrust_bounds[1]:
rospy.logwarn(
'Tried to command thrust ({}) outside of physical thrust bounds ({})'
.format(thrust, thruster_model.thrust_bounds))
if name in self.failed_thrusters:
if not np.isclose(thrust, 0):
rospy.logwarn(
'ThrusterDriver: commanding non-zero thrust to offline thruster ('
+ name + ')')
effort = target_port.thruster_info[name].get_effort_from_thrust(thrust)
# We immediately get thruster_status back
thruster_status = target_port.command_thruster(name, effort)
# Keep track of thrusters going online or offline
offline_on_port = target_port.get_offline_thruster_names()
for offline in offline_on_port:
if offline not in self.failed_thrusters:
self.failed_thrusters.add(offline) # Thruster went offline
for failed in copy.deepcopy(self.failed_thrusters):
if (failed in target_port.get_declared_thruster_names()
and failed not in offline_on_port
and failed not in self.deactivated_thrusters):
self.failed_thrusters.remove(failed) # Thruster came online
# Don't try to do anything if the thruster status is bad
if thruster_status is None:
return
message_contents = [
'rpm', 'bus_v', 'bus_i', 'temp', 'fault', 'command_tx_count',
'status_rx_count', 'command_latency_avg'
]
message_keyword_args = {
key: thruster_status[key]
for key in message_contents
}
power = thruster_status['bus_v'] * thruster_status['bus_i']
self.status_publishers[name].publish(
ThrusterStatus(header=Header(stamp=rospy.Time.now()),
name=name,
node_id=thruster_model.node_id,
power=power,
effort=effort,
thrust=thrust,
**message_keyword_args))
# Will publish bus_voltage and raise alarm if necessary
self.bus_voltage_monitor.add_reading(message_keyword_args['bus_v'],
rospy.Time.now())
# Undervolt/overvolt faults are unreliable (might not still be true - David)
if message_keyword_args['fault'] > 2:
fault_codes = {
(1 << 0): 'UNDERVOLT',
(1 << 1): 'OVERRVOLT',
(1 << 2): 'OVERCURRENT',
(1 << 3): 'OVERTEMP',
(1 << 4): 'STALL',
(1 << 5): 'STALL_WARN',
}
fault = int(message_keyword_args['fault'])
faults = []
for code, fault_name in fault_codes.items():
if code & fault != 0:
faults.append(fault_name)
rospy.logwarn(
"Thruster: {} has entered fault with status {}".format(
name, message_keyword_args))
rospy.logwarn("Fault causes are: {}".format(faults))
return
def thrust_cb(self, msg):
'''
Callback for receiving thrust commands
These messages contain a list of instructions, one for each thruster
If there are any updates to the list of failed thrusters, it will raise and alarm
'''
failed_before = {x for x in self.failed_thrusters}
for thrust_cmd in list(msg.thruster_commands):
self.command_thruster(thrust_cmd.name, thrust_cmd.thrust)
# Raise or clear 'thruster-out' alarm
if not self.failed_thrusters == failed_before:
rospy.logdebug('Failed thrusters:', self.failed_thrusters)
self.update_thruster_out_alarm()
def stop(self):
''' Commands 0 thrust to all thrusters '''
for port in self.ports.values():
for thruster_name in port.online_thruster_names.copy():
self.command_thruster(thruster_name, 0.0)
def fail_thruster(self, srv):
''' Makes a thruster unavailable for thrust allocation '''
# So that thrust is not allocated to the thruster
self.failed_thrusters.add(srv.thruster_name)
# So that it won't come back online even if comms are good
self.deactivated_thrusters.add(srv.thruster_name)
# So that thruster_mapper updates the B-matrix
self.update_thruster_out_alarm()
return {}
def unfail_thruster(self, srv):
''' Undoes effect of self.fail_thruster '''
self.failed_thrusters.remove(srv.thruster_name)
self.deactivated_thrusters.remove(srv.thruster_name)
self.update_thruster_out_alarm()
return {}
class ThrusterDriver(object):
_dropped_timeout = 1.0 # s
_window_duration = 30.0 # s
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 load_effort_to_thrust_map(self, path):
'''Load the effort to thrust mapping:
- Map force inputs from Newtons to [-1, 1] required by the thruster
'''
try:
_file = file(path)
except IOError as e:
rospy.logerr(
"Could not find thruster configuration file at {}".format(
path))
raise (e)
json_data = json.load(_file)
newtons = json_data['calibration_data']['newtons']
thruster_input = json_data['calibration_data']['thruster_input']
return newtons, thruster_input
@thread_lock(lock)
def load_thruster_layout(self, ports, thruster_definitions):
'''Load and handle the thruster layout'''
port_dict = {}
# These alarms require this service to be available before things will work
rospy.wait_for_service("update_thruster_layout")
self.thruster_out_alarm.clear_alarm(parameters={'clear_all': True})
for port_info in ports:
thruster_port = thruster_comm_factory(port_info,
thruster_definitions,
fake=self.make_fake)
# Add the thrusters to the thruster dict
for thruster_name in port_info['thruster_names']:
if thruster_name in thruster_port.missing_thrusters:
rospy.logerr("{} IS MISSING!".format(thruster_name))
self.alert_thruster_loss(
thruster_name, "Motor ID was not found on it's port.")
else:
rospy.loginfo("{} registered".format(thruster_name))
self.thruster_heartbeats[thruster_name] = None
port_dict[thruster_name] = thruster_port
return port_dict
def get_thruster_info(self, srv):
'''
Get the thruster info for a particular thruster ID
Right now, this is only the min and max thrust data
'''
# Unused right now
# query_id = srv.thruster_id
min_thrust = min(self.interpolate.x)
max_thrust = max(self.interpolate.x)
thruster_info = ThrusterInfoResponse(min_force=min_thrust,
max_force=max_thrust)
return thruster_info
def publish_bus_voltage(self, *args):
''' Publishes bus voltage estimate and raises bus_voltage alarm if needed '''
since_voltage = rospy.Time.now(
) - self.bus_voltage_estimator.get_last_update_time()
if (since_voltage) > rospy.Duration(0.5):
self.stop() # for safety
bus_voltage = self.bus_voltage_estimator.get_voltage_estimate()
if bus_voltage is not None:
msg = Float64(bus_voltage)
self.bus_voltage_pub.publish(msg)
self.check_bus_voltage(
bus_voltage) # also checks the severity of the bus voltage
def check_bus_voltage(self, voltage):
''' Raises bus_voltage alarm with a corresponding severity given a bus voltage '''
severity = None
if voltage < self.warn_voltage:
severity = 3
if voltage < self.kill_voltage:
severity = 5
if severity is not None:
self.bus_voltage_alarm.raise_alarm(
problem_description='Bus voltage has fallen to {}'.format(
voltage),
parameters={
'bus_voltage': voltage,
},
severity=severity)
def check_alarm_status(self, alarm):
# If someone else cleared this alarm, we need to make sure to raise it again
if not alarm.raised and len(
self.failed_thrusters) != 0 and not alarm.parameters.get(
"clear_all", False):
self.alert_thruster_loss(self.failed_thrusters[0], "Timed out")
def check_for_drops(self, *args):
for name, time in self.thruster_heartbeats.items():
if time is None:
# Thruster wasn't registered on startup
continue
elif rospy.Time.now() - time > rospy.Duration(
self._dropped_timeout):
rospy.logwarn(
"TIMEOUT, No recent response from: {}.".format(name))
if name not in self.failed_thrusters:
self.alert_thruster_loss(name, "Timed out")
# Check if the thruster is back up
self.command_thruster(name, 0)
elif name in self.failed_thrusters:
rospy.logwarn("Thruster {} has come back online".format(name))
self.alert_thruster_unloss(name)
def alert_thruster_unloss(self, thruster_name):
if thruster_name in self.failed_thrusters:
self.failed_thrusters.remove(thruster_name)
if len(self.failed_thrusters) == 0:
self.thruster_out_alarm.clear_alarm(parameters={"clear_all"})
else:
severity = 3 if len(self.failed_thrusters) <= rospy.get_param(
"thruster_loss_limit", 2) else 5
rospy.logerr(self.failed_thrusters)
self.thruster_out_alarm.raise_alarm(parameters={
'thruster_names':
self.failed_thrusters,
},
severity=severity)
def alert_thruster_loss(self, thruster_name, last_update):
if thruster_name not in self.failed_thrusters:
self.failed_thrusters.append(thruster_name)
# Severity rises to 5 if too many thrusters are out
severity = 3 if len(self.failed_thrusters) <= rospy.get_param(
"thruster_loss_limit", 2) else 5
rospy.logerr(self.failed_thrusters)
self.thruster_out_alarm.raise_alarm(
problem_description='Thruster {} has failed'.format(thruster_name),
parameters={
'thruster_names': self.failed_thrusters,
'last_update': last_update
},
severity=severity)
def fail_thruster(self, srv):
self.alert_thruster_loss(srv.thruster_name, None)
return FailThrusterResponse()
@thread_lock(lock)
def command_thruster(self, name, force):
'''Issue a a force command (in Newtons) to a named thruster
Example names are BLR, FLH, etc.
'''
target_port = self.port_dict[name]
margin_factor = 0.8
clipped_force = np.clip(force, margin_factor * min(self.interpolate.x),
margin_factor * max(self.interpolate.x))
normalized_force = self.interpolate(clipped_force)
if name in self.failed_thrusters:
normalized_force = 0
# We immediately get thruster_status back
thruster_status = target_port.command_thruster(name, normalized_force)
# Don't try to do anything if the thruster status is bad
if thruster_status is None:
return
message_contents = [
'rpm',
'bus_voltage',
'bus_current',
'temperature',
'fault',
'response_node_id',
]
message_keyword_args = {
key: thruster_status[key]
for key in message_contents
}
self.thruster_heartbeats[name] = rospy.Time.now()
self.status_pub.publish(
ThrusterStatus(header=Header(stamp=rospy.Time.now()),
name=name,
**message_keyword_args))
# TODO: TEST
self.bus_voltage_estimator.add_reading(
message_keyword_args['bus_voltage'], rospy.Time.now())
return
# Undervolt/overvolt faults are unreliable
if message_keyword_args['fault'] > 2:
fault_codes = {
(1 << 0): 'UNDERVOLT',
(1 << 1): 'OVERRVOLT',
(1 << 2): 'OVERCURRENT',
(1 << 3): 'OVERTEMP',
(1 << 4): 'STALL',
(1 << 5): 'STALL_WARN',
}
fault = int(message_keyword_args['fault'])
faults = []
for code, fault_name in fault_codes.items():
if code & fault != 0:
faults.append(fault_name)
rospy.logwarn(
"Thruster: {} has entered fault with status {}".format(
name, message_keyword_args))
rospy.logwarn("Fault causes are: {}".format(faults))
self.alert_thruster_loss(name, message_keyword_args)
def thrust_cb(self, msg):
'''Callback for receiving thrust commands
These messages contain a list of instructions, one for each thruster
'''
for thrust_cmd in list(msg.thruster_commands):
self.command_thruster(thrust_cmd.name, thrust_cmd.thrust)
def stop(self):
''' Commands 0 thrust to all thrusters '''
for name in self.port_dict.keys():
if name not in self.failed_thrusters:
self.command_thruster(name, 0.0)
class BusVoltageMonitor(object):
'''
Class that estimates sub8's thruster bus voltage.
As of May 2017, this is just a simple rolling average with a constant width sliding
window. However add_reading and get_estimate methods are left for when smarter
filtering is needed
'''
VMAX = 50 # volts
VMIN = 0 # volts
class VoltageReading(object):
def __init__(self, voltage, time):
self.v = voltage
self.t = time
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 add_reading(self, voltage, time):
''' Adds voltage readings to buffer '''
voltage = float(voltage)
# Only add if it makes sense (the M5's will give nonsense feedback at times)
if voltage >= self.VMIN and voltage <= self.VMAX:
self.buffer.append(self.VoltageReading(voltage, time))
self.prune_buffer()
# check bus voltage if enough time has passed
if rospy.Time.now() - self.last_estimate_time > self.ESTIMATION_PERIOD:
self.check_bus_voltage()
def prune_buffer(self):
''' Removes readings older than the window_duration from buffer '''
for reading in self.buffer:
age = rospy.Time.now() - reading.t
if age > self.WINDOW_DURATION:
self.buffer.remove(reading)
def get_voltage_estimate(self):
''' Returns average voltage in buffer '''
voltages = []
if len(self.buffer) == 0:
return None
for r in self.buffer:
voltages.append(r.v)
return np.mean(voltages)
def check_bus_voltage(self):
''' Publishes bus_voltage estimate and raises alarm if necessary '''
bus_voltage = self.get_voltage_estimate()
if bus_voltage is None:
return
self.bus_voltage_pub.publish(Float64(bus_voltage))
severity = None
if bus_voltage < self.warn_voltage:
severity = 3
if bus_voltage < self.kill_voltage:
severity = 5
if severity is not None and self.cached_severity != severity:
self.bus_voltage_alarm.raise_alarm(
problem_description='Bus voltage has fallen to {}'.format(
bus_voltage),
parameters={'bus_voltage': bus_voltage},
severity=severity)
self.cached_severity = severity
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)
if __name__ == "__main__":
rospy.init_node("callback_test")
ab = AlarmBroadcaster("test_alarm")
al = AlarmListener("test_alarm")
ab.clear_alarm()
rospy.sleep(0.1)
al.add_callback(cb1)
print "Inited"
assert al.is_cleared()
rospy.loginfo("Raise Test")
ab.raise_alarm()
rospy.sleep(0.1)
assert al.is_raised()
assert cb1_ran
cb1_ran = False
al.clear_callbacks()
al.add_callback(cb1, severity_required=2)
al.add_callback(cb2, call_when_raised=False)
rospy.loginfo("Severity Fail Test")
ab.raise_alarm(severity=3)
rospy.sleep(0.1)
assert not cb1_ran
assert not cb2_ran
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 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
#!/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"
class ThrusterDriver(object):
_dropped_timeout = 1.0 # s
_window_duration = 30.0 # s
_NODE_NAME = rospy.get_name()
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)
@thread_lock(lock)
def load_thruster_ports(self, ports_layout, thruster_definitions):
''' Loads a dictionary ThrusterPort objects '''
self.ports = {} # ThrusterPort objects
self.thruster_to_port_map = {} # node_id to ThrusterPort
rospack = rospkg.RosPack()
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'")
# Instantiate thruster comms port
for port_info in ports_layout:
port_name = port_info['port']
self.ports[port_name] = thruster_comm_factory(port_info, thruster_definitions, fake=self.make_fake)
# Add the thrusters to the thruster dict and configure if present
for thruster_name in port_info['thruster_names']:
self.thruster_to_port_map[thruster_name] = port_info['port']
if thruster_name not in self.ports[port_name].online_thruster_names:
rospy.logerr("ThrusterDriver: {} IS MISSING!".format(thruster_name))
else:
rospy.loginfo("ThrusterDriver: {} registered".format(thruster_name))
# Set firmware settings
port = self.ports[port_name]
node_id = thruster_definitions[thruster_name]['node_id']
config_path = (rospack.get_path('sub8_videoray_m5_thruster') + '/config/firmware_settings/' +
thruster_name + '.yaml')
rospy.loginfo('Configuring {} with settings specified in {}.'.format(thruster_name,
config_path))
port.set_registers_from_dict(node_id=node_id,
reg_dict=rosparam.load_file(config_path)[0][0])
port.reboot_thruster(node_id) # Necessary for some settings to take effect
def get_thruster_info(self, srv):
''' Get the thruster info for a particular thruster name '''
query_name = srv.thruster_name
info = self.ports[self.thruster_to_port_map[query_name]].thruster_info[query_name]
thruster_info = ThrusterInfoResponse(
node_id=info.node_id,
min_force=info.thrust_bounds[0],
max_force=info.thrust_bounds[1],
position=numpy_to_point(info.position),
direction=Vector3(*info.direction)
)
return thruster_info
def check_alarm_status(self, alarm):
# If someone else cleared this alarm, we need to make sure to raise it again
if not alarm.raised and alarm.node_name != self._NODE_NAME:
self.update_thruster_out_alarm()
def update_thruster_out_alarm(self):
'''
Raises or clears the thruster out alarm
Updates the 'offline_thruster_names' parameter accordingly
Sets the severity to the number of failed thrusters (clipped at 5)
'''
offline_names = list(self.failed_thrusters)
if len(self.failed_thrusters) > 0:
self.thruster_out_alarm.raise_alarm(
node_name=self._NODE_NAME,
parameters={'offline_thruster_names': offline_names},
severity=int(np.clip(len(self.failed_thrusters), 1, 5)))
else:
self.thruster_out_alarm.clear_alarm(
node_name=self._NODE_NAME,
parameters={'offline_thruster_names': offline_names})
@thread_lock(lock)
def command_thruster(self, name, thrust):
'''
Issue a a force command (in Newtons) to a named thruster
Example names are BLR, FLH, etc.
Raises RuntimeError if a thrust value outside of the configured thrust bounds is commanded
Raises UnavailableThrusterException if a thruster that is offline is commanded a non-zero thrust
'''
port_name = self.thruster_to_port_map[name]
target_port = self.ports[port_name]
thruster_model = target_port.thruster_info[name]
if thrust < thruster_model.thrust_bounds[0] or thrust > thruster_model.thrust_bounds[1]:
rospy.logwarn('Tried to command thrust ({}) outside of physical thrust bounds ({})'.format(
thrust, thruster_model.thrust_bounds))
if name in self.failed_thrusters:
if not np.isclose(thrust, 0):
rospy.logwarn('ThrusterDriver: commanding non-zero thrust to offline thruster (' + name + ')')
effort = target_port.thruster_info[name].get_effort_from_thrust(thrust)
# We immediately get thruster_status back
thruster_status = target_port.command_thruster(name, effort)
# Keep track of thrusters going online or offline
offline_on_port = target_port.get_offline_thruster_names()
for offline in offline_on_port:
if offline not in self.failed_thrusters:
self.failed_thrusters.add(offline) # Thruster went offline
for failed in copy.deepcopy(self.failed_thrusters):
if (failed in target_port.get_declared_thruster_names() and
failed not in offline_on_port and
failed not in self.deactivated_thrusters):
self.failed_thrusters.remove(failed) # Thruster came online
# Don't try to do anything if the thruster status is bad
if thruster_status is None:
return
message_contents = [
'rpm',
'bus_v',
'bus_i',
'temp',
'fault',
'command_tx_count',
'status_rx_count',
'command_latency_avg'
]
message_keyword_args = {key: thruster_status[key] for key in message_contents}
power = thruster_status['bus_v'] * thruster_status['bus_i']
self.status_publishers[name].publish(
ThrusterStatus(
header=Header(stamp=rospy.Time.now()),
name=name,
node_id=thruster_model.node_id,
power=power,
effort=effort,
thrust=thrust,
**message_keyword_args
)
)
# Will publish bus_voltage and raise alarm if necessary
self.bus_voltage_monitor.add_reading(message_keyword_args['bus_v'], rospy.Time.now())
# Undervolt/overvolt faults are unreliable (might not still be true - David)
if message_keyword_args['fault'] > 2:
fault_codes = {
(1 << 0): 'UNDERVOLT',
(1 << 1): 'OVERRVOLT',
(1 << 2): 'OVERCURRENT',
(1 << 3): 'OVERTEMP',
(1 << 4): 'STALL',
(1 << 5): 'STALL_WARN',
}
fault = int(message_keyword_args['fault'])
faults = []
for code, fault_name in fault_codes.items():
if code & fault != 0:
faults.append(fault_name)
rospy.logwarn("Thruster: {} has entered fault with status {}".format(name, message_keyword_args))
rospy.logwarn("Fault causes are: {}".format(faults))
return
def thrust_cb(self, msg):
'''
Callback for receiving thrust commands
These messages contain a list of instructions, one for each thruster
If there are any updates to the list of failed thrusters, it will raise and alarm
'''
failed_before = {x for x in self.failed_thrusters}
for thrust_cmd in list(msg.thruster_commands):
self.command_thruster(thrust_cmd.name, thrust_cmd.thrust)
# Raise or clear 'thruster-out' alarm
if not self.failed_thrusters == failed_before:
rospy.logdebug('Failed thrusters:', self.failed_thrusters)
self.update_thruster_out_alarm()
def stop(self):
''' Commands 0 thrust to all thrusters '''
for port in self.ports.values():
for thruster_name in port.online_thruster_names.copy():
self.command_thruster(thruster_name, 0.0)
def fail_thruster(self, srv):
''' Makes a thruster unavailable for thrust allocation '''
# So that thrust is not allocated to the thruster
self.failed_thrusters.add(srv.thruster_name)
# So that it won't come back online even if comms are good
self.deactivated_thrusters.add(srv.thruster_name)
# So that thruster_mapper updates the B-matrix
self.update_thruster_out_alarm()
return {}
def unfail_thruster(self, srv):
''' Undoes effect of self.fail_thruster '''
self.failed_thrusters.remove(srv.thruster_name)
self.deactivated_thrusters.remove(srv.thruster_name)
self.update_thruster_out_alarm()
return {}
