Merge branch 'MS2' of https://github.com/SUTURO/pycram into MS2

demos/pycram_receptionist_demo/tests/MS3_testable.py

@@ -1,3 +1,5 @@
+import rospy
+
 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
@@ -23,102 +25,167 @@
 
 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()
+def demo_tst():
+    with real_robot:
 
-    # NavigateAction([misc.pose_door]).resolve().perform()
-    # giskardpy.opendoor()
+        # 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()
 
-    TalkingMotion("Welcome, please come in").resolve().perform()
+        # subscriber not needed anymore
+        bell_subscriber.unregister()
 
-    # look for human
-    # TODO: test new technique
-    DetectAction(technique='human', state='start').resolve().perform()
-    rospy.loginfo("human detected")
+        # NavigateAction([misc.pose_door]).resolve().perform()
+        # giskardpy.opendoor()
 
-    # 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)
+        TalkingMotion("Welcome, please come in").resolve().perform()
 
-    # signal to start listening
-    pub_nlp.publish("start listening")
+        # look for human
+        # TODO: test new technique
+        DetectAction(technique='human', state='start').resolve().perform()
+        rospy.loginfo("human detected")
 
-    # receives name and drink via topic
-    rospy.Subscriber("nlp_out", String, misc.talk_request_nlp)
+        # 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)
 
-    # failure handling
-    rospy.Subscriber("nlp_feedback", Bool, misc.talk_error)
+        # signal to start listening
+        pub_nlp.publish("start listening")
 
-    while misc.guest1.name == "guest1":
-        rospy.sleep(1)
+        # receives name and drink via topic
+        rospy.Subscriber("nlp_out", String, misc.talk_request_nlp)
 
-    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")
+        # failure handling
+        rospy.Subscriber("nlp_feedback", Bool, misc.talk_error)
 
-    rospy.Subscriber("nlp_confirmation", Bool, misc.name_cb)
+        while misc.guest1.name == "guest1":
+            rospy.sleep(1)
 
-    while not misc.understood_name:
+        TalkingMotion("it is so noisy here, please confirm if i got your name right").resolve().perform()
         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()
+        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='start').resolve().perform()
+        rospy.sleep(5)
+        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(2)
+
+        # point to free place
+        # giskardpy.point_to_seat
+
+        pose_host = PoseStamped()
+        pose_host.header.frame_id = "/map"
+        pose_host.pose.position.x = 1
+        # pose_host.pose.position.y = 5.9
+        pose_host.pose.position.y = 2
+        pose_host.pose.position.z = 1
+
+        pose_guest = PoseStamped()
+        pose_guest.header.frame_id = "/map"
+        pose_guest.pose.position.x = 1
+        # pose_guest.pose.position.y = 4.7
+        pose_guest.pose.position.y = -0.4
+        pose_guest.pose.position.x = 1
+
+        misc.host.set_pose(pose_host)
+        misc.host.set_drink("beer")
+        misc.guest1.set_pose(pose_guest)
+
+        # introduce humans and look at them
+        giskardpy.move_head_to_human()
+        print("täsr" + misc.host.fav_drink)
+        misc.introduce(pose_b=pose_host, pose_a=pose_guest)
+
+
+def demo_tst2():
+    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")
+        #giskardpy.move_head_to_human()
+        #rospy.sleep(7)
+        #DetectAction(technique='human', state='stop').resolve().perform()
+
+        pose_host = PoseStamped()
+        pose_host.header.frame_id = 'map'
+        pose_host.pose.position.x = 1.0
+        pose_host.pose.position.y = 5.9
+        pose_host.pose.position.z = 0.9
+
+        pose_guest = PoseStamped()
+        pose_guest.header.frame_id = 'map'
+        pose_guest.pose.position.x = 1.0
+        pose_guest.pose.position.y = 4.7
+        pose_guest.pose.position.z = 1.0
+
+        misc.host.set_pose(pose_host)
+        misc.host.set_drink("beer")
+
+        misc.guest1.set_pose(pose_guest)
+
+        # introduce humans and look at them
+        giskardpy.move_head_to_human()
+        rospy.sleep(3)
+        misc.introduce(pose_b=pose_host, pose_a=pose_guest)
+
+        rospy.sleep(2)
+        TalkingMotion("Introducing again").resolve().perform()
+        rospy.sleep(2)
+
+        misc.introduce(pose_b=pose_host, pose_a=pose_guest)
+
+
+demo_tst2()

demos/pycram_receptionist_demo/tests/perception_humans.py

@@ -29,7 +29,7 @@
 
 
 
-def test():
+def p():
     with real_robot:
 
         DetectAction(technique='human', state='start').resolve().perform()
@@ -52,4 +52,4 @@ def test():
 
 
 if __name__ == '__main__':
-    test()
+    p()

demos/pycram_receptionist_demo/utils/misc.py

@@ -1,6 +1,9 @@
+import rospy
+
 from pycram.designators.motion_designator import TalkingMotion
 from pycram.designators.object_designator import *
 from pycram.helper import axis_angle_to_quaternion
+#from ..tests.MS3_testable import *
 
 # Publisher for NLP
 pub_nlp = rospy.Publisher('/startListener', String, queue_size=10)
@@ -95,16 +98,21 @@ def introduce(name_a: Optional[str] = guest1.name, drink_a: Optional[str] = gues
     # TODO: needs to be tested!
     if pose_b:
         pub_pose.publish(pose_b)
+        rospy.sleep(3.5)
     TalkingMotion(f"Hey, {name_b}").resolve().perform()
 
+
     if pose_a:
-        pub_pose.publish(pose_b)
+        pub_pose.publish(pose_a)
+        rospy.sleep(3.5)
     TalkingMotion(f" This is {name_a} and their favorite drink is {drink_a}").resolve().perform()
     rospy.sleep(2)
     TalkingMotion(f"Hey, {name_a}").resolve().perform()
 
+
     if pose_b:
         pub_pose.publish(pose_b)
+        rospy.sleep(3.5)
     TalkingMotion(f" This is {name_b} and their favorite drink is {drink_b}").resolve().perform()
 
     rospy.sleep(3)

src/pycram/designators/object_designator.py

@@ -232,4 +232,5 @@ def set_pose(self, new_pose):
         function for changing pose of human
         :param new_pose: new pose of human
         """
+        print("in set pose")
         self.pose = new_pose

src/pycram/external_interfaces/robokudo.py

@@ -160,6 +160,8 @@ def active_callback():
 
     def done_callback(state, result):
         rospy.loginfo("Finished perceiving")
+        global human_bool
+        human_bool = True
         global query_result
         query_result = result
 
@@ -174,19 +176,18 @@ def callback(pose):
         human_bool = True
         human_pose = pose
 
-    def listener():
-        rospy.Subscriber("/human_pose", PoseStamped, callback)
-
-
-    object_goal = goal_msg = QueryGoal()
 
     client = actionlib.SimpleActionClient('robokudo/query', QueryAction)
     rospy.loginfo("Waiting for action server")
     client.wait_for_server()
+    object_goal = goal_msg = QueryGoal()
+    client.send_goal(object_goal, active_cb=active_callback, done_cb=done_callback, feedback_cb=feedback_callback)
+
+    # if no human is detected
     human_bool = False
     waiting_human = False
-    client.send_goal(object_goal, active_cb=active_callback, done_cb=done_callback, feedback_cb=feedback_callback)
-    listener()
+    rospy.Subscriber("/human_pose", PoseStamped, callback)
+
     while not human_bool:
         rospy.loginfo_throttle(3, "Waiting for human to be detected")
         pub = rospy.Publisher('/talk_request', Voice, queue_size=10)
@@ -196,12 +197,13 @@ def listener():
         if waiting_human:
             pub.publish(texttospeech)
         waiting_human = True
-        rospy.sleep(4)
+        rospy.sleep(5)
         pass
 
     return human_pose
 
 
+
 def stop_queryHuman() -> Any:
     """
        Sends a query to RoboKudo to stop human detection
@@ -212,6 +214,7 @@ def stop_queryHuman() -> Any:
     client = actionlib.SimpleActionClient('robokudo/query', QueryAction)
     client.wait_for_server()
     client.cancel_all_goals()
+    print("get status: " + client.get_goal_status_text())
     rospy.loginfo("cancelled current goal")
 
 

src/pycram/process_modules/hsrb_process_modules.py

@@ -345,7 +345,7 @@ class HSRBDetectingReal(ProcessModule):
 
     def _execute(self, desig: DetectingMotion.Motion) -> Any:
         # todo at the moment perception ignores searching for a specific object type so we do as well on real
-        if desig.technique == 'human':
+        if desig.technique == 'human' and (desig.state == 'start' or desig.state == None):
             human_pose = queryHuman()
             pose = Pose.from_pose_stamped(human_pose)
             pose.position.z = 0