Merge pull request SUTURO#53 from Julislz/MS2

 [Receptionist] code for testing new perception features

demos/pycram_receptionist_demo/tests/MS3_testable.py

@@ -130,7 +130,6 @@ def demo_tst():
 
         # 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)
 
 

demos/pycram_receptionist_demo/tests/perception_humans.py

@@ -32,6 +32,13 @@
 def p():
     with real_robot:
 
+        # to signal the start of demo
+        TalkingMotion("Hello, i am ready for the test").resolve().perform()
+
+        # does the code work with the manipulation feature?
+        giskardpy.move_head_to_human()
+
+        # old Detection Query
         DetectAction(technique='human', state='start').resolve().perform()
 
         rospy.loginfo("human detected")
@@ -48,6 +55,18 @@ def p():
 
         print("------------------------------- stop")
 
+        # new Query to detect attributes of a human
+        TalkingMotion("detecting attributes now").resolve().perform()
+        attributes = DetectAction(technique='attributes').resolve().perform()
+        rospy.loginfo("Attributes: " + str(attributes))
+        rospy.sleep(5)
+
+        # new Query for free seat
+        #TalkingMotion("detecting free seat now").resolve().perform()
+        #seat = DetectAction(technique='location', state='seat1').resolve().perform()
+        #rospy.loginfo("seat bool: " + str(attributes))
+        #rospy.sleep(5)
+
         print("end")
 
 

src/pycram/external_interfaces/robokudo.py

@@ -176,7 +176,7 @@ def callback(pose):
         human_bool = True
         human_pose = pose
 
-
+    # create client to communicate with perception
     client = actionlib.SimpleActionClient('robokudo/query', QueryAction)
     rospy.loginfo("Waiting for action server")
     client.wait_for_server()
@@ -218,9 +218,10 @@ def stop_queryHuman() -> Any:
     rospy.loginfo("cancelled current goal")
 
 
-def seat_queryHuman() -> Any:
+def seat_queryHuman(seat: str) -> Any:
     """
-    Sends a query to RoboKudo to look for a free place to sit and look for a human
+    Sends a query to RoboKudo to check if a place is free to sit
+    :param seat: name of the seat/region, that will be checked
     """
     init_robokudo_interface()
     from robokudo_msgs.msg import QueryAction, QueryGoal, QueryResult
@@ -235,15 +236,14 @@ def done_callback(state, result: QueryResult):
         global query_result
         query_result = result.res
 
-
     # fill Query with information so that perception looks for a seat
     object_goal = QueryGoal()
-    object_goal.obj.location = "seat" # aktivate region filter
-    object_goal.obj.attribute = ["attributes"] # gender, bright/dark top, x, hat or no hat
+    object_goal.obj.location = seat # aktivate region filter
 
     client = actionlib.SimpleActionClient('robokudo/query', QueryAction)
     rospy.loginfo("Waiting for action server")
     client.wait_for_server()
+    rospy.loginfo("Query to Perception: " + str(object_goal))
     client.send_goal(object_goal, active_cb=active_callback, done_cb=done_callback)
     # TODO: necessary?
     client.wait_for_result()
@@ -268,15 +268,14 @@ def done_callback(state, result: QueryResult):
         global query_result
         query_result = result
 
-
-
     object_goal = QueryGoal()
     # Perception will detect gender, clothes, skin color, age
     object_goal.obj.attribute = ["attributes"]
 
     client = actionlib.SimpleActionClient('robokudo/query', QueryAction)
     rospy.loginfo("Waiting for action server")
     client.wait_for_server()
+    rospy.loginfo("Query to Perception: " + str(object_goal))
     client.send_goal(object_goal, active_cb=active_callback, done_cb=done_callback)
     # TODO: necessary?
     client.wait_for_result()

src/pycram/process_modules/hsrb_process_modules.py

@@ -362,11 +362,12 @@ def _execute(self, desig: DetectingMotion.Motion) -> Any:
             return "stopped"
         elif desig.technique == 'location':
             # TODO: test what and how Perception returns Query msg and make it fit rest of code
-            seat_human_pose = seat_queryHuman()
+            seat = desig.state
+            seat_human_pose = seat_queryHuman(seat)
             return seat_human_pose
         elif desig.technique == 'attributes':
-            seat_human_pose = attributes_queryHuman()
-            return seat_human_pose
+            human_pose_attr = attributes_queryHuman()
+            return human_pose_attr
 
 
         query_result = queryEmpty(ObjectDesignatorDescription(types=[desig.object_type]))