<commit_before>#!/usr/bin/env python

PKG = 'sub8_alarm'
NAME = 'test_alarm'

import sys
import rospy
import rostest
import unittest
import numpy as np
from sub8_msgs.msg import Thrust, ThrusterCmd
from sub8_msgs.srv import FailThruster
from geometry_msgs.msg import WrenchStamped, Wrench, Vector3
from sub8_ros_tools import wait_for_subscriber
import time


class TestAlarmIntegration(unittest.TestCase):
    def setUp(self):
        self.last_thrust_cmd = None
        self.called = False

    def thrust_callback(self, msg):
        self.called = True
        self.last_thrust_cmd = msg

    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))

    @unittest.skip("Not ready")
    def test_kill(self):
        '''Test a 'kill' alarm'''
        pass

    def test_unkill(self):
        '''Test an unkill alarm'''
        pass


if __name__ == '__main__':
    rospy.init_node(NAME, anonymous=True)
    rostest.rosrun(PKG, NAME, TestAlarmIntegration, sys.argv)<commit_msg>ALARMS: Make integration test more realistic<commit_after>#!/usr/bin/env python

PKG = 'sub8_alarm'
NAME = 'test_alarm'

import sys
import rospy
import rostest
import unittest
import numpy as np
from sub8_msgs.msg import Thrust, ThrusterCmd
from sub8_msgs.srv import FailThruster
from geometry_msgs.msg import WrenchStamped, Wrench, Vector3
from sub8_ros_tools import wait_for_subscriber
import time


class TestAlarmIntegration(unittest.TestCase):
    def setUp(self):
        self.last_thrust_cmd = None
        self.called = False

    def thrust_callback(self, msg):
        self.called = True
        self.last_thrust_cmd = msg

    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', 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)

        time.sleep(0.1)
        give_up_time = time.time() + 1.5
        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))

    @unittest.skip("Not ready")
    def test_kill(self):
        '''Test a 'kill' alarm'''
        pass

    @unittest.skip("Not ready")
    def test_unkill(self):
        '''Test an unkill alarm'''
        pass


if __name__ == '__main__':
    rospy.init_node(NAME, anonymous=True)
    rostest.rosrun(PKG, NAME, TestAlarmIntegration, sys.argv)
{'fraction_non_alphanumeric': 0.09292622795372653, 'fraction_numerical': 0.013464820785131804, 'mean_word_length': 2.6322314049586777, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4936483', 'n_tokens_mistral': 1741, 'n_tokens_neox': 1681, 'n_words': 390}