From d097cfc24f0ff7af53636d7ca8056a58dfbf1732 Mon Sep 17 00:00:00 2001 From: juschulz Date: Thu, 28 Mar 2024 10:49:38 +0100 Subject: [PATCH 1/2] [Receptionist] perception query change --- src/pycram/external_interfaces/robokudo.py | 7 ++++--- src/pycram/process_modules/hsrb_process_modules.py | 3 ++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index d3a7888f6..f22bc2b7d 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -215,9 +215,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,7 +236,7 @@ def done_callback(state, result: QueryResult): # 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.location = seat # aktivate region filter object_goal.obj.attribute = ["attributes"] # gender, bright/dark top, x, hat or no hat client = actionlib.SimpleActionClient('robokudo/query', QueryAction) diff --git a/src/pycram/process_modules/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py index 4be45f66b..a757189af 100644 --- a/src/pycram/process_modules/hsrb_process_modules.py +++ b/src/pycram/process_modules/hsrb_process_modules.py @@ -362,7 +362,8 @@ 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() From f0c44ce18cf0603831a05f3e9e769c7914bfe4e3 Mon Sep 17 00:00:00 2001 From: juschulz Date: Thu, 28 Mar 2024 11:34:44 +0100 Subject: [PATCH 2/2] [Receptionist] Perception Test for new features --- .../tests/MS3_testable.py | 1 - .../tests/perception_humans.py | 19 +++++++++++++++++++ src/pycram/external_interfaces/robokudo.py | 8 +++----- .../process_modules/hsrb_process_modules.py | 4 ++-- 4 files changed, 24 insertions(+), 8 deletions(-) diff --git a/demos/pycram_receptionist_demo/tests/MS3_testable.py b/demos/pycram_receptionist_demo/tests/MS3_testable.py index 1e6732c62..433aad05a 100644 --- a/demos/pycram_receptionist_demo/tests/MS3_testable.py +++ b/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) diff --git a/demos/pycram_receptionist_demo/tests/perception_humans.py b/demos/pycram_receptionist_demo/tests/perception_humans.py index 72ef0dcab..aef7683fa 100644 --- a/demos/pycram_receptionist_demo/tests/perception_humans.py +++ b/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") diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index a1d687f04..64fc684ea 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/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() @@ -236,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 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() @@ -269,8 +268,6 @@ 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"] @@ -278,6 +275,7 @@ def done_callback(state, result: QueryResult): 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() diff --git a/src/pycram/process_modules/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py index 73fc1bfa9..44a31cbf5 100644 --- a/src/pycram/process_modules/hsrb_process_modules.py +++ b/src/pycram/process_modules/hsrb_process_modules.py @@ -366,8 +366,8 @@ def _execute(self, desig: DetectingMotion.Motion) -> Any: 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]))