diff --git a/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py b/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py index 7f9cdd78c..2034d25d5 100644 --- a/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py +++ b/demos/pycram_receptionist_demo/Ms3_receptionist_demo.py @@ -69,7 +69,14 @@ rospy.Subscriber("nlp_confirmation", Bool, misc.name_cb) - while not misc.understood: + 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) diff --git a/demos/pycram_receptionist_demo/tests/Humanfluent.py b/demos/pycram_receptionist_demo/tests/Humanfluent.py deleted file mode 100644 index 44f21324f..000000000 --- a/demos/pycram_receptionist_demo/tests/Humanfluent.py +++ /dev/null @@ -1,35 +0,0 @@ -import rospy - -from pycram.fluent import Fluent -from pycram.process_module import real_robot -from std_msgs.msg import String - - -class HumanDescription: - - def __init__(self, name, fav_drink): - # TODO: coordinate with Perception on what is easy to implement - # characteristics to consider: height, hair color, and age. - self.human_pose = Fluent() - self.name = name - self.fav_drink = fav_drink # self.shirt_color = shirt_color # self.gender = gender - - self.human_pose_sub = rospy.Subscriber("/human_pose", String, self.human_pose_cb) - - def human_pose_cb(self, HumanPoseMsg): - self.human_pose.set_value(HumanPoseMsg.data) - - -with real_robot: - host = HumanDescription("Bob", fav_drink="Coffee") - #detect - # Perception, detectfirst guest - # If detect true then - - host.human_pose.set_value("True") - - # While loop, human is detected - while host.human_pose.get_value(): - print(host.human_pose.get_value()) - - rospy.loginfo("human not detected anymore") \ No newline at end of file diff --git a/demos/pycram_receptionist_demo/tests/MS3_testable.py b/demos/pycram_receptionist_demo/tests/MS3_testable.py new file mode 100644 index 000000000..0e740d2f8 --- /dev/null +++ b/demos/pycram_receptionist_demo/tests/MS3_testable.py @@ -0,0 +1,124 @@ +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 +from pycram.process_module import real_robot +import pycram.external_interfaces.giskard as giskardpy +from pycram.ros.robot_state_updater import RobotStateUpdater +from pycram.ros.viz_marker_publisher import VizMarkerPublisher +from pycram.designators.location_designator import * +from pycram.designators.object_designator import * +from pycram.bullet_world import BulletWorld, Object +from std_msgs.msg import String, Bool + +world = BulletWorld("DIRECT") +v = VizMarkerPublisher() + +robot = Object("hsrb", "robot", "../../resources/" + robot_description.name + ".urdf") +robot_desig = ObjectDesignatorDescription(names=["hsrb"]).resolve() +robot.set_color([0.5, 0.5, 0.9, 1]) + +kitchen = Object("kitchen", "environment", "kitchen.urdf") +giskardpy.init_giskard_interface() +RobotStateUpdater("/tf", "/giskard_joint_states") + +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() + + # 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") + + # 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) + + # signal to start listening + pub_nlp.publish("start listening") + + # receives name and drink via topic + rospy.Subscriber("nlp_out", String, misc.talk_request_nlp) + + # failure handling + rospy.Subscriber("nlp_feedback", Bool, misc.talk_error) + + while misc.guest1.name == "guest1": + rospy.sleep(1) + + 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") + + 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='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() diff --git a/demos/pycram_receptionist_demo/tests/fortoday.py b/demos/pycram_receptionist_demo/tests/fortoday.py deleted file mode 100644 index f37491bd9..000000000 --- a/demos/pycram_receptionist_demo/tests/fortoday.py +++ /dev/null @@ -1,22 +0,0 @@ -import rospy - -from pycram.designators.action_designator import DetectAction, NavigateAction -from pycram.designators.motion_designator import TalkingMotion -from pycram.fluent import Fluent -from demos.pycram_receptionist_demo.utils.misc import * -from pycram.helper import axis_angle_to_quaternion -from pycram.process_module import real_robot -import pycram.external_interfaces.giskard as giskardpy -from pycram.external_interfaces.knowrob import get_guest_info -from pycram.ros.robot_state_updater import RobotStateUpdater -from pycram.ros.viz_marker_publisher import VizMarkerPublisher -from pycram.designators.location_designator import * -from pycram.designators.object_designator import * -from pycram.bullet_world import BulletWorld, Object -from std_msgs.msg import String, Bool -from demos.pycram_receptionist_demo.deprecated import talk_actions -import pycram.external_interfaces.navigate as moveBase - -x = get_guest_info("1.0") -print(x[0]) -print(x[1]) \ No newline at end of file diff --git a/demos/pycram_receptionist_demo/utils/misc.py b/demos/pycram_receptionist_demo/utils/misc.py index ff3297144..68f0d6aab 100644 --- a/demos/pycram_receptionist_demo/utils/misc.py +++ b/demos/pycram_receptionist_demo/utils/misc.py @@ -5,8 +5,10 @@ # Publisher for NLP pub_nlp = rospy.Publisher('/startListener', String, queue_size=10) pub_pose = rospy.Publisher('/human_pose', PoseStamped, queue_size=10) -understood = False -doorbell = False +understood_name = False +understood_drink = False +# TODO: set to False when NLP has implemented that feature +doorbell = True # Declare variables for humans @@ -16,12 +18,12 @@ # Pose variables # Pose in front of the couch, HSR looks in direction of couch -pose_couch = Pose([3, 5, 0], [0, 0, 1, 0]) +pose_couch = Pose([2.7, 5, 0], [0, 0, 1, 0]) pose_door = Pose([1.4, 0.25, 0], [0, 0, 1, 0]) # Pose in the passage between kitchen and living room robot_orientation = axis_angle_to_quaternion([0, 0, 1], 90) -pose_kitchen_to_couch = Pose([4.2, 3, 0], robot_orientation) +pose_kitchen_to_couch = Pose([4.35, 3, 0], robot_orientation) def talk_request(data: list): @@ -48,22 +50,25 @@ def talk_request_nlp(data: String): :param data: String "name, drink" """ print(guest1.name) + data_list = data.data.split(",") + name, drink = data_list 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() + # 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 + elif not understood_name: toyas_text = f"is your name {name} ?" TalkingMotion(toyas_text).resolve().perform() pub_nlp.publish("start") + elif understood_name and not understood_drink: + toyas_text = f"is your favorite drink {drink} ?" + TalkingMotion(toyas_text).resolve().perform() + pub_nlp.publish("start") + def talk_error(data): @@ -119,12 +124,16 @@ def name_cb(data): callback function for a subscriber to NLP script. is called when name was correctly understood """ - global understood - if data.data: - TalkingMotion("perfect, nice to meet you").resolve().perform() - understood = True + global understood_name + global understood_drink + if data.data and not understood_name: + TalkingMotion("perfect").resolve().perform() + understood_name = True + elif data.data and not understood_drink: + TalkingMotion("alright, thank you and nice to meet you").resolve().perform() + understood_drink = True else: - TalkingMotion("i am sorry, please repeat your name loud and clear").resolve().perform() + TalkingMotion("i am sorry, please repeat yourself loud and clear").resolve().perform() rospy.sleep(1) # TODO: only hear for Name pub_nlp.publish("start listening")