def setUp(self):
rospy.init_node('state_fusion_validation_test')
self.agent = Agent(strategy='normal')
self.fusion_validate = Publisher('mock/fusion_validate', String)
self.target = mock_msgs.create_victim_info(id=1, probability=0.65)
self.agent.target.set(self.target)
self.agent.set_breakpoint('exploration')
def setUp(self):
self.agent = Agent(strategy='normal')
self.agent.machine.add_state('test_validate_gui')
self.agent.machine.add_transition('operator_responded', 'off',
'test_validate_gui')
self.validate_gui_mock = Publisher('mock/validate_gui', String)
self.gui_result = Publisher('mock/gui_result', Bool)
sleep(2)
class TestOperatorValidationState(unittest.TestCase):
""" Tests for the operator validation state. """
def setUp(self):
rospy.init_node('operator_validation_state')
self.agent = Agent(strategy='normal')
self.gui = Publisher('mock/validate_gui', String)
self.gui_result = Publisher('mock/gui_result', Bool)
self.agent.set_breakpoint('fusion_validation')
def test_to_fusion_validation_by_success(self):
self.agent.target.set(mock_msgs.create_victim_info())
if not rospy.is_shutdown():
rospy.sleep(1)
self.gui.publish('success:1')
self.agent.to_operator_validation()
self.assertEqual(self.agent.state, 'fusion_validation')
def test_to_fusion_validation_by_abort(self):
self.agent.target.set(mock_msgs.create_victim_info())
if not rospy.is_shutdown():
rospy.sleep(1)
self.gui.publish('abort:1')
self.agent.to_operator_validation()
self.assertEqual(self.agent.state, 'fusion_validation')
self.assertFalse(self.agent.gui_result.victimValid)
def test_update_victims_valid(self):
""" Expecting to add the valid victim """
if not rospy.is_shutdown():
rospy.sleep(1)
self.gui_result.publish(True)
self.gui.publish('success:2')
msg = mock_msgs.create_victim_info(id=5, probability=0.8)
self.agent.target.set(msg)
self.agent.to_operator_validation()
self.assertTrue(self.agent.gui_result.victimValid)
self.assertEqual(self.agent.state, 'fusion_validation')
def test_update_victims_invalid(self):
""" Expecting to ignore this victim """
if not rospy.is_shutdown():
rospy.sleep(1)
self.gui_result.publish(False)
self.gui.publish('success:2')
msg = mock_msgs.create_victim_info(id=5, probability=0.2)
self.agent.target.set(msg)
self.agent.to_operator_validation()
self.assertEqual(self.agent.state, 'fusion_validation')
self.assertFalse(self.agent.gui_result.victimValid)
def setUp(self):
rospy.init_node('state_identification_test')
self.move_base_mock = Publisher('mock/feedback_move_base', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('victim_deletion')
self.agent.set_breakpoint('sensor_hold')
self.agent.target.set(
mock_msgs.create_victim_info(id=1, probability=0.4))
self.events = [
self.agent.move_base_success, self.agent.move_base_retry,
self.agent.move_base_feedback, self.agent.move_base_resend
]
def setUp(self):
rospy.init_node('state_epxloration_test')
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('identification')
self.agent.set_breakpoint('end')
self.agent.set_breakpoint('init')
self.explorer = Publisher('mock/explorer', String)
self.agent.preempt_end_effector = lambda: True
self.agent.preempt_explorer = lambda: True
self.events = [
self.agent.exploration_success, self.agent.exploration_retry,
self.agent.poi_found
]
class TestEndState(unittest.TestCase):
""" Tests for the end state. """
def setUp(self):
rospy.init_node('state_end_test')
self.agent = Agent(strategy='normal')
def test_global_state_change(self):
final = RobotModeMsg.MODE_TELEOPERATED_LOCOMOTION
self.agent.to_end()
self.assertEqual(self.agent.state_changer.get_current_state(), final)
self.assertEqual(self.agent.state, 'off')
class ChooseTarget(unittest.TestCase):
""" Test choose_target function. """
def setUp(self):
self.agent = Agent(testing=True)
self.agent.current_pose = mock_msgs.create_pose(x=0, y=0)
def test_empty_targets(self):
""" Should throw an error and return None. """
targets = []
next_target = self.agent.choose_target(targets)
self.assertIsNone(next_target)
def test_with_one_target(self):
""" Should return the one target. """
target = mock_msgs.create_victim_info()
next_target = self.agent.choose_target([target])
self.assertEqual(target, next_target)
def test_equal_distance_targets(self):
""" Should return the first target. """
target1 = mock_msgs.create_victim_info(id=1)
target1.victimPose = mock_msgs.create_pose_stamped(x=1, y=1)
target2 = mock_msgs.create_victim_info(id=2)
target2.victimPose = mock_msgs.create_pose_stamped(x=1, y=1)
next_target = self.agent.choose_target([target1, target2])
self.assertEqual(next_target, target1)
def test_with_normal_targets(self):
""" Should return the nearest target. """
target1 = mock_msgs.create_victim_info(id=1)
target1.victimPose = mock_msgs.create_pose_stamped(x=1, y=1)
target2 = mock_msgs.create_victim_info(id=2)
target2.victimPose = mock_msgs.create_pose_stamped(x=2, y=2)
target3 = mock_msgs.create_victim_info(id=3)
target3.victimPose = mock_msgs.create_pose_stamped(x=3, y=3)
next_target = self.agent.choose_target([target1, target2, target3])
self.assertEqual(next_target, target1)
class TestInitState(unittest.TestCase):
""" Tests for the init state. """
def setUp(self):
rospy.init_node('state_init_test')
self.effector_mock = Publisher('mock/effector', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('exploration')
def test_init_to_exploration(self):
self.effector_mock.publish('success:1')
self.agent.to_init()
self.assertEqual(self.agent.state, 'exploration')
def test_initialization_with_effector_failure(self):
self.effector_mock.publish('abort:1')
# If the end effector is not responsive the init
# task will loop forever. Using this decorator
# we limit the execution time of the task.
# Wrap your function and test the wrapper.
@TimeLimiter(timeout=5)
def init_wrapper():
self.agent.to_init()
self.assertRaises(TimeoutException, init_wrapper)
self.effector_mock.publish('preempt:1')
@TimeLimiter(timeout=5)
def init_wrapper():
self.agent.to_init()
self.assertRaises(TimeoutException, init_wrapper)
def test_global_state_change(self):
self.effector_mock.publish('success:1')
initial_state = RobotModeMsg.MODE_OFF
final_state = RobotModeMsg.MODE_START_AUTONOMOUS
self.assertEqual(self.agent.state_changer.get_current_state(),
initial_state)
self.agent.wake_up()
self.assertEqual(self.agent.state_changer.get_current_state(),
final_state)
def setUp(self):
# Register the mock servers.
rospy.init_node('unit_base_control')
self.move_base_mock = Publisher('mock/feedback_move_base', String)
self.agent = Agent(strategy='normal')
target = mock_msgs.create_victim_info(id=8, probability=0.65)
self.agent.target_victim = target
class TestVictimDeletionState(unittest.TestCase):
""" Tests for the victim deletion state. """
def setUp(self):
rospy.init_node('state_victim_deletion_test')
self.delete_victim_mock = Publisher('mock/delete_victim', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('exploration')
def test_delete_victim_success(self):
target = mock_msgs.create_victim_info(id=1, probability=0.7)
self.agent.available_targets = [target]
self.agent.target.set(target)
if not rospy.is_shutdown():
sleep(1)
self.delete_victim_mock.publish('success:1')
self.agent.to_victim_deletion()
self.assertEqual(self.agent.state, 'exploration')
self.assertTrue(self.agent.target.is_empty)
self.assertIsNot(self.agent.available_targets, [])
def test_delete_victim_fail(self):
target = mock_msgs.create_victim_info(id=1, probability=0.7)
self.agent.available_targets = [target]
self.agent.target.set(target)
if not rospy.is_shutdown():
sleep(1)
self.delete_victim_mock.publish('abort:1')
self.agent.to_victim_deletion()
self.assertEqual(self.agent.state, 'exploration')
self.assertTrue(self.agent.target.is_empty)
self.assertIsNot(self.agent.available_targets, [])
class TestExplorer(unittest.TestCase):
""" Tests for the explorer action client """
def setUp(self):
# Register the mock servers.
rospy.init_node('unit_explorer')
self.explorer_mock = Publisher('mock/explorer', String)
self.agent = Agent(strategy='normal')
def test_explore(self):
self.explorer_mock.publish('success:4')
self.agent.explore()
self.assertTrue(self.agent.explorer.exploration_pending.is_set())
self.assertEqual(self.agent.explorer.client.get_state(),
GoalStatus.PENDING)
def test_preempt(self):
self.explorer_mock.publish('success:3')
self.agent.explore()
self.agent.preempt_explorer()
self.assertFalse(self.agent.explorer.exploration_pending.is_set())
def test_explorer_abort(self):
self.explorer_mock.publish('abort:1')
self.agent.explore()
sleep(3)
self.assertEqual(self.agent.explorer.client.get_state(),
GoalStatus.ABORTED)
self.assertFalse(self.agent.explorer.exploration_pending.is_set())
def test_explorer_success(self):
self.explorer_mock.publish('success:1')
self.agent.explore()
sleep(3)
self.assertEqual(self.agent.explorer.client.get_state(),
GoalStatus.SUCCEEDED)
self.assertFalse(self.agent.explorer.exploration_pending.is_set())
def setUp(self):
rospy.init_node('state_victim_deletion_test')
self.delete_victim_mock = Publisher('mock/delete_victim', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('exploration')
class TestIdentificationState(unittest.TestCase):
""" Tests for the identification state. """
def setUp(self):
rospy.init_node('state_identification_test')
self.move_base_mock = Publisher('mock/feedback_move_base', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('victim_deletion')
self.agent.set_breakpoint('sensor_hold')
self.agent.target.set(
mock_msgs.create_victim_info(id=1, probability=0.4))
self.events = [
self.agent.move_base_success, self.agent.move_base_retry,
self.agent.move_base_feedback, self.agent.move_base_resend
]
def send_updated_pose(self, delay):
pose_stamped = PoseStamped()
pose_stamped.pose.position.x = 20
pose_stamped.pose.position.y = 20
target = mock_msgs.create_victim_info(id=1, probability=0.5)
target.victimPose = pose_stamped
msg = WorldModel()
msg.victims = [target]
msg.visitedVictims = []
sleep(delay)
self.agent.receive_world_model(msg)
def send_identified_victim(self, delay, probability):
target = mock_msgs.create_victim_info(id=1, probability=probability)
msg = WorldModel()
msg.victims = [target]
msg.visitedVictims = []
sleep(delay)
self.agent.receive_world_model(msg)
def test_global_state_change(self):
""" The global state should be MODE_IDENTIFICATION. """
self.move_base_mock.publish('success:10')
final = RobotModeMsg.MODE_IDENTIFICATION
self.agent.to_identification()
sleep(5)
self.assertEqual(self.agent.state, 'identification')
self.assertItemsEqual(self.agent.dispatcher.listeners_all(),
self.events)
self.assertEqual(self.agent.state_changer.get_current_state(), final)
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_identification_with_move_base(self):
""" The base has moved to the victim's pose. """
self.move_base_mock.publish('success:1')
self.agent.to_identification()
sleep(5)
self.assertEqual(self.agent.state, 'sensor_hold')
self.assertEqual(self.agent.dispatcher.listeners_all(), [])
self.assertFalse(self.agent.target.is_verified())
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_identification_with_high_probability(self):
""" The move_base has failed but the victim has high probability. """
self.move_base_mock.publish('abort:2')
Thread(target=self.send_identified_victim, args=(5, 0.8)).start()
self.agent.to_identification()
sleep(15)
self.assertEqual(self.agent.state, 'sensor_hold')
self.assertEqual(self.agent.dispatcher.listeners_all(), [])
self.assertTrue(self.agent.target.is_identified())
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_identification_with_convergence(self):
""" The agent is close enough to the victim to be identified. """
if not rospy.is_shutdown():
self.move_base_mock.publish('abort_feedback:2')
sleep(1)
Thread(target=self.send_identified_victim, args=(3, 0.5)).start()
self.agent.to_identification()
sleep(30)
self.assertEqual(self.agent.state, 'sensor_hold')
self.assertTrue(self.agent.base_converged.is_set())
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_abort_victim(self):
self.move_base_mock.publish('abort:1')
Thread(target=self.send_identified_victim, args=(3, 0.4)).start()
self.agent.to_identification()
sleep(10)
self.assertEqual(self.agent.state, 'victim_deletion')
self.assertFalse(self.agent.target.is_identified())
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_unresponsive_move_base(self):
conf.MOVE_BASE_TIMEOUT = 5
if not rospy.is_shutdown():
self.move_base_mock.publish('success:7')
self.agent.to_identification()
sleep(10)
self.assertEqual(self.agent.state, 'victim_deletion')
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_update_move_base(self):
if not rospy.is_shutdown():
self.move_base_mock.publish('execute:10')
Thread(target=self.send_updated_pose, args=(7, )).start()
self.agent.to_identification()
sleep(15)
x = self.agent.target.info.victimPose.pose.position.x
y = self.agent.target.info.victimPose.pose.position.y
self.assertEqual(x, 20)
self.assertEqual(y, 20)
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_update_move_base_timer(self):
conf.MOVE_BASE_TIMEOUT = 25
if not rospy.is_shutdown():
self.move_base_mock.publish('execute:10')
with patch.object(self.agent.navigator.dispatcher, 'emit') as mock:
Thread(target=self.send_updated_pose, args=(5, )).start()
self.agent.to_identification()
sleep(10)
x = self.agent.target.info.victimPose.pose.position.x
y = self.agent.target.info.victimPose.pose.position.y
pose = Pose()
pose.position.x = 20
pose.position.y = 20
self.assertEqual(x, 20)
self.assertEqual(y, 20)
mock.assert_any_call('move_base.resend', pose)
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def test_no_move_base_update(self):
self.agent.target.info.victimPose.pose.position.x = 20.1
self.agent.target.info.victimPose.pose.position.y = 20
if not rospy.is_shutdown():
self.move_base_mock.publish('success:10')
Thread(target=self.send_updated_pose, args=(2, )).start()
with patch.object(self.agent, 'approach_target') as mock:
self.agent.to_identification()
sleep(15)
x = self.agent.target.info.victimPose.pose.position.x
y = self.agent.target.info.victimPose.pose.position.y
# The mock should be called only once because the updated goal is
# within the acceptable limits
self.assertEqual(mock.call_count, 1)
self.assertEqual(x, 20)
self.assertEqual(y, 20)
self.assertTrue(type(self.agent.available_targets), list)
self.assertGreater(len(self.agent.available_targets), 1)
def setUp(self):
self.agent = Agent(testing=True)
self.agent.current_pose = mock_msgs.create_pose(x=0, y=0)
class TestValidateGUI(unittest.TestCase):
""" Tests the validate gui client """
def setUp(self):
self.agent = Agent(strategy='normal')
self.agent.machine.add_state('test_validate_gui')
self.agent.machine.add_transition('operator_responded', 'off',
'test_validate_gui')
self.validate_gui_mock = Publisher('mock/validate_gui', String)
self.gui_result = Publisher('mock/gui_result', Bool)
sleep(2)
def test_validated_true(self):
""" The operator has stated this victim isvalid """
self.agent.set_breakpoint('fusion_validation')
msg = mock_msgs.create_victim_info(id=5)
self.agent.target_victim = msg
self.gui_result.publish(True)
self.validate_gui_mock.publish('success:2')
self.agent.wait_for_operator()
self.assertTrue(self.agent.gui_result.victimValid)
def test_validated_false(self):
""" The operator has stated this victim is not valid """
self.agent.set_breakpoint('fusion_validation')
msg = mock_msgs.create_victim_info(id=5)
self.agent.target_victim = msg
self.gui_result.publish(False)
self.validate_gui_mock.publish('success:2')
self.agent.wait_for_operator()
self.assertFalse(self.agent.gui_result.victimValid)
def test_validation_aborted(self):
self.agent.set_breakpoint('fusion_validation')
self.validate_gui_mock.publish('abort:2')
msg = mock_msgs.create_victim_info(id=5)
self.agent.target_victim = msg
self.agent.wait_for_operator()
self.assertEqual(self.agent.gui_client.get_state(), GoalStatus.ABORTED)
class WorldModelSub(unittest.TestCase):
""" Tests for the agent callbacks. """
def setUp(self):
self.agent = Agent(testing=True)
self.world_model = WorldModelPub()
@unittest.skip('ROS dependent')
def test_connection(self):
""" The receive_world_model should be called. """
# Set up testing mocks
self.agent.target = None
self.agent.state = 'exploration'
Thread(target=self.world_model.send_random).start()
sleep(3)
self.assertIsNot(self.agent.current_victims, [])
self.assertIsNot(self.agent.visited_victims, [])
self.assertIsNotNone(self.agent.target)
def test_update_target(self):
""" Tests that the target is updated. """
target = mock_msgs.create_victim_info(id=1, probability=0.2)
self.agent.target.set(target)
visited = [mock_msgs.create_victim_info() for i in range(2)]
current_victims = [target]
model = mock_msgs.create_world_model(current_victims, visited)
with patch.object(self.agent.target, 'update') as mock:
with patch.object(self.agent.dispatcher, 'emit') as emitter:
self.agent.receive_world_model(model)
self.assertTrue(mock.called)
self.assertTrue(emitter.called)
self.assertFalse(self.agent.target.is_verified())
self.assertFalse(self.agent.target.is_identified())
def test_choose_new_victim(self):
""" Tests that the target is updated. """
target = mock_msgs.create_victim_info(id=1, probability=0.2)
visited = [mock_msgs.create_victim_info() for i in range(2)]
current_victims = [target]
model = mock_msgs.create_world_model(current_victims, visited)
with patch.object(self.agent.target, 'update') as mock_update:
with patch.object(self.agent.dispatcher, 'emit') as emitter:
self.agent.receive_world_model(model)
self.assertFalse(mock_update.called)
self.assertTrue(emitter.called)
self.assertEqual(self.agent.target.info, target)
self.assertFalse(self.agent.target.is_verified())
self.assertFalse(self.agent.target.is_identified())
def test_non_existent_victim(self):
target = mock_msgs.create_victim_info(id=1, probability=0.2)
target2 = mock_msgs.create_victim_info(id=2, probability=0.2)
visited = [mock_msgs.create_victim_info() for i in range(2)]
current_victims = [target2]
model = mock_msgs.create_world_model(current_victims, visited)
self.agent.target.set(target)
with patch.object(self.agent.target, 'update') as mock_update:
with patch.object(self.agent.dispatcher, 'emit') as emitter:
self.agent.receive_world_model(model)
self.assertFalse(mock_update.called)
self.assertFalse(emitter.called)
self.assertTrue(self.agent.target.is_empty)
self.assertFalse(self.agent.target.is_verified())
self.assertFalse(self.agent.target.is_identified())
def test_identification_threshold(self):
""" Tests that the target is identified. """
conf.IDENTIFICATION_THRESHOLD = 0.6
conf.VERIFICATION_THRESHOLD = 0.8
target = mock_msgs.create_victim_info(probability=0.7)
self.agent.target.set(target)
visited = [mock_msgs.create_victim_info() for i in range(2)]
current_victims = [target]
model = mock_msgs.create_world_model(current_victims, visited)
self.agent.receive_world_model(model)
self.assertFalse(self.agent.target.is_empty)
self.assertTrue(self.agent.target.is_identified())
self.assertFalse(self.agent.target.is_verified())
def test_verification_threshold(self):
""" Tests that the target is verified. """
conf.IDENTIFICATION_THRESHOLD = 0.6
conf.VERIFICATION_THRESHOLD = 0.8
target = mock_msgs.create_victim_info(probability=0.9)
self.agent.target.set(target)
visited = [mock_msgs.create_victim_info() for i in range(2)]
current_victims = [target]
model = mock_msgs.create_world_model(current_victims, visited)
self.agent.receive_world_model(model)
self.assertFalse(self.agent.target.is_empty)
self.assertTrue(self.agent.target.is_identified())
self.assertTrue(self.agent.target.is_verified())
def setUp(self):
self.agent = Agent(testing=True)
self.world_model = WorldModelPub()
class TestEndEffector(unittest.TestCase):
""" Tests for the end effector action client. """
def setUp(self):
# Register the mock servers.
self.effector_mock = Publisher('mock/effector', String)
self.agent = Agent(strategy='normal')
def test_park_end_effector(self):
self.effector_mock.publish('abort:1')
@TimeLimiter(timeout=5)
def infinite_delay():
self.agent.park_end_effector()
self.assertRaises(TimeoutException, infinite_delay)
self.effector_mock.publish('success:1')
self.agent.park_end_effector()
self.assertEqual(self.agent.end_effector_client.get_state(),
GoalStatus.SUCCEEDED)
def test_end_effector(self):
@TimeLimiter(timeout=5)
def infinite_delay():
self.agent.test_end_effector()
self.assertRaises(TimeoutException, infinite_delay)
self.effector_mock.publish('success:1')
self.agent.test_end_effector()
self.assertEqual(self.agent.end_effector_client.get_state(),
GoalStatus.SUCCEEDED)
def test_scan(self):
self.effector_mock.publish('abort:1')
self.agent.scan()
sleep(3)
self.assertEqual(self.agent.end_effector_client.get_state(),
GoalStatus.ABORTED)
self.effector_mock.publish('success:1')
self.agent.scan()
sleep(3)
self.assertEqual(self.agent.end_effector_client.get_state(),
GoalStatus.SUCCEEDED)
def test_point_sensors(self):
self.agent.target_victim = mock_msgs.create_victim_info()
self.effector_mock.publish('abort:1')
self.agent.point_sensors()
sleep(3)
self.assertEqual(self.agent.end_effector_client.get_state(),
GoalStatus.ABORTED)
self.effector_mock.publish('success:1')
self.agent.point_sensors()
sleep(3)
self.assertEqual(self.agent.end_effector_client.get_state(),
GoalStatus.SUCCEEDED)
#!/usr/bin/env python
import rospy
import sys
from pandora_fsm import Agent
if __name__ == '__main__':
if len(sys.argv) > 1:
strategy = sys.argv[1]
else:
strategy = 'normal'
rospy.init_node('agent_standalone')
agent = Agent(strategy=strategy)
# Start the agent.
agent.wake_up()
rospy.spin()
def setUp(self):
# Register the mock servers.
self.effector_mock = Publisher('mock/effector', String)
self.agent = Agent(strategy='normal')
def setUp(self):
# Register the mock servers.
rospy.init_node('unit_explorer')
self.explorer_mock = Publisher('mock/explorer', String)
self.agent = Agent(strategy='normal')
def setUp(self):
rospy.init_node('state_init_test')
self.effector_mock = Publisher('mock/effector', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('exploration')
class TestFusionValidationState(unittest.TestCase):
""" Tests for the fusion validation state. """
def setUp(self):
rospy.init_node('state_fusion_validation_test')
self.agent = Agent(strategy='normal')
self.fusion_validate = Publisher('mock/fusion_validate', String)
self.target = mock_msgs.create_victim_info(id=1, probability=0.65)
self.agent.target.set(self.target)
self.agent.set_breakpoint('exploration')
def test_true_positive(self):
self.agent.target.verified.set()
if not rospy.is_shutdown():
rospy.sleep(1)
self.fusion_validate.publish('success:1')
self.agent.gui_result.victimValid = True
self.agent.to_fusion_validation()
self.assertEqual(self.agent.state, 'exploration')
self.assertFalse(self.agent.gui_result.victimValid)
self.assertTrue(self.agent.target.is_empty)
self.assertFalse(self.agent.target.is_verified())
self.assertNotIn(self.agent.available_targets, [])
def test_false_positive(self):
self.agent.target.verified.set()
if not rospy.is_shutdown():
rospy.sleep(1)
self.fusion_validate.publish('success:1')
self.agent.gui_result.victimValid = False
self.agent.to_fusion_validation()
self.assertEqual(self.agent.state, 'exploration')
self.assertFalse(self.agent.gui_result.victimValid)
self.assertTrue(self.agent.target.is_empty)
self.assertFalse(self.agent.target.is_verified())
self.assertNotIn(self.agent.available_targets, [])
def test_true_negative(self):
if not rospy.is_shutdown():
rospy.sleep(1)
self.fusion_validate.publish('success:1')
self.agent.gui_result.victimValid = False
self.agent.to_fusion_validation()
self.assertEqual(self.agent.state, 'exploration')
self.assertFalse(self.agent.gui_result.victimValid)
self.assertTrue(self.agent.target.is_empty)
self.assertFalse(self.agent.target.is_verified())
self.assertNotIn(self.agent.available_targets, [])
def test_false_negative(self):
if not rospy.is_shutdown():
rospy.sleep(1)
self.fusion_validate.publish('success:1')
self.agent.gui_result.victimValid = True
self.agent.to_fusion_validation()
self.assertEqual(self.agent.state, 'exploration')
self.assertFalse(self.agent.gui_result.victimValid)
self.assertTrue(self.agent.target.is_empty)
self.assertFalse(self.agent.target.is_verified())
self.assertNotIn(self.agent.available_targets, [])
def test_invalid_victim(self):
if not rospy.is_shutdown():
rospy.sleep(1)
self.fusion_validate.publish('abort:1')
self.agent.gui_result.victimValid = False
self.agent.to_fusion_validation()
self.assertEqual(self.agent.state, 'exploration')
self.assertFalse(self.agent.gui_result.victimValid)
self.assertTrue(self.agent.target.is_empty)
self.assertNotIn(self.agent.available_targets, [])
def setUp(self):
rospy.init_node('operator_validation_state')
self.agent = Agent(strategy='normal')
self.gui = Publisher('mock/validate_gui', String)
self.gui_result = Publisher('mock/gui_result', Bool)
self.agent.set_breakpoint('fusion_validation')
def setUp(self):
rospy.init_node('state_end_test')
self.agent = Agent(strategy='normal')
class TestExplorationState(unittest.TestCase):
""" Tests for the exploration state. """
def setUp(self):
rospy.init_node('state_epxloration_test')
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('identification')
self.agent.set_breakpoint('end')
self.agent.set_breakpoint('init')
self.explorer = Publisher('mock/explorer', String)
self.agent.preempt_end_effector = lambda: True
self.agent.preempt_explorer = lambda: True
self.events = [
self.agent.exploration_success, self.agent.exploration_retry,
self.agent.poi_found
]
def random_victims(self, times, delay):
""" Spawn a thread and send a potential victim instead of using a
mock Publisher which is not so predictable or easy to configure.
"""
visited = [mock_msgs.create_victim_info() for i in range(0, 3)]
victim = [mock_msgs.create_victim_info()]
model = mock_msgs.create_world_model(victim, visited)
for i in range(times):
sleep(delay)
self.agent.receive_world_model(model)
def test_target_found_after_exploration(self):
""" A target has been discovered after exploration. """
self.agent.disable_events()
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:20')
Thread(target=self.random_victims, args=(
3,
2,
)).start()
self.agent.to_exploration()
sleep(5)
self.assertFalse(self.agent.target.is_empty)
self.assertEqual(self.agent.dispatcher.listeners_all(), [])
self.assertEqual(self.agent.state, 'identification')
def test_to_end(self):
# This goal will move the agent to the end state.
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:1')
self.agent.to_exploration()
sleep(5)
self.assertEqual(self.agent.state, 'end')
self.assertEqual(self.agent.dispatcher.listeners_all(), [])
def test_long_wait_for_victim(self):
""" The agent will wait for a victim indefinitely. """
# Long goals that will not affect the test.
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:20')
self.agent.to_exploration()
sleep(10)
self.assertEqual(self.agent.state, 'exploration')
self.assertItemsEqual(self.agent.dispatcher.listeners_all(),
self.events)
self.assertTrue(self.agent.target.is_empty)
def test_retry_on_explorer_abort(self):
""" The agent will keep sending goals if the explorer fails. """
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('abort:1')
with patch.object(self.agent.explorer.dispatcher, 'emit') as mock:
self.agent.to_exploration()
sleep(7)
mock.assert_called_with('exploration.retry')
self.assertItemsEqual(self.agent.dispatcher.listeners_all(),
self.events)
self.assertEqual(self.agent.state, 'exploration')
def test_retry_on_explorer_reject(self):
""" The agent will keep sending goals if the explorer fails. """
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('reject:1')
with patch.object(self.agent.explorer.dispatcher, 'emit') as mock:
self.agent.to_exploration()
sleep(7)
mock.assert_called_with('exploration.retry')
self.assertItemsEqual(self.agent.dispatcher.listeners_all(),
self.events)
self.assertEqual(self.agent.state, 'exploration')
def test_poi_found_once(self):
""" Poi_found should be called only once. """
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:20')
Thread(target=self.random_victims, args=(
15,
0.1,
)).start()
with patch.object(logger, 'warning') as mock:
self.agent.to_exploration()
sleep(5)
# Only one thread should acquire the lock.
mock.assert_called_with('A point of interest has been discovered.')
self.assertEqual(mock.call_count, 1)
self.assertEqual(self.agent.state, 'identification')
def test_global_state_change(self):
""" The global state should be MODE_EXPLORATION_RESCUE """
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:1')
final = RobotModeMsg.MODE_EXPLORATION_RESCUE
self.agent.to_exploration()
sleep(10)
self.assertEqual(self.agent.state_changer.get_current_state(), final)
self.assertEqual(self.agent.dispatcher.listeners_all(), [])
self.assertEqual(self.agent.state, 'end')
def test_with_target_on_enter(self):
""" A target has been acquired before exploration. """
target = mock_msgs.create_victim_info()
self.agent.available_targets = [target]
self.agent.target.set(target)
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:20')
self.agent.to_exploration()
sleep(5)
self.assertEqual(self.agent.state, 'identification')
def test_with_available_targets(self):
""" There are available targets before exploration. """
target = mock_msgs.create_victim_info()
self.agent.available_targets = [target]
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:20')
self.agent.to_exploration()
sleep(10)
self.assertEqual(self.agent.state, 'identification')
self.assertFalse(self.agent.target.is_empty)
def test_with_wrong_target(self):
""" The target is not in the available_targets. """
target = mock_msgs.create_victim_info()
self.agent.target.set(target)
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:20')
self.agent.to_exploration()
sleep(5)
self.assertEqual(self.agent.state, 'exploration')
def test_wrong_target_with_available_victims(self):
""" Choose a target after the wrong one. """
target = mock_msgs.create_victim_info()
target2 = mock_msgs.create_victim_info()
self.agent.available_targets = [target2]
self.agent.target.set(target)
if not rospy.is_shutdown():
sleep(1)
self.explorer.publish('success:20')
self.agent.to_exploration()
sleep(5)
self.assertEqual(self.agent.state, 'identification')
def setUp(self):
""" Initialization """
self.agent = Agent(testing=True)
