From 79993b8f82d073323399302d69cb0b5977edf0ae Mon Sep 17 00:00:00 2001 From: juschulz Date: Thu, 1 Feb 2024 14:29:14 +0100 Subject: [PATCH] [File structure changes/ Human added to Object Designator/ some Code changes in receptionist tests and Fluent implemented] --- demos/pycram_receptionist_demo/M2_Demo.py | 4 +- .../Ms2_testable_stuff.py | 172 ------------------ demos/pycram_receptionist_demo/random.py | 43 ----- .../tests/Ms2_testable_stuff.py | 141 ++++++++++++++ .../tests/__init__.py | 0 .../tests/connection_knowledge.py | 136 ++++++++++++++ .../tests/perception_humans.py | 40 ++++ demos/pycram_receptionist_demo/utils/misc.py | 20 +- src/pycram/designators/object_designator.py | 37 ++++ src/pycram/external_interfaces/knowrob.py | 6 +- 10 files changed, 365 insertions(+), 234 deletions(-) delete mode 100644 demos/pycram_receptionist_demo/Ms2_testable_stuff.py delete mode 100644 demos/pycram_receptionist_demo/random.py create mode 100644 demos/pycram_receptionist_demo/tests/Ms2_testable_stuff.py create mode 100644 demos/pycram_receptionist_demo/tests/__init__.py create mode 100644 demos/pycram_receptionist_demo/tests/connection_knowledge.py create mode 100644 demos/pycram_receptionist_demo/tests/perception_humans.py diff --git a/demos/pycram_receptionist_demo/M2_Demo.py b/demos/pycram_receptionist_demo/M2_Demo.py index 218475f84..33564385c 100644 --- a/demos/pycram_receptionist_demo/M2_Demo.py +++ b/demos/pycram_receptionist_demo/M2_Demo.py @@ -68,8 +68,8 @@ def introduce(name1, drink1, name2, drink2): """ first="Hey" + name2 + " This is " + name1 + "and the favorite drink of your guest is " + drink1 second = name1 + "This is " + name2 + "his favorite drink is " + drink2 - TalkingMotion(first) - TalkingMotion(second) + TalkingMotion(first).resolve.perform() + TalkingMotion(second).resolve.perform() diff --git a/demos/pycram_receptionist_demo/Ms2_testable_stuff.py b/demos/pycram_receptionist_demo/Ms2_testable_stuff.py deleted file mode 100644 index 83221fc56..000000000 --- a/demos/pycram_receptionist_demo/Ms2_testable_stuff.py +++ /dev/null @@ -1,172 +0,0 @@ -import rospy -from pycram.designators.action_designator import DetectAction, NavigateAction -from pycram.designators.motion_designator import TalkingMotion -from pycram.helper import axis_angle_to_quaternion -from pycram.process_module import real_robot -import pycram.external_interfaces.giskard as giskardpy -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 -from demos.pycram_receptionist_demo.deprecated import talk_actions -import pycram.external_interfaces.navigate as moveBase -world = BulletWorld("DIRECT") -# /pycram/viz_marker topic bei Marker Array -v = VizMarkerPublisher() - -world.set_gravity([0, 0, -9.8]) -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") -robot.set_joint_state(robot_description.torso_joint, 0.24) -kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) -milk = Object("Milkpack", "milk", "milk.stl", pose=Pose([-2.7, 2.3, 0.43]), color=[1, 0, 0, 1]) - -giskardpy.init_giskard_interface() - - -# giskardpy.sync_worlds() -# RobotStateUpdater("/tf", "/joint_states") - - -class HumanDescription: - - def __init__(self, name, fav_drink): - self.name = name - self.fav_drink = fav_drink - #self.shirt_color = shirt_color - #self.gender = gender - # TODO: coordinate with Perception on what is easy to implement - # characteristics to consider: height, hair color, and age. - - -def talk_request(data: String): - """ - callback function that takes the data from nlp (name and drink) and lets the robot talk - :param data: String "name drink" - """ - - name_drink = data.data.split(" ") - talk_actions.name_drink_talker(name_drink) - rospy.loginfo("nlp data:" + name_drink[0] + " " + name_drink[1]) - - rospy.loginfo("stop looking now") - giskardpy.stop_looking() - rospy.loginfo("Navigating now") - NavigateAction([Pose([3, 5, 0], [0, 0, 1, 1])]).resolve().perform() - - -def talk_error(data): - """ - callback function if no name/drink was heard - """ - - error_msgs = "i could not hear you, please repeat" - TalkingMotion(error_msgs).resolve().perform() - pub_nlp.publish("start listening") - -def introduce(name1, drink1, name2, drink2): - """ - Text for robot to introduce two people to each other - """ - first="Hey" + name2 + " This is " + name1 + "and the favorite drink of your guest is " + drink1 - second = name1 + "This is " + name2 + "his favorite drink is " + drink2 - TalkingMotion(first) - TalkingMotion(second) - - -def demo_test(area): - with real_robot: - host = HumanDescription("Bob", fav_drink="Coffee") - pub_nlp = rospy.Publisher('/startListener', String, queue_size=10) - robot_orientation_couch = axis_angle_to_quaternion([0, 0, 1], 0) - pose_couch = Pose([3, 5, 0], robot_orientation_couch) - - robot_orientation_from_couch = axis_angle_to_quaternion([0, 0, 1], -90) - pose_from_couch = Pose([4.2, 3.8, 0], robot_orientation_from_couch) - - robot_orientation = axis_angle_to_quaternion([0, 0, 1], 90) - pose_kitchen_to_couch = Pose([4.2, 3, 0], robot_orientation) - - pose_home = Pose([3, 1.7, 0], robot_orientation) - - DetectAction(technique='default', state='start').resolve().perform() - # Perception, detect first guest - #todo richtigen fluent erstellen - fluentvariable = True - while not fluentvariable: - - #TODO: funktioniert Designator in While Bedingung??? - TalkingMotion("Please step in front of me").resolve.perform() - rospy.sleep(5) - - - rospy.loginfo("human detected") - - # look at guest and introduction - giskardpy.move_head_to_human() - TalkingMotion("Hello, i am Toya and my favorite drink is oil. What about you, talk to me?").resolve().perform() - - # reicht sleep 1? - rospy.sleep(1) - - # signal to start listening - pub_nlp.publish("start listening") - #TODO: How to erst weiter machen, wenn Knowledge Daten geschickt hat - - TalkingMotion("Hey i will stop looking now").resolve().perform() - - - rospy.loginfo("stop looking now") - giskardpy.stop_looking() - rospy.loginfo("Navigating now") - TalkingMotion("navigating to couch area now, pls step away").resolve().perform() - - - if area == 'to_couch': - NavigateAction([pose_kitchen_to_couch]).resolve().perform() - NavigateAction([pose_couch]).resolve().perform() - elif area == 'from_couch': - NavigateAction([pose_from_couch]).resolve().perform() - NavigateAction([pose_home]).resolve().perform() - - - # failure handling - #rospy.Subscriber("nlp_feedback", Bool, talk_error) - - # receives name and drink via topic - # rospy.Subscriber("nlp_out", String, talk_request) - - - -def nav_test(): - with real_robot: - robot_orientation = axis_angle_to_quaternion([0, 0, 1], 90) - test_pose1 = Pose([4.2, 3, 0], robot_orientation) - test_pose = Pose([3, 5, 0], [0, 0, 0, 1]) - moveBase.queryPoseNav(test_pose1) - moveBase.queryPoseNav(test_pose) - - - - -#demo_test('from_couch') -#demo_test('to_couch') - - # receives name and drink via topic - #rospy.Subscriber("nlp_out", String, talk_request) - - - -#1. rasa run --enable-api -> start Rasa Server -#2. python3 activate_language_processing.py -> NLP -#3. roslaunch suturo_bringup suturo_bringup.launch -> Map -#4. roslaunch_hsr_velocity_controller unloas_my_controller.launch -#5. roslaunch giskardpy giskardpy_hsr_real_vel.launch -> Giskard -#starten -#6. rosrun robokudo main.py _ae=humandetection_demo_ros_pkg=milestone1 -> Perception -#7. run demo in Pycharm -> Planning - - diff --git a/demos/pycram_receptionist_demo/random.py b/demos/pycram_receptionist_demo/random.py deleted file mode 100644 index 54f075067..000000000 --- a/demos/pycram_receptionist_demo/random.py +++ /dev/null @@ -1,43 +0,0 @@ -import rospy -from std_msgs.msg import String -from std_srvs.srv import IsKnown - -#from pycram.external_interfaces.knowrob import instances_of, get_guest_info -#import random - -#def client_call(id): -# rospy.init_node("client_node") -# rospy.wait_for_service("serviceName") -# rate = rospy.Rate(1) -# while not rospy.s_shutdown(): -# try: -# service_variable = rospy.ServiceProxy("SeviceName", Type) -# respone = service_variable(parameterForService) -# rate.sleep() -# except rospy.ServiceExcepion as e: -# print("Service call failed") - - -def get_guest_info(id): - """ - function that uses Knowledge Service to get Name and drink from new guest via ID - """ - - #TODO: Service für Name und Getränk mit ID einbauen, wenn dieser fertig - rospy.wait_for_service('name_server') - print("debug") - try: - #TODO: Service class herausfinden - print("in try") - info_service = rospy.ServiceProxy('name_server', IsKnown) - print("serverProxy") - guest_data = info_service(id) #guest_data = List der Form ["name", "drink"] - print("end") - return guest_data - except rospy.ServiceException as e: - print("Service call failed") - - -if __name__ == '__main__': - print("starte test") - print(get_guest_info("4.0")) diff --git a/demos/pycram_receptionist_demo/tests/Ms2_testable_stuff.py b/demos/pycram_receptionist_demo/tests/Ms2_testable_stuff.py new file mode 100644 index 000000000..787c719ce --- /dev/null +++ b/demos/pycram_receptionist_demo/tests/Ms2_testable_stuff.py @@ -0,0 +1,141 @@ +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 +from demos.pycram_receptionist_demo.deprecated import talk_actions +import pycram.external_interfaces.navigate as moveBase + +world = BulletWorld("DIRECT") +# /pycram/viz_marker topic bei Marker Array +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]) + +# carefull that u spawn the correct kitchen +kitchen = Object("kitchen", "environment", "kitchen.urdf") +giskardpy.init_giskard_interface() +RobotStateUpdater("/tf", "/giskard_joint_states") + +robot_orientation_couch = axis_angle_to_quaternion([0, 0, 1], 0) +pose_couch = Pose([3, 5, 0], robot_orientation_couch) + +robot_orientation_from_couch = axis_angle_to_quaternion([0, 0, 1], -90) +pose_from_couch = Pose([4.2, 3.8, 0], robot_orientation_from_couch) + +robot_orientation = axis_angle_to_quaternion([0, 0, 1], 90) +pose_kitchen_to_couch = Pose([4.2, 3, 0], robot_orientation) + +pose_home = Pose([3, 1.7, 0], robot_orientation) + +pub_nlp = rospy.Publisher('/startListener', String, queue_size=10) + + +class HumanDescription: + + def __init__(self, name, fav_drink: Optional = None): + # 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) + + def set_name(self, new_name): + self.name = new_name + + + +def demo_test(area): + with real_robot: + host = HumanDescription("Bob", fav_drink="Coffee") + guest1 = HumanDescription("guest1") + + # Perception, detect first guest -> First detect guest, then listen + DetectAction(technique='human', state='start').resolve().perform() + + # While loop, human is detected + while not guest1.human_pose: + TalkingMotion("Please step in front of me").resolve.perform() + rospy.sleep(5) + + rospy.loginfo("human detected") + + # look at guest and introduction + 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") + rospy.sleep(5) + + guest_data = get_guest_info(1) + while guest_data == "No name saved under this ID!": + talk_error("no name") + guest_data = get_guest_info(1) + rospy.sleep(3) + + + + + rospy.loginfo("stop looking now") + giskardpy.stop_looking() + rospy.loginfo("Navigating now") + TalkingMotion("navigating to couch area now, pls step away").resolve().perform() + + if area == 'to_couch': + NavigateAction([pose_kitchen_to_couch]).resolve().perform() + NavigateAction([pose_couch]).resolve().perform() + elif area == 'from_couch': + NavigateAction([pose_from_couch]).resolve().perform() + NavigateAction([pose_home]).resolve().perform() + + # failure handling + # rospy.Subscriber("nlp_feedback", Bool, talk_error) + + # receives name and drink via topic + # rospy.Subscriber("nlp_out", String, talk_request) + + +def nav_test(): + with real_robot: + robot_orientation = axis_angle_to_quaternion([0, 0, 1], 90) + test_pose1 = Pose([4.2, 3, 0], robot_orientation) + test_pose = Pose([3, 5, 0], [0, 0, 0, 1]) + moveBase.queryPoseNav(test_pose1) + moveBase.queryPoseNav(test_pose) + +# demo_test('from_couch') +# demo_test('to_couch') + +# receives name and drink via topic +# rospy.Subscriber("nlp_out", String, talk_request) + + +# 1. rasa run --enable-api -> start Rasa Server +# 2. python3 activate_language_processing.py -> NLP +# 3. roslaunch suturo_bringup suturo_bringup.launch -> Map +# 4. roslaunch_hsr_velocity_controller unloas_my_controller.launch +# 5. roslaunch giskardpy giskardpy_hsr_real_vel.launch -> Giskard +# starten +# 6. rosrun robokudo main.py _ae=humandetection_demo_ros_pkg=milestone1 -> Perception +# 7. run demo in Pycharm -> Planning diff --git a/demos/pycram_receptionist_demo/tests/__init__.py b/demos/pycram_receptionist_demo/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/demos/pycram_receptionist_demo/tests/connection_knowledge.py b/demos/pycram_receptionist_demo/tests/connection_knowledge.py new file mode 100644 index 000000000..a99d1ca90 --- /dev/null +++ b/demos/pycram_receptionist_demo/tests/connection_knowledge.py @@ -0,0 +1,136 @@ +import rospy +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 +from demos.pycram_receptionist_demo.deprecated import talk_actions +import pycram.external_interfaces.navigate as moveBase + +world = BulletWorld("DIRECT") +# /pycram/viz_marker topic bei Marker Array +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]) + +# carefull that u spawn the correct kitchen +kitchen = Object("kitchen", "environment", "kitchen.urdf") +giskardpy.init_giskard_interface() +RobotStateUpdater("/tf", "/giskard_joint_states") + +robot_orientation_couch = axis_angle_to_quaternion([0, 0, 1], 0) +pose_couch = Pose([3, 5, 0], robot_orientation_couch) + +robot_orientation_from_couch = axis_angle_to_quaternion([0, 0, 1], -90) +pose_from_couch = Pose([4.2, 3.8, 0], robot_orientation_from_couch) + +robot_orientation = axis_angle_to_quaternion([0, 0, 1], 90) +pose_kitchen_to_couch = Pose([4.2, 3, 0], robot_orientation) + +pose_home = Pose([3, 1.7, 0], robot_orientation) + +pub_nlp = rospy.Publisher('/startListener', String, queue_size=10) + + +class HumanDescription: + + def __init__(self, name, fav_drink: Optional = None): + # 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) + + def set_name(self, new_name): + self.name = new_name + + + +def demo_test(area): + with real_robot: + host = HumanDescription("Bob", fav_drink="Coffee") + guest1 = HumanDescription("guest1") + + # Perception, detect first guest -> First detect guest, then listen + DetectAction(technique='human', state='start').resolve().perform() + + # While loop, human is detected + while not guest1.human_pose: + TalkingMotion("Please step in front of me").resolve.perform() + rospy.sleep(5) + + rospy.loginfo("human detected") + + # look at guest and introduction + 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") + rospy.sleep(5) + + guest_data = get_guest_info(1) + while guest_data == "No name saved under this ID!": + talk_error("no name") + guest_data = get_guest_info(1) + rospy.sleep(3) + + +def get_guest_info_old(id): + """ + function that uses Knowledge Service to get Name and drink from new guest via ID + """ + + #TODO: Service für Name und Getränk mit ID einbauen, wenn dieser fertig + rospy.wait_for_service('name_server') + print("debug") + try: + #TODO: Service class herausfinden + print("in try") + info_service = rospy.ServiceProxy('name_server', String) #isKnown + print("serverProxy") + guest_data = info_service(id) #guest_data = List der Form ["name", "drink"] + print("end") + return guest_data + except rospy.ServiceException as e: + print("Service call failed") + + + +def client_test(data): + try: + zahl = int(data) + ergebnis = 10 / zahl + print("Ergebnis:", ergebnis) + return "klappt" + + except ZeroDivisionError: + print("Fehler: Division durch Null ist nicht erlaubt.") + + + +if __name__ == '__main__': + if client_test(1): + print("wtf!") + else: + print("klappt das?") diff --git a/demos/pycram_receptionist_demo/tests/perception_humans.py b/demos/pycram_receptionist_demo/tests/perception_humans.py new file mode 100644 index 000000000..5c7d3e262 --- /dev/null +++ b/demos/pycram_receptionist_demo/tests/perception_humans.py @@ -0,0 +1,40 @@ +import rospy + +from pycram.designators.action_designator import DetectAction, NavigateAction +from pycram.designators.motion_designator import TalkingMotion +from pycram.fluent import Fluent +from pycram.helper import axis_angle_to_quaternion +from pycram.process_module import semi_real_robot, 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 +from demos.pycram_receptionist_demo.deprecated import talk_actions +import pycram.external_interfaces.navigate as moveBase + +world = BulletWorld("DIRECT") +# /pycram/viz_marker topic bei Marker Array +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]) + +# carefull that u spawn the correct kitchen +kitchen = Object("kitchen", "environment", "kitchen.urdf") +giskardpy.init_giskard_interface() +RobotStateUpdater("/tf", "/giskard_joint_states") + +def test(): + with real_robot: + DetectAction(technique='human', state='start').resolve().perform() + + rospy.loginfo("human detected") + + + +if __name__ == '__main__': + test() \ 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 815ff30c4..6de7352e6 100644 --- a/demos/pycram_receptionist_demo/utils/misc.py +++ b/demos/pycram_receptionist_demo/utils/misc.py @@ -7,26 +7,20 @@ from demos.pycram_receptionist_demo.deprecated import talk_actions from deprecated import deprecated +pub_nlp = rospy.Publisher('/startListener', String, queue_size=10) -@deprecated(reason="Newst version uses the knowledge interface") -def talk_request(data: String): +def talk_request(name_drink: List): """ callback function that takes the data from nlp (name and drink) and lets the robot talk - :param data: String "name drink" + :param name_drink: List ["name", "drink"] """ - - name_drink = data.data.split(" ") - talk_actions.name_drink_talker(name_drink) rospy.loginfo("nlp data:" + name_drink[0] + " " + name_drink[1]) - rospy.loginfo("stop looking now") - giskardpy.stop_looking() - rospy.loginfo("Navigating now") - NavigateAction([Pose([3, 5, 0], [0, 0, 1, 1])]).resolve().perform() -@deprecated(reason="Newst version uses the knowledge interface") + + def talk_error(data): """ callback function if no name/drink was heard @@ -44,5 +38,5 @@ def introduce(name1, drink1, name2, drink2): """ first = "Hey" + name2 + " This is " + name1 + "and the favorite drink of your guest is " + drink1 second = name1 + "This is " + name2 + "his favorite drink is " + drink2 - TalkingMotion(first) - TalkingMotion(second) + TalkingMotion(first).resolve.perform() + TalkingMotion(second).resolve.perform() diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 02a58ef41..3e3e0aa67 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,11 +1,15 @@ import dataclasses from typing import List, Union, Optional, Callable, Tuple, Iterable + +import rospy import sqlalchemy.orm from ..bullet_world import BulletWorld, Object as BulletWorldObject from ..designator import DesignatorDescription, ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) from ..pose import Pose +from pycram.fluent import Fluent +from std_msgs.msg import String from ..external_interfaces.robokudo import query @@ -175,3 +179,36 @@ def __iter__(self): # if bullet_obj.get_pose().dist(obj_deisg.pose) < 0.05: # obj_deisg.bullet_world_object = bullet_obj # yield obj_deisg + +class HumanDescription: + """ + Class that represents humans. this class does not spawn a human in a simulation. + """ + def __init__(self, name, fav_drink: Optional = None): + """ + :param name: name of human + :param fav_drink: favorite drink of human + """ + + # 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): + """ + callback function for human_pose Subscriber. + sets the attribute human_pose when someone (e.g. Perception/Robokudo) publishes on the topic + :param HumanPoseMsg: received message + """ + self.human_pose.set_value(HumanPoseMsg.data) + + def set_name(self, new_name): + """ + function for changing name of human + :param new_name: new name of human + """ + self.name = new_name \ No newline at end of file diff --git a/src/pycram/external_interfaces/knowrob.py b/src/pycram/external_interfaces/knowrob.py index b8288c449..050122aac 100644 --- a/src/pycram/external_interfaces/knowrob.py +++ b/src/pycram/external_interfaces/knowrob.py @@ -131,6 +131,8 @@ def knowrob_string_to_pose(pose_as_string: str) -> List[float]: def get_guest_info(id): """ function that uses Knowledge Service to get Name and drink from new guest via ID + :param id: integer for person + :return: ["name", " drink"] """ rospy.wait_for_service('name_server') @@ -142,7 +144,3 @@ def get_guest_info(id): except rospy.ServiceException as e: print("Service call failed") - - - -