def setUpClass(self):
rospy.init_node('pau2motors_test')
wait_for('pau2motors')
os.system(
'rosrun dynamic_reconfigure dynparam set /pau2motors reload True')
self.queue = MessageQueue()
self.pub = rospy.Publisher('/listen_topic', pau, queue_size=1)
def test_btree_off(self):
queue = MessageQueue()
queue.subscribe('/blender_api/set_gesture', SetGesture)
time.sleep(1)
self.assertTrue(queue.queue.empty())
self.behavior_switch('btree_on')
msg = queue.get()
self.assertTrue(msg is not None)
def test_btree_off(self):
queue = MessageQueue()
queue.subscribe('/blender_api/set_gesture', SetGesture)
time.sleep(1)
self.assertTrue(queue.queue.empty())
self.behavior_switch('btree_on')
msg = queue.get()
self.assertTrue(msg is not None)
def record(device, serial_port_data_file, pau_data_file, gesture, duration):
raw_data_file = os.path.join(CWD, 'serial_port_data.raw')
recorder = SerialPortRecorder(device, raw_data_file)
rospy.init_node("record")
rospy.wait_for_service('/blender_api/get_param')
rospy.wait_for_service('/blender_api/set_param')
blender_get_param = rospy.ServiceProxy('/blender_api/get_param', GetParam)
blender_set_param = rospy.ServiceProxy('/blender_api/set_param', SetParam)
blender_set_param("bpy.context.scene['commandListenerActive']", "False")
time.sleep(2)
os.system('cat < {}'.format(device))
blender_set_param("bpy.context.scene['commandListenerActive']", "True")
recorder.start()
print "Start recording"
# publish animation message
queue = MessageQueue()
queue.subscribe('/blender_api/get_pau', pau)
pub = rospy.Publisher('/blender_api/set_gesture', SetGesture, latch=True)
pub.publish(SetGesture(gesture, 1, 1, 1))
timestamps = OrderedDict()
timestamps['publish_set_gesture_msg'] = time.time()
current_gestures = None
while current_gestures is None:
current_gestures = eval(blender_get_param("self.getGestures()").value)
time.sleep(0.01)
timestamps['blender_play_gestures'] = time.time()
motor_msg = rospy.wait_for_message('/sophia/safe/head/command', MotorCommand)
timestamps['motor_command_arrive'] = time.time()
time.sleep(duration) # wait for animation to finish
blender_set_param("bpy.context.scene['commandListenerActive']", "False")
recorder.stop()
print "Stop recording"
pau_messages = queue.tolist()
shapekeys = eval(blender_get_param("self.getFaceData()").value)
pau_df = pd.DataFrame(columns = shapekeys.keys())
for pau_message in pau_messages:
pau_df.loc[len(pau_df)] = pau_message.m_coeffs
pau_df.to_csv(pau_data_file, index=False)
print "Write pau message to %s" % pau_data_file
timestamps['first_serial_port_data'] = recorder.start_time
timestamps['last_serial_port_data'] = recorder.stop_time
for k, v in timestamps.items():
print k, datetime.fromtimestamp(v)
parse_raw_data(raw_data_file, serial_port_data_file)
os.remove(raw_data_file)
def test_btree_on(self):
timeout = 30
queue = MessageQueue()
queue.subscribe('/blender_api/set_gesture', SetGesture)
time.sleep(1)
self.behavior_switch('btree_off')
pub, msg_class = rostopic.create_publisher(
'/camera/face_event', 'pi_face_tracker/FaceEvent', True)
pub.publish(msg_class('new_face', 1))
time.sleep(timeout)
self.assertTrue(queue.queue.empty())
def test_btree_on(self):
timeout = 30
queue = MessageQueue()
queue.subscribe('/blender_api/set_gesture', SetGesture)
time.sleep(1)
self.behavior_switch('btree_off')
pub, msg_class = rostopic.create_publisher(
'/camera/face_event', 'pi_face_tracker/FaceEvent', True)
pub.publish(msg_class('new_face', 1))
time.sleep(timeout)
self.assertTrue(queue.queue.empty())
def test_face_interaction1(self):
positive_gestures = [
x.strip() for x in self.behavior_config.get(
'gesture', 'positive_gestures').split(',')
]
pub, msg_class = rostopic.create_publisher(
'/blender_api/available_emotion_states',
'blender_api_msgs/AvailableEmotionStates', True)
pub.publish(msg_class(['happy']))
queue = MessageQueue()
queue.subscribe('/blender_api/set_emotion_state', EmotionState)
queue.subscribe('/blender_api/set_gesture', SetGesture)
time.sleep(1)
self.behavior_switch('btree_on')
pub, msg_class = rostopic.create_publisher(
'/camera/face_event', 'pi_face_tracker/FaceEvent', True)
pub.publish(msg_class('new_face', 1))
time.sleep(20)
self.assertTrue(not queue.queue.empty())
emotions = []
gestures = []
while not queue.queue.empty():
msg = queue.get(timeout=1)
if isinstance(msg, EmotionState):
emotions.append(msg.name)
elif isinstance(msg, SetGesture):
gestures.append(msg.name)
self.assertTrue(set(['happy']) | set(emotions))
self.assertTrue(set(positive_gestures) & set(gestures))
def test_face_interaction1(self):
positive_gestures = [
x.strip() for x in self.behavior_config.get(
'gesture', 'positive_gestures').split(',')]
pub, msg_class = rostopic.create_publisher(
'/blender_api/available_emotion_states',
'blender_api_msgs/AvailableEmotionStates', True)
pub.publish(msg_class(['happy']))
queue = MessageQueue()
queue.subscribe('/blender_api/set_emotion_state', EmotionState)
queue.subscribe('/blender_api/set_gesture', SetGesture)
time.sleep(1)
self.behavior_switch('btree_on')
pub, msg_class = rostopic.create_publisher(
'/camera/face_event', 'pi_face_tracker/FaceEvent', True)
pub.publish(msg_class('new_face', 1))
time.sleep(20)
self.assertTrue(not queue.queue.empty())
emotions = []
gestures = []
while not queue.queue.empty():
msg = queue.get(timeout=1)
if isinstance(msg, EmotionState):
emotions.append(msg.name)
elif isinstance(msg, SetGesture):
gestures.append(msg.name)
self.assertTrue(set(['happy']) | set(emotions))
self.assertTrue(set(positive_gestures) & set(gestures))
class Pau2MotorsTest(unittest.TestCase):
@classmethod
def setUpClass(self):
rospy.init_node('pau2motors_test')
wait_for('pau2motors')
os.system('rosrun dynamic_reconfigure dynparam set /pau2motors reload True')
self.queue = MessageQueue()
self.pub = rospy.Publisher('/listen_topic', pau, queue_size=1)
def check_msg(self, msg, expect):
self.queue.clear()
self.pub.publish(msg)
msg = self.queue.get(2)
self.assertEqual(msg.data, expect)
def test_motor1(self):
msg = pau()
msg.m_headRotation.x = 0.5
msg.m_headRotation.y = 0.5
msg.m_headRotation.z = 0.0
msg.m_headRotation.w = 1.0
sub = self.queue.subscribe('/pub_topic_controller/command', Float64)
rospy.sleep(2)
self.check_msg(msg, math.asin(0.5))
sub.unregister()
def test_motor2(self):
msg = pau()
msg.m_neckRotation.x = 1.0
msg.m_neckRotation.y = 0.5
msg.m_neckRotation.z = 0.0
msg.m_neckRotation.w = 1.0
sub = self.queue.subscribe('/pub_topic2_controller/command', Float64)
rospy.sleep(2)
self.check_msg(msg, 0.5) # max = 0.5
sub.unregister()
class Pau2MotorsTest(unittest.TestCase):
@classmethod
def setUpClass(self):
rospy.init_node('pau2motors_test')
wait_for('pau2motors')
os.system('rosrun dynamic_reconfigure dynparam set /pau2motors reload True')
self.queue = MessageQueue()
self.pub = rospy.Publisher('/listen_topic', pau, queue_size=1)
def check_msg(self, msg, expect):
self.queue.clear()
self.pub.publish(msg)
msg = self.queue.get(2)
self.assertEqual(msg.data, expect)
def test_motor1(self):
msg = pau()
msg.m_headRotation.x = 0.5
msg.m_headRotation.y = 0.5
msg.m_headRotation.z = 0.0
msg.m_headRotation.w = 1.0
sub = self.queue.subscribe('/pub_topic_controller/command', Float64)
rospy.sleep(2)
self.check_msg(msg, math.asin(0.5))
sub.unregister()
def test_motor2(self):
msg = pau()
msg.m_neckRotation.x = 1.0
msg.m_neckRotation.y = 0.5
msg.m_neckRotation.z = 0.0
msg.m_neckRotation.w = 1.0
sub = self.queue.subscribe('/pub_topic2_controller/command', Float64)
rospy.sleep(2)
self.check_msg(msg, 0.5) # max = 0.5
sub.unregister()
def setUpClass(self):
rospy.init_node('pau2motors_test')
wait_for('pau2motors')
os.system('rosrun dynamic_reconfigure dynparam set /pau2motors reload True')
self.queue = MessageQueue()
self.pub = rospy.Publisher('/listen_topic', pau, queue_size=1)
