diff --git a/demos/pycram_receptionist_demo/tests/MS3_testable.py b/demos/pycram_receptionist_demo/tests/MS3_testable.py index 0e740d2f8..1e6732c62 100644 --- a/demos/pycram_receptionist_demo/tests/MS3_testable.py +++ b/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() diff --git a/demos/pycram_receptionist_demo/tests/perception_humans.py b/demos/pycram_receptionist_demo/tests/perception_humans.py index 4a47e6cdf..72ef0dcab 100644 --- a/demos/pycram_receptionist_demo/tests/perception_humans.py +++ b/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() diff --git a/demos/pycram_receptionist_demo/utils/misc.py b/demos/pycram_receptionist_demo/utils/misc.py index 68f0d6aab..51863675b 100644 --- a/demos/pycram_receptionist_demo/utils/misc.py +++ b/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) diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index a7867a0ed..0270cb513 100644 --- a/src/pycram/designators/object_designator.py +++ b/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 diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index f22bc2b7d..a1d687f04 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/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") diff --git a/src/pycram/process_modules/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py index a757189af..73fc1bfa9 100644 --- a/src/pycram/process_modules/hsrb_process_modules.py +++ b/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