[Receptionist] Testfile for thursday

demos/pycram_receptionist_demo/Ms3_receptionist_demo.py

@@ -69,7 +69,14 @@
 
     rospy.Subscriber("nlp_confirmation", Bool, misc.name_cb)
 
-    while not misc.understood:
+    while not misc.understood_name:
+        rospy.sleep(1)
+
+    # TODO: implement HRI for fav drink
+    TalkingMotion("is your favorite drink " + misc.guest1.fav_drink + "?").resolve().perform()
+    pub_nlp.publish("start")
+
+    while not misc.understood_drink:
         rospy.sleep(1)
 
 

demos/pycram_receptionist_demo/tests/MS3_testable.py

@@ -0,0 +1,124 @@
+from pycram.designators.action_designator import DetectAction, NavigateAction, LookAtAction
+from pycram.designators.motion_designator import TalkingMotion
+import demos.pycram_receptionist_demo.utils.misc as misc
+from pycram.process_module import real_robot
+import pycram.external_interfaces.giskard as giskardpy
+from pycram.ros.robot_state_updater import RobotStateUpdater
+from pycram.ros.viz_marker_publisher import VizMarkerPublisher
+from pycram.designators.location_designator import *
+from pycram.designators.object_designator import *
+from pycram.bullet_world import BulletWorld, Object
+from std_msgs.msg import String, Bool
+
+world = BulletWorld("DIRECT")
+v = VizMarkerPublisher()
+
+robot = Object("hsrb", "robot", "../../resources/" + robot_description.name + ".urdf")
+robot_desig = ObjectDesignatorDescription(names=["hsrb"]).resolve()
+robot.set_color([0.5, 0.5, 0.9, 1])
+
+kitchen = Object("kitchen", "environment", "kitchen.urdf")
+giskardpy.init_giskard_interface()
+RobotStateUpdater("/tf", "/giskard_joint_states")
+
+pub_nlp = rospy.Publisher('/startListener', String, queue_size=10)
+
+with real_robot:
+
+    # wait for doorbell
+    bell_subscriber = rospy.Subscriber("doorbell", Bool, misc.doorbell_cb)
+    while not misc.doorbell:
+        # TODO: spin or sleep better?
+        # TODO: Failure Handling, when no bell is heard for a longer period of time
+        rospy.spin()
+
+    # subscriber not needed anymore
+    bell_subscriber.unregister()
+
+    # NavigateAction([misc.pose_door]).resolve().perform()
+    # giskardpy.opendoor()
+
+    TalkingMotion("Welcome, please come in").resolve().perform()
+
+    # look for human
+    # TODO: test new technique
+    DetectAction(technique='human', state='start').resolve().perform()
+    rospy.loginfo("human detected")
+
+    # look at guest and introduce
+    giskardpy.move_head_to_human()
+    TalkingMotion("Hello, i am Toya and my favorite drink is oil. What about you, talk to me?").resolve().perform()
+    rospy.sleep(1)
+
+    # signal to start listening
+    pub_nlp.publish("start listening")
+
+    # receives name and drink via topic
+    rospy.Subscriber("nlp_out", String, misc.talk_request_nlp)
+
+    # failure handling
+    rospy.Subscriber("nlp_feedback", Bool, misc.talk_error)
+
+    while misc.guest1.name == "guest1":
+        rospy.sleep(1)
+
+    TalkingMotion("it is so noisy here, please confirm if i got your name right").resolve().perform()
+    rospy.sleep(1)
+    TalkingMotion("is your name " + misc.guest1.name + "?").resolve().perform()
+    pub_nlp.publish("start")
+
+    rospy.Subscriber("nlp_confirmation", Bool, misc.name_cb)
+
+    while not misc.understood_name:
+        rospy.sleep(1)
+
+    # TODO: implement HRI for fav drink
+    TalkingMotion("is your favorite drink " + misc.guest1.fav_drink + "?").resolve().perform()
+    pub_nlp.publish("start")
+
+    while not misc.understood_drink:
+        rospy.sleep(1)
+
+    # stop looking
+    TalkingMotion("i will show you the living room now").resolve().perform()
+    rospy.sleep(1)
+    TalkingMotion("please step out of the way and follow me").resolve().perform()
+    rospy.loginfo("stop looking now")
+    # TODO: look in direction of navigation maybe?
+    giskardpy.stop_looking()
+
+    # stop perceiving human
+    rospy.loginfo("stop detecting")
+    DetectAction(technique='human', state='stop').resolve().perform()
+
+    # lead human to living room
+    # TODO: check if rospy.sleep is needed and how long
+    rospy.sleep(2)
+    #NavigateAction([misc.pose_kitchen_to_couch]).resolve().perform()
+    #NavigateAction([misc.pose_couch]).resolve().perform()
+    TalkingMotion("Welcome to the living room").resolve().perform()
+    rospy.sleep(1)
+
+    TalkingMotion("please take a seat next to your host").resolve().perform()
+    rospy.sleep(4)
+
+
+    # point to free place
+    # giskardpy.point_to_seat
+
+    pose_host = PoseStamped
+    pose_host.pose.position.x = 1
+    pose_host.pose.position.y = 5.9
+    pose_host.pose.position.x = 1
+
+    pose_guest = PoseStamped
+    pose_guest.pose.position.x = 1
+    pose_guest.pose.position.y = 4.7
+    pose_guest.pose.position.x = 1
+
+    misc.host.set_pose(pose_host)
+    misc.guest1.set_pose(pose_guest)
+
+    # introduce humans and look at them
+    giskardpy.move_head_to_human()
+    misc.introduce()

demos/pycram_receptionist_demo/utils/misc.py

@@ -5,8 +5,10 @@
 # Publisher for NLP
 pub_nlp = rospy.Publisher('/startListener', String, queue_size=10)
 pub_pose = rospy.Publisher('/human_pose', PoseStamped, queue_size=10)
-understood = False
-doorbell = False
+understood_name = False
+understood_drink = False
+# TODO: set to False when NLP has implemented that feature
+doorbell = True
 
 
 # Declare variables for humans
@@ -16,12 +18,12 @@
 
 # Pose variables
 # Pose in front of the couch, HSR looks in direction of couch
-pose_couch = Pose([3, 5, 0], [0, 0, 1, 0])
+pose_couch = Pose([2.7, 5, 0], [0, 0, 1, 0])
 pose_door = Pose([1.4, 0.25, 0], [0, 0, 1, 0])
 
 # Pose in the passage between kitchen and living room
 robot_orientation = axis_angle_to_quaternion([0, 0, 1], 90)
-pose_kitchen_to_couch = Pose([4.2, 3, 0], robot_orientation)
+pose_kitchen_to_couch = Pose([4.35, 3, 0], robot_orientation)
 
 
 def talk_request(data: list):
@@ -48,22 +50,25 @@ def talk_request_nlp(data: String):
     :param data: String "name, drink"
     """
     print(guest1.name)
+    data_list = data.data.split(",")
+    name, drink = data_list
     if guest1.name == "guest1":
-        data_list = data.data.split(",")
-        name, drink = data_list
         toyas_text = f"Hey {name}, your favorite drink is {drink}"
 
-        TalkingMotion(toyas_text).resolve().perform()
+        # TalkingMotion(toyas_text).resolve().perform()
         rospy.sleep(2)
 
         guest1.set_name(name)
         guest1.set_drink(drink)
-    else:
-        data_list = data.data.split(",")
-        name, drink = data_list
+    elif not understood_name:
         toyas_text = f"is your name {name} ?"
         TalkingMotion(toyas_text).resolve().perform()
         pub_nlp.publish("start")
+    elif understood_name and not understood_drink:
+        toyas_text = f"is your favorite drink {drink} ?"
+        TalkingMotion(toyas_text).resolve().perform()
+        pub_nlp.publish("start")
+
 
 
 def talk_error(data):
@@ -119,12 +124,16 @@ def name_cb(data):
     callback function for a subscriber to NLP script.
     is called when name was correctly understood
     """
-    global understood
-    if data.data:
-        TalkingMotion("perfect, nice to meet you").resolve().perform()
-        understood = True
+    global understood_name
+    global understood_drink
+    if data.data and not understood_name:
+        TalkingMotion("perfect").resolve().perform()
+        understood_name = True
+    elif data.data and not understood_drink:
+        TalkingMotion("alright, thank you and nice to meet you").resolve().perform()
+        understood_drink = True
     else:
-        TalkingMotion("i am sorry, please repeat your name loud and clear").resolve().perform()
+        TalkingMotion("i am sorry, please repeat yourself loud and clear").resolve().perform()
         rospy.sleep(1)
         # TODO: only hear for Name
         pub_nlp.publish("start listening")