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)
                         )
Example #3
0
    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))
Example #4
0
    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
Example #5
0
    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))