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 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_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)
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):
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_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 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_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 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
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_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 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_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)
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 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 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_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 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 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_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, [])
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 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)
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)
