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 StationHold(HandlerBase): alarm_name = 'station-hold' def __init__(self): self.move_client = SimpleActionClient('/move_to', MoveAction) self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect) self.broadcaster = AlarmBroadcaster(self.alarm_name) def _client_cb(self, terminal_state, result): if terminal_state != 3: rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format( TerminalState.to_string(terminal_state), result)) return try: self.change_wrench('autonomous') rospy.loginfo('Now station holding') self.broadcaster.clear_alarm() except rospy.ServiceException as e: rospy.logwarn('Station hold changing wrench failed: {}'.format(e)) def raised(self, alarm): rospy.loginfo("Attempting to station hold") station_hold_goal = MoveGoal() station_hold_goal.move_type = 'hold' self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb) def cleared(self, alarm): # When cleared, do nothing and just wait for new goal / custom wrench pass
class StationHold(HandlerBase): alarm_name = 'station-hold' def __init__(self): self.move_client = SimpleActionClient('/move_to', MoveAction) self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect) self.broadcaster = AlarmBroadcaster(self.alarm_name) def _client_cb(self, terminal_state, result): if terminal_state != 3: rospy.logwarn( 'Station hold goal failed (Status={}, Result={})'.format( TerminalState.to_string(terminal_state), result)) return try: self.change_wrench('autonomous') rospy.loginfo('Now station holding') self.broadcaster.clear_alarm() except rospy.ServiceException as e: rospy.logwarn('Station hold changing wrench failed: {}'.format(e)) def raised(self, alarm): rospy.loginfo("Attempting to station hold") station_hold_goal = MoveGoal() station_hold_goal.move_type = 'hold' self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb) def cleared(self, alarm): # When cleared, do nothing and just wait for new goal / custom wrench pass
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 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_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 HeightOverBottom(HandlerBase): alarm_name = "height-over-bottom" def __init__(self): self._killed = False self._update_height() self.ab = AlarmBroadcaster(self.alarm_name, node_name="height_over_bottom_kill") # Keep track of the current height self._last_height = 100 set_last_height = lambda msg: setattr(self, "_last_height", msg.range) rospy.Subscriber("/dvl/range", RangeStamped, set_last_height) # Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing. rospy.Timer(rospy.Duration(5), self._update_height) # This should smooth out random dips below the limit rospy.Timer(rospy.Duration(0.5), self._do_check) def _do_check(self, *args): if self._last_height <= self._height_to_kill and not self._killed: rospy.logwarn("SUB TOO LOW!") self.ab.raise_alarm(problem_description="The sub was too low: {}".format(self._last_height), parameters={"height": self._last_height}, severity=5 ) elif self._last_height >= self._height_to_kill and self._killed: rospy.logwarn("REVIVING") self.ab.clear_alarm() def _update_height(self, *args): self._height_to_kill = rospy.get_param("/height_over_bottom", 0.4) def raised(self, alarm): self._killed = True def cleared(self, alarm): self._killed = False
class StationHold(HandlerBase): alarm_name = 'station-hold' def __init__(self): self.task_client = TaskClient() self.broadcaster = AlarmBroadcaster(self.alarm_name) def _client_cb(self, terminal_state, result): if terminal_state != 3: rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format( TerminalState.to_string(terminal_state), result.result)) return rospy.loginfo('Station holding!') self.broadcaster.clear_alarm() def raised(self, alarm): rospy.loginfo("Attempting to station hold") self.task_client.run_task('StationHold', done_cb=self._client_cb) def cleared(self, alarm): # When cleared, do nothing and just wait for new goal / custom wrench pass
class StationHold(HandlerBase): alarm_name = 'station-hold' def __init__(self): self.task_client = TaskClient() self.broadcaster = AlarmBroadcaster(self.alarm_name) def _client_cb(self, terminal_state, result): if terminal_state != 3: rospy.logwarn( 'Station hold goal failed (Status={}, Result={})'.format( TerminalState.to_string(terminal_state), result.result)) return rospy.loginfo('Station holding!') self.broadcaster.clear_alarm() def raised(self, alarm): rospy.loginfo("Attempting to station hold") self.task_client.run_task('StationHold', done_cb=self._client_cb) def cleared(self, alarm): # When cleared, do nothing and just wait for new goal / custom wrench pass
def 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 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 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 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 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 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 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 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._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 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 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 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.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 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 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 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 = {}
#!/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 {}