def check_msg(self, msg_str, expects):
pub, msg_class = rostopic.create_publisher('/blender_api/get_pau',
'pau2motors/pau', True)
msg = msg_class()
fill_message_args(msg, yaml.load(msg_str))
pub.publish(msg)
msg = wait_for_message('/joint_states_combined', JointState, 2)
for key in ['position', 'name']:
self.assertListEqual(list(getattr(msg, key)), expects[key])
pub.unregister()
def check_msg(self, msg_str, expects):
pub, msg_class = rostopic.create_publisher(
'/blender_api/get_pau', 'pau2motors/pau', True)
msg = msg_class()
fill_message_args(msg, yaml.load(msg_str))
pub.publish(msg)
msg = wait_for_message('/joint_states_combined', JointState, 2)
for key in ['position', 'name']:
self.assertListEqual(list(getattr(msg, key)), expects[key])
pub.unregister()
def test_face_tf(self):
pub, msg_class = rostopic.create_publisher('/camera/face_locations',
'pi_face_tracker/Faces',
True)
msg = msg_class()
msg_str = """faces:
- id: 1
point:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
attention: 0.990000009537
- id: 2
point:
x: 0.807249662369
y: -0.00428759906469
z: 0.035407830253
attention: 0.990000009537"""
fill_message_args(msg, [yaml.load(msg_str)])
pub.publish(msg)
msg = wait_for_message('/tf', tfMessage, 5)
expect_str = """
transforms:
-
header:
seq: 0
stamp:
nsecs: 752310037
frame_id: camera
child_frame_id: face_base1
transform:
translation:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
expect = tfMessage()
fill_message_args(expect, [yaml.load(expect_str)])
#self.assertEqual(msg.transforms[0].child_frame_id,
# expect.transforms[0].child_frame_id)
self.assertEqual(msg.transforms[0].transform,
expect.transforms[0].transform)
pub.unregister()
def test_face_tf(self):
pub, msg_class = rostopic.create_publisher(
'/camera/face_locations', 'pi_face_tracker/Faces', True)
msg = msg_class()
msg_str = """faces:
- id: 1
point:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
attention: 0.990000009537
- id: 2
point:
x: 0.807249662369
y: -0.00428759906469
z: 0.035407830253
attention: 0.990000009537"""
fill_message_args(msg, [yaml.load(msg_str)])
pub.publish(msg)
msg = wait_for_message('/tf', tfMessage, 5)
expect_str = """
transforms:
-
header:
seq: 0
stamp:
nsecs: 752310037
frame_id: camera
child_frame_id: face_base1
transform:
translation:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
expect = tfMessage()
fill_message_args(expect, [yaml.load(expect_str)])
#self.assertEqual(msg.transforms[0].child_frame_id,
# expect.transforms[0].child_frame_id)
self.assertEqual(msg.transforms[0].transform,
expect.transforms[0].transform)
pub.unregister()
def test_face_in_out(self):
face_event_msg, face_location_msg = None, None
fps = 25
wait = 1.0/fps
for _, _, _ in rosbag_msg_generator(get_rosbag_file('face')):
face_location_msg, face_event_msg = wait_for_messages(
['/camera/face_locations', '/camera/face_event'],
[Faces, FaceEvent], wait)
if face_location_msg and face_location_msg.faces:
self.assertEqual(len(face_location_msg.faces), 1)
face = face_location_msg.faces[0]
self.assertGreater(face.attention, 0.99)
if face_event_msg:
self.assertEqual(face_event_msg.face_event, 'new_face')
for _, _, _ in rosbag_msg_generator(get_rosbag_file('empty')):
face_event_msg = wait_for_message(
'/camera/face_event', FaceEvent, wait)
if face_event_msg:
self.assertEqual(face_event_msg.face_event, 'lost_face')
def test_face_in_out(self):
face_event_msg, face_location_msg = None, None
fps = 25
wait = 1.0/fps
for _, _, _ in rosbag_msg_generator(get_rosbag_file('face')):
face_location_msg, face_event_msg = wait_for_messages(
['/camera/face_locations', '/camera/face_event'],
[Faces, FaceEvent], wait)
if face_location_msg and face_location_msg.faces:
self.assertEqual(len(face_location_msg.faces), 1)
face = face_location_msg.faces[0]
self.assertGreater(face.attention, 0.99)
if face_event_msg:
self.assertEqual(face_event_msg.face_event, 'new_face')
for _, _, _ in rosbag_msg_generator(get_rosbag_file('empty')):
face_event_msg = wait_for_message(
'/camera/face_event', FaceEvent, wait)
if face_event_msg:
self.assertEqual(face_event_msg.face_event, 'lost_face')
