diff --git a/demos/pycram_receptionist_demo/MS3_Demo.py b/demos/pycram_receptionist_demo/MS3_Demo.py new file mode 100644 index 000000000..2d251b498 --- /dev/null +++ b/demos/pycram_receptionist_demo/MS3_Demo.py @@ -0,0 +1,28 @@ +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 + +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]) + +kitchen = Object("kitchen", "environment", "kitchen.urdf") +giskardpy.init_giskard_interface() +RobotStateUpdater("/tf", "/giskard_joint_states") \ 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 6964f888c..91c45bc88 100644 --- a/demos/pycram_receptionist_demo/utils/misc.py +++ b/demos/pycram_receptionist_demo/utils/misc.py @@ -1,42 +1,34 @@ -import rospy -from pycram.designators.action_designator import NavigateAction from pycram.designators.motion_designator import TalkingMotion -import pycram.external_interfaces.giskard as giskardpy from pycram.designators.object_designator import * from std_msgs.msg import String -from demos.pycram_receptionist_demo.deprecated import talk_actions -from deprecated import deprecated +from pycram.helper import axis_angle_to_quaternion + pub_nlp = rospy.Publisher('/startListener', String, queue_size=10) -def talk_request(data: list): - """ - callback function that takes the data from nlp (name and drink) and lets the robot talk - :param data: String "name drink" - """ +# pose variables - rospy.loginfo("in callback success") - toyas_text = "Hey " + data[0] + " your favorite drink is " + data[ - 1] - TalkingMotion(toyas_text).resolve().perform() - rospy.sleep(1) - TalkingMotion("nice to meet you").resolve().perform() +# pose in front of the couch, hsr looks in direction of couch +pose_couch = Pose([3, 5, 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) -def talk_request_nlp(data: str): +# talk functions +def talk_request(data: list): """ - callback function that takes the data from nlp (name and drink) and lets the robot talk - :param data: String "name drink" + function that takes the data from nlp (name and drink) and lets the robot talk + :param data: list ["name" "drink"] """ - rospy.loginfo("in callback success") - data = data.split(",") - toyas_text = "Hey " + data[0] + " your favorite drink is " + data[ - 1] + name, drink = data + toyas_text = f"Hey {name}, your favorite drink is {drink}" TalkingMotion(toyas_text).resolve().perform() rospy.sleep(1) - TalkingMotion("nice to meet you").resolve().perform() + TalkingMotion("Nice to meet you").resolve().perform() def talk_error(data): @@ -54,8 +46,8 @@ def introduce(name1, drink1, host_name, host_drink): """ Text for robot to introduce two people to each other """ - first = "Hey " + str(host_name) + " This is " + str(name1) + " and the favorite drink of your guest is " + str(drink1) + first = "Hey " + str(host_name) + " This is " + str(name1) + " and the favorite drink of your guest is " + str(drink1) second = str(name1) + " This is " + str(host_name) + " his favorite drink is " + str(host_drink) TalkingMotion(first).resolve().perform() rospy.sleep(3)