From 5c2616604574d7bf10677e4c2aea3a4b3390586a Mon Sep 17 00:00:00 2001 From: Lucas Bassett-Audain Date: Mon, 5 Jun 2017 19:35:11 -0400 Subject: [PATCH 1/4] B_MATRIX KILL: Fix merge issue --- .../nodes/thruster_driver.py | 86 ++++++++++++++++++- 1 file changed, 85 insertions(+), 1 deletion(-) diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py index 50efd01c..cce5ae5d 100755 --- a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py +++ b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py @@ -9,7 +9,7 @@ from std_msgs.msg import Header, Float64 from sub8_msgs.msg import Thrust, ThrusterStatus from mil_ros_tools import wait_for_param, thread_lock -from sub8_msgs.srv import ThrusterInfo, ThrusterInfoResponse, FailThruster, FailThrusterResponse +from sub8_msgs.srv import ThrusterInfo, ThrusterInfoResponse, FailThruster, FailThrusterResponse, UpdateThrusterLayout, BMatrix from sub8_thruster_comm import thruster_comm_factory from ros_alarms import AlarmBroadcaster, AlarmListener lock = threading.Lock() @@ -337,6 +337,90 @@ def stop(self): if name not in self.failed_thrusters: self.command_thruster(name, 0.0) + 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: + rospy.wait_for_service('update_thruster_layout') + try: + update = rospy.ServiceProxy('update_thruster_layout', UpdateThrusterLayout) + rospy.wait_for_service('b_matrix') + try: + B = rospy.ServiceProxy('b_matrix', BMatrix) + B = np.asmatrix(B) + if (np.linalg.matrix_rank(B) < 6): + self.thruster_out_alarm.raise_alarm( + parameters={ + 'thruster_names': self.failed_thrusters + "B MATRIX IS SINGULAR", + }, + severity=5 + ) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + except rospy.ServiceException, e: + print "Service call failed: %s"%e + 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) + rospy.wait_for_service('update_thruster_layout') + try: + update = rospy.ServiceProxy('update_thruster_layout') + rospy.wait_for_service('b_matrix') + try: + B = rospy.ServiceProxy('b_matrix') + B = np.asmatrix(B) + if (np.linalg.matrix_rank(B) < 6): + self.thruster_out_alarm.raise_alarm( + parameters={ + 'thruster_names': self.failed_thrusters + "B MATRIX IS SINGULAR", + }, + severity=5 + ) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + except rospy.ServiceException, e: + print "Service call failed: %s"%e + # 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 alert_bus_voltage(self, voltage): + severity = 3 + # Kill if the voltage goes too low + if self.bus_voltage < rospy.get_param("/battery/kill_voltage", 44.0): + severity = 5 + + self.bus_voltage_alarm.raise_alarm( + problem_description='Bus voltage has fallen to {}'.format(voltage), + parameters={ + 'bus_voltage': voltage, + }, + severity=severity + ) + + def fail_thruster(self, srv): + self.alert_thruster_loss(srv.thruster_name, None) + return FailThrusterResponse() + if __name__ == '__main__': PKG = 'sub8_videoray_m5_thruster' From 6c8a10285d98a78843dc479daa6e5a84ec3d0f9c Mon Sep 17 00:00:00 2001 From: Lucas Bassett-Audain Date: Fri, 26 May 2017 15:21:03 -0400 Subject: [PATCH 2/4] THRUSTER ALARM: Update B matrix in mapper when a thruster goes out --- gnc/sub8_thruster_mapper/nodes/mapper.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/gnc/sub8_thruster_mapper/nodes/mapper.py b/gnc/sub8_thruster_mapper/nodes/mapper.py index 8cffc07d..23a211b3 100755 --- a/gnc/sub8_thruster_mapper/nodes/mapper.py +++ b/gnc/sub8_thruster_mapper/nodes/mapper.py @@ -63,10 +63,14 @@ def update_layout(self, srv): self.max_thrusts = np.copy(self.default_max_thrusts) for thruster_name in self.dropped_thrusters: - thruster_index = self.thruster_name_map.index(thruster_name) - self.min_thrusts[thruster_index] = -self.min_commandable_thrust * 0.5 - self.max_thrusts[thruster_index] = self.min_commandable_thrust * 0.5 - + try: + thruster_index = self.thruster_name_map.index(thruster_name) + self.min_thrusts[thruster_index] = -self.min_commandable_thrust * 0.5 + self.max_thrusts[thruster_index] = self.min_commandable_thrust * 0.5 + except: + print "Failed" + self.thruster_layout['thrusters'].pop(thruster_name, "None") + self.B = self.generate_B(self.thruster_layout) rospy.logwarn("Layout updated") return UpdateThrusterLayoutResponse() @@ -123,6 +127,7 @@ def generate_B(self, layout): ) self.num_thrusters += 1 B.append(wrench_column) + return np.transpose(np.array(B)) def map(self, wrench): From d9268fb725f25b7452bd42be073bfe758b6a96b9 Mon Sep 17 00:00:00 2001 From: Lucas Bassett-Audain Date: Fri, 26 May 2017 15:21:56 -0400 Subject: [PATCH 3/4] THRUSTER ALARM: Fix B matrix service call and rank check --- .../nodes/thruster_driver.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py index cce5ae5d..4fea50fc 100755 --- a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py +++ b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py @@ -373,17 +373,18 @@ def alert_thruster_unloss(self, thruster_name): def alert_thruster_loss(self, thruster_name, last_update): if thruster_name not in self.failed_thrusters: self.failed_thrusters.append(thruster_name) - rospy.wait_for_service('update_thruster_layout') try: - update = rospy.ServiceProxy('update_thruster_layout') - rospy.wait_for_service('b_matrix') + update = rospy.ServiceProxy('update_thruster_layout', UpdateThrusterLayout) + update() try: - B = rospy.ServiceProxy('b_matrix') - B = np.asmatrix(B) + get_b = rospy.ServiceProxy('b_matrix', BMatrix) + b_response = get_b() + B = np.asmatrix(b_response.B).reshape(6, -1) if (np.linalg.matrix_rank(B) < 6): self.thruster_out_alarm.raise_alarm( parameters={ - 'thruster_names': self.failed_thrusters + "B MATRIX IS SINGULAR", + 'thruster_names': self.failed_thrusters, + 'issue': "B MATRIX IS SINGULAR", }, severity=5 ) From 2876a0e4d6ef3a6cf458a05dfb9c5a660091e9a7 Mon Sep 17 00:00:00 2001 From: Lucas Bassett-Audain Date: Fri, 26 May 2017 15:42:17 -0400 Subject: [PATCH 4/4] THRUSTER ALARM: Clean up try logic --- .../nodes/thruster_driver.py | 115 +++++++----------- 1 file changed, 41 insertions(+), 74 deletions(-) diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py index 4fea50fc..b990bd09 100755 --- a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py +++ b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py @@ -221,38 +221,6 @@ def check_for_drops(self, *args): 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() @@ -342,33 +310,34 @@ def alert_thruster_unloss(self, thruster_name): self.failed_thrusters.remove(thruster_name) if len(self.failed_thrusters) == 0: self.thruster_out_alarm.clear_alarm(parameters={"clear_all"}) - else: - rospy.wait_for_service('update_thruster_layout') - try: - update = rospy.ServiceProxy('update_thruster_layout', UpdateThrusterLayout) - rospy.wait_for_service('b_matrix') - try: - B = rospy.ServiceProxy('b_matrix', BMatrix) - B = np.asmatrix(B) - if (np.linalg.matrix_rank(B) < 6): - self.thruster_out_alarm.raise_alarm( - parameters={ - 'thruster_names': self.failed_thrusters + "B MATRIX IS SINGULAR", - }, - severity=5 - ) - except rospy.ServiceException, e: - print "Service call failed: %s"%e - except rospy.ServiceException, e: - print "Service call failed: %s"%e - 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 - ) + try: + update = rospy.ServiceProxy('update_thruster_layout', UpdateThrusterLayout) + update() + get_b = rospy.ServiceProxy('b_matrix', BMatrix) + b_proxy = get_b() + B = np.asmatrix(b_proxy.B).reshape(6, -1) + if (np.linalg.matrix_rank(B) < 6): + self.thruster_out_alarm.raise_alarm( + parameters={ + 'thruster_names': self.failed_thrusters, + 'issue': "B MATRIX IS SINGULAR", + }, + severity=5 + ) + + except rospy.ServiceException, e: + rospy.logerr("Service call failed: %s"%e) + # 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 alert_thruster_loss(self, thruster_name, last_update): if thruster_name not in self.failed_thrusters: @@ -376,22 +345,20 @@ def alert_thruster_loss(self, thruster_name, last_update): try: update = rospy.ServiceProxy('update_thruster_layout', UpdateThrusterLayout) update() - try: - get_b = rospy.ServiceProxy('b_matrix', BMatrix) - b_response = get_b() - B = np.asmatrix(b_response.B).reshape(6, -1) - if (np.linalg.matrix_rank(B) < 6): - self.thruster_out_alarm.raise_alarm( - parameters={ - 'thruster_names': self.failed_thrusters, - 'issue': "B MATRIX IS SINGULAR", - }, - severity=5 - ) - except rospy.ServiceException, e: - print "Service call failed: %s"%e + get_b = rospy.ServiceProxy('b_matrix', BMatrix) + b_proxy = get_b() + B = np.asmatrix(b_proxy.B).reshape(6, -1) + if (np.linalg.matrix_rank(B) < 6): + self.thruster_out_alarm.raise_alarm( + parameters={ + 'thruster_names': self.failed_thrusters, + 'issue': "B MATRIX IS SINGULAR", + }, + severity=5 + ) + except rospy.ServiceException, e: - print "Service call failed: %s"%e + rospy.logerr("Service call failed: %s"%e) # 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)