def test_lose_thruster(self): '''Trigger a thruster loss and verify that it is no longer used Behavior: 1. Trigger the thruster failure alarm (By manually failing a simulated thruster) 2. Issue a wrench command 3. Verify that the navigator_msgs/Thrust command does not contain a command to that thruster ''' rospy.Subscriber("/thrusters/thrust", Thrust, self.thrust_callback) wrench_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1, latch=True) thruster_to_fail = 'BRV' # TODO: Compress this anymore subscribed = wait_for_subscriber('thruster_mapper', 'wrench', timeout=10.0) self.assertTrue( subscribed, "{} did not not come up in time".format('thruster_mapper')) # Fail a thruster # TODO: Raise if waiting fails rospy.wait_for_service('fail_thruster', timeout=5.0) fail_thruster_proxy = rospy.ServiceProxy('fail_thruster', FailThruster) fail_thruster_proxy(thruster_to_fail) # This will raise an alarm! rospy.sleep(0.2) # Wait some time for the update to finish # Spend up to 0.2 seconds waiting for a response (Bide time until the mapper finishes reacting) # Looping because mapper ignores wrenches until it finishes remapping thrusters give_up_time = time.time() + 0.2 while not (rospy.is_shutdown() and (time.time() < give_up_time)): wrench_pub.publish( WrenchStamped( # The actual force/torque is irrelevant, we still send a 0 command for every active thruster wrench=Wrench(force=Vector3(10.0, 10.0, 10.0), torque=Vector3(10.0, 10.0, 10.0)))) if self.called: break time.sleep(0.1) self.assertTrue(self.called) self.assertIsNotNone( self.last_thrust_cmd, msg="Never got thrust command after issuing wrench") names = [] for cmd in self.last_thrust_cmd.thruster_commands: names.append(cmd.name) self.assertNotIn( thruster_to_fail, names, msg= "Thruster mapper continued to issue commands to {} after thruster loss alarm" .format(thruster_to_fail))
def test_lose_thruster(self): '''Trigger a thruster loss and verify that it is no longer used Behavior: 1. Trigger the thruster failure alarm (By manually failing a simulated thruster) 2. Issue a wrench command 3. Verify that the sub8_msgs/Thrust command does not contain a command to that thruster ''' rospy.Subscriber("/thrusters/thrust", Thrust, self.thrust_callback) wrench_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1, latch=True) thruster_to_fail = 'BRV' # TODO: Compress this anymore subscribed = wait_for_subscriber('thruster_mapper', 'wrench', timeout=10.0) self.assertTrue( subscribed, "{} did not not come up in time".format('thruster_mapper') ) # Fail a thruster # TODO: Raise if waiting fails rospy.wait_for_service('fail_thruster', timeout=5.0) fail_thruster_proxy = rospy.ServiceProxy('fail_thruster', FailThruster) fail_thruster_proxy(thruster_to_fail) # This will raise an alarm! rospy.sleep(0.2) # Wait some time for the update to finish # Spend up to 0.2 seconds waiting for a response (Bide time until the mapper finishes reacting) # Looping because mapper ignores wrenches until it finishes remapping thrusters give_up_time = time.time() + 0.2 while not(rospy.is_shutdown() and (time.time() < give_up_time)): wrench_pub.publish(WrenchStamped( # The actual force/torque is irrelevant, we still send a 0 command for every active thruster wrench=Wrench( force=Vector3(10.0, 10.0, 10.0), torque=Vector3(10.0, 10.0, 10.0) ) )) if self.called: break time.sleep(0.1) self.assertTrue(self.called) self.assertIsNotNone(self.last_thrust_cmd, msg="Never got thrust command after issuing wrench") names = [] for cmd in self.last_thrust_cmd.thruster_commands: names.append(cmd.name) self.assertNotIn(thruster_to_fail, names, msg="Thruster mapper continued to issue commands to {} after thruster loss alarm".format( thruster_to_fail) )
def test_lose_thruster(self): '''Trigger a thruster loss and verify that it is no longer used''' rospy.Subscriber("/thrusters/thrust", Thrust, self.thrust_callback) wrench_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1, latch=True) thruster_to_fail = 'BRV' # TODO: Compress this anymore subscribed = wait_for_subscriber('thruster_mapper', 'wrench') self.assertTrue( subscribed, "{} did not not come up in time".format('thruster_mapper')) # Fail a thruster # TODO: Raise if waiting fails rospy.wait_for_service('fail_thruster', timeout=5.0) fail_thruster_proxy = rospy.ServiceProxy('fail_thruster', FailThruster) fail_thruster_proxy(thruster_to_fail) time.sleep(0.1) wrench_pub.publish( WrenchStamped( # The actual force/torque is irrelevant, we still send a 0 command for every active thruster wrench=Wrench(force=Vector3(10.0, 10.0, 10.0), torque=Vector3(10.0, 10.0, 10.0)))) time.sleep(0.1) self.assertTrue(self.called) self.assertIsNotNone( self.last_thrust_cmd, msg="Never got thrust command after issuing wrench") names = [] for cmd in self.last_thrust_cmd.thruster_commands: names.append(cmd.name) self.assertNotIn( thruster_to_fail, names, msg= "Thruster mapper continued to issue commands to {} after thruster loss alarm" .format(thruster_to_fail))
def test_map_good(self): '''Test desired wrenches that are known to be achievable ''' target_node = 'thruster_mapper' target_topic = '/wrench' subscribed = wait_for_subscriber(target_node, target_topic) self.assertTrue( subscribed, "{} did not not come up in time".format(target_node) ) thrust_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1, latch=True) wrenches = [ np.zeros(6), np.arange(6) * 5, np.arange(6)[::-1] * 5, np.arange(6) * -5, np.arange(6)[::-1] * -5, np.arange(6)[::-1] * -100, np.arange(6)[::-1] * 100, ] rospy.Subscriber("/thrusters/thrust", Thrust, self.thrust_callback) time.sleep(0.1) # Wait for the Subscriber to 'wake up' for num, wrench in enumerate(wrenches): wrench_msg = WrenchStamped( wrench=Wrench( force=Vector3(*wrench[:3]), torque=Vector3(*wrench[3:]) ) ) thrust_pub.publish(wrench_msg) timeout_t = time.time() + 0.01 while not rospy.is_shutdown() and time.time() < timeout_t and not self.got_msg: time.sleep(0.01) self.assertEqual(len(self.test_data) - 1, num, msg="Could not compute wrench for " + str(wrench) + " within timeout") self.got_msg = False rospy.sleep(0.06) # Wait the timeout period
def test_lose_thruster(self): '''Trigger a thruster loss and verify that it is no longer used''' rospy.Subscriber("/thrusters/thrust", Thrust, self.thrust_callback) wrench_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1, latch=True) thruster_to_fail = 'BRV' # TODO: Compress this anymore subscribed = wait_for_subscriber('thruster_mapper', 'wrench') self.assertTrue( subscribed, "{} did not not come up in time".format('thruster_mapper') ) # Fail a thruster # TODO: Raise if waiting fails rospy.wait_for_service('fail_thruster', timeout=5.0) fail_thruster_proxy = rospy.ServiceProxy('fail_thruster', FailThruster) fail_thruster_proxy(thruster_to_fail) time.sleep(0.1) wrench_pub.publish(WrenchStamped( # The actual force/torque is irrelevant, we still send a 0 command for every active thruster wrench=Wrench( force=Vector3(10.0, 10.0, 10.0), torque=Vector3(10.0, 10.0, 10.0) ) )) time.sleep(0.1) self.assertTrue(self.called) self.assertIsNotNone(self.last_thrust_cmd, msg="Never got thrust command after issuing wrench") names = [] for cmd in self.last_thrust_cmd.thruster_commands: names.append(cmd.name) self.assertNotIn(thruster_to_fail, names, msg="Thruster mapper continued to issue commands to {} after thruster loss alarm".format(thruster_to_fail))