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