From a48531b060f543729f74852c9a3f61855079d426 Mon Sep 17 00:00:00 2001
From: Celina SuturoLaptop <celina5@uni-bremen.de>
Date: Wed, 20 Mar 2024 13:48:16 +0100
Subject: [PATCH] [Receptionist] Yes/No function implemented, plan to switch
 from Subscribers to one Service from NLP.

---
 .../Ms3_receptionist_demo.py                  | 25 +++++++-----
 demos/pycram_receptionist_demo/utils/misc.py  | 40 ++++++++++++++-----
 2 files changed, 46 insertions(+), 19 deletions(-)

diff --git a/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py b/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py
index 82d1ae3bf..b60b4bc70 100644
--- a/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py
+++ b/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py
@@ -28,13 +28,13 @@
 with real_robot:
 
     #wait for doorbell
-    bell_subscriber = rospy.Subscriber("doorbell", Bool, doorbell_cb)
-    while not doorbell:
-        # TODO: spin or sleep better?
-        rospy.spin()
+    #bell_subscriber = rospy.Subscriber("doorbell", Bool, doorbell_cb)
+    #while not doorbell:
+    #    # TODO: spin or sleep better?
+    #    rospy.spin()
 
     # subscriber not needed anymore
-    bell_subscriber.unregister()
+    #bell_subscriber.unregister()
 
     # question: is the hsr standing in front of the door already??
     # giskardpy.opendoor()
@@ -59,14 +59,21 @@
     # failure handling
     rospy.Subscriber("nlp_feedback", Bool, talk_error)
 
-    while not understood:
+    while guest1.name == "guest1":
         rospy.sleep(1)
+        print(guest1.name)
+        print(understood)
 
     TalkingMotion("it is so noisy here, please confirm if i got your name right").resolve().perform()
     rospy.sleep(1)
     TalkingMotion("is your name " + guest1.name + "?").resolve().perform()
+    pub_nlp.publish("start")
 
-    rospy.Subscriber("name_confirm", Bool, name_cb)
+    rospy.Subscriber("nlp_confirmation", Bool, name_cb)
+
+    while not guest1.understood:
+        rospy.sleep(1)
+        print(str(guest1.understood))
 
 
     # stop looking
@@ -83,8 +90,8 @@
     # lead human to living room
     # TODO: check if rospy.sleep is needed and how long
     rospy.sleep(2)
-    NavigateAction([pose_kitchen_to_couch]).resolve().perform()
-    NavigateAction([pose_couch]).resolve().perform()
+    #NavigateAction([pose_kitchen_to_couch]).resolve().perform()
+    #NavigateAction([pose_couch]).resolve().perform()
     TalkingMotion("Welcome to the living room").resolve().perform()
     rospy.sleep(1)
 
diff --git a/demos/pycram_receptionist_demo/utils/misc.py b/demos/pycram_receptionist_demo/utils/misc.py
index 6b452e25a..4407b29f2 100644
--- a/demos/pycram_receptionist_demo/utils/misc.py
+++ b/demos/pycram_receptionist_demo/utils/misc.py
@@ -6,6 +6,7 @@
 pub_nlp = rospy.Publisher('/startListener', String, queue_size=10)
 understood = False
 doorbell = False
+name = False
 
 # Declare variables for humans
 host = HumanDescription("Yannis", fav_drink="ice tea")
@@ -44,16 +45,26 @@ def talk_request_nlp(data: String):
     function that takes the data from nlp (name and drink) and lets the robot talk
     :param data: String "name, drink"
     """
-    global understood
-    data_list = data.data.split(",")
-    name, drink = data_list
-    toyas_text = f"Hey {name}, your favorite drink is {drink}"
+    print(guest1.name)
+    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()
+        rospy.sleep(2)
+
+        guest1.set_name(name)
+        guest1.set_drink(drink)
+    else:
+        data_list = data.data.split(",")
+        name, drink = data_list
+        toyas_text = f"is your name {name} ?"
+        TalkingMotion(toyas_text).resolve().perform()
+        pub_nlp.publish("start")
+
 
-    TalkingMotion(toyas_text).resolve().perform()
-    rospy.sleep(2)
-    TalkingMotion("Nice to meet you").resolve().perform()
 
-    understood = True
 
 
 def talk_error(data):
@@ -85,13 +96,22 @@ def doorbell_cb(data):
     global doorbell
     doorbell = True
 
+
 def name_cb(data):
     """
     callback function for a subscriber to NLP script.
     is called when name was correctly understood
     """
-    global name
-    name = data
+    print("name cb:" + str(data))
+    if data.data:
+        TalkingMotion("perfect, nice to meet you").resolve().perform()
+        guest1.set_understood(True)
+    else:
+        TalkingMotion("i am sorry, please repeat your name loud and clear").resolve().perform()
+        rospy.sleep(1)
+        # TODO: only hear for Name
+        pub_nlp.publish("start listening")
+