From 78ddd76c1d856ec0815c32df6d5d56dd1327f809 Mon Sep 17 00:00:00 2001 From: meike Date: Fri, 6 Dec 2024 13:13:24 +0100 Subject: [PATCH 1/2] [MS1-Restaurant] Started to implement the beginnung of the NLP functions for the challenge --- .../pycram_hsrb_real_test_demos/restaurant.py | 29 +++++----- .../utils/nlp_restaurant.py | 57 +++++++++++++++---- 2 files changed, 62 insertions(+), 24 deletions(-) diff --git a/demos/pycram_hsrb_real_test_demos/restaurant.py b/demos/pycram_hsrb_real_test_demos/restaurant.py index ab7d9c95f..018e71c0c 100644 --- a/demos/pycram_hsrb_real_test_demos/restaurant.py +++ b/demos/pycram_hsrb_real_test_demos/restaurant.py @@ -9,6 +9,8 @@ from std_msgs.msg import String from demos.pycram_hsrb_real_test_demos.utils.misc_restaurant import Restaurant + +from pycram.demos.pycram_hsrb_real_test_demos.utils.NLPRestaurant import NLPRestaurant # from pycram.demos.pycram_hsrb_real_test_demos.utils.misc_restaurant import Restaurant, monitor_func from pycram.designators.action_designator import ParkArmsAction, DetectAction, LookAtAction, MoveTorsoAction, \ NavigateAction @@ -25,7 +27,6 @@ from demos.pycram_hsrb_real_test_demos.utils.startup import startup import pycram.external_interfaces.giskard as giskardpy import pycram.external_interfaces.robokudo as robokudo -from demos.pycram_hsrb_real_test_demos.utils.nlp_restaurant import nlp_restaurant import rospy import tf from geometry_msgs.msg import PoseStamped @@ -41,7 +42,7 @@ rospy.loginfo("You can start your demo now") pub_nlp = rospy.Publisher('/startListener', String, queue_size=16) -nlp = nlp_restaurant() +nlp = NLPRestaurant() ########################################################################### @@ -51,6 +52,11 @@ order = ('Cola', 2) +########################################################################### +#NLP Functions + +########################################################################## + def monitor_found_waving(): global human_pose if human_pose: @@ -114,11 +120,6 @@ def looperino(): -def detect_obstacles(): - def laser_scan_cb(msg): - ranges = list(msg.ranges) - print(ranges) - laser_subscriber = rospy.Subscriber("hsrb/base_scan", LaserScan, laser_scan_cb ) def demo(step): @@ -151,15 +152,15 @@ def demo(step): customer = CustomerDescription(customerCounter, drive_pose) marker.publish(Pose.from_pose_stamped(drive_pose), color=[1, 1, 0, 1], name="human_waving_pose") move.pub_now(navpose=drive_pose) - print(customer.pose) print(customer.id) - # if step <= 2: # Order step - # image_switch_publisher.pub_now(ImageEnum.ORDER.value) - # MoveTorsoAction([0.1]).resolve().perform() - # TalkingMotion("What do you want to order?").perform() # NLP Placeholder - # customer.set_order("Cola",1) - # TalkingMotion(f"You want to order {customer.order[0]} in the following amount {customer.order[1]}").perform() + if step <= 2: # Order step + image_switch_publisher.pub_now(ImageEnum.ORDER.value) + MoveTorsoAction([0.1]).resolve().perform() + TalkingMotion("What do you want to order?").perform() # NLP Placeholder + nlp_restaurant.get_order(customer) + #customer.set_order("Cola",1) + #TalkingMotion(f"You want to order {customer.order[0]} in the following amount {customer.order[1]}").perform() # if step <= 3: # Drive back step # image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) # TalkingMotion("I will drive back now and return with your order").perform() diff --git a/demos/pycram_hsrb_real_test_demos/utils/nlp_restaurant.py b/demos/pycram_hsrb_real_test_demos/utils/nlp_restaurant.py index ac51bcd39..3087edf29 100644 --- a/demos/pycram_hsrb_real_test_demos/utils/nlp_restaurant.py +++ b/demos/pycram_hsrb_real_test_demos/utils/nlp_restaurant.py @@ -1,3 +1,4 @@ +import json import time import rospy @@ -9,27 +10,34 @@ from pycram.designators.object_designator import CustomerDescription from pycram.utilities.robocup_utils import ImageSwitchPublisher -response = [None, None, None] +response = [None, None] callback = False +dict_response = {} timeout = 10 - +global tmp_order image_switch_publisher = ImageSwitchPublisher() -class nlp_restaurant: +class NLPRestaurant: def __init__(self): self.nlp_pub = rospy.Publisher('/startListener', String, queue_size=16) self.sub_nlp = rospy.Subscriber("nlp_out", String, self.data_cb) self.response = ["", ""] self.callback = False + self.direct_data = String("") + + def stnd_msg_to_dict(self, msg: String): + tmp_dump = json.loads(msg.data) + print(tmp_dump) + return tmp_dump def data_cb(self, data): """ function to receive data from nlp via /nlp_out topic """ - + print(type(data)) image_switch_publisher = ImageSwitchPublisher() - + self.direct_data = data image_switch_publisher.pub_now(ImageEnum.HI.value) self.response = data.data.split(",") for ele in self.response: @@ -38,9 +46,31 @@ def data_cb(self, data): print(self.response) self.callback = True - def repeat_order(self, customer: CustomerDescription): - TalkingMotion("Do you want to order the following items?").perform() - TalkingMotion(f"{customer.order[1]}{customer.order[0]} ").perform() + def order_repeat(self, customer: CustomerDescription): + global tmp_order + self.callback = False + tries= 0 + + while tries <= 2: + TalkingMotion("I am sorry, please repeat your order").perform() + rospy.sleep(1.2) + self.nlp_pub.publish("start") + + start_time = time.time() + while not self.callback: + if time.time() - start_time == timeout: + print("customer needs to repeat") + image_switch_publisher.pub_now(ImageEnum.JREPEAT.value) + image_switch_publisher.pub_now(ImageEnum.HI.value) + self.callback = False + if self.response != ["None"]: + tmp_order = self.stnd_msg_to_dict(self.direct_data) + if self.response[0] == "" and tmp_order is not None: + return tmp_order + tries += 1 + + + def get_order(self, customer: CustomerDescription): """ Method to order food if Toya successfully arrived at a customer. @@ -68,8 +98,15 @@ def get_order(self, customer: CustomerDescription): self.callback = False if self.response[0] == "": - if self.response[1].strip() != "None" and self.response[2].strip() is not None: - customer.set_order(self.response[1], self.response[2]) + tmp_order = self.stnd_msg_to_dict(self.direct_data) + print(tmp_order) + if len(tmp_order) != 0: + for ord in tmp_order: + if ord[0] is not None and ord[1] is not None: + customer.set_order(ord[0], ord[1]) + print(customer.order) + else: + self.order_repeat(customer) else: order = False amount = False From 02e8977aea62dc63c8e02c4856c7302ed0325fd6 Mon Sep 17 00:00:00 2001 From: meike Date: Tue, 17 Dec 2024 12:40:41 +0100 Subject: [PATCH 2/2] [MS1-Restaurant] Cleaned code up a little bit and added new setter methods for customer --- .../pycram_hsrb_real_test_demos/restaurant.py | 297 ++---------------- src/pycram/designators/object_designator.py | 33 +- 2 files changed, 47 insertions(+), 283 deletions(-) diff --git a/demos/pycram_hsrb_real_test_demos/restaurant.py b/demos/pycram_hsrb_real_test_demos/restaurant.py index 018e71c0c..c826dfe3b 100644 --- a/demos/pycram_hsrb_real_test_demos/restaurant.py +++ b/demos/pycram_hsrb_real_test_demos/restaurant.py @@ -69,7 +69,7 @@ def monitor_found_waving(): def transform_camera_to_x(pose, frame_x): """ - transforms the pose with given frame_x, orientation will be head ori and z is minu 1.3 + transforms the pose with given frame_x, orientation will be head ori and z is minus 1.3 """ pose.pose.position.z -= 1.3 pose.header.frame_id = "hsrb/" + frame_x @@ -86,7 +86,7 @@ def detect_waving(): """ talk = True global human_pose - #text_to_speech_publisher.pub_now("Please wave you hand. I will come to you", talk) + text_to_speech_publisher.pub_now("Please wave you hand. I will come to you", talk) human_pose = DetectAction(technique='waving', state='start').resolve().perform() @@ -114,7 +114,7 @@ def looperino(): look_point_in_map.z])]).resolve().perform() if human_pose: break - # Executes in paralel and breaks if human bool gets true + # Executes in parallel and breaks if human_pose != None plan = Code(detect_waving) | Code(looperino) plan.perform() @@ -123,305 +123,58 @@ def looperino(): def demo(step): + # Variables for the current customer global customer customerCounter = 0 with real_robot: talk = True start_pose = robot.get_pose() - #TalkingMotion("start demo").perform() + # Saves the original pose to be able to collect the order kitchenPose = start_pose if step <= 0: + # So that Toya can perceive its surrounding without having its limbs in the way config_for_placing = {'arm_lift_joint': -1, 'arm_flex_joint': -0.16, 'arm_roll_joint': -0.0145, 'wrist_flex_joint': -1.417, 'wrist_roll_joint': 0.0} - perceive_conf = {'arm_lift_joint': 0.20, 'wrist_flex_joint': 1.8, 'arm_roll_joint': -1, } pakerino(config=config_for_placing) - # todo: eign müsste sie schräg stehen weil sonst arm in weg und sie muss auf 0.5 hoch das ist nur zum gucken! nicht fahren MoveTorsoAction([0.1]).resolve().perform() ParkArmsAction([Arms.LEFT]).resolve().perform() if step <= 1: - # text_to_speech_publisher.pub_now("Starting Restaurant Demo.", talk) image_switch_publisher.pub_now(ImageEnum.WAVING.value) + # Searching for a human look_around(20, start_pose, talk) + # Preparing to move to detected human MoveTorsoAction([0]).resolve().perform() + # Transformation of detected pose to nearest frame link drive_pose = transform_camera_to_x(human_pose, "head_rgbd_sensor_link") if human_pose is not None: + # Saving important data in customer variable image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) customerCounter += 1 customer = CustomerDescription(customerCounter, drive_pose) marker.publish(Pose.from_pose_stamped(drive_pose), color=[1, 1, 0, 1], name="human_waving_pose") move.pub_now(navpose=drive_pose) - print(customer.pose) - print(customer.id) if step <= 2: # Order step image_switch_publisher.pub_now(ImageEnum.ORDER.value) MoveTorsoAction([0.1]).resolve().perform() TalkingMotion("What do you want to order?").perform() # NLP Placeholder - nlp_restaurant.get_order(customer) - #customer.set_order("Cola",1) - #TalkingMotion(f"You want to order {customer.order[0]} in the following amount {customer.order[1]}").perform() - # if step <= 3: # Drive back step - # image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) - # TalkingMotion("I will drive back now and return with your order").perform() - # MoveTorsoAction([0]).resolve().perform() - # move.pub_now(navpose=kitchenPose) - # TalkingMotion(f"Please prepare the following order for customer {customer.id}. {customer.order[1]} {customer.order[0]}, please").perform() - # # print("I am back") - # if step <= 4: - # # Present the order - # TalkingMotion(f"The customer with the ID {customer.id} wants to order {customer.order[1]} {customer.order[0]}").perform() - # # wait for the cook - # rospy.sleep(1.5) - # move.pub_now(navpose=customer.pose) - # print("demo-done") + customer.set_order("Cola",1) + TalkingMotion(f"You want to order {customer.order[0]} in the following amount {customer.order[1]}").perform() + if step <= 3: # Drive back step + image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) + TalkingMotion("I will drive back now and return with your order").perform() + MoveTorsoAction([0]).resolve().perform() + move.pub_now(navpose=kitchenPose) + TalkingMotion(f"Please prepare the following order for customer {customer.id}. {customer.order[1]} {customer.order[0]}, please").perform() + if step <= 4: + # Present the order + TalkingMotion(f"The customer with the ID {customer.id} wants to order {customer.order[1]} {customer.order[0]}").perform() + # wait for the cook + rospy.sleep(1.5) + move.pub_now(navpose=customer.pose) + print("demo-done") demo(0) -# while not success: -# try: -# look_around(1, start_pose, talk) - -# pose = Pose([new_human_p.point.x, new_human_p.point.y, 0]) -# -# print(type(new_human_p)) -# new_human_p = DetectAction(technique='waving', state='stop').resolve().perform() -# -# if pose is not None: -# human_bool = True -# #human_pose_inM = Pose[(new_human_p.point.x, new_human_p.point.y, new_human_p.z)] -# print(pose) - -# MoveTorsoAction([0]).resolve().perform() -# tf_pose_bl = tf_listener.transformPose( -# "hsrb/" + RobotDescription.current_robot_description.base_link, pose) -# NavigateAction([Pose([tf_pose_bl.pose.position.x - 0.2, tf_pose_bl.pose.position.y - 0.2, 0])]) - - -# print("Human Point", new_human_p) -# print("Human Point in Map", human_point_in_map) -# print("Human Pose in Map", human_pose_in_map) -# print("Human Pose in Bl", human_pose_in_bl) -# print("Toyas Pose in frame", restaurant.toya_pose) - -# if human_pose_in_bl is not None: -# human_bool = True -# NavigateAction(target_locations=[Pose([human_pose_in_map.position.x-0.25, human_pose_in_map.position.y-0.25, 0])]).resolve().perform() -# -# except Exception as e: -# print(e) -# try: -# MoveTorsoAction([0]).resolve().perform() # Damit Toya eingefahren ist bevor sie losfährt -# plan = Code(lambda: rospy.sleep(1)) * 99999999 >> Monitor(monitor_func) -# plan.perform() -# except SensorMonitoringCondition: -# text_to_speech_publisher.pub_now("Finding my way. Please wait.", talk) -# image_switch_publisher.pub_now(ImageEnum.HI.value) -# -# if step <= 2: -# text_to_speech_publisher.pub_now("Driving.", talk) -# image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) -# try: -# plan = Code(lambda: NavigateAction([human_pose_in_map.pose.position.x-0.2, human_pose_in_map.pose.position.y-0.2, 0]).resolve().perform()) >> Monitor(monitor_func) -# plan.perform() -# except SensorMonitoringCondition: -# print("ABBRUCH ALARM ALARM") -# #TODO schauen, ob move interrupt vielleicht schon in einem Designator festgelegt worden ist -# move.interrupt() - -# try: -# plan = Code(lambda: rospy.sleep(1)) * 99999999 >> Monitor(monitor_func) -# plan.perform() -# except SensorMonitoringCondition: -# text_to_speech_publisher.pub_now("Finding my way. Please wait.", talk) -# image_switch_publisher.pub_now(ImageEnum.HI.value) -# -# -# # human_poseTm = transform_pose(human_pose, "head_rgbd_sensor_rgb_frame", "map") -# human_p = human_poseTm -# human_p.pose.position.z = 0 -# human_p.pose.orientation.x = 0 -# human_p.pose.orientation.y = 0 -# human_p.pose.orientation.z = 0 -# # restaurant.human_pose = None -# human_pose = robokudo.query_waving_human() -# human_poseTm = transform_pose(human_pose, "head_rgbd_sensor_rgb_frame", "map") -# human_p = human_poseTm -# human_p.pose.position.z = 0 -# human_p.pose.orientation.x = 0 -# human_p.pose.orientation.y = 0 -# human_p.pose.orientation.z = 0 -# human_p.pose.orientation.w = 1 -# human_p.pose.orientation.w = 1 -# -# if human_p: -# success = True -# restaurant.human_pose = human_p -# # print(human_p.pose.position.x) - - -# except Exception as e: -# print("An error occurred from perception:", e) -# if step <= 2: -# text_to_speech_publisher.pub_now("Driving", talk) -# image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) -# world.current_bullet_world.add_vis_axis(Pose.from_pose_stamped(human_p)) -# #print(restaurant.distance()) -# marker.publish(Pose.from_pose_stamped(human_p)) -# world.current_bullet_world.add_vis_axis(Pose.from_pose_stamped(human_p)) -# try: -# plan = Code(lambda: move.pub_now(human_p)) >> Monitor(monitor_func) -# plan.perform() -# except SensorMonitoringCondition: -# print("ABBRUCH") -# move.interrupt() -# # robot_pose_to_human = Pose.from_pose_stamped(calc_distance(human_p, 0.5)) - -# world.current_bullet_world.add_vis_axis(robot_pose_to_human) -# move.query_pose_nav(robot_pose_to_human) -# image_switch_publisher.pub_now(ImageEnum.ORDER.value) -# text_to_speech_publisher.pub_now("Hi what do you want to have.", talk) -# # nlp stuff -# if step <= 3: -# text_to_speech_publisher.pub_now("Driving", talk) -# image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) -# move.query_pose_nav(start_pose) -# # nlp stuff insert here -# image_switch_publisher.pub_now(ImageEnum.HANDOVER.value) -# text_to_speech_publisher.pub_now("The guest wants: apple, milk", talk) -# -# if step <= 4: -# text_to_speech_publisher.pub_now("Driving", talk) -# image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) -# move.query_pose_nav(robot_pose_to_human) -# # nlp stuff insert here -# image_switch_publisher.pub_now(ImageEnum.HANDOVER.value) -# text_to_speech_publisher.pub_now("Take out your order", talk) -# -# if step <= 5: -# text_to_speech_publisher.pub_now("Driving", talk) -# image_switch_publisher.pub_now(ImageEnum.DRIVINGBACK.value) -# move.query_pose_nav(start_pose) -# demo(step=1) - - -# human_pose = detect_waving(talk) -# if not human_pose: -# # rospy.sleep(1) - -# for w in range(10, -10, -increase): -# human_pose = detect_waving(talk) -# if not human_pose: -# angle = w / 10 -# look_pose = Pose([tmp_x, tmp_y, tmp_z], -# frame="hsrb/" + RobotDescription.current_robot_description.base_link) -# look_pose_in_map = tf_listener.transformPose("/map", look_pose) -# look_point_in_map = look_pose_in_map.pose.position -# -# LookAtAction( -# [Pose([look_point_in_map.x + angle, look_point_in_map.y + angle, -# look_point_in_map.z])]).resolve().perform() -# # rospy.sleep(1) -# - -# -# def monitor_func_human(): -# der = fts.get_last_value() -# if abs(der.wrench.force.x) > 10.30: -# print("sensor") -# return SensorMonitoringCondition -# if not human.human_pose.get_value(): -# print("human") -# return HumanNotFoundCondition -# return False -# -# -# def monitor_func(): -# der = fts.get_last_value() -# if abs(der.wrench.force.x) > 10.30: -# return SensorMonitoringCondition -# return False -# -# -# def callback(point): -# global human_bool -# human_bool = True -# print("Human detected") - -# class Human: -# """ -# Class that represents humans. This class does not spawn a human in a simulation. -# """ -# -# def __init__(self): -# self.human_pose = Fluent() -# -# self.last_msg_time = time.time() -# -# self.threshold = 5.0 # seconds -# -# # Subscriber to the human pose topic -# self.human_pose_sub = rospy.Subscriber("/cml_human_pose", PointStamped, self.human_pose_cb) -# -# # Timer to check for no message -# self.timer = rospy.Timer(rospy.Duration(1), self.check_for_no_message) -# -# def check_for_no_message(self, event): -# current_time = time.time() -# if (current_time - self.last_msg_time) > self.threshold: -# # rospy.loginfo("No messages received for %s seconds", self.threshold) -# self.human_pose.set_value(False) -# -# 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.last_msg_time = time.time() -# -# if HumanPoseMsg: -# self.human_pose.set_value(True) -# else: -# self.human_pose.set_value(False) -# -# -# class Restaurant: -# def __init__(self): -# self.toya_pose = Fluent() -# self.human_pose = None -# self.toya_pose_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.toya_pose_cb) -# self.robot = robot -# self.rospy = rospy -# -# def toya_pose_cb(self, msg): -# # print("updating") -# self.toya_pose.set_value(robot.get_pose()) -# self.rospy.sleep(0.5) -# -# def set_human_pose(self, pose: Pose): -# self.human_pose = pose -# -# def distance(self): -# print("toya pose:" + str(self.toya_pose.get_value().pose)) -# if self.human_pose: -# dis = math.sqrt((self.human_pose.pose.position.x - self.toya_pose.get_value().pose.position.x) ** 2 + -# (self.human_pose.pose.position.y - self.toya_pose.get_value().pose.position.y) ** 2) -# print("dis: " + str(dis)) -# return dis -# else: -# self.rospy.logerr("Cant calculate distance, no human pose found") -# -# -# def monitor_func(restaurant: Restaurant): -# if restaurant.distance() < 1: -# return SensorMonitoringCondition -# return False -# - -# restaurant = Restaurant() - -# def human_pose_cb(data): -# query_result = data -# return data \ No newline at end of file diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 87fd85dfc..8228ab781 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -250,19 +250,30 @@ def set_attributes(self, attribute_list): self.attributes = attribute_list -class CustomerDiscription(HumanDescription): - def __init__(self, id: int, pose: Pose): - self.id = id - self.pose = pose - - - def get_pose(self): - return self.pose +class CustomerDescription(HumanDescription): + def __init__(self, id: int, pose: Pose, order: Optional = None): + super(CustomerDescription, self).__init__(id, pose) + self.order = order def set_pose(self, new_pose): + """ + Function for setting the pose of the customer. + :param new_pose: new pose of customer + """ self.pose = new_pose - print(self.pose) - def get_id(self): - return id + + def set_id(self, new_id): + """ + Function for changing id of customer. + :param new_id: new id of customer + """ self.id = new_id + + def set_order(self, item:str, amount: int): + """ + Function for changing order of customer. + :param item: The desired item to order + :param amount: The amount of order + """ + self.order = (item, amount) \ No newline at end of file