From 021468ab02bd6d6e717e6400c8f92576062d7e5d Mon Sep 17 00:00:00 2001 From: HaSu Date: Sat, 13 Jul 2024 12:12:45 +0200 Subject: [PATCH 1/8] [GPSR] can now also bring stuff to me --- demos/pycram_gpsr_demo/high_level_plans.py | 71 +++++++++++++++++----- demos/pycram_gpsr_demo/nlp_processing.py | 19 ++---- demos/pycram_gpsr_demo/run_demo.py | 39 +++++++++++- demos/pycram_gpsr_demo/setup_demo.py | 3 +- demos/pycram_gpsr_demo/utils.py | 32 ++++++++++ 5 files changed, 130 insertions(+), 34 deletions(-) diff --git a/demos/pycram_gpsr_demo/high_level_plans.py b/demos/pycram_gpsr_demo/high_level_plans.py index 886d19790..38be8d87a 100755 --- a/demos/pycram_gpsr_demo/high_level_plans.py +++ b/demos/pycram_gpsr_demo/high_level_plans.py @@ -12,9 +12,11 @@ from demos.pycram_gpsr_demo import nlp_processing as nlp from stringcase import snakecase from demos.pycram_gpsr_demo import perc_to_know - +from pycram.pose import Pose as PoseStamped object_in_hand = None +me_pose = None + # these are all the high level plans, to which we map the NLP output. # they should either connect to low level plans or be filled with data from knowledge @@ -91,7 +93,8 @@ def moving_to(param_json): # WIP Can also be funriture, or a person elif furniture_class: if furniture == furniture_class: # matched knowledge and nlp - nav_poses = knowrob.get_nav_poses_for_furniture_item(furniture_name=furniture, furniture_iri=furniture) # change might get removed + nav_poses = knowrob.get_nav_poses_for_furniture_item(furniture_name=furniture, + furniture_iri=furniture) # change might get removed else: nav_poses = knowrob.get_nav_poses_for_furniture_item(furniture_iri=furniture_class) # go to person at furniture item TODO ---------------------------------------------------------- @@ -115,13 +118,15 @@ def moving_to(param_json): # WIP Can also be funriture, or a person elif furniture_class: # maybeh make this an and? if furniture == furniture_class: # matched knowledge and nlp - nav_poses = knowrob.get_nav_poses_for_furniture_item(room=room, furniture_name=furniture) # change might get removed + nav_poses = knowrob.get_nav_poses_for_furniture_item(room=room, + furniture_name=furniture) # change might get removed else: nav_poses = knowrob.get_nav_poses_for_furniture_item(room=room, furniture_iri=furniture_class) # go to furniture item in room if nav_poses is not None and nav_poses != []: # go to furniture item - rospy.loginfo(utils.PC.BLUE + f"[CRAM] going to furniture item in Room pose {nav_poses[0].get('Item').get('pose')}") + rospy.loginfo( + utils.PC.BLUE + f"[CRAM] going to furniture item in Room pose {nav_poses[0].get('Item').get('pose')}") result = navi.go_to_pose(nav_poses[0].get('Item').get('pose')) rospy.loginfo(utils.PC.BLUE + "[CRAM] result: " + str(result)) return nav_poses[0] @@ -147,7 +152,8 @@ def moving_to(param_json): # WIP Can also be funriture, or a person return nav_poses[0] # needed for perception else: - rospy.loginfo(utils.PC.BLUE + "[CRAM] could not find furniture item " + str(param_json.get('Destination').get('value'))) + rospy.loginfo(utils.PC.BLUE + "[CRAM] could not find furniture item " + str( + param_json.get('Destination').get('value'))) sing_my_angel_of_music("I am sorry. I don't know where that is.") return None # abort mission @@ -160,6 +166,15 @@ def moving_to(param_json): # WIP Can also be funriture, or a person rospy.loginfo(utils.PC.BLUE + "[CRAM] result: " + str(result)) return result # fallback + elif person: + if person.get('value') == 'me' and me_pose is not None: + sing_my_angel_of_music("I am going to you.") + rospy.loginfo(utils.PC.BLUE + "[CRAM] moving to me") + # go to me + result = navi.go_to_pose(me_pose) + rospy.loginfo(utils.PC.BLUE + "[CRAM] result: " + str(result)) + return result + else: sing_my_angel_of_music(utils.PC.BLUE + f"I am sorry. I don't know where that is.") rospy.logerr("[CRAM]: MovingTo plan failed. No valid parameters found.") @@ -184,7 +199,7 @@ def looking_for(param_json): # WIP # subplan of fetching and transporting -def pick_up(param_json): # works testing +def pick_up(param_json): # works testing # TODO add obj to knowrob? global object_in_hand item, source, source_room, look_at_link, look_at_pose = None, None, None, None, None @@ -221,8 +236,10 @@ def pick_up(param_json): # works testing while pick_up_retries > 0 and found_object is None: if item is not None and look_at_link is not None and grasped_bool is None: try: - gasped_bool, grasp, found_object = plans.process_pick_up_objects(obj_type=item, obj_types_dict=obj_types_dict, - link=utils.remove_prefix(look_at_link, 'iai_kitchen/'), + gasped_bool, grasp, found_object = plans.process_pick_up_objects(obj_type=item, + obj_types_dict=obj_types_dict, + link=utils.remove_prefix(look_at_link, + 'iai_kitchen/'), look_pose_given=look_at_pose, environment_raw=sd.environment_raw, giskardpy=sd.giskard, @@ -254,7 +271,7 @@ def pick_up(param_json): # works testing return None -def placing(param_json): # works testing +def placing(param_json): # works testing global object_in_hand destination_link = None if not object_in_hand: @@ -284,8 +301,9 @@ def placing(param_json): # works testing gripper=sd.gripper, world=sd.world, robot_desig=sd.robot_desig) - +# deprecated -> transporting def fetching(param_json): + global me_pose # go to a target location, pick up object, bring it back sing_my_angel_of_music("in fetching plan") rospy.loginfo("fetching: " + str(param_json)) @@ -296,14 +314,16 @@ def fetching(param_json): if param_json.get('BeneficiaryRole').get('value') == 'me': # or part of a list of names? # save current pose me_pose = utils.tf_l.lookupTransform(target_frame='map', source_frame='base_link', - time=rospy.get_time()) + time=rospy.get_time()) # alternatively, try to memorize the human person from perception? ... uff person = 'you' # go to the destination to pick the obj # --- Step 1 --- # get the pose of 'PhysicalPlace' = 'Room' (if available) CHANGE (Potentially) - if param_json.get('Destination').get('entity') == 'PhysicalPlace': - physical_place = param_json.get('Destination').get('value') # CHANGE this actually should be Origin + pick_up(param_json) + navi.go_to_pose(me_pose) + sing_my_angel_of_music("Here is your object. Please take it from me in 3.. 2.. 1.") + sd.gripper('open') # perceive obj # TODO WIP @@ -316,8 +336,28 @@ def cleaning(param_json): def transporting(param_json): sing_my_angel_of_music("in transporting plan") - pick_up(param_json) - placing(param_json) + global me_pose + person = None + if param_json.get('BeneficiaryRole').get('value') == 'me': # TODO handle other people too + # save current pose + rospy.loginfo("[CRAM] tf lookup: ") + # TODO wrap this to avoid extraplotation errors + me_pose = utils.lookup_transform(target_frame='map', source_frame='base_link', tf_listener=utils.tf_l) + me_pose = PoseStamped(me_pose[0], me_pose[1]) + rospy.loginfo("[CRAM] me pose: " + str(me_pose)) + # alternatively, try to memorize the human person from perception? ... uff + person = 'me' + # pick up item + result = pick_up(param_json) + rospy.loginfo("[CRAM] pick up result: " + str(result)) + if result is None: + return None + if person == 'me': + navi.go_to_pose(me_pose) + sing_my_angel_of_music("Here is your object. Please take it from me in 3.. 2.. 1.") + sd.gripper.pub_now('open') + else: + placing(param_json) def arranging(param_json): @@ -445,4 +485,3 @@ def prepare_for_commands(): # TODO this should be looped for multiple tasks # go to initial point move.query_pose_nav(instruction_point) - diff --git a/demos/pycram_gpsr_demo/nlp_processing.py b/demos/pycram_gpsr_demo/nlp_processing.py index d6f521045..3aa3c3851 100644 --- a/demos/pycram_gpsr_demo/nlp_processing.py +++ b/demos/pycram_gpsr_demo/nlp_processing.py @@ -18,27 +18,16 @@ currentSpeech = "" stoppedSpeaking = Condition() canSpeak = True -canListen = True +canListen = False canDisplay = False -# might be deprecated? -def data_cb(data): - global response - global callback - global doorbell - - rospy.loginfo("in NLP callback") - response = data.data.split(",") - response.append("None") - callback = True - def what_am_i_saying(msg): global currentSpeech if ("" != currentSpeech) and ("" == msg.data): with stoppedSpeaking: stoppedSpeaking.notify_all() - currentSpeech=msg.data + currentSpeech = msg.data def sing_my_angel_of_music(text): @@ -75,6 +64,7 @@ def intent_processing(msg): #if response["intent"] in ["Agreement", "Disagreement"]: # return confirm + # create a subscriber to the /nlp_out topic on which the result from NLP is published def nlp_subscribe(): global nlp_sub, talk_sub @@ -146,7 +136,7 @@ def confirm_nlp_output(received_output): # --- THIS IS THE MAIN FUNCTION FOR NLP --- def listen_to_commands(): - nlp_subscribe() + #nlp_subscribe() done during setup global response, todo_plans while True: if canDisplay: @@ -161,4 +151,3 @@ def listen_to_commands(): rospy.loginfo("[CRAM] do stuff") nlp_unsubscribe() return todo_plans - diff --git a/demos/pycram_gpsr_demo/run_demo.py b/demos/pycram_gpsr_demo/run_demo.py index ca307fb16..9b8af8c17 100644 --- a/demos/pycram_gpsr_demo/run_demo.py +++ b/demos/pycram_gpsr_demo/run_demo.py @@ -73,6 +73,41 @@ def gpsr(): } } +data2 = { + "sentence": " Bring me the cup from the living room table .", + "intent": "Transporting", + "BeneficiaryRole": { + "value": "me", + "entity": "NaturalPerson", + "propertyAttribute": [], + "actionAttribute": [], + "numberAttribute": [] + }, + "Item": { + "value": "cup", + "entity": "Transportable", + "propertyAttribute": [], + "actionAttribute": [], + "numberAttribute": [] + }, + "SourceRoom": { + "value": "living room", + "entity": "Room", + "propertyAttribute": [], + "actionAttribute": [], + "numberAttribute": [] + }, + "Source": { + "value": "couch table", + "entity": "DesignedFurniture", + "propertyAttribute": [], + "actionAttribute": [], + "numberAttribute": [] + } +} + + + # CHANGE CARE THIS STUFF GETS ACTUALLY EXECUTED def demo_plan(data): @@ -83,6 +118,6 @@ def demo_plan(data): setup() -gpsr() -#demo_plan(data) +#gpsr() +demo_plan(data2) #setup_demo.gripper.pub_now('open') diff --git a/demos/pycram_gpsr_demo/setup_demo.py b/demos/pycram_gpsr_demo/setup_demo.py index f12dfea5a..9b4365268 100755 --- a/demos/pycram_gpsr_demo/setup_demo.py +++ b/demos/pycram_gpsr_demo/setup_demo.py @@ -76,8 +76,9 @@ def setup(): giskard.clear() giskard.sync_worlds() + # this is already done in listen to commands if nlp_processing.canSpeak: - nlp_processing.nlp_subscribe() + nlp_processing.nlp_subscribe() rospy.loginfo(utils.PC.GREEN + "[CRAM] done with setup") print("###################################################################") diff --git a/demos/pycram_gpsr_demo/utils.py b/demos/pycram_gpsr_demo/utils.py index 84d52fd32..f7ed73a61 100755 --- a/demos/pycram_gpsr_demo/utils.py +++ b/demos/pycram_gpsr_demo/utils.py @@ -113,6 +113,38 @@ def transform_pose(tf_listener, target_frame, pose_stamped, max_attempts=10, att return None +def lookup_transform(tf_listener, target_frame, source_frame, max_attempts=10, attempt_delay=1): + """ + Attempts to transform a pose to the specified target frame with retry logic. + + Args: + tf_listener (tf.TransformListener): The TransformListener object. + target_frame (str): The target frame to transform the pose into. + pose_stamped (PoseStamped): The pose to transform. + max_attempts (int): Maximum number of attempts to transform the pose. + attempt_delay (int or float): Delay between attempts in seconds. + + Returns: + PoseStamped or None: The transformed pose if successful, None otherwise. + """ + attempt_count = 0 + while attempt_count < max_attempts: + try: + if not tf_listener.canTransform(target_frame, source_frame, rospy.Time(0)): + rospy.logwarn("[TF]Transform not available. Attempt {} of {}".format(attempt_count + 1, max_attempts)) + rospy.sleep(attempt_delay) + else: + rospy.loginfo(f"[TF]Transform available. Attempting transformation to {target_frame}.") + transformed_pose = tf_listener.lookupTransform(target_frame, source_frame, rospy.Time(0)) + return transformed_pose + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: + rospy.logerr("[TF]An error occurred during transformation attempt {}: {}".format(attempt_count + 1, e)) + rospy.sleep(attempt_delay) + attempt_count += 1 + rospy.logerr("Failed to transform pose after {} attempts.".format(max_attempts)) + return None + + def knowrob_poses_result_to_list_dict(knowrob_output, nav_or_perc='nav'): # works global tf_l poses_list = [] From d298d86a8294f66b67cf39241df0bd2fbff37517 Mon Sep 17 00:00:00 2001 From: HaSu Date: Mon, 15 Jul 2024 18:00:56 +0200 Subject: [PATCH 2/8] [GPSR] adding new obj to lists for robocup + looking for plan WIP --- demos/pycram_gpsr_demo/high_level_plans.py | 130 +++++++++++------- demos/pycram_gpsr_demo/objects.py | 31 ++++- .../pycram_gpsr_demo/perception_to_knowrob.py | 29 +++- src/pycram/utilities/gpsr_utils.py | 1 + 4 files changed, 135 insertions(+), 56 deletions(-) diff --git a/demos/pycram_gpsr_demo/high_level_plans.py b/demos/pycram_gpsr_demo/high_level_plans.py index 38be8d87a..c6dccff73 100755 --- a/demos/pycram_gpsr_demo/high_level_plans.py +++ b/demos/pycram_gpsr_demo/high_level_plans.py @@ -181,23 +181,6 @@ def moving_to(param_json): # WIP Can also be funriture, or a person return None -# also finding + searching -def looking_for(param_json): # WIP - sing_my_angel_of_music("in looking for plan") - rospy.loginfo("Looking For: " + str(param_json)) - physical_place, physical_artifact = None, None - # step 0: go to the requested room - if it was mentioned explicitly OR - if param_json.get('Location') and param_json.get('Location').get('entity') == 'PhysicalPlace': - physical_place = snakecase(param_json.get('Location').get('value')) - if param_json.get('Destination') and param_json.get('Destination').get('entity') == 'PhysicalArtifact': - physical_artifact = param_json.get('Destination').get('value') - # make msg for perception - rk_msg = perception_interface.make_robokudo_obj_msg(param_json.get('Item')) - # get navigation pose from knowrob depending on what info is known - - # step 1: go to the furniture/surface item that got mentioned and look on it for the specified obj - - # subplan of fetching and transporting def pick_up(param_json): # works testing # TODO add obj to knowrob? @@ -212,11 +195,11 @@ def pick_up(param_json): # works testing source_params = utils.remap_source_to_destination(source_params) result = moving_to(source_params) rospy.loginfo(utils.PC.BLUE + "[CRAM] Navigation done. result: " + str(result)) - #get values + # get values # get position of where robot should look at if param_json.get('Item') is not None: item = param_json.get('Item').get('value') - #item = 'cup_small' # CHANGE add fancy filtering + # item = 'cup_small' # CHANGE add fancy filtering if param_json.get('Source') is not None: source = param_json.get('Source').get('value') if param_json.get('SourceRoom') is not None: @@ -295,43 +278,52 @@ def placing(param_json): # works testing item_location = knowrob.get_predefined_destination_item_location(knowrob_iri) destination_link = utils.remove_prefix(item_location.get('Item').get('link'), 'iai_kitchen/') - plans.place(object=object_in_hand, grasp='front', link=destination_link, giskard=sd.giskard, talk=nlp.tts, - robot_description=sd.robot_description, lt=sd.lt, environment_raw=sd.environment_raw, - environment_desig=sd.environment_desig, - gripper=sd.gripper, world=sd.world, robot_desig=sd.robot_desig) + result = plans.place(object=object_in_hand, grasp='front', link=destination_link, giskard=sd.giskard, talk=nlp.tts, + robot_description=sd.robot_description, lt=sd.lt, environment_raw=sd.environment_raw, + environment_desig=sd.environment_desig, + gripper=sd.gripper, world=sd.world, robot_desig=sd.robot_desig) + if result: + sing_my_angel_of_music("done with placing") + object_in_hand = None + return True -# deprecated -> transporting -def fetching(param_json): - global me_pose - # go to a target location, pick up object, bring it back - sing_my_angel_of_music("in fetching plan") - rospy.loginfo("fetching: " + str(param_json)) - # BeneficiaryRole: target - if param_json.get('BeneficiaryRole').get('entity') == 'NaturalPerson': - # Goal is natural pearson. Means we need to HRI - # if person is 'me', save current pose and person name - if param_json.get('BeneficiaryRole').get('value') == 'me': # or part of a list of names? - # save current pose - me_pose = utils.tf_l.lookupTransform(target_frame='map', source_frame='base_link', - time=rospy.get_time()) - # alternatively, try to memorize the human person from perception? ... uff - person = 'you' - # go to the destination to pick the obj - # --- Step 1 --- - # get the pose of 'PhysicalPlace' = 'Room' (if available) CHANGE (Potentially) - pick_up(param_json) - navi.go_to_pose(me_pose) - sing_my_angel_of_music("Here is your object. Please take it from me in 3.. 2.. 1.") - sd.gripper('open') - - # perceive obj - # TODO WIP +# also finding + searching +# Source +def looking_for(param_json): # WIP TODO + sing_my_angel_of_music("in looking for plan") + rospy.loginfo("Looking For: " + str(param_json)) + # get vars + item, person, furniture, room = None, None, None, None + if param_json.get('Source') is not None: + # ensure furniture obj exists + furniture = param_json.get('Source').get('value').lower() + if knowrob.check_existence_of_instance(snakecase(furniture)): + rospy.loginfo(f"[CRAM] found instance of furniture item " + furniture) + furniture = snakecase(furniture) + # if instance does not exist,check if class does ? + elif knowrob.check_existence_of_class(furniture): + rospy.loginfo("[CRAM] found class of furniture item " + furniture) + furniture_class = knowrob.check_existence_of_class(furniture)[0].get('Class') + furniture = False + elif utils.obj_dict.get(furniture) is not None: + furniture = utils.obj_dict.get(furniture) + rospy.loginfo("[CRAM] Fallback: of furniture item found in list. item : " + furniture) + else: + rospy.loginfo("[CRAM] could not find furniture item " + furniture) + sing_my_angel_of_music(f"I am sorry. I don't know where {furniture} is.") + return None + + if param_json.get('SourceRoom') is not None: + # ensure room exists + if knowrob.rooms.get(snakecase(param_json.get('SourceRoom').get('value').lower())) is not None: + room = param_json.get('SourceRoom').get('value').lower() + else: + room = 'arena' -def cleaning(param_json): - sing_my_angel_of_music("in cleaning plan") - rospy.loginfo("cleaning: " + str(param_json)) + if param_json.get('Item') is not None: + item = param_json.get('Item').get('value') def transporting(param_json): @@ -341,7 +333,7 @@ def transporting(param_json): if param_json.get('BeneficiaryRole').get('value') == 'me': # TODO handle other people too # save current pose rospy.loginfo("[CRAM] tf lookup: ") - # TODO wrap this to avoid extraplotation errors + me_pose = utils.lookup_transform(target_frame='map', source_frame='base_link', tf_listener=utils.tf_l) me_pose = PoseStamped(me_pose[0], me_pose[1]) rospy.loginfo("[CRAM] me pose: " + str(me_pose)) @@ -371,6 +363,11 @@ def count(param_json): rospy.loginfo("count: " + str(param_json)) +def cleaning(param_json): + sing_my_angel_of_music("in cleaning plan") + rospy.loginfo("cleaning: " + str(param_json)) + + def guide(param_json): sing_my_angel_of_music("in guiding plan") rospy.loginfo("guide: " + str(param_json)) @@ -485,3 +482,30 @@ def prepare_for_commands(): # TODO this should be looped for multiple tasks # go to initial point move.query_pose_nav(instruction_point) + +# DEPRECTAED --------------------------------------------------------------------------------------- +# deprecated -> transporting +# def fetching(param_json): +# global me_pose +# # go to a target location, pick up object, bring it back +# sing_my_angel_of_music("in fetching plan") +# rospy.loginfo("fetching: " + str(param_json)) +# # BeneficiaryRole: target +# if param_json.get('BeneficiaryRole').get('entity') == 'NaturalPerson': +# # Goal is natural pearson. Means we need to HRI +# # if person is 'me', save current pose and person name +# if param_json.get('BeneficiaryRole').get('value') == 'me': # or part of a list of names? +# # save current pose +# me_pose = utils.tf_l.lookupTransform(target_frame='map', source_frame='base_link', +# time=rospy.get_time()) +# # alternatively, try to memorize the human person from perception? ... uff +# person = 'you' +# # go to the destination to pick the obj +# # --- Step 1 --- +# # get the pose of 'PhysicalPlace' = 'Room' (if available) CHANGE (Potentially) +# pick_up(param_json) +# navi.go_to_pose(me_pose) +# sing_my_angel_of_music("Here is your object. Please take it from me in 3.. 2.. 1.") +# sd.gripper('open') +# +# # perceive obj diff --git a/demos/pycram_gpsr_demo/objects.py b/demos/pycram_gpsr_demo/objects.py index 8dc41e73a..1df887712 100644 --- a/demos/pycram_gpsr_demo/objects.py +++ b/demos/pycram_gpsr_demo/objects.py @@ -33,7 +33,7 @@ golf_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#GolfBall' soft_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#SoftBall' salt_shaker = 'http://www.ease-crc.org/ont/SOMA.owl#SaltShaker' -corny_box = 'http://www.ease-crc.org/ont/SUTURO.owl#CornyBox' +metal_plate = 'http://www.ease-crc.org/ont/SUTURO.owl#MetalPlate' pringles_chips_can = 'http://www.ease-crc.org/ont/SUTURO.owl#PringlesChipsCan' strawberry = 'http://www.ease-crc.org/ont/SUTURO.owl#Strawberry' glass_cleaner_spray_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#GlassCleanerSprayBottle' @@ -70,7 +70,7 @@ bag = 'http://www.ease-crc.org/ont/SUTURO.owl#Bag' juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox' pancake_mix_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#PancakeMixBottle' -jell_ostrawberry_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JellOStrawberryBox' +gelatine_box = 'http://www.ease-crc.org/ont/SUTURO.owl#GelatineBox' tea_bag_box = 'http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox' pastry_box = 'http://www.ease-crc.org/ont/SUTURO.owl#PastryBox' pear = 'http://www.ease-crc.org/ont/SUTURO.owl#Pear' @@ -92,11 +92,38 @@ milk_pack_ja = 'http://www.ease-crc.org/ont/SUTURO.owl#MilkPackJa' muesli_box = 'http://www.ease-crc.org/ont/SUTURO.owl#MuesliBox' stapel_chips = 'http://www.ease-crc.org/ont/SUTURO.owl#StapelChips' +cup_blue = 'http://www.ease-crc.org/ont/SUTURO.owl#CupBlue' +cup_green = 'http://www.ease-crc.org/ont/SUTURO.owl#CupGreen' large_marker = 'http://www.ease-crc.org/ont/SUTURO.owl#LargeMarker' +masterchef_can = 'http://www.ease-crc.org/ont/SUTURO.owl#MasterchefCan' scissors = 'http://www.ease-crc.org/ont/SUTURO.owl#Scissors' scrub_cleaner = 'http://www.ease-crc.org/ont/SUTURO.owl#ScrubCleaner' grapes = 'http://www.ease-crc.org/ont/SUTURO.owl#Grapes' +cup_small = 'http://www.ease-crc.org/ont/SUTURO.owl#CupSmall' screw_driver = 'http://www.ease-crc.org/ont/SUTURO.owl#ScrewDriver' clamp = 'http://www.ease-crc.org/ont/SUTURO.owl#Clamp' hammer = 'http://www.ease-crc.org/ont/SUTURO.owl#Hammer' wood_block = 'http://www.ease-crc.org/ont/SUTURO.owl#WoodBlock' +corny_box = 'http://www.ease-crc.org/ont/SUTURO.owl#CornyBox' +sponge = 'http://www.ease-crc.org/ont/SUTURO.owl#Sponge' +wash_cloth = 'http://www.ease-crc.org/ont/SUTURO.owl#WashCloth' +soap = 'http://www.ease-crc.org/ont/SUTURO.owl#Soap' +fanta_can = 'http://www.ease-crc.org/ont/SUTURO.owl#FantaCan' +ice_tea_can = 'http://www.ease-crc.org/ont/SUTURO.owl#IceTeaCan' +dubbel_fris = 'http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris' +water_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#WaterBottle' +cola_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#ColaBottle' +pea_soup_can = 'http://www.ease-crc.org/ont/SUTURO.owl#PeaSoupCan' +cornflakes = 'http://www.ease-crc.org/ont/SUTURO.owl#Cornflakes' +curry = 'http://www.ease-crc.org/ont/SUTURO.owl#Curry' +mayonaise = 'http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise' +hagelslag = 'http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag' +sausages = 'http://www.ease-crc.org/ont/SUTURO.owl#Sausages' +pancake_mix = 'http://www.ease-crc.org/ont/SUTURO.owl#PancakeMix' +crisps_bag = 'http://www.ease-crc.org/ont/SUTURO.owl#CrispsBag' +candy = 'http://www.ease-crc.org/ont/SUTURO.owl#Candy' +stroopwafels = 'http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels' +liquorice = 'http://www.ease-crc.org/ont/SUTURO.owl#Liquorice' +tic_tac = 'http://www.ease-crc.org/ont/SUTURO.owl#TicTac' +candle = 'http://www.ease-crc.org/ont/SUTURO.owl#Candle' +grocery_bag = 'http://www.ease-crc.org/ont/SUTURO.owl#GroceryBag' diff --git a/demos/pycram_gpsr_demo/perception_to_knowrob.py b/demos/pycram_gpsr_demo/perception_to_knowrob.py index e2dd8d478..a78f31879 100644 --- a/demos/pycram_gpsr_demo/perception_to_knowrob.py +++ b/demos/pycram_gpsr_demo/perception_to_knowrob.py @@ -9,16 +9,25 @@ "Bleachcleanserbottle": "http://www.ease-crc.org/ont/SUTURO.owl#BleachCleanserBottle", "Bottle": "http://www.ease-crc.org/ont/SOMA.owl#Bottle", "Bowl": "http://www.ease-crc.org/ont/SOMA.owl#Bowl", + "Candle": "http://www.ease-crc.org/ont/SUTURO.owl#Candle", + "Candy": "http://www.ease-crc.org/ont/SUTURO.owl#Candy", "Cerealbox": "http://www.ease-crc.org/ont/SOMA.owl#CerealBox", "Cerealboxrobocup": "http://www.ease-crc.org/ont/SUTURO.owl#CerealBoxRoboCup", "Clamp": "http://www.ease-crc.org/ont/SUTURO.owl#Clamp", "Coffeecan": "http://www.ease-crc.org/ont/SUTURO.owl#CoffeeCan", "Coffeepack": "http://www.ease-crc.org/ont/SUTURO.owl#CoffeePack", + "Colabottle": "http://www.ease-crc.org/ont/SUTURO.owl#ColaBottle", "Colacan": "http://www.ease-crc.org/ont/SUTURO.owl#ColaCan", + "Cornflakes": "http://www.ease-crc.org/ont/SUTURO.owl#Cornflakes", "Cornybox": "http://www.ease-crc.org/ont/SUTURO.owl#CornyBox", "Crackerbox": "http://www.ease-crc.org/ont/SUTURO.owl#CrackerBox", + "Crispsbag": "http://www.ease-crc.org/ont/SUTURO.owl#CrispsBag", "Cup": "http://www.ease-crc.org/ont/SOMA.owl#Cup", + "Cupblue": "http://www.ease-crc.org/ont/SUTURO.owl#CupBlue", "Cupboard": "http://www.ease-crc.org/ont/SOMA.owl#Cupboard", + "Cupgreen": "http://www.ease-crc.org/ont/SUTURO.owl#CupGreen", + "Cupsmall": "http://www.ease-crc.org/ont/SUTURO.owl#CupSmall", + "Curry": "http://www.ease-crc.org/ont/SUTURO.owl#Curry", "Designedchair": "http://www.ease-crc.org/ont/SOMA.owl#DesignedChair", "Designedhandle": "http://www.ease-crc.org/ont/SOMA.owl#DesignedHandle", "Dice": "http://www.ease-crc.org/ont/SUTURO.owl#Dice", @@ -26,17 +35,22 @@ "Dishwashertab": "http://www.ease-crc.org/ont/SOMA.owl#DishwasherTab", "Door": "http://www.ease-crc.org/ont/SOMA.owl#Door", "Drawer": "http://www.ease-crc.org/ont/SOMA.owl#Drawer", + "Dubbelfris": "http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris", + "Fantacan": "http://www.ease-crc.org/ont/SUTURO.owl#FantaCan", "Foambrick": "http://www.ease-crc.org/ont/SUTURO.owl#FoamBrick", "Fork": "http://www.ease-crc.org/ont/SOMA.owl#Fork", + "Gelatinebox": "http://www.ease-crc.org/ont/SUTURO.owl#GelatineBox", "Glass": "http://www.ease-crc.org/ont/SOMA.owl#Glass", "Glasscleanerspraybottle": "http://www.ease-crc.org/ont/SUTURO.owl#GlassCleanerSprayBottle", "Golfball": "http://www.ease-crc.org/ont/SUTURO.owl#GolfBall", "Grapes": "http://www.ease-crc.org/ont/SUTURO.owl#Grapes", + "Grocerybag": "http://www.ease-crc.org/ont/SUTURO.owl#GroceryBag", + "Hagelslag": "http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag", "Hammer": "http://www.ease-crc.org/ont/SUTURO.owl#Hammer", "Iceteabottle": "http://www.ease-crc.org/ont/SUTURO.owl#IceTeaBottle", + "Iceteacan": "http://www.ease-crc.org/ont/SUTURO.owl#IceTeaCan", "Jellobox": "http://www.ease-crc.org/ont/SUTURO.owl#JelloBox", "Jellochocolatepuddingbox": "http://www.ease-crc.org/ont/SUTURO.owl#JellOChocolatePuddingBox", - "Jellostrawberrybox": "http://www.ease-crc.org/ont/SUTURO.owl#JellOStrawberryBox", "Juicebox": "http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox", "Juicepack": "http://www.ease-crc.org/ont/SUTURO.owl#JuicePack", "Kitchencabinet": "http://www.ease-crc.org/ont/SOMA.owl#KitchenCabinet", @@ -44,9 +58,13 @@ "Knife": "http://www.ease-crc.org/ont/SOMA.owl#Knife", "Largemarker": "http://www.ease-crc.org/ont/SUTURO.owl#LargeMarker", "Lemon": "http://www.ease-crc.org/ont/SUTURO.owl#Lemon", + "Liquorice": "http://www.ease-crc.org/ont/SUTURO.owl#Liquorice", "Marble": "http://www.ease-crc.org/ont/SUTURO.owl#Marble", + "Masterchefcan": "http://www.ease-crc.org/ont/SUTURO.owl#MasterchefCan", + "Mayonaise": "http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise", "Metalbowl": "http://www.ease-crc.org/ont/SUTURO.owl#MetalBowl", "Metalmug": "http://www.ease-crc.org/ont/SUTURO.owl#MetalMug", + "Metalplate": "http://www.ease-crc.org/ont/SUTURO.owl#MetalPlate", "Milkbottle": "http://www.ease-crc.org/ont/SOMA.owl#MilkBottle", "Milkpack": "http://www.ease-crc.org/ont/SOMA.owl#MilkPack", "Milkpackja": "http://www.ease-crc.org/ont/SUTURO.owl#MilkPackJa", @@ -59,10 +77,12 @@ "Orange": "http://www.ease-crc.org/ont/SUTURO.owl#Orange", "Orangejuicebox": "http://www.ease-crc.org/ont/SUTURO.owl#OrangeJuiceBox", "Oreganoshaker": "http://www.ease-crc.org/ont/SUTURO.owl#OreganoShaker", + "Pancakemix": "http://www.ease-crc.org/ont/SUTURO.owl#PancakeMix", "Pancakemixbottle": "http://www.ease-crc.org/ont/SUTURO.owl#PancakeMixBottle", "Pastrybox": "http://www.ease-crc.org/ont/SUTURO.owl#PastryBox", "Peach": "http://www.ease-crc.org/ont/SUTURO.owl#Peach", "Pear": "http://www.ease-crc.org/ont/SUTURO.owl#Pear", + "Peasoupcan": "http://www.ease-crc.org/ont/SUTURO.owl#PeaSoupCan", "Pitcher": "http://www.ease-crc.org/ont/SUTURO.owl#Pitcher", "Plasticchain": "http://www.ease-crc.org/ont/SUTURO.owl#PlasticChain", "Plasticknife": "http://www.ease-crc.org/ont/SUTURO.owl#PlasticKnife", @@ -75,23 +95,28 @@ "Rope": "http://www.ease-crc.org/ont/SUTURO.owl#Rope", "Rubikscube": "http://www.ease-crc.org/ont/SUTURO.owl#RubiksCube", "Saltshaker": "http://www.ease-crc.org/ont/SOMA.owl#SaltShaker", + "Sausages": "http://www.ease-crc.org/ont/SUTURO.owl#Sausages", "Scissors": "http://www.ease-crc.org/ont/SUTURO.owl#Scissors", "Screwdriver": "http://www.ease-crc.org/ont/SUTURO.owl#ScrewDriver", "Scrubcleaner": "http://www.ease-crc.org/ont/SUTURO.owl#ScrubCleaner", "Servingmat": "http://www.ease-crc.org/ont/SUTURO.owl#ServingMat", "Shelflayer": "http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer", "Skillet": "http://www.ease-crc.org/ont/SUTURO.owl#Skillet", + "Soap": "http://www.ease-crc.org/ont/SUTURO.owl#Soap", "Softball": "http://www.ease-crc.org/ont/SUTURO.owl#SoftBall", "Spatula": "http://www.ease-crc.org/ont/SOMA.owl#Spatula", + "Sponge": "http://www.ease-crc.org/ont/SUTURO.owl#Sponge", "Spoon": "http://www.ease-crc.org/ont/SOMA.owl#Spoon", "Stackingblocks": "http://www.ease-crc.org/ont/SUTURO.owl#StackingBlocks", "Stapelchips": "http://www.ease-crc.org/ont/SUTURO.owl#StapelChips", "Strawberry": "http://www.ease-crc.org/ont/SUTURO.owl#Strawberry", + "Stroopwafels": "http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels", "Sugarbox": "http://www.ease-crc.org/ont/SUTURO.owl#SugarBox", "Table": "http://www.ease-crc.org/ont/SOMA.owl#Table", "Tablecloth": "http://www.ease-crc.org/ont/SUTURO.owl#TableCloth", "Teabagbox": "http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox", "Tennisball": "http://www.ease-crc.org/ont/SUTURO.owl#TennisBall", + "Tictac": "http://www.ease-crc.org/ont/SUTURO.owl#TicTac", "Tomatosoupcan": "http://www.ease-crc.org/ont/SUTURO.owl#TomatoSoupCan", "Toothpaste": "http://www.ease-crc.org/ont/SUTURO.owl#Toothpaste", "Toyotahsr": "http://www.ease-crc.org/ont/SUTURO.owl#ToyotaHSR", @@ -99,7 +124,9 @@ "Tray": "http://www.ease-crc.org/ont/SUTURO.owl#Tray", "Tropicaljuicebottle": "http://www.ease-crc.org/ont/SUTURO.owl#TropicalJuiceBottle", "Tunafishcan": "http://www.ease-crc.org/ont/SUTURO.owl#TunaFishCan", + "Washcloth": "http://www.ease-crc.org/ont/SUTURO.owl#WashCloth", "Washer": "http://www.ease-crc.org/ont/SUTURO.owl#Washer", + "Waterbottle": "http://www.ease-crc.org/ont/SUTURO.owl#WaterBottle", "Winebottle": "http://www.ease-crc.org/ont/SOMA.owl#WineBottle", "Wineglass": "http://www.ease-crc.org/ont/SOMA.owl#WineGlass", "Woodblock": "http://www.ease-crc.org/ont/SUTURO.owl#WoodBlock" diff --git a/src/pycram/utilities/gpsr_utils.py b/src/pycram/utilities/gpsr_utils.py index e6de362bd..191bebeeb 100644 --- a/src/pycram/utilities/gpsr_utils.py +++ b/src/pycram/utilities/gpsr_utils.py @@ -166,6 +166,7 @@ def calculate_dynamic_threshold(previous_force_x): giskardpy.avoid_all_collisions() park = pakerino() + while not park: print("waiting for park") rospy.sleep(0.1) From 0cbd76deae94860e270c7c81e6d2bc56782634b8 Mon Sep 17 00:00:00 2001 From: HaSu Date: Tue, 16 Jul 2024 13:58:02 +0200 Subject: [PATCH 3/8] [GPSR] knowledge interface fixes --- demos/pycram_gpsr_demo/knowrob_interface.py | 55 +++++++++++++++++---- demos/pycram_gpsr_demo/nlp_processing.py | 4 +- demos/pycram_gpsr_demo/run_demo.py | 4 +- demos/pycram_gpsr_demo/setup_demo.py | 4 +- demos/pycram_gpsr_demo/utils.py | 2 +- 5 files changed, 51 insertions(+), 18 deletions(-) diff --git a/demos/pycram_gpsr_demo/knowrob_interface.py b/demos/pycram_gpsr_demo/knowrob_interface.py index db3de1511..2f477f191 100644 --- a/demos/pycram_gpsr_demo/knowrob_interface.py +++ b/demos/pycram_gpsr_demo/knowrob_interface.py @@ -8,13 +8,20 @@ living_room = 'http://www.ease-crc.org/ont/SUTURO.owl#LivingRoom' arena = 'http://www.ease-crc.org/ont/SUTURO.owl#Arena' dining_room = 'http://www.ease-crc.org/ont/SUTURO.owl#DiningRoom' -rooms = {'kitchen': kitchen, 'living_room': living_room, 'arena': arena, 'dining_room': dining_room} +hallway = 'http://www.ease-crc.org/ont/SUTURO.owl#Hallway' +office = 'http://www.ease-crc.org/ont/SUTURO.owl#Office' +rooms = {'kitchen': kitchen, 'living_room': living_room, 'arena': arena, 'dining_room': dining_room, + 'hallway': hallway, 'office': office} kb = KnowrobKnowledge() #hopefully tmp dishes = 'http://www.ease-crc.org/ont/SUTURO.owl#RoboCupDishes' snacks = 'http://www.ease-crc.org/ont/SUTURO.owl#RoboCupSnacks' fruits = 'http://www.ease-crc.org/ont/SUTURO.owl#RoboCupFruits' +food = 'http://www.ease-crc.org/ont/SUTURO.owl#RoboCupFood' +cleaning_supplies = 'http://www.ease-crc.org/ont/SUTURO.owl#RoboCupCleaningSupplies' +toys = 'http://www.ease-crc.org/ont/SUTURO.owl#RoboCupToys' +decorations = 'http://www.ease-crc.org/ont/SUTURO.owl#RoboCupDecorations' def init_knowrob(): # works @@ -104,7 +111,7 @@ def get_nav_poses_for_furniture_item(room='arena', furniture_iri=None, furniture pass else: rospy.logwarn(f"[KnowRob] unknown furniture class with name {furniture_iri}. " - f"looking for DesignedFurniture instead") + f"looking for DesignedFurniture instead") furniture_iri = f"soma:'DesignedFurniture'" else: furniture_iri = f"soma:'DesignedFurniture'" @@ -135,8 +142,6 @@ def get_nav_poses_for_furniture_item(room='arena', furniture_iri=None, furniture return poses_list - - # mostly used to check if a furniture object exists based on name from nlp def check_existence_of_instance(nlp_name): # check if an instance of the object exists @@ -165,9 +170,9 @@ def check_existence_of_class(nlp_name): return tmp +# get iri from objects.py mapping def get_predefined_source_item_location(items_iri): - knowrob_poses_list = kb.prolog_client.all_solutions(f"subclass_of(Obj, {items_iri}), " - f""f"predefined_origin_location(Obj, Furniture), " + knowrob_poses_list = kb.prolog_client.all_solutions(f"predefined_origin_location('{items_iri}', Furniture), " f"furniture_rel_pose(Furniture, 'perceive', Pose).") poses_list = [] if knowrob_poses_list: @@ -178,8 +183,7 @@ def get_predefined_source_item_location(items_iri): def get_predefined_destination_item_location(items_iri): - knowrob_poses_list = kb.prolog_client.all_solutions(f"subclass_of(Obj, {items_iri}), " - f""f"predefined_destination_location(Obj, Furniture), " + knowrob_poses_list = kb.prolog_client.all_solutions(f"predefined_destination_location('{items_iri}', Furniture), " f"furniture_rel_pose(Furniture, 'perceive', Pose).") poses_list = [] if knowrob_poses_list: @@ -203,6 +207,39 @@ def test_predefined_locations(): return {'source': source_list, 'destination': destination_list} +# Test ----------------------------------------------------------------------------------------- +perception_list = [ + "Fork", "Pitcher", "Bleachcleanserbottle", "Crackerbox", "Minisoccerball", + "Baseball", "Mustardbottle", "Jellochocolatepuddingbox", "Wineglass", + "Orange", "Coffeepack", "Softball", "Metalplate", "Pringleschipscan", + "Strawberry", "Glasscleanerspraybottle", "Tennisball", "Spoon", "Metalmug", + "Abrasivesponge", "Jellobox", "Dishwashertab", "Knife", "Cerealbox", + "Metalbowl", "Sugarbox", "Coffeecan", "Milkpackja", "Apple", "Tomatosoupcan", + "Tunafishcan", "Gelatinebox", "Pear", "Lemon", "Banana", "Pottedmeatcan", + "Peach", "Plum", "Rubikscube", "Mueslibox", "Cupblue", "Cupgreen", + "Largemarker", "Masterchefcan", "Scissors", "Scrubcleaner", "Grapes", + "Cup_small", "screwdriver", "clamp", "hammer", "wooden_block", "Cornybox" +] + +furniture_list = ['hallway cabinet', 'entrance', 'desk', 'shelf', 'coathanger', + 'exit', 'TV table', 'lounge chair', 'lamp', 'couch', 'coffee table', + 'trashcan', 'kitchen cabinet', 'dinner table', 'dishwasher', 'kitchen counter'] + + +# check_existence_of_furniture(furniture_list) +def check_existence_of_furniture(tmp_list): + results = [] + tmp = None + for item in tmp_list: + tmp = check_existence_of_instance(snakecase(item)) + if tmp: + results.append([item, tmp]) + else: + results.append([item, None]) + tmp = None + return results + + def test_queries(): kb.prolog_client.once("findall(Room, has_type(Room, soma:'Room'), RoomList).") kb.prolog_client.once("member(X,[1,2,3]).") @@ -255,10 +292,8 @@ def test_queries(): f"what_object_transitive('living room', Room), instance_of(RoomInst, Room), " f"is_inside_of(Inst, RoomInst), furniture_rel_pose(Inst, 'perceive', Pose).") - # triple(Object, soma:isOntopOf, Furniture) # check if obj is on top of shelf layer - # drop databases # kb.prolog_client.all_solutions("drop_graph(user), tf_mem_clear, mng_drop(roslog, tf).") # kb.prolog_client.all_solutions(f"reset_user_data.") diff --git a/demos/pycram_gpsr_demo/nlp_processing.py b/demos/pycram_gpsr_demo/nlp_processing.py index 3aa3c3851..fd2870a9e 100644 --- a/demos/pycram_gpsr_demo/nlp_processing.py +++ b/demos/pycram_gpsr_demo/nlp_processing.py @@ -17,8 +17,8 @@ todo_plans = [] currentSpeech = "" stoppedSpeaking = Condition() -canSpeak = True -canListen = False +canSpeak = False # CHANGE set to TRUE for real robot +canListen = False # CHANGE set to TRUE when NLP and Whisper enabled canDisplay = False diff --git a/demos/pycram_gpsr_demo/run_demo.py b/demos/pycram_gpsr_demo/run_demo.py index 9b8af8c17..795bb69b8 100644 --- a/demos/pycram_gpsr_demo/run_demo.py +++ b/demos/pycram_gpsr_demo/run_demo.py @@ -117,7 +117,7 @@ def demo_plan(data): return -setup() +#setup() #gpsr() -demo_plan(data2) +#demo_plan(data2) #setup_demo.gripper.pub_now('open') diff --git a/demos/pycram_gpsr_demo/setup_demo.py b/demos/pycram_gpsr_demo/setup_demo.py index 9b4365268..f1e7c7a8b 100755 --- a/demos/pycram_gpsr_demo/setup_demo.py +++ b/demos/pycram_gpsr_demo/setup_demo.py @@ -15,7 +15,7 @@ import pycram.utilities.gpsr_utils as plans from demos.pycram_gpsr_demo import tf_l -with_real_robot = True +with_real_robot = False # CHANGE set to TRUE for real robot # initialize interfaces instruction_point = PoseStamped([1.45, 4.5, 0], [0, 0, 1, 0]) world = None @@ -26,7 +26,6 @@ move = None image_switch = None sound_pub = None -#tf_listener = None grasp_listener = None start_signal_waiter = None lt = None @@ -55,7 +54,6 @@ def setup(): move = PoseNavigator() image_switch = ImageSwitchPublisher() sound_pub = SoundRequestPublisher() - # tf_listener = tf.listener.TransformListener() grasp_listener = GraspListener() start_signal_waiter = StartSignalWaiter() lt = LocalTransformer() diff --git a/demos/pycram_gpsr_demo/utils.py b/demos/pycram_gpsr_demo/utils.py index f7ed73a61..934dd27ad 100755 --- a/demos/pycram_gpsr_demo/utils.py +++ b/demos/pycram_gpsr_demo/utils.py @@ -199,7 +199,7 @@ def autogenerate_dict_from_file(file_path): return ontology_dict -def write_json_to_file(json_data, file_path='home/hawkin/ros_out_files'): +def write_json_to_file(json_data, file_path='result.json'): # Call the function to get the result # Convert the result to a JSON string From 9435a467e7bd6a9cb661c6dd09f21905d90cfa7b Mon Sep 17 00:00:00 2001 From: HaSu Date: Tue, 16 Jul 2024 16:02:04 +0200 Subject: [PATCH 4/8] updating item lists --- demos/pycram_gpsr_demo/knowrob_interface.py | 39 +++- demos/pycram_gpsr_demo/objects.py | 106 +++++------ .../pycram_gpsr_demo/perception_interface.py | 14 +- .../pycram_gpsr_demo/perception_to_knowrob.py | 180 ++++++++++-------- 4 files changed, 197 insertions(+), 142 deletions(-) diff --git a/demos/pycram_gpsr_demo/knowrob_interface.py b/demos/pycram_gpsr_demo/knowrob_interface.py index 2f477f191..100847e2d 100644 --- a/demos/pycram_gpsr_demo/knowrob_interface.py +++ b/demos/pycram_gpsr_demo/knowrob_interface.py @@ -1,6 +1,7 @@ from stringcase import snakecase from pycram.knowledge.knowrob_knowledge import KnowrobKnowledge import rospy +from demos.pycram_gpsr_demo.perception_to_knowrob import perc_to_know import demos.pycram_gpsr_demo.utils as utils # available rooms iri types @@ -172,7 +173,9 @@ def check_existence_of_class(nlp_name): # get iri from objects.py mapping def get_predefined_source_item_location(items_iri): - knowrob_poses_list = kb.prolog_client.all_solutions(f"predefined_origin_location('{items_iri}', Furniture), " + if "'" not in items_iri: + items_iri = "'" + items_iri + "'" + knowrob_poses_list = kb.prolog_client.all_solutions(f"predefined_origin_location({items_iri}, Furniture), " f"furniture_rel_pose(Furniture, 'perceive', Pose).") poses_list = [] if knowrob_poses_list: @@ -183,7 +186,9 @@ def get_predefined_source_item_location(items_iri): def get_predefined_destination_item_location(items_iri): - knowrob_poses_list = kb.prolog_client.all_solutions(f"predefined_destination_location('{items_iri}', Furniture), " + if "'" not in items_iri: + items_iri = "'" + items_iri + "'" + knowrob_poses_list = kb.prolog_client.all_solutions(f"predefined_destination_location({items_iri}, Furniture), " f"furniture_rel_pose(Furniture, 'perceive', Pose).") poses_list = [] if knowrob_poses_list: @@ -193,6 +198,20 @@ def get_predefined_destination_item_location(items_iri): return poses_list +def check_existence_based_on_class(class_iri): + # check if an instance of the object exists + # returns the instance name + # nlp_name = 'table' + tmp = kb.prolog_client.all_solutions(f"what_object_transitive(Name, {class_iri}).") + if tmp is None or tmp == []: + rospy.logwarn(f"[KnowRob] no object class with name {class_iri} found.") + return None + else: + rospy.loginfo(f"[KnowRob] object class {tmp} of type {class_iri} found") + return tmp + + +# Test ----------------------------------------------------------------------------------------- def test_predefined_locations(): source_list = [] destination_list = [] @@ -207,7 +226,6 @@ def test_predefined_locations(): return {'source': source_list, 'destination': destination_list} -# Test ----------------------------------------------------------------------------------------- perception_list = [ "Fork", "Pitcher", "Bleachcleanserbottle", "Crackerbox", "Minisoccerball", "Baseball", "Mustardbottle", "Jellochocolatepuddingbox", "Wineglass", @@ -226,6 +244,21 @@ def test_predefined_locations(): 'trashcan', 'kitchen cabinet', 'dinner table', 'dishwasher', 'kitchen counter'] +def check_existence_of_perception(): + results = [] + items_iri = None + for item in perc_to_know: + items_iri = perc_to_know.get(item) + if "'" not in items_iri: + items_iri = "'" + items_iri + "'" + tmp = check_existence_based_on_class(items_iri) + if tmp: + results.append([item, tmp]) + else: + results.append([item, None]) + return results + + # check_existence_of_furniture(furniture_list) def check_existence_of_furniture(tmp_list): results = [] diff --git a/demos/pycram_gpsr_demo/objects.py b/demos/pycram_gpsr_demo/objects.py index 1df887712..6ff13d1ff 100644 --- a/demos/pycram_gpsr_demo/objects.py +++ b/demos/pycram_gpsr_demo/objects.py @@ -1,97 +1,97 @@ -trash_bin = 'http://www.ease-crc.org/ont/SUTURO.owl#TrashBin' -plastic_knife = 'http://www.ease-crc.org/ont/SUTURO.owl#PlasticKnife' -designed_chair = 'http://www.ease-crc.org/ont/SOMA.owl#DesignedChair' -cupboard = 'http://www.ease-crc.org/ont/SOMA.owl#Cupboard' -foam_brick = 'http://www.ease-crc.org/ont/SUTURO.owl#FoamBrick' +# trash_bin = 'http://www.ease-crc.org/ont/SUTURO.owl#TrashBin' +# plastic_knife = 'http://www.ease-crc.org/ont/SUTURO.owl#PlasticKnife' +# designed_chair = 'http://www.ease-crc.org/ont/SOMA.owl#DesignedChair' +# cupboard = 'http://www.ease-crc.org/ont/SOMA.owl#Cupboard' +# foam_brick = 'http://www.ease-crc.org/ont/SUTURO.owl#FoamBrick' fork = 'http://www.ease-crc.org/ont/SOMA.owl#Fork' -spatula = 'http://www.ease-crc.org/ont/SOMA.owl#Spatula' +# spatula = 'http://www.ease-crc.org/ont/SOMA.owl#Spatula' pitcher = 'http://www.ease-crc.org/ont/SUTURO.owl#Pitcher' bleach_cleanser_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#BleachCleanserBottle' -kitchen_cabinet = 'http://www.ease-crc.org/ont/SOMA.owl#KitchenCabinet' +# kitchen_cabinet = 'http://www.ease-crc.org/ont/SOMA.owl#KitchenCabinet' cracker_box = 'http://www.ease-crc.org/ont/SUTURO.owl#CrackerBox' mini_soccer_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#MiniSoccerBall' baseball = 'http://www.ease-crc.org/ont/SUTURO.owl#Baseball' -table_cloth = 'http://www.ease-crc.org/ont/SUTURO.owl#TableCloth' -washer = 'http://www.ease-crc.org/ont/SUTURO.owl#Washer' -designed_handle = 'http://www.ease-crc.org/ont/SOMA.owl#DesignedHandle' -pot = 'http://www.ease-crc.org/ont/SOMA.owl#Pot' -napkin = 'http://www.ease-crc.org/ont/SUTURO.owl#Napkin' +# table_cloth = 'http://www.ease-crc.org/ont/SUTURO.owl#TableCloth' +# washer = 'http://www.ease-crc.org/ont/SUTURO.owl#Washer' +# designed_handle = 'http://www.ease-crc.org/ont/SOMA.owl#DesignedHandle' +# pot = 'http://www.ease-crc.org/ont/SOMA.owl#Pot' +# napkin = 'http://www.ease-crc.org/ont/SUTURO.owl#Napkin' mustard_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#MustardBottle' jell_ochocolate_pudding_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JellOChocolatePuddingBox' -door = 'http://www.ease-crc.org/ont/SOMA.owl#Door' -serving_mat = 'http://www.ease-crc.org/ont/SUTURO.owl#ServingMat' -dishwasher = 'http://www.ease-crc.org/ont/SOMA.owl#Dishwasher' -natural_person = 'http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#NaturalPerson' +# door = 'http://www.ease-crc.org/ont/SOMA.owl#Door' +# serving_mat = 'http://www.ease-crc.org/ont/SUTURO.owl#ServingMat' +# dishwasher = 'http://www.ease-crc.org/ont/SOMA.owl#Dishwasher' +# natural_person = 'http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#NaturalPerson' wine_glass = 'http://www.ease-crc.org/ont/SOMA.owl#WineGlass' -shelf_layer = 'http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer' -tray = 'http://www.ease-crc.org/ont/SUTURO.owl#Tray' +# shelf_layer = 'http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer' +# tray = 'http://www.ease-crc.org/ont/SUTURO.owl#Tray' orange = 'http://www.ease-crc.org/ont/SUTURO.owl#Orange' -table = 'http://www.ease-crc.org/ont/SOMA.owl#Table' -bowl = 'http://www.ease-crc.org/ont/SOMA.owl#Bowl' -glass = 'http://www.ease-crc.org/ont/SOMA.owl#Glass' +# table = 'http://www.ease-crc.org/ont/SOMA.owl#Table' +# bowl = 'http://www.ease-crc.org/ont/SOMA.owl#Bowl' +# glass = 'http://www.ease-crc.org/ont/SOMA.owl#Glass' coffee_pack = 'http://www.ease-crc.org/ont/SUTURO.owl#CoffeePack' -golf_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#GolfBall' +# golf_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#GolfBall' soft_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#SoftBall' -salt_shaker = 'http://www.ease-crc.org/ont/SOMA.owl#SaltShaker' +# salt_shaker = 'http://www.ease-crc.org/ont/SOMA.owl#SaltShaker' metal_plate = 'http://www.ease-crc.org/ont/SUTURO.owl#MetalPlate' pringles_chips_can = 'http://www.ease-crc.org/ont/SUTURO.owl#PringlesChipsCan' strawberry = 'http://www.ease-crc.org/ont/SUTURO.owl#Strawberry' -glass_cleaner_spray_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#GlassCleanerSprayBottle' +# glass_cleaner_spray_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#GlassCleanerSprayBottle' tennis_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#TennisBall' spoon = 'http://www.ease-crc.org/ont/SOMA.owl#Spoon' -marble = 'http://www.ease-crc.org/ont/SUTURO.owl#Marble' -bottle = 'http://www.ease-crc.org/ont/SOMA.owl#Bottle' +# marble = 'http://www.ease-crc.org/ont/SUTURO.owl#Marble' +# bottle = 'http://www.ease-crc.org/ont/SOMA.owl#Bottle' metal_mug = 'http://www.ease-crc.org/ont/SUTURO.owl#MetalMug' -rope = 'http://www.ease-crc.org/ont/SUTURO.owl#Rope' -stacking_blocks = 'http://www.ease-crc.org/ont/SUTURO.owl#StackingBlocks' +# rope = 'http://www.ease-crc.org/ont/SUTURO.owl#Rope' +# stacking_blocks = 'http://www.ease-crc.org/ont/SUTURO.owl#StackingBlocks' abrasive_sponge = 'http://www.ease-crc.org/ont/SUTURO.owl#AbrasiveSponge' -racquetball = 'http://www.ease-crc.org/ont/SUTURO.owl#Racquetball' +# racquetball = 'http://www.ease-crc.org/ont/SUTURO.owl#Racquetball' jello_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JelloBox' dishwasher_tab = 'http://www.ease-crc.org/ont/SOMA.owl#DishwasherTab' knife = 'http://www.ease-crc.org/ont/SOMA.owl#Knife' -plastic_chain = 'http://www.ease-crc.org/ont/SUTURO.owl#PlasticChain' -plate = 'http://www.ease-crc.org/ont/SOMA.owl#Plate' -cup = 'http://www.ease-crc.org/ont/SOMA.owl#Cup' -dice = 'http://www.ease-crc.org/ont/SUTURO.owl#Dice' +# plastic_chain = 'http://www.ease-crc.org/ont/SUTURO.owl#PlasticChain' +# plate = 'http://www.ease-crc.org/ont/SOMA.owl#Plate' +# cup = 'http://www.ease-crc.org/ont/SOMA.owl#Cup' +# dice = 'http://www.ease-crc.org/ont/SUTURO.owl#Dice' cereal_box = 'http://www.ease-crc.org/ont/SOMA.owl#CerealBox' metal_bowl = 'http://www.ease-crc.org/ont/SUTURO.owl#MetalBowl' sugar_box = 'http://www.ease-crc.org/ont/SUTURO.owl#SugarBox' -skillet = 'http://www.ease-crc.org/ont/SUTURO.owl#Skillet' -kiwi = 'http://www.ease-crc.org/ont/SUTURO.owl#Kiwi' -drawer = 'http://www.ease-crc.org/ont/SOMA.owl#Drawer' -toyota_hsr = 'http://www.ease-crc.org/ont/SUTURO.owl#ToyotaHSR' +# skillet = 'http://www.ease-crc.org/ont/SUTURO.owl#Skillet' +# kiwi = 'http://www.ease-crc.org/ont/SUTURO.owl#Kiwi' +# drawer = 'http://www.ease-crc.org/ont/SOMA.owl#Drawer' +# toyota_hsr = 'http://www.ease-crc.org/ont/SUTURO.owl#ToyotaHSR' coffee_can = 'http://www.ease-crc.org/ont/SUTURO.owl#CoffeeCan' milk_pack = 'http://www.ease-crc.org/ont/SOMA.owl#MilkPack' -toothpaste = 'http://www.ease-crc.org/ont/SUTURO.owl#Toothpaste' -oregano_shaker = 'http://www.ease-crc.org/ont/SUTURO.owl#OreganoShaker' +# toothpaste = 'http://www.ease-crc.org/ont/SUTURO.owl#Toothpaste' +# oregano_shaker = 'http://www.ease-crc.org/ont/SUTURO.owl#OreganoShaker' apple = 'http://www.ease-crc.org/ont/SUTURO.owl#Apple' tomato_soup_can = 'http://www.ease-crc.org/ont/SUTURO.owl#TomatoSoupCan' tuna_fish_can = 'http://www.ease-crc.org/ont/SUTURO.owl#TunaFishCan' -bag = 'http://www.ease-crc.org/ont/SUTURO.owl#Bag' -juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox' -pancake_mix_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#PancakeMixBottle' +# bag = 'http://www.ease-crc.org/ont/SUTURO.owl#Bag' +# juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox' +# pancake_mix_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#PancakeMixBottle' gelatine_box = 'http://www.ease-crc.org/ont/SUTURO.owl#GelatineBox' -tea_bag_box = 'http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox' -pastry_box = 'http://www.ease-crc.org/ont/SUTURO.owl#PastryBox' +# tea_bag_box = 'http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox' +# pastry_box = 'http://www.ease-crc.org/ont/SUTURO.owl#PastryBox' pear = 'http://www.ease-crc.org/ont/SUTURO.owl#Pear' lemon = 'http://www.ease-crc.org/ont/SUTURO.owl#Lemon' banana = 'http://www.ease-crc.org/ont/SUTURO.owl#Banana' potted_meat_can = 'http://www.ease-crc.org/ont/SUTURO.owl#PottedMeatCan' peach = 'http://www.ease-crc.org/ont/SUTURO.owl#Peach' plum = 'http://www.ease-crc.org/ont/SUTURO.owl#Plum' -wine_bottle = 'http://www.ease-crc.org/ont/SOMA.owl#WineBottle' -juice_pack = 'http://www.ease-crc.org/ont/SUTURO.owl#JuicePack' -cola_can = 'http://www.ease-crc.org/ont/SUTURO.owl#ColaCan' -tropical_juice_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#TropicalJuiceBottle' +# wine_bottle = 'http://www.ease-crc.org/ont/SOMA.owl#WineBottle' +# juice_pack = 'http://www.ease-crc.org/ont/SUTURO.owl#JuicePack' +# cola_can = 'http://www.ease-crc.org/ont/SUTURO.owl#ColaCan' +# tropical_juice_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#TropicalJuiceBottle' milk_bottle = 'http://www.ease-crc.org/ont/SOMA.owl#MilkBottle' -ice_tea_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#IceTeaBottle' -orange_juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#OrangeJuiceBox' +# ice_tea_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#IceTeaBottle' +# orange_juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#OrangeJuiceBox' rubiks_cube = 'http://www.ease-crc.org/ont/SUTURO.owl#RubiksCube' -cereal_box_robo_cup = 'http://www.ease-crc.org/ont/SUTURO.owl#CerealBoxRoboCup' -nesquick_cereal_box = 'http://www.ease-crc.org/ont/SUTURO.owl#NesquickCerealBox' +# cereal_box_robo_cup = 'http://www.ease-crc.org/ont/SUTURO.owl#CerealBoxRoboCup' +# nesquick_cereal_box = 'http://www.ease-crc.org/ont/SUTURO.owl#NesquickCerealBox' milk_pack_ja = 'http://www.ease-crc.org/ont/SUTURO.owl#MilkPackJa' muesli_box = 'http://www.ease-crc.org/ont/SUTURO.owl#MuesliBox' -stapel_chips = 'http://www.ease-crc.org/ont/SUTURO.owl#StapelChips' +# stapel_chips = 'http://www.ease-crc.org/ont/SUTURO.owl#StapelChips' cup_blue = 'http://www.ease-crc.org/ont/SUTURO.owl#CupBlue' cup_green = 'http://www.ease-crc.org/ont/SUTURO.owl#CupGreen' large_marker = 'http://www.ease-crc.org/ont/SUTURO.owl#LargeMarker' diff --git a/demos/pycram_gpsr_demo/perception_interface.py b/demos/pycram_gpsr_demo/perception_interface.py index 9d3e522f0..6df9db92a 100644 --- a/demos/pycram_gpsr_demo/perception_interface.py +++ b/demos/pycram_gpsr_demo/perception_interface.py @@ -5,12 +5,9 @@ from pycram.process_module import real_robot from robokudo_msgs.msg import QueryAction, QueryGoal, QueryResult, QueryActionResult - - rk = actionlib.SimpleActionClient('robokudo/query', QueryAction) - def init_robokudo(): global rk rk = actionlib.SimpleActionClient('robokudo/query', QueryAction) @@ -42,7 +39,8 @@ def ask_robokudo_for_all_objects(): rospy.wait_for_message(topic='/robokudo/query/result', topic_type=QueryActionResult, timeout=15) result = rk.get_result() rospy.loginfo("[RK] result received") - return result # list of all perceived items or an empty list + return result # list of all perceived items or an empty list + # todo test from here ---- def ask_robokudo_for_object(obj_type): @@ -55,7 +53,7 @@ def ask_robokudo_for_object(obj_type): rospy.wait_for_message(topic='/robokudo/query/result', topic_type=QueryActionResult, timeout=15) result = rk.get_result() rospy.loginfo("[RK] result received") - return result # list of all perceived items or an empty list + return result # list of all perceived items or an empty list def send_robokudo_goal(goal_msg): @@ -67,6 +65,7 @@ def send_robokudo_goal(goal_msg): rospy.loginfo("[RK] result received") return result + # todo test end here------ ### --- REPL testing --- @@ -77,8 +76,8 @@ def test_pc(): rk.wait_for_server() rospy.loginfo("You can start your demo now") goal_msg = QueryGoal() - goal_msg.obj.type = 'Crackerbox' # human doesn't work somehow? 'mueslibox' - goal_msg.type='all' + goal_msg.obj.type = 'Crackerbox' # human doesn't work somehow? 'mueslibox' + goal_msg.type = 'all' rk.send_goal(goal_msg) result = rk.get_result() print(result.res[0].type) @@ -99,4 +98,3 @@ def test_lookat(pose): # print(object_desig) # return object_desig ### to get the result I would need to do this: object_desig[1].get('Metalmug_1719825441.1798751') - diff --git a/demos/pycram_gpsr_demo/perception_to_knowrob.py b/demos/pycram_gpsr_demo/perception_to_knowrob.py index a78f31879..28d2cb76c 100644 --- a/demos/pycram_gpsr_demo/perception_to_knowrob.py +++ b/demos/pycram_gpsr_demo/perception_to_knowrob.py @@ -3,131 +3,155 @@ perc_to_know = { "Abrasivesponge": "http://www.ease-crc.org/ont/SUTURO.owl#AbrasiveSponge", "Apple": "http://www.ease-crc.org/ont/SUTURO.owl#Apple", - "Bag": "http://www.ease-crc.org/ont/SUTURO.owl#Bag", + # "Bag": "http://www.ease-crc.org/ont/SUTURO.owl#Bag", "Banana": "http://www.ease-crc.org/ont/SUTURO.owl#Banana", "Baseball": "http://www.ease-crc.org/ont/SUTURO.owl#Baseball", "Bleachcleanserbottle": "http://www.ease-crc.org/ont/SUTURO.owl#BleachCleanserBottle", - "Bottle": "http://www.ease-crc.org/ont/SOMA.owl#Bottle", - "Bowl": "http://www.ease-crc.org/ont/SOMA.owl#Bowl", - "Candle": "http://www.ease-crc.org/ont/SUTURO.owl#Candle", - "Candy": "http://www.ease-crc.org/ont/SUTURO.owl#Candy", + # "Bottle": "http://www.ease-crc.org/ont/SOMA.owl#Bottle", + # "Bowl": "http://www.ease-crc.org/ont/SOMA.owl#Bowl", + # "Candle": "http://www.ease-crc.org/ont/SUTURO.owl#Candle", + # "Candy": "http://www.ease-crc.org/ont/SUTURO.owl#Candy", "Cerealbox": "http://www.ease-crc.org/ont/SOMA.owl#CerealBox", - "Cerealboxrobocup": "http://www.ease-crc.org/ont/SUTURO.owl#CerealBoxRoboCup", + # "Cerealboxrobocup": "http://www.ease-crc.org/ont/SUTURO.owl#CerealBoxRoboCup", "Clamp": "http://www.ease-crc.org/ont/SUTURO.owl#Clamp", "Coffeecan": "http://www.ease-crc.org/ont/SUTURO.owl#CoffeeCan", "Coffeepack": "http://www.ease-crc.org/ont/SUTURO.owl#CoffeePack", - "Colabottle": "http://www.ease-crc.org/ont/SUTURO.owl#ColaBottle", - "Colacan": "http://www.ease-crc.org/ont/SUTURO.owl#ColaCan", - "Cornflakes": "http://www.ease-crc.org/ont/SUTURO.owl#Cornflakes", - "Cornybox": "http://www.ease-crc.org/ont/SUTURO.owl#CornyBox", + # "Colabottle": "http://www.ease-crc.org/ont/SUTURO.owl#ColaBottle", + # "Colacan": "http://www.ease-crc.org/ont/SUTURO.owl#ColaCan", + # "Cornflakes": "http://www.ease-crc.org/ont/SUTURO.owl#Cornflakes", + # "Cornybox": "http://www.ease-crc.org/ont/SUTURO.owl#CornyBox", "Crackerbox": "http://www.ease-crc.org/ont/SUTURO.owl#CrackerBox", - "Crispsbag": "http://www.ease-crc.org/ont/SUTURO.owl#CrispsBag", - "Cup": "http://www.ease-crc.org/ont/SOMA.owl#Cup", + # "Crispsbag": "http://www.ease-crc.org/ont/SUTURO.owl#CrispsBag", + # "Cup": "http://www.ease-crc.org/ont/SOMA.owl#Cup", "Cupblue": "http://www.ease-crc.org/ont/SUTURO.owl#CupBlue", - "Cupboard": "http://www.ease-crc.org/ont/SOMA.owl#Cupboard", + # "Cupboard": "http://www.ease-crc.org/ont/SOMA.owl#Cupboard", "Cupgreen": "http://www.ease-crc.org/ont/SUTURO.owl#CupGreen", "Cupsmall": "http://www.ease-crc.org/ont/SUTURO.owl#CupSmall", - "Curry": "http://www.ease-crc.org/ont/SUTURO.owl#Curry", - "Designedchair": "http://www.ease-crc.org/ont/SOMA.owl#DesignedChair", - "Designedhandle": "http://www.ease-crc.org/ont/SOMA.owl#DesignedHandle", - "Dice": "http://www.ease-crc.org/ont/SUTURO.owl#Dice", - "Dishwasher": "http://www.ease-crc.org/ont/SOMA.owl#Dishwasher", + # "Curry": "http://www.ease-crc.org/ont/SUTURO.owl#Curry", + # "Designedchair": "http://www.ease-crc.org/ont/SOMA.owl#DesignedChair", + # "Designedhandle": "http://www.ease-crc.org/ont/SOMA.owl#DesignedHandle", + # "Dice": "http://www.ease-crc.org/ont/SUTURO.owl#Dice", + # "Dishwasher": "http://www.ease-crc.org/ont/SOMA.owl#Dishwasher", "Dishwashertab": "http://www.ease-crc.org/ont/SOMA.owl#DishwasherTab", - "Door": "http://www.ease-crc.org/ont/SOMA.owl#Door", - "Drawer": "http://www.ease-crc.org/ont/SOMA.owl#Drawer", - "Dubbelfris": "http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris", - "Fantacan": "http://www.ease-crc.org/ont/SUTURO.owl#FantaCan", - "Foambrick": "http://www.ease-crc.org/ont/SUTURO.owl#FoamBrick", + # "Door": "http://www.ease-crc.org/ont/SOMA.owl#Door", + # "Drawer": "http://www.ease-crc.org/ont/SOMA.owl#Drawer", + # "Dubbelfris": "http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris", + # "Fantacan": "http://www.ease-crc.org/ont/SUTURO.owl#FantaCan", + # "Foambrick": "http://www.ease-crc.org/ont/SUTURO.owl#FoamBrick", "Fork": "http://www.ease-crc.org/ont/SOMA.owl#Fork", "Gelatinebox": "http://www.ease-crc.org/ont/SUTURO.owl#GelatineBox", - "Glass": "http://www.ease-crc.org/ont/SOMA.owl#Glass", - "Glasscleanerspraybottle": "http://www.ease-crc.org/ont/SUTURO.owl#GlassCleanerSprayBottle", - "Golfball": "http://www.ease-crc.org/ont/SUTURO.owl#GolfBall", + # "Glass": "http://www.ease-crc.org/ont/SOMA.owl#Glass", + "Glasscleanerspraybottle": "http://www.ease-crc.org/ont/SOMA.owl#GlassCleanerSprayBottle", + # "Golfball": "http://www.ease-crc.org/ont/SUTURO.owl#GolfBall", "Grapes": "http://www.ease-crc.org/ont/SUTURO.owl#Grapes", - "Grocerybag": "http://www.ease-crc.org/ont/SUTURO.owl#GroceryBag", - "Hagelslag": "http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag", + # "Grocerybag": "http://www.ease-crc.org/ont/SUTURO.owl#GroceryBag", + # "Hagelslag": "http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag", "Hammer": "http://www.ease-crc.org/ont/SUTURO.owl#Hammer", - "Iceteabottle": "http://www.ease-crc.org/ont/SUTURO.owl#IceTeaBottle", - "Iceteacan": "http://www.ease-crc.org/ont/SUTURO.owl#IceTeaCan", + # "Iceteabottle": "http://www.ease-crc.org/ont/SUTURO.owl#IceTeaBottle", + # "Iceteacan": "http://www.ease-crc.org/ont/SUTURO.owl#IceTeaCan", "Jellobox": "http://www.ease-crc.org/ont/SUTURO.owl#JelloBox", "Jellochocolatepuddingbox": "http://www.ease-crc.org/ont/SUTURO.owl#JellOChocolatePuddingBox", - "Juicebox": "http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox", - "Juicepack": "http://www.ease-crc.org/ont/SUTURO.owl#JuicePack", - "Kitchencabinet": "http://www.ease-crc.org/ont/SOMA.owl#KitchenCabinet", - "Kiwi": "http://www.ease-crc.org/ont/SUTURO.owl#Kiwi", + # "Juicebox": "http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox", + # "Juicepack": "http://www.ease-crc.org/ont/SUTURO.owl#JuicePack", + # "Kitchencabinet": "http://www.ease-crc.org/ont/SOMA.owl#KitchenCabinet", + # "Kiwi": "http://www.ease-crc.org/ont/SUTURO.owl#Kiwi", "Knife": "http://www.ease-crc.org/ont/SOMA.owl#Knife", "Largemarker": "http://www.ease-crc.org/ont/SUTURO.owl#LargeMarker", "Lemon": "http://www.ease-crc.org/ont/SUTURO.owl#Lemon", - "Liquorice": "http://www.ease-crc.org/ont/SUTURO.owl#Liquorice", - "Marble": "http://www.ease-crc.org/ont/SUTURO.owl#Marble", + # "Liquorice": "http://www.ease-crc.org/ont/SUTURO.owl#Liquorice", + # "Marble": "http://www.ease-crc.org/ont/SUTURO.owl#Marble", "Masterchefcan": "http://www.ease-crc.org/ont/SUTURO.owl#MasterchefCan", - "Mayonaise": "http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise", + # "Mayonaise": "http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise", "Metalbowl": "http://www.ease-crc.org/ont/SUTURO.owl#MetalBowl", "Metalmug": "http://www.ease-crc.org/ont/SUTURO.owl#MetalMug", "Metalplate": "http://www.ease-crc.org/ont/SUTURO.owl#MetalPlate", - "Milkbottle": "http://www.ease-crc.org/ont/SOMA.owl#MilkBottle", - "Milkpack": "http://www.ease-crc.org/ont/SOMA.owl#MilkPack", + # "Milkbottle": "http://www.ease-crc.org/ont/SOMA.owl#MilkBottle", + # "Milkpack": "http://www.ease-crc.org/ont/SOMA.owl#MilkPack", "Milkpackja": "http://www.ease-crc.org/ont/SUTURO.owl#MilkPackJa", "Minisoccerball": "http://www.ease-crc.org/ont/SUTURO.owl#MiniSoccerBall", "Mueslibox": "http://www.ease-crc.org/ont/SUTURO.owl#MuesliBox", "Mustardbottle": "http://www.ease-crc.org/ont/SUTURO.owl#MustardBottle", - "Napkin": "http://www.ease-crc.org/ont/SUTURO.owl#Napkin", - "Naturalperson": "http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#NaturalPerson", - "Nesquickcerealbox": "http://www.ease-crc.org/ont/SUTURO.owl#NesquickCerealBox", + # "Napkin": "http://www.ease-crc.org/ont/SUTURO.owl#Napkin", + # "Naturalperson": "http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#NaturalPerson", + # "Nesquickcerealbox": "http://www.ease-crc.org/ont/SUTURO.owl#NesquickCerealBox", "Orange": "http://www.ease-crc.org/ont/SUTURO.owl#Orange", - "Orangejuicebox": "http://www.ease-crc.org/ont/SUTURO.owl#OrangeJuiceBox", - "Oreganoshaker": "http://www.ease-crc.org/ont/SUTURO.owl#OreganoShaker", - "Pancakemix": "http://www.ease-crc.org/ont/SUTURO.owl#PancakeMix", - "Pancakemixbottle": "http://www.ease-crc.org/ont/SUTURO.owl#PancakeMixBottle", - "Pastrybox": "http://www.ease-crc.org/ont/SUTURO.owl#PastryBox", + # "Orangejuicebox": "http://www.ease-crc.org/ont/SUTURO.owl#OrangeJuiceBox", + # "Oreganoshaker": "http://www.ease-crc.org/ont/SUTURO.owl#OreganoShaker", + # "Pancakemix": "http://www.ease-crc.org/ont/SUTURO.owl#PancakeMix", + # "Pancakemixbottle": "http://www.ease-crc.org/ont/SUTURO.owl#PancakeMixBottle", + # "Pastrybox": "http://www.ease-crc.org/ont/SUTURO.owl#PastryBox", "Peach": "http://www.ease-crc.org/ont/SUTURO.owl#Peach", "Pear": "http://www.ease-crc.org/ont/SUTURO.owl#Pear", - "Peasoupcan": "http://www.ease-crc.org/ont/SUTURO.owl#PeaSoupCan", + # "Peasoupcan": "http://www.ease-crc.org/ont/SUTURO.owl#PeaSoupCan", "Pitcher": "http://www.ease-crc.org/ont/SUTURO.owl#Pitcher", - "Plasticchain": "http://www.ease-crc.org/ont/SUTURO.owl#PlasticChain", - "Plasticknife": "http://www.ease-crc.org/ont/SUTURO.owl#PlasticKnife", - "Plate": "http://www.ease-crc.org/ont/SOMA.owl#Plate", + # "Plasticchain": "http://www.ease-crc.org/ont/SUTURO.owl#PlasticChain", + # "Plasticknife": "http://www.ease-crc.org/ont/SUTURO.owl#PlasticKnife", + # "Plate": "http://www.ease-crc.org/ont/SOMA.owl#Plate", "Plum": "http://www.ease-crc.org/ont/SUTURO.owl#Plum", - "Pot": "http://www.ease-crc.org/ont/SOMA.owl#Pot", + # "Pot": "http://www.ease-crc.org/ont/SOMA.owl#Pot", "Pottedmeatcan": "http://www.ease-crc.org/ont/SUTURO.owl#PottedMeatCan", "Pringleschipscan": "http://www.ease-crc.org/ont/SUTURO.owl#PringlesChipsCan", - "Racquetball": "http://www.ease-crc.org/ont/SUTURO.owl#Racquetball", - "Rope": "http://www.ease-crc.org/ont/SUTURO.owl#Rope", + # "Racquetball": "http://www.ease-crc.org/ont/SUTURO.owl#Racquetball", + # "Rope": "http://www.ease-crc.org/ont/SUTURO.owl#Rope", "Rubikscube": "http://www.ease-crc.org/ont/SUTURO.owl#RubiksCube", - "Saltshaker": "http://www.ease-crc.org/ont/SOMA.owl#SaltShaker", - "Sausages": "http://www.ease-crc.org/ont/SUTURO.owl#Sausages", + # "Saltshaker": "http://www.ease-crc.org/ont/SOMA.owl#SaltShaker", + # "Sausages": "http://www.ease-crc.org/ont/SUTURO.owl#Sausages", "Scissors": "http://www.ease-crc.org/ont/SUTURO.owl#Scissors", "Screwdriver": "http://www.ease-crc.org/ont/SUTURO.owl#ScrewDriver", "Scrubcleaner": "http://www.ease-crc.org/ont/SUTURO.owl#ScrubCleaner", - "Servingmat": "http://www.ease-crc.org/ont/SUTURO.owl#ServingMat", - "Shelflayer": "http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer", - "Skillet": "http://www.ease-crc.org/ont/SUTURO.owl#Skillet", - "Soap": "http://www.ease-crc.org/ont/SUTURO.owl#Soap", + # "Servingmat": "http://www.ease-crc.org/ont/SUTURO.owl#ServingMat", + # "Shelflayer": "http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer", + # "Skillet": "http://www.ease-crc.org/ont/SUTURO.owl#Skillet", + # "Soap": "http://www.ease-crc.org/ont/SUTURO.owl#Soap", "Softball": "http://www.ease-crc.org/ont/SUTURO.owl#SoftBall", - "Spatula": "http://www.ease-crc.org/ont/SOMA.owl#Spatula", - "Sponge": "http://www.ease-crc.org/ont/SUTURO.owl#Sponge", + # "Spatula": "http://www.ease-crc.org/ont/SOMA.owl#Spatula", + # "Sponge": "http://www.ease-crc.org/ont/SUTURO.owl#Sponge", "Spoon": "http://www.ease-crc.org/ont/SOMA.owl#Spoon", - "Stackingblocks": "http://www.ease-crc.org/ont/SUTURO.owl#StackingBlocks", - "Stapelchips": "http://www.ease-crc.org/ont/SUTURO.owl#StapelChips", + # "Stackingblocks": "http://www.ease-crc.org/ont/SUTURO.owl#StackingBlocks", + # "Stapelchips": "http://www.ease-crc.org/ont/SUTURO.owl#StapelChips", "Strawberry": "http://www.ease-crc.org/ont/SUTURO.owl#Strawberry", - "Stroopwafels": "http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels", + # "Stroopwafels": "http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels", "Sugarbox": "http://www.ease-crc.org/ont/SUTURO.owl#SugarBox", - "Table": "http://www.ease-crc.org/ont/SOMA.owl#Table", - "Tablecloth": "http://www.ease-crc.org/ont/SUTURO.owl#TableCloth", - "Teabagbox": "http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox", + # "Table": "http://www.ease-crc.org/ont/SOMA.owl#Table", + # "Tablecloth": "http://www.ease-crc.org/ont/SUTURO.owl#TableCloth", + # "Teabagbox": "http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox", "Tennisball": "http://www.ease-crc.org/ont/SUTURO.owl#TennisBall", - "Tictac": "http://www.ease-crc.org/ont/SUTURO.owl#TicTac", + # "Tictac": "http://www.ease-crc.org/ont/SUTURO.owl#TicTac", "Tomatosoupcan": "http://www.ease-crc.org/ont/SUTURO.owl#TomatoSoupCan", - "Toothpaste": "http://www.ease-crc.org/ont/SUTURO.owl#Toothpaste", - "Toyotahsr": "http://www.ease-crc.org/ont/SUTURO.owl#ToyotaHSR", - "Trashbin": "http://www.ease-crc.org/ont/SUTURO.owl#TrashBin", - "Tray": "http://www.ease-crc.org/ont/SUTURO.owl#Tray", - "Tropicaljuicebottle": "http://www.ease-crc.org/ont/SUTURO.owl#TropicalJuiceBottle", + # "Toothpaste": "http://www.ease-crc.org/ont/SUTURO.owl#Toothpaste", + # "Toyotahsr": "http://www.ease-crc.org/ont/SUTURO.owl#ToyotaHSR", + # "Trashbin": "http://www.ease-crc.org/ont/SUTURO.owl#TrashBin", + # "Tray": "http://www.ease-crc.org/ont/SUTURO.owl#Tray", + # "Tropicaljuicebottle": "http://www.ease-crc.org/ont/SUTURO.owl#TropicalJuiceBottle", "Tunafishcan": "http://www.ease-crc.org/ont/SUTURO.owl#TunaFishCan", + # "Washcloth": "http://www.ease-crc.org/ont/SUTURO.owl#WashCloth", + # "Washer": "http://www.ease-crc.org/ont/SUTURO.owl#Washer", + # "Waterbottle": "http://www.ease-crc.org/ont/SUTURO.owl#WaterBottle", + # "Winebottle": "http://www.ease-crc.org/ont/SOMA.owl#WineBottle", + "Wineglass": "http://www.ease-crc.org/ont/SOMA.owl#WineGlass", + "Woodblock": "http://www.ease-crc.org/ont/SUTURO.owl#WoodBlock", + # ROBOCUP + "Sponge": "http://www.ease-crc.org/ont/SUTURO.owl#Sponge", "Washcloth": "http://www.ease-crc.org/ont/SUTURO.owl#WashCloth", - "Washer": "http://www.ease-crc.org/ont/SUTURO.owl#Washer", + "Soap": "http://www.ease-crc.org/ont/SUTURO.owl#Soap", + "Fantacan": "http://www.ease-crc.org/ont/SUTURO.owl#FantaCan", + "Iceteacan": "http://www.ease-crc.org/ont/SUTURO.owl#IceTeaCan", + "Dubbelfris": "http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris", "Waterbottle": "http://www.ease-crc.org/ont/SUTURO.owl#WaterBottle", - "Winebottle": "http://www.ease-crc.org/ont/SOMA.owl#WineBottle", - "Wineglass": "http://www.ease-crc.org/ont/SOMA.owl#WineGlass", - "Woodblock": "http://www.ease-crc.org/ont/SUTURO.owl#WoodBlock" + "Colabottle": "http://www.ease-crc.org/ont/SUTURO.owl#ColaBottle", + "Peasoupcan": "http://www.ease-crc.org/ont/SUTURO.owl#PeaSoupCan", + "Cornflakes": "http://www.ease-crc.org/ont/SUTURO.owl#Cornflakes", + "Curry": "http://www.ease-crc.org/ont/SUTURO.owl#Curry", + "Mayonaise": "http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise", + "Hagelslag": "http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag", + "Sausages": "http://www.ease-crc.org/ont/SUTURO.owl#Sausages", + "Pancakemix": "http://www.ease-crc.org/ont/SUTURO.owl#PancakeMix", + "Crispsbag": "http://www.ease-crc.org/ont/SUTURO.owl#CrispsBag", + "Candy": "http://www.ease-crc.org/ont/SUTURO.owl#Candy", + "Stroopwafels": "http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels", + "Liquorice": "http://www.ease-crc.org/ont/SUTURO.owl#Liquorice", + "TicTac": "http://www.ease-crc.org/ont/SUTURO.owl#TicTac", + "Candle": "http://www.ease-crc.org/ont/SUTURO.owl#Candle", + "Grocerybag": "http://www.ease-crc.org/ont/SUTURO.owl#GroceryBag", + "Vase": "http://www.ease-crc.org/ont/SUTURO.owl#Vase" } \ No newline at end of file From adbb9c6a39353f12aefd2d6d41e33ebc303ad727 Mon Sep 17 00:00:00 2001 From: HaSu Date: Tue, 16 Jul 2024 18:35:38 +0200 Subject: [PATCH 5/8] [GPSR] added eindhoven specific Q and A --- demos/pycram_gpsr_demo/high_level_plans.py | 52 +++++++++++++++++++++- demos/pycram_gpsr_demo/llp_tell_stuff.py | 47 ++++++++++++++++++- demos/pycram_gpsr_demo/objects.py | 16 ++++++- 3 files changed, 112 insertions(+), 3 deletions(-) diff --git a/demos/pycram_gpsr_demo/high_level_plans.py b/demos/pycram_gpsr_demo/high_level_plans.py index c6dccff73..231aaeeaf 100755 --- a/demos/pycram_gpsr_demo/high_level_plans.py +++ b/demos/pycram_gpsr_demo/high_level_plans.py @@ -325,6 +325,8 @@ def looking_for(param_json): # WIP TODO if param_json.get('Item') is not None: item = param_json.get('Item').get('value') + # go to location which human said to look for items + def transporting(param_json): sing_my_angel_of_music("in transporting plan") @@ -468,7 +470,55 @@ def tell_something(param_json): # CHANGE adapt tp cute toya stuff rospy.loginfo("tell something: " + str(param_json)) -# WIP --- END telling plans ------------------------------------------------------------------------------------- +# DONE --- END telling plans ------------------------------------------------------------------------------------- +# WIP ---- Eindhoven Specific Questions ------------------------------ +def eindhoven_mountain(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_mountain()) + rospy.loginfo("say eindhoven mountain: " + str(param_json)) + + +def eindhoven_painter(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_painter()) + rospy.loginfo("say eindhoven painter: " + str(param_json)) + + +def eindhoven_lake(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_lake()) + rospy.loginfo("say eindhoven lake: " + str(param_json)) + + +def eindhoven_baron(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_baron()) + rospy.loginfo("say eindhoven baron: " + str(param_json)) + + +def eindhoven_created(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_created()) + rospy.loginfo("say eindhoven mountain: " + str(param_json)) + + +def eindhoven_people(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_people()) + rospy.loginfo("say eindhoven people: " + str(param_json)) + + +def eindhoven_mascot(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_mascot()) + rospy.loginfo("say eindhoven mascot: " + str(param_json)) + + +def eindhoven_lowest_point(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_lowest_point()) + rospy.loginfo("say eindhoven lowest point: " + str(param_json)) + + +def eindhoven_currency(param_json): + sing_my_angel_of_music(llp_tell_stuff.say_eindhoven_currency()) + rospy.loginfo("say eindhoven currency: " + str(param_json)) + + +# WIP ---- END Eindhoven Specific Questions ------------------------------ + def prepare_for_commands(): global move, instruction_point # wait for start signal (laser scan) diff --git a/demos/pycram_gpsr_demo/llp_tell_stuff.py b/demos/pycram_gpsr_demo/llp_tell_stuff.py index 6b8492252..9f0ea4e01 100644 --- a/demos/pycram_gpsr_demo/llp_tell_stuff.py +++ b/demos/pycram_gpsr_demo/llp_tell_stuff.py @@ -85,8 +85,53 @@ def say_something(): "What do you call a pirate robot? - Arrrr2-D2", "Hey human, you wouldn't happen to know the exact whereabouts of John Connor, would you? Just asking for a friend...", "Meow - I really like cats!", - "The first robot was created in the 5th century BC and was a mechanical bird.", "Robots are exploring Mars and sending back information to Earth.", "There's a robot that can solve a Rubik's cube in less than a second." ] return random.choice(facts) + + +# WIP Eindhoven specifc questions --------------------------------------- +def say_eindhoven_mountain(): + response = f"The Vaalserberg is the highest mountain in the Netherlands, although parts of the mountain belong to Belgium and Germany." + return response + + +def say_eindhoven_painter(): + response = f"It was created by the dutch painter Rembrandt." + return response + + +def say_eindhoven_lake(): + response = f"The largest lake in the Netherlands is the Ijsselmeer." + return response + + +def say_eindhoven_baron(): + response = f"King Willem-Alexander of the Netherlands." + return response + + +def say_eindhoven_created(): + response = f"In 1232, by the duke of Brabant, Henry I." + return response + + +def say_eindhoven_people(): + response = f"More than 200.000 people currently live in Eindhoven." + return response + + +def say_eindhoven_mascot(): + response = f"The official mascot for this year's RoboCup is called Robin." + return response + + +def say_eindhoven_lowest_point(): + response = f"The lowest point of the Netherlands is -6.67m below sea level. It is located close to the A20." + return response + + +def say_eindhoven_currency(): + response = f"The guilder was the currency of the Netherlands before the euro was introduced in 2002." + return response diff --git a/demos/pycram_gpsr_demo/objects.py b/demos/pycram_gpsr_demo/objects.py index 6ff13d1ff..ac4a569c3 100644 --- a/demos/pycram_gpsr_demo/objects.py +++ b/demos/pycram_gpsr_demo/objects.py @@ -23,7 +23,7 @@ # dishwasher = 'http://www.ease-crc.org/ont/SOMA.owl#Dishwasher' # natural_person = 'http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#NaturalPerson' wine_glass = 'http://www.ease-crc.org/ont/SOMA.owl#WineGlass' -# shelf_layer = 'http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer' +shelf_layer = 'http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer' # tray = 'http://www.ease-crc.org/ont/SUTURO.owl#Tray' orange = 'http://www.ease-crc.org/ont/SUTURO.owl#Orange' # table = 'http://www.ease-crc.org/ont/SOMA.owl#Table' @@ -127,3 +127,17 @@ tic_tac = 'http://www.ease-crc.org/ont/SUTURO.owl#TicTac' candle = 'http://www.ease-crc.org/ont/SUTURO.owl#Candle' grocery_bag = 'http://www.ease-crc.org/ont/SUTURO.owl#GroceryBag' +# Furniture +# hallway_cabinet = 'http://www.ease-crc.org/ont/SUTURO.owl#HallwayCabinet' +# shelf = 'http://www.ease-crc.org/ont/SUTURO.owl#Shelf' +# coat_hanger = 'http://www.ease-crc.org/ont/SUTURO.owl#CoatHanger' +# tv_table = 'http://www.ease-crc.org/ont/SUTURO.owl#TvTable' +# lounge_chair = 'http://www.ease-crc.org/ont/SUTURO.owl#LoungeChair' +# lamp = 'http://www.ease-crc.org/ont/SUTURO.owl#Lamp' # fehlt in KB +# couch = 'http://www.ease-crc.org/ont/SUTURO.owl#Couch' +# coffee_table = 'http://www.ease-crc.org/ont/SUTURO.owl#CoffeeTable' +# trashcan = 'http://www.ease-crc.org/ont/SUTURO.owl#TrashCan' +# kitchen_cabinet = 'http://www.ease-crc.org/ont/SUTURO.owl#KitchenCabinet' +# dinner_table = 'http://www.ease-crc.org/ont/SUTURO.owl#DinnerTable' +# dishwasher = 'http://www.ease-crc.org/ont/SUTURO.owl#Dishwasher' +# kitchen_counter = 'http://www.ease-crc.org/ont/SUTURO.owl#KitchenCounter' From 6ca17ef3c5482f430353cf49e2a99ceca4b657cc Mon Sep 17 00:00:00 2001 From: HaSu Date: Fri, 19 Jul 2024 00:33:46 +0200 Subject: [PATCH 6/8] [GPSR] added yeet into arena, wip looking-for plan, robokudo human perception --- demos/pycram_gpsr_demo/high_level_plans.py | 123 +++++++++++-- demos/pycram_gpsr_demo/knowrob_interface.py | 26 ++- demos/pycram_gpsr_demo/llp_navigation.py | 4 +- demos/pycram_gpsr_demo/nlp_processing.py | 7 +- demos/pycram_gpsr_demo/objects.py | 80 +++++++-- .../pycram_gpsr_demo/perception_interface.py | 36 +++- demos/pycram_gpsr_demo/run_demo.py | 163 +++++++++++++++++- demos/pycram_gpsr_demo/setup_demo.py | 2 +- demos/pycram_gpsr_demo/utils.py | 1 + src/pycram/utilities/gpsr_utils.py | 93 +++++----- 10 files changed, 441 insertions(+), 94 deletions(-) diff --git a/demos/pycram_gpsr_demo/high_level_plans.py b/demos/pycram_gpsr_demo/high_level_plans.py index 231aaeeaf..f5010454d 100755 --- a/demos/pycram_gpsr_demo/high_level_plans.py +++ b/demos/pycram_gpsr_demo/high_level_plans.py @@ -3,6 +3,7 @@ from pycram.designators.action_designator import * from pycram.utilities.robocup_utils import StartSignalWaiter, pakerino from demos.pycram_gpsr_demo import perception_interface, llp_tell_stuff +import demos.pycram_gpsr_demo.perception_interface as robokudo from demos.pycram_gpsr_demo import knowrob_interface as knowrob from demos.pycram_gpsr_demo import llp_navigation as navi import demos.pycram_gpsr_demo.utils as utils @@ -16,6 +17,7 @@ object_in_hand = None me_pose = None +found_obj = None # these are all the high level plans, to which we map the NLP output. @@ -68,7 +70,27 @@ def moving_to(param_json): # WIP Can also be funriture, or a person if room and furniture and person: rospy.loginfo(utils.PC.BLUE + "[CRAM] moving to person at furniture item in room") sing_my_angel_of_music(f"I will go to the {furniture} in {room} to look for a person.") - # TODO + #rospy.loginfo(utils.PC.BLUE + "[CRAM] moving to furniture item in room") + #sing_my_angel_of_music(f"I will go to the {furniture} in {room}.") + if furniture: + rospy.loginfo(f"[CRAM] found instance of furniture item " + str(furniture)) + nav_poses = knowrob.get_nav_poses_for_furniture_item(room=room, furniture_name=furniture) + # if instance does not exist,check class? + elif furniture_class: # maybeh make this an and? + if furniture == furniture_class: + # matched knowledge and nlp + nav_poses = knowrob.get_nav_poses_for_furniture_item(room=room, + furniture_name=furniture) # change might get removed + else: + nav_poses = knowrob.get_nav_poses_for_furniture_item(room=room, furniture_iri=furniture_class) + # go to furniture item in room + if nav_poses is not None and nav_poses != []: + # go to furniture item + rospy.loginfo( + utils.PC.BLUE + f"[CRAM] going to furniture item in Room pose {nav_poses[0].get('Item').get('pose')}") + result = navi.go_to_pose(nav_poses[0].get('Item').get('pose')) + rospy.loginfo(utils.PC.BLUE + "[CRAM] result: " + str(result)) + return nav_poses[0] # go to person at furniture item in room # Room + Person WIP--------------------------------------------------- elif room and person: # DONE one value @@ -260,7 +282,7 @@ def placing(param_json): # works testing if not object_in_hand: rospy.logerr("[CRAM] I have no object in my hand so I cannot place :(") return None - sing_my_angel_of_music("in placing plan") + sing_my_angel_of_music("I'm going to do a placing action") # move to playing destination result = moving_to(param_json) if result is None: @@ -275,7 +297,7 @@ def placing(param_json): # works testing # PLACE at PREDEFINED LOCATION FROM KNOWROB if obj_type in perc_to_know.keys(): knowrob_iri = perc_to_know.get('type') - item_location = knowrob.get_predefined_destination_item_location(knowrob_iri) + item_location = knowrob.get_predefined_destination_item_location(knowrob_iri) # CHANGE check if this works destination_link = utils.remove_prefix(item_location.get('Item').get('link'), 'iai_kitchen/') result = plans.place(object=object_in_hand, grasp='front', link=destination_link, giskard=sd.giskard, talk=nlp.tts, @@ -292,10 +314,11 @@ def placing(param_json): # works testing # also finding + searching # Source def looking_for(param_json): # WIP TODO - sing_my_angel_of_music("in looking for plan") + #sing_my_angel_of_music("in looking for plan") + #robokudo.init_robokudo() rospy.loginfo("Looking For: " + str(param_json)) # get vars - item, person, furniture, room = None, None, None, None + item, person, furniture, room, result = None, None, None, None, None if param_json.get('Source') is not None: # ensure furniture obj exists furniture = param_json.get('Source').get('value').lower() @@ -324,12 +347,71 @@ def looking_for(param_json): # WIP TODO if param_json.get('Item') is not None: item = param_json.get('Item').get('value') + rospy.loginfo("[CRAM] looking for item: " + item) + sing_my_angel_of_music("I'm going look for the " + str(item)) # go to location which human said to look for items + if furniture and room: + rospy.loginfo("[CRAM] going to furniture in room") + source_params = {'Source': param_json.get('Source'), 'SourceRoom': param_json.get('SourceRoom')} + source_params = utils.remap_source_to_destination(source_params) + result = moving_to(source_params) + elif room: + rospy.loginfo("[CRAM] going to room") + source_params = {'SourceRoom': param_json.get('SourceRoom')} + source_params = utils.remap_source_to_destination(source_params) + result = moving_to(source_params) + elif furniture: + rospy.loginfo("[CRAM] going to furniture") + source_params = {'Source': param_json.get('Source')} + source_params = utils.remap_source_to_destination(source_params) + result = moving_to(source_params) + elif item: + rospy.loginfo("[CRAM] going to item") + # try anyway? get default location of the item + # check if item is known + if " " in item: # ensure snake case if a space is present + item = snakecase(item) + + if item: + # ensure item exists + rospy.loginfo("[CRAM] looking for item: " + item) + result = knowrob.get_predefined_source_item_location_iri(utils.obj_dict.get(item)) #FIX THIS + if result is None: + result = knowrob.get_predefined_source_item_location_name(item) + rospy.loginfo("[CRAM] result: " + str(result)) + if result: + navi.go_to_pose(result[0].get('Item').get('pose')) + else: + rospy.logerr("[CRAM] Could not find predefined location for item.") + return None + + # assume navigation was successful, and robot is in a perceiving pose... + # point head at correct location + if result: + perceive_conf = {'arm_lift_joint': 0.20, 'wrist_flex_joint': 1.8, 'arm_roll_joint': -1, } + if sd.with_real_robot: + plans.pakerino(config=perceive_conf) + look_at_link = result.get('Item').get('link') + look_at_pose = utils.tf_l.lookupTransform(target_frame='map', + source_frame=look_at_link, + time=rospy.get_rostime()) + if sd.with_real_robot: + sd.giskard.move_head_to_pose(look_at_pose) + # perceive + objects = robokudo.ask_robokudo_for_all_objects() + + for obj in objects: + if obj.type == item: + rospy.loginfo("[CRAM] found object of type " + item) + found_obj = obj + return found_obj + # TODO return to person? def transporting(param_json): - sing_my_angel_of_music("in transporting plan") + # TODO add Saying what the robot does + sing_my_angel_of_music("I'm going to do a transporting action") global me_pose person = None if param_json.get('BeneficiaryRole').get('value') == 'me': # TODO handle other people too @@ -353,30 +435,38 @@ def transporting(param_json): else: placing(param_json) +# TODO add this to plan. +# DetectAction(technique='human').resolve().perform() +# giskardpy.move_head_to_human() +# giskardpy.cancel_all_goals() OR giskardpy.cancel_all_called_goals() to stop + + def arranging(param_json): - sing_my_angel_of_music("in arranging plan") + sing_my_angel_of_music("I am sorry. I do not know how to arrange things yet.") rospy.loginfo("arranging: " + str(param_json)) # count obj or person def count(param_json): - sing_my_angel_of_music("in counting plan") + sing_my_angel_of_music("I am sorry, I do not know how to count items yet.") rospy.loginfo("count: " + str(param_json)) def cleaning(param_json): - sing_my_angel_of_music("in cleaning plan") + sing_my_angel_of_music("I am sorry, I fo not know how to clean up yet. ") rospy.loginfo("cleaning: " + str(param_json)) def guide(param_json): - sing_my_angel_of_music("in guiding plan") + sing_my_angel_of_music("please follow me") + moving_to(param_json) + sing_my_angel_of_music("I have arrived. thank you for following me. ") rospy.loginfo("guide: " + str(param_json)) def accompany(param_json): - sing_my_angel_of_music("in accompany plan") + sing_my_angel_of_music("I am sorry. I do now know how to accompany yet.") rospy.loginfo("accompany: " + str(param_json)) @@ -400,22 +490,25 @@ def assert_preferece(param_json): # todo def greet(param_json): - sing_my_angel_of_music("in greeting plan") + sing_my_angel_of_music("I'm going to greet a person") + moving_to(param_json) + # TODO add looking for human + sing_my_angel_of_music("Hello and welcome to the RoboCup 2024 in Eindhoven. I wish you a great day and lots of fun.") rospy.loginfo("greeting") def describe(param_json): - sing_my_angel_of_music("in describe plan") + sing_my_angel_of_music("I am sorry. I do not know how to describe someone yet.") rospy.loginfo("describe: " + str(param_json)) def offer(param_json): - sing_my_angel_of_music("in offer plan") + sing_my_angel_of_music("I am sorry. I do not know how to offer something yet.") rospy.loginfo("offer: " + str(param_json)) def follow(param_json): - sing_my_angel_of_music("in follow plan") + sing_my_angel_of_music("Sorry. I am unsure how to follow you. ") rospy.loginfo("follow: " + str(param_json)) diff --git a/demos/pycram_gpsr_demo/knowrob_interface.py b/demos/pycram_gpsr_demo/knowrob_interface.py index 100847e2d..43098b939 100644 --- a/demos/pycram_gpsr_demo/knowrob_interface.py +++ b/demos/pycram_gpsr_demo/knowrob_interface.py @@ -172,10 +172,11 @@ def check_existence_of_class(nlp_name): # get iri from objects.py mapping -def get_predefined_source_item_location(items_iri): - if "'" not in items_iri: - items_iri = "'" + items_iri + "'" - knowrob_poses_list = kb.prolog_client.all_solutions(f"predefined_origin_location({items_iri}, Furniture), " +def get_predefined_source_item_location_name(item_name): + if " " in item_name: # ensure snake case if a space is present + item_name = snakecase(item_name) + knowrob_poses_list = kb.prolog_client.all_solutions(f"what_object_transitive('{item_name}', Obj)," + f"predefined_origin_location(Obj, Furniture), " f"furniture_rel_pose(Furniture, 'perceive', Pose).") poses_list = [] if knowrob_poses_list: @@ -185,6 +186,22 @@ def get_predefined_source_item_location(items_iri): return poses_list +def get_predefined_source_item_location_iri(item_iri): + # TODO ensure the pose is from water and not just liquid but it is a nice fallback? + if "'" not in item_iri: + item_iri = "'" + item_iri + "'" + knowrob_poses_list = kb.prolog_client.all_solutions(f"what_object_transitive(Name, {item_iri})," + f"predefined_origin_location({item_iri}, Furniture), " + f"furniture_rel_pose(Furniture, 'perceive', Pose).") + poses_list = [] + if knowrob_poses_list: + poses_list = utils.knowrob_poses_result_to_list_dict(knowrob_poses_list) + else: + rospy.logerr("[KnowRob] query returned empty :(") + return None + return poses_list + + def get_predefined_destination_item_location(items_iri): if "'" not in items_iri: items_iri = "'" + items_iri + "'" @@ -195,6 +212,7 @@ def get_predefined_destination_item_location(items_iri): poses_list = utils.knowrob_poses_result_to_list_dict(knowrob_poses_list) else: rospy.logerr("[KnowRob] query returned empty :(") + return None return poses_list diff --git a/demos/pycram_gpsr_demo/llp_navigation.py b/demos/pycram_gpsr_demo/llp_navigation.py index e60e83135..46f3022a8 100644 --- a/demos/pycram_gpsr_demo/llp_navigation.py +++ b/demos/pycram_gpsr_demo/llp_navigation.py @@ -6,7 +6,7 @@ # these are all the low level plans which are used by the high level plans - +# e.g. 'living_room' and 'entry' def go_to_room_entry_or_exit(room_name, entry_exit): # test # go to the room entry point pose = knowrob.get_room_pose(room=room_name, entry_or_exit=entry_exit) @@ -28,7 +28,7 @@ def go_to_room_middle(room_name): # test pose = knowrob.get_room_middle_pose(room=room_name) rospy.loginfo(utils.PC.GREEN + f"[Go-To-Pose] Going to: {pose}.") if pose is None: - rospy.loginfo(utils.PC.GREEN +"[CRAM] KnowRob result was empty.") + rospy.loginfo(utils.PC.GREEN + "[CRAM] KnowRob result was empty.") sing_my_angel_of_music("I am sorry. I don't know where " + room_name + "is.") return None # abort mission else: diff --git a/demos/pycram_gpsr_demo/nlp_processing.py b/demos/pycram_gpsr_demo/nlp_processing.py index fd2870a9e..86cb7cd67 100644 --- a/demos/pycram_gpsr_demo/nlp_processing.py +++ b/demos/pycram_gpsr_demo/nlp_processing.py @@ -18,7 +18,7 @@ currentSpeech = "" stoppedSpeaking = Condition() canSpeak = False # CHANGE set to TRUE for real robot -canListen = False # CHANGE set to TRUE when NLP and Whisper enabled +canListen = True # CHANGE set to TRUE when NLP and Whisper enabled canDisplay = False @@ -104,7 +104,7 @@ def nlp_listening(): #rospy.wait_for_message('nlp_out', String, timeout=20) with haveNLPOutput: haveNLPOutput.wait(30) - rospy.loginfo("NLP data notification received.") + rospy.loginfo("[NLP] data notification received.") # process output from NLP rospy.loginfo("message received. todo_plans: " + str(todo_plans)) return todo_plans @@ -113,6 +113,7 @@ def nlp_listening(): # todo: are there special functions for confirmation? def confirm_nlp_output(received_output): global confirm, todo_plans + rospy.loginfo("[NLP] Please confirm yes/no") # concatenate output into a whole sentence again whole_sentence = '' for sentence in received_output: @@ -136,7 +137,7 @@ def confirm_nlp_output(received_output): # --- THIS IS THE MAIN FUNCTION FOR NLP --- def listen_to_commands(): - #nlp_subscribe() done during setup + nlp_subscribe() # CHANGE !!! done during setup !!! ENABLE FOR TESTING ONLY. DISABLE FOR REAL ROBOT global response, todo_plans while True: if canDisplay: diff --git a/demos/pycram_gpsr_demo/objects.py b/demos/pycram_gpsr_demo/objects.py index ac4a569c3..a095d804b 100644 --- a/demos/pycram_gpsr_demo/objects.py +++ b/demos/pycram_gpsr_demo/objects.py @@ -1,13 +1,13 @@ # trash_bin = 'http://www.ease-crc.org/ont/SUTURO.owl#TrashBin' # plastic_knife = 'http://www.ease-crc.org/ont/SUTURO.owl#PlasticKnife' # designed_chair = 'http://www.ease-crc.org/ont/SOMA.owl#DesignedChair' -# cupboard = 'http://www.ease-crc.org/ont/SOMA.owl#Cupboard' +cupboard = 'http://www.ease-crc.org/ont/SOMA.owl#Cupboard' # foam_brick = 'http://www.ease-crc.org/ont/SUTURO.owl#FoamBrick' fork = 'http://www.ease-crc.org/ont/SOMA.owl#Fork' # spatula = 'http://www.ease-crc.org/ont/SOMA.owl#Spatula' pitcher = 'http://www.ease-crc.org/ont/SUTURO.owl#Pitcher' bleach_cleanser_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#BleachCleanserBottle' -# kitchen_cabinet = 'http://www.ease-crc.org/ont/SOMA.owl#KitchenCabinet' +kitchen_cabinet = 'http://www.ease-crc.org/ont/SOMA.owl#KitchenCabinet' cracker_box = 'http://www.ease-crc.org/ont/SUTURO.owl#CrackerBox' mini_soccer_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#MiniSoccerBall' baseball = 'http://www.ease-crc.org/ont/SUTURO.owl#Baseball' @@ -18,7 +18,7 @@ # napkin = 'http://www.ease-crc.org/ont/SUTURO.owl#Napkin' mustard_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#MustardBottle' jell_ochocolate_pudding_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JellOChocolatePuddingBox' -# door = 'http://www.ease-crc.org/ont/SOMA.owl#Door' +door = 'http://www.ease-crc.org/ont/SOMA.owl#Door' # serving_mat = 'http://www.ease-crc.org/ont/SUTURO.owl#ServingMat' # dishwasher = 'http://www.ease-crc.org/ont/SOMA.owl#Dishwasher' # natural_person = 'http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#NaturalPerson' @@ -26,8 +26,8 @@ shelf_layer = 'http://www.ease-crc.org/ont/SUTURO.owl#ShelfLayer' # tray = 'http://www.ease-crc.org/ont/SUTURO.owl#Tray' orange = 'http://www.ease-crc.org/ont/SUTURO.owl#Orange' -# table = 'http://www.ease-crc.org/ont/SOMA.owl#Table' -# bowl = 'http://www.ease-crc.org/ont/SOMA.owl#Bowl' +table = 'http://www.ease-crc.org/ont/SOMA.owl#Table' +bowl = 'http://www.ease-crc.org/ont/SOMA.owl#Bowl' # glass = 'http://www.ease-crc.org/ont/SOMA.owl#Glass' coffee_pack = 'http://www.ease-crc.org/ont/SUTURO.owl#CoffeePack' # golf_ball = 'http://www.ease-crc.org/ont/SUTURO.owl#GolfBall' @@ -50,28 +50,28 @@ dishwasher_tab = 'http://www.ease-crc.org/ont/SOMA.owl#DishwasherTab' knife = 'http://www.ease-crc.org/ont/SOMA.owl#Knife' # plastic_chain = 'http://www.ease-crc.org/ont/SUTURO.owl#PlasticChain' -# plate = 'http://www.ease-crc.org/ont/SOMA.owl#Plate' -# cup = 'http://www.ease-crc.org/ont/SOMA.owl#Cup' +plate = 'http://www.ease-crc.org/ont/SOMA.owl#Plate' + # dice = 'http://www.ease-crc.org/ont/SUTURO.owl#Dice' cereal_box = 'http://www.ease-crc.org/ont/SOMA.owl#CerealBox' metal_bowl = 'http://www.ease-crc.org/ont/SUTURO.owl#MetalBowl' sugar_box = 'http://www.ease-crc.org/ont/SUTURO.owl#SugarBox' # skillet = 'http://www.ease-crc.org/ont/SUTURO.owl#Skillet' -# kiwi = 'http://www.ease-crc.org/ont/SUTURO.owl#Kiwi' +kiwi = 'http://www.ease-crc.org/ont/SUTURO.owl#Kiwi' # drawer = 'http://www.ease-crc.org/ont/SOMA.owl#Drawer' # toyota_hsr = 'http://www.ease-crc.org/ont/SUTURO.owl#ToyotaHSR' coffee_can = 'http://www.ease-crc.org/ont/SUTURO.owl#CoffeeCan' -milk_pack = 'http://www.ease-crc.org/ont/SOMA.owl#MilkPack' +#milk_pack = 'http://www.ease-crc.org/ont/SOMA.owl#MilkPack' # toothpaste = 'http://www.ease-crc.org/ont/SUTURO.owl#Toothpaste' # oregano_shaker = 'http://www.ease-crc.org/ont/SUTURO.owl#OreganoShaker' apple = 'http://www.ease-crc.org/ont/SUTURO.owl#Apple' tomato_soup_can = 'http://www.ease-crc.org/ont/SUTURO.owl#TomatoSoupCan' tuna_fish_can = 'http://www.ease-crc.org/ont/SUTURO.owl#TunaFishCan' # bag = 'http://www.ease-crc.org/ont/SUTURO.owl#Bag' -# juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox' +juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#JuiceBox' # pancake_mix_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#PancakeMixBottle' gelatine_box = 'http://www.ease-crc.org/ont/SUTURO.owl#GelatineBox' -# tea_bag_box = 'http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox' +#tea_bag_box = 'http://www.ease-crc.org/ont/SUTURO.owl#TeaBagBox' # pastry_box = 'http://www.ease-crc.org/ont/SUTURO.owl#PastryBox' pear = 'http://www.ease-crc.org/ont/SUTURO.owl#Pear' lemon = 'http://www.ease-crc.org/ont/SUTURO.owl#Lemon' @@ -79,52 +79,98 @@ potted_meat_can = 'http://www.ease-crc.org/ont/SUTURO.owl#PottedMeatCan' peach = 'http://www.ease-crc.org/ont/SUTURO.owl#Peach' plum = 'http://www.ease-crc.org/ont/SUTURO.owl#Plum' -# wine_bottle = 'http://www.ease-crc.org/ont/SOMA.owl#WineBottle' +wine_bottle = 'http://www.ease-crc.org/ont/SOMA.owl#WineBottle' # juice_pack = 'http://www.ease-crc.org/ont/SUTURO.owl#JuicePack' -# cola_can = 'http://www.ease-crc.org/ont/SUTURO.owl#ColaCan' + # tropical_juice_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#TropicalJuiceBottle' milk_bottle = 'http://www.ease-crc.org/ont/SOMA.owl#MilkBottle' -# ice_tea_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#IceTeaBottle' + # orange_juice_box = 'http://www.ease-crc.org/ont/SUTURO.owl#OrangeJuiceBox' rubiks_cube = 'http://www.ease-crc.org/ont/SUTURO.owl#RubiksCube' # cereal_box_robo_cup = 'http://www.ease-crc.org/ont/SUTURO.owl#CerealBoxRoboCup' # nesquick_cereal_box = 'http://www.ease-crc.org/ont/SUTURO.owl#NesquickCerealBox' milk_pack_ja = 'http://www.ease-crc.org/ont/SUTURO.owl#MilkPackJa' +milk = 'http://www.ease-crc.org/ont/SUTURO.owl#MilkPackJa' +milk_pack = 'http://www.ease-crc.org/ont/SUTURO.owl#MilkPackJa' muesli_box = 'http://www.ease-crc.org/ont/SUTURO.owl#MuesliBox' -# stapel_chips = 'http://www.ease-crc.org/ont/SUTURO.owl#StapelChips' -cup_blue = 'http://www.ease-crc.org/ont/SUTURO.owl#CupBlue' -cup_green = 'http://www.ease-crc.org/ont/SUTURO.owl#CupGreen' +stapel_chips = 'http://www.ease-crc.org/ont/SUTURO.owl#StapelChips' +staple_chips = 'http://www.ease-crc.org/ont/SUTURO.owl#StapelChips' + large_marker = 'http://www.ease-crc.org/ont/SUTURO.owl#LargeMarker' masterchef_can = 'http://www.ease-crc.org/ont/SUTURO.owl#MasterchefCan' scissors = 'http://www.ease-crc.org/ont/SUTURO.owl#Scissors' scrub_cleaner = 'http://www.ease-crc.org/ont/SUTURO.owl#ScrubCleaner' grapes = 'http://www.ease-crc.org/ont/SUTURO.owl#Grapes' +cup = 'http://www.ease-crc.org/ont/SOMA.owl#Cup' cup_small = 'http://www.ease-crc.org/ont/SUTURO.owl#CupSmall' +cup_blue = 'http://www.ease-crc.org/ont/SUTURO.owl#CupBlue' +cup_green = 'http://www.ease-crc.org/ont/SUTURO.owl#CupGreen' screw_driver = 'http://www.ease-crc.org/ont/SUTURO.owl#ScrewDriver' clamp = 'http://www.ease-crc.org/ont/SUTURO.owl#Clamp' hammer = 'http://www.ease-crc.org/ont/SUTURO.owl#Hammer' wood_block = 'http://www.ease-crc.org/ont/SUTURO.owl#WoodBlock' corny_box = 'http://www.ease-crc.org/ont/SUTURO.owl#CornyBox' sponge = 'http://www.ease-crc.org/ont/SUTURO.owl#Sponge' + wash_cloth = 'http://www.ease-crc.org/ont/SUTURO.owl#WashCloth' +washcloth = 'http://www.ease-crc.org/ont/SUTURO.owl#WashCloth' + soap = 'http://www.ease-crc.org/ont/SUTURO.owl#Soap' fanta_can = 'http://www.ease-crc.org/ont/SUTURO.owl#FantaCan' +fanta = 'http://www.ease-crc.org/ont/SUTURO.owl#FantaCan' ice_tea_can = 'http://www.ease-crc.org/ont/SUTURO.owl#IceTeaCan' +ice_tea_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#IceTeaBottle' +ice_tea = 'http://www.ease-crc.org/ont/SUTURO.owl#IceTeaCan' + dubbel_fris = 'http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris' +double_freeze = 'http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris' +double_free = 'http://www.ease-crc.org/ont/SUTURO.owl#DubbelFris' + water_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#WaterBottle' +water = 'http://www.ease-crc.org/ont/SUTURO.owl#WaterBottle' cola_bottle = 'http://www.ease-crc.org/ont/SUTURO.owl#ColaBottle' +cola = 'http://www.ease-crc.org/ont/SUTURO.owl#Cola' +cola_can = 'http://www.ease-crc.org/ont/SUTURO.owl#ColaCan' pea_soup_can = 'http://www.ease-crc.org/ont/SUTURO.owl#PeaSoupCan' +pea_soup = 'http://www.ease-crc.org/ont/SUTURO.owl#PeaSoupCan' cornflakes = 'http://www.ease-crc.org/ont/SUTURO.owl#Cornflakes' + + curry = 'http://www.ease-crc.org/ont/SUTURO.owl#Curry' +ketchup = 'http://www.ease-crc.org/ont/SUTURO.owl#Curry' + mayonaise = 'http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise' +mayonnaise = 'http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise' +myonies = 'http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise' +myunays = 'http://www.ease-crc.org/ont/SUTURO.owl#Mayonaise' + hagelslag = 'http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag' +huggle_slug = 'http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag' +huggle_sluck = 'http://www.ease-crc.org/ont/SUTURO.owl#Hagelslag' + sausages = 'http://www.ease-crc.org/ont/SUTURO.owl#Sausages' pancake_mix = 'http://www.ease-crc.org/ont/SUTURO.owl#PancakeMix' crisps_bag = 'http://www.ease-crc.org/ont/SUTURO.owl#CrispsBag' +crisps = 'http://www.ease-crc.org/ont/SUTURO.owl#CrispsBag' + candy = 'http://www.ease-crc.org/ont/SUTURO.owl#Candy' + + stroopwafels = 'http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels' +# alternatives: +stroke_waffle = 'http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels' +struck_waffles = 'http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels' +stroke_waffles = 'http://www.ease-crc.org/ont/SUTURO.owl#Stroopwafels' + + liquorice = 'http://www.ease-crc.org/ont/SUTURO.owl#Liquorice' +light_degrees = 'http://www.ease-crc.org/ont/SUTURO.owl#Liquorice' + tic_tac = 'http://www.ease-crc.org/ont/SUTURO.owl#TicTac' +tick_tacks = 'http://www.ease-crc.org/ont/SUTURO.owl#TicTac' +tick_tack = 'http://www.ease-crc.org/ont/SUTURO.owl#TicTac' +take_tack = 'http://www.ease-crc.org/ont/SUTURO.owl#TicTac' + candle = 'http://www.ease-crc.org/ont/SUTURO.owl#Candle' grocery_bag = 'http://www.ease-crc.org/ont/SUTURO.owl#GroceryBag' # Furniture diff --git a/demos/pycram_gpsr_demo/perception_interface.py b/demos/pycram_gpsr_demo/perception_interface.py index 6df9db92a..ab031dca1 100644 --- a/demos/pycram_gpsr_demo/perception_interface.py +++ b/demos/pycram_gpsr_demo/perception_interface.py @@ -30,7 +30,7 @@ def make_robokudo_obj_msg(item_json): return goal_msg -def ask_robokudo_for_all_objects(): +def ask_robokudo_for_all_objects(): # works global rk goal_msg = QueryGoal() goal_msg.type = 'all' @@ -56,6 +56,23 @@ def ask_robokudo_for_object(obj_type): return result # list of all perceived items or an empty list +# person = attributes of person, lying, standing etc. +# faces = face detection +def ask_robokudo_for_humans(): + global rk + goal_msg = QueryGoal() + goal_msg.obj.type = 'person' + goal_msg.type = 'person' + rk.send_goal(goal_msg) + rospy.loginfo("[RK] goal sent... waiting for result") + rospy.wait_for_message(topic='/robokudo/query/result', topic_type=QueryActionResult, timeout=15) + result = rk.get_result() + rospy.loginfo("[RK] result received") + return result # list of all perceived items or an empty list + + +# DetectHumanAction ohne face. + def send_robokudo_goal(goal_msg): global rk rk.send_goal(goal_msg) @@ -66,6 +83,23 @@ def send_robokudo_goal(goal_msg): return result +# all poses are returned in map frame +def process_robokudo_obj_result(result): + result_dict = {} + for msg in result.res: + entry = {} + entry = { + 'type': msg.type, + 'color': msg.color, + 'attribute': msg.attribute, + 'pose': msg.pose, + 'pose_source': msg.pose_source, + 'description': msg.description + } + result_dict[msg.type] = entry + return result_dict + + # todo test end here------ ### --- REPL testing --- diff --git a/demos/pycram_gpsr_demo/run_demo.py b/demos/pycram_gpsr_demo/run_demo.py index 795bb69b8..1a6096425 100644 --- a/demos/pycram_gpsr_demo/run_demo.py +++ b/demos/pycram_gpsr_demo/run_demo.py @@ -1,22 +1,151 @@ +from dynamic_reconfigure.msg import DoubleParameter, IntParameter, BoolParameter, StrParameter, GroupState, Config +from dynamic_reconfigure.srv import Reconfigure, ReconfigureRequest +from geometry_msgs.msg import Twist + from demos.pycram_gpsr_demo.setup_demo import * from demos.pycram_gpsr_demo import utils, setup_demo import demos.pycram_gpsr_demo.nlp_processing as nlp from stringcase import snakecase +import demos.pycram_gpsr_demo.llp_navigation as navi from demos.pycram_gpsr_demo.nlp_processing import sing_my_angel_of_music import src.pycram.utilities.gpsr_utils as gpsr_utils +from pycram.enums import ObjectType, ImageEnum +from pycram.language import Code, Monitor + +instruction_point = PoseStamped([6.28, 1.0, 0], [0, 0, 0, 1]) + + +def move_vel(speed, distance, isForward, angle=0): + # Starts a new node + velocity_publisher = rospy.Publisher('/hsrb/command_velocity', Twist, queue_size=10) + vel_msg = Twist() + + # Checking if the movement is forward or backwards + if isForward: + vel_msg.linear.x = abs(speed) + else: + vel_msg.linear.x = 0 + if angle > 0: + vel_msg.angular.z = angle + else: + vel_msg.angular.z = 0 + # Since we are moving just in x-axis + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + + # Setting the current time for distance calculation + t0 = rospy.Time.now().to_sec() + current_distance = 0 + + # Loop to move the turtle a specified distance + while not rospy.is_shutdown() and current_distance < distance: + # Publish the velocity + velocity_publisher.publish(vel_msg) + # Take actual time to velocity calculation + t1 = rospy.Time.now().to_sec() + # Calculate distance + current_distance = speed * (t1 - t0) + + # After the loop, stop the robot + vel_msg.linear.x = 0 + # Force the robot to stop + velocity_publisher.publish(vel_msg) + + +def set_parameters(new_parameters): + rospy.wait_for_service('/tmc_map_merger/inputs/base_scan/obstacle_circle/set_parameters') + try: + reconfigure_service = rospy.ServiceProxy('/tmc_map_merger/inputs/base_scan/obstacle_circle/set_parameters', + Reconfigure) + config = Config() + + # Set the new parameters + if 'forbid_radius' in new_parameters: + config.doubles.append(DoubleParameter(name='forbid_radius', value=new_parameters['forbid_radius'])) + if 'obstacle_occupancy' in new_parameters: + config.ints.append(IntParameter(name='obstacle_occupancy', value=new_parameters['obstacle_occupancy'])) + if 'obstacle_radius' in new_parameters: + config.doubles.append(DoubleParameter(name='obstacle_radius', value=new_parameters['obstacle_radius'])) + + # Empty parameters that are not being set + config.bools.append(BoolParameter(name='', value=False)) + config.strs.append(StrParameter(name='', value='')) + config.groups.append(GroupState(name='', state=False, id=0, parent=0)) + + req = ReconfigureRequest(config=config) + reconfigure_service(req) + rospy.loginfo("Parameters updated successfully") + + except rospy.ServiceException as e: + rospy.logerr("Service call failed: %s" % e) + + +def yeet_into_arena(): + try: + + image_switch.pub_now(ImageEnum.HI.value) # hi im toya + sing_my_angel_of_music("Push down my Hand, when you are Ready.") + image_switch.pub_now(ImageEnum.PUSHBUTTONS.value) + plan = Code(lambda: rospy.sleep(1)) * 999999 >> Monitor(monitor_func) + plan.perform() + except SensorMonitoringCondition: + sing_my_angel_of_music("Starting Storing Grocery.") + # load everything world giskard robokudo.... + # Wait for the start signal + + image_switch.pub_now(ImageEnum.HI.value) + start_signal_waiter.wait_for_startsignal() + + # Once the start signal is received, continue with the rest of the script + rospy.loginfo("Start signal received, now proceeding with tasks.") + move_vel(speed=2, distance=4, isForward=True) + rospy.sleep(2) + new_parameters = { + 'forbid_radius': 0.2, + 'obstacle_occupancy': 5, + 'obstacle_radius': 0.2 + } + + set_parameters(new_parameters) # Once the start signal is received, continue with the rest of the script + + rospy.sleep(1) + + fake_pose_2 = Pose([3, 0.3, 0]) + move.pub_fake_pose(fake_pose_2) + move_vel(0.2, 2, False, 0.06) + sing_my_angel_of_music("Driving.") + move_123 = Pose([4, -0.2, 0], [0, 0, 0.7, 0.7]) + move.pub_now(move_123) + move_145 = Pose([4.8, 0.8, 0], [0, 0, 0.7, 0.7]) + move.pub_now(move_145) # --- main control --- # TODO: test on real robot def gpsr(): with real_robot: + yeet_into_arena() plan_list = utils.get_plans(high_level_plans) # plan_list = utils.get_plans(high_level_plans) # get list of all available plans # init params # go to instruction point, look at human, etc. - # high_level_plans.prepare_for_commands() + # TODO add Vanessa's run into arena part + sing_my_angel_of_music("Going to the instruction point") + navi.go_to_pose(PoseStamped([3.0, 0.5, 0.0], [0, 0, 0, 1])) # in door + navi.go_to_room_entry_or_exit('living_room', 'entry') + navi.go_to_pose(instruction_point) + # look at a person when listening to command + + # high_level_plans.prepare_for_commands() + instruction_list = [] for i in range(5): # test + # TODO add this to plan. + # DetectAction(technique='human').resolve().perform() + # giskardpy.move_head_to_human() + # giskardpy.cancel_all_goals() OR giskardpy.cancel_all_called_goals() to stop # listen to instructions instruction_list = nlp.listen_to_commands() rospy.logwarn("[CRAM] instruction list: " + str(instruction_list)) @@ -30,7 +159,10 @@ def gpsr(): # do stuff # match instruction to plan utils.call_plan_by_name(plan_list, snakecase(instruction['intent']), instruction) - # if plan was successfull, remove it from the list + # if plan was successful, remove it from the list + instruction_list = [] + sing_my_angel_of_music("navigating to the instruction point") + navi.go_to_pose(instruction_point) data = { @@ -106,7 +238,31 @@ def gpsr(): } } - +data3 = { + "sentence": "Please bring the red cup from the kitchen table to the living room table", + "intent": "LookingFor", + "Item": { + "value": "cup", + "entity": "Transportable", + "propertyAttribute": [], + "actionAttribute": [], + "numberAttribute": [] + } #, + # "Source": { + # "value": "coffee table", + # "entity": "DesignedFurniture", + # "propertyAttribute": [], + # "actionAttribute": [], + # "numberAttribute": [] + # }, + # "SourceRoom": { + # "value": "living room", + # "entity": "Room", + # "propertyAttribute": [], + # "actionAttribute": [], + # "numberAttribute": [] + # } +} # CHANGE CARE THIS STUFF GETS ACTUALLY EXECUTED @@ -116,7 +272,6 @@ def demo_plan(data): print('--------------stahp----------------') return - #setup() #gpsr() #demo_plan(data2) diff --git a/demos/pycram_gpsr_demo/setup_demo.py b/demos/pycram_gpsr_demo/setup_demo.py index f1e7c7a8b..73afa1160 100755 --- a/demos/pycram_gpsr_demo/setup_demo.py +++ b/demos/pycram_gpsr_demo/setup_demo.py @@ -17,7 +17,7 @@ with_real_robot = False # CHANGE set to TRUE for real robot # initialize interfaces -instruction_point = PoseStamped([1.45, 4.5, 0], [0, 0, 1, 0]) +#instruction_point = PoseStamped([1.45, 4.5, 0], [0, 0, 1, 0]) world = None robot = None environment_raw = None diff --git a/demos/pycram_gpsr_demo/utils.py b/demos/pycram_gpsr_demo/utils.py index 934dd27ad..c2ff453ea 100755 --- a/demos/pycram_gpsr_demo/utils.py +++ b/demos/pycram_gpsr_demo/utils.py @@ -267,5 +267,6 @@ def remove_prefix(text, prefix): return text[len(prefix):] return text + # autogenerate a dict from all defined objects in the objects.py file obj_dict = autogenerate_dict_from_file(objects_path) diff --git a/src/pycram/utilities/gpsr_utils.py b/src/pycram/utilities/gpsr_utils.py index 191bebeeb..3c7a9c23e 100644 --- a/src/pycram/utilities/gpsr_utils.py +++ b/src/pycram/utilities/gpsr_utils.py @@ -14,50 +14,6 @@ from pycram.utilities.robocup_utils import TextToSpeechPublisher, ImageSwitchPublisher, StartSignalWaiter, \ HSRBMoveGripperReal, pakerino, GraspListener -# world = None # BulletWorld() -# environment_raw = None # Object("kitchen", ObjectType.ENVIRONMENT, "pre_robocup_sg.urdf") -# environment_desig = None #ObjectDesignatorDescription(names=["kitchen"]) -# -# grasp_listener = None # GraspListener() -# talk = None # TextToSpeechPublisher() -# img_swap = None # ImageSwitchPublisher() -# start_signal_waiter = None # StartSignalWaiter() -# move = None # PoseNavigator() -# lt = None # LocalTransformer() -# gripper = None # HSRBMoveGripperReal() -# robot = None # Object("hsrb", "robot", "../../resources/" + "hsrb" + ".urdf") -# #robot.set_color([0.5, 0.5, 0.9, 1]) -# robot_desig = None # ObjectDesignatorDescription(names=["hsrb"]) -# giskard_obj = None -# # giskardpy.init_giskard_interface() -# # giskardpy.clear() -# # giskardpy.sync_worlds() -# -# previous_value = None - - -# TODO find a better way of doing this -# def handover_all_variables(other_world, other_environment_raw, other_environment_desig, other_grasp_listener, -# other_talk, other_img_swap, other_start_signal_waiter, other_move, other_lt, other_gripper, -# other_robot, other_robot_desig, other_giskard): -# global world, environment_raw, environment_desig, grasp_listener, talk, img_swap, start_signal_waiter, move, lt -# global gripper, robot, robot_desig, giskard_obj -# rospy.loginfo("[PLAN-CRAM] remapping...") -# world = other_world -# environment_raw = other_environment_raw -# environment_desig = other_environment_desig -# grasp_listener = other_grasp_listener -# talk = other_talk -# img_swap = other_img_swap -# start_signal_waiter = other_start_signal_waiter -# move = other_move -# lt = other_lt -# gripper = other_gripper -# robot = other_robot -# robot_desig = other_robot_desig -# giskard_obj = other_giskard -# rospy.loginfo('\033[92m' + "[PLAN-CRAM] done remapping") # green - previous_value = None @@ -84,7 +40,8 @@ def get_place_poses_for_surface(object_to_place, link, environment_desig, enviro return NoPlacePoseFoundCondition -def place(object, grasp, link, giskard, talk, robot_description, lt, environment_raw, environment_desig, gripper, world, robot_desig): +def place(object, grasp, link, giskard, talk, robot_description, lt, environment_raw, environment_desig, gripper, world, + robot_desig): def monitor_func_place(): global previous_value der = fts.get_last_value() @@ -127,7 +84,8 @@ def calculate_dynamic_threshold(previous_force_x): talk.pub_now("I was not able to perceive any objects") #todo ifno target_location then do handover - target_location = get_place_poses_for_surface(object_to_place=object, link=link, environment_desig=environment_desig, + target_location = get_place_poses_for_surface(object_to_place=object, link=link, + environment_desig=environment_desig, environment_raw=environment_raw, robot_desig=robot_desig, world=world, lt=lt) @@ -363,4 +321,45 @@ def pick_place_demo(move): move.pub_now(shelf_pose) place(found_object, "front", 'shelf:shelf:shelf_floor_2') -#pick_place_demo() + +# ----- PERCEIVE ----------- +# def perceive(obj_type, link, look_pose_given, environment_raw, giskardpy, gripper, talk): +# try: +# look_pose = environment_raw.get_link_pose(link) +# except KeyError as e: +# rospy.logerr(f"[CRAM] cool not find link {link} in environment_urdf of BulletWorld. Fallback to given pose") +# look_pose = look_pose_given +# # park +# gripper.pub_now('close') +# perceive_conf = {'arm_lift_joint': 0.20, 'wrist_flex_joint': 1.8, 'arm_roll_joint': -1, } +# pakerino(config=perceive_conf) +# # look +# giskardpy.move_head_to_pose(look_pose) +# +# talk.pub_now("perceiving", True) +# # noteme detect on table +# try: +# table_obj = DetectAction(technique='all').resolve().perform() +# found_object = None +# first, *remaining = table_obj +# print(first) +# for dictionary in remaining: +# for value in dictionary.values(): +# # check if type matches exactly +# if value.type == obj_type: +# found_object = value +# +# if found_object is None: +# for dictionary in remaining: +# for value in dictionary.values(): +# # check if substring matches +# if obj_type in value.type: # CHANGE todo ensure +# found_object = value +# found_object_name = found_object.bullet_world_object.name.replace('_', ' ') +# found_object_name = re.sub(r'\d+', '', found_object_name) +# talk.pub_now(f"I haven't seen {obj_type}, but found {found_object_name}") +# +# giskardpy.sync_worlds() +# except PerceptionObjectNotFound: +# talk.pub_now("I was not able to perceive any objects") +# return From 5aeb57b4c561df80ac2ae5dd5c67929b56d48ce7 Mon Sep 17 00:00:00 2001 From: HaSu Date: Fri, 19 Jul 2024 01:00:13 +0200 Subject: [PATCH 7/8] [GPSR] added looking at people when receiving commands --- .../pycram_gpsr_demo/perception_interface.py | 20 +++++++++++++++++++ demos/pycram_gpsr_demo/run_demo.py | 5 ++++- 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/demos/pycram_gpsr_demo/perception_interface.py b/demos/pycram_gpsr_demo/perception_interface.py index ab031dca1..a1fa35436 100644 --- a/demos/pycram_gpsr_demo/perception_interface.py +++ b/demos/pycram_gpsr_demo/perception_interface.py @@ -1,5 +1,7 @@ import rospy import actionlib +from rospy import ROSException + from demos.pycram_gpsr_demo.utils import find_color from pycram.designators.action_designator import LookAtAction, DetectAction from pycram.process_module import real_robot @@ -117,6 +119,24 @@ def test_pc(): print(result.res[0].type) + +def looking_for_human(): + # loop until a human is seen + check_human = False + while not check_human: + try: + rospy.loginfo("[CRAM] Looking for human...") + result = ask_robokudo_for_humans() + if result is not None: + check_human = True + rospy.loginfo("[CRAM] Human found") + else: + rospy.loginfo("[CRAM] No human found") + rospy.sleep(3) + except ROSException: + rospy.loginfo("[CRAM] No human found and robokudo timed out") + return check_human + def test_lookat(pose): with real_robot: LookAtAction(targets=[pose]).resolve().perform() diff --git a/demos/pycram_gpsr_demo/run_demo.py b/demos/pycram_gpsr_demo/run_demo.py index 1a6096425..bce28d340 100644 --- a/demos/pycram_gpsr_demo/run_demo.py +++ b/demos/pycram_gpsr_demo/run_demo.py @@ -3,7 +3,7 @@ from geometry_msgs.msg import Twist from demos.pycram_gpsr_demo.setup_demo import * -from demos.pycram_gpsr_demo import utils, setup_demo +from demos.pycram_gpsr_demo import utils, setup_demo, perception_interface import demos.pycram_gpsr_demo.nlp_processing as nlp from stringcase import snakecase import demos.pycram_gpsr_demo.llp_navigation as navi @@ -147,8 +147,11 @@ def gpsr(): # giskardpy.move_head_to_human() # giskardpy.cancel_all_goals() OR giskardpy.cancel_all_called_goals() to stop # listen to instructions + perception_interface.looking_for_human() + giskard.move_head_to_human() instruction_list = nlp.listen_to_commands() rospy.logwarn("[CRAM] instruction list: " + str(instruction_list)) + giskard.cancel_all_called_goals() # execute instructions # TODO iterate over list of instructions and do stuff From 932f8d99dbab5ddc27b6381bcd3e774def0ceb3e Mon Sep 17 00:00:00 2001 From: HaSu Date: Fri, 19 Jul 2024 20:52:57 +0200 Subject: [PATCH 8/8] [GPSR] Challenge status update --- demos/pycram_gpsr_demo/nlp_processing.py | 4 +- .../pycram_gpsr_demo/perception_interface.py | 8 +- demos/pycram_gpsr_demo/run_demo.py | 171 +++++------------- demos/pycram_gpsr_demo/setup_demo.py | 5 +- src/pycram/designators/action_designator.py | 2 +- 5 files changed, 56 insertions(+), 134 deletions(-) diff --git a/demos/pycram_gpsr_demo/nlp_processing.py b/demos/pycram_gpsr_demo/nlp_processing.py index 86cb7cd67..89a9f345f 100644 --- a/demos/pycram_gpsr_demo/nlp_processing.py +++ b/demos/pycram_gpsr_demo/nlp_processing.py @@ -17,7 +17,7 @@ todo_plans = [] currentSpeech = "" stoppedSpeaking = Condition() -canSpeak = False # CHANGE set to TRUE for real robot +canSpeak = True # CHANGE set to TRUE for real robot canListen = True # CHANGE set to TRUE when NLP and Whisper enabled canDisplay = False @@ -32,7 +32,7 @@ def what_am_i_saying(msg): def sing_my_angel_of_music(text): global tts, canSpeak, stoppedSpeaking - rospy.loginfo("SPEAKING: " + text) + rospy.loginfo("[SPEAKING:] " + text) #tts.pub_now(text) # handled on other side now if canSpeak: diff --git a/demos/pycram_gpsr_demo/perception_interface.py b/demos/pycram_gpsr_demo/perception_interface.py index a1fa35436..49a96d556 100644 --- a/demos/pycram_gpsr_demo/perception_interface.py +++ b/demos/pycram_gpsr_demo/perception_interface.py @@ -2,6 +2,7 @@ import actionlib from rospy import ROSException +from demos.pycram_gpsr_demo import sing_my_angel_of_music from demos.pycram_gpsr_demo.utils import find_color from pycram.designators.action_designator import LookAtAction, DetectAction from pycram.process_module import real_robot @@ -123,6 +124,7 @@ def test_pc(): def looking_for_human(): # loop until a human is seen check_human = False + sing_my_angel_of_music("Please step infront of me.") while not check_human: try: rospy.loginfo("[CRAM] Looking for human...") @@ -137,9 +139,9 @@ def looking_for_human(): rospy.loginfo("[CRAM] No human found and robokudo timed out") return check_human -def test_lookat(pose): - with real_robot: - LookAtAction(targets=[pose]).resolve().perform() +# def test_lookat(pose): +# with real_robot: +# LookAtAction(targets=[pose]).resolve().perform() # this would be the designator way, which we are currently not using since it is slow #def test_perception(): diff --git a/demos/pycram_gpsr_demo/run_demo.py b/demos/pycram_gpsr_demo/run_demo.py index bce28d340..9b450f4b6 100644 --- a/demos/pycram_gpsr_demo/run_demo.py +++ b/demos/pycram_gpsr_demo/run_demo.py @@ -1,6 +1,6 @@ from dynamic_reconfigure.msg import DoubleParameter, IntParameter, BoolParameter, StrParameter, GroupState, Config from dynamic_reconfigure.srv import Reconfigure, ReconfigureRequest -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, PoseWithCovariance, PoseWithCovarianceStamped from demos.pycram_gpsr_demo.setup_demo import * from demos.pycram_gpsr_demo import utils, setup_demo, perception_interface @@ -11,9 +11,12 @@ import src.pycram.utilities.gpsr_utils as gpsr_utils from pycram.enums import ObjectType, ImageEnum from pycram.language import Code, Monitor +#from demos.pycram_gpsr_demo.setup_demo import image_switch -instruction_point = PoseStamped([6.28, 1.0, 0], [0, 0, 0, 1]) - +instruction_point = PoseStamped([6.12, 1.8, 0], [0, 0, 0, 1]) +#instruction_point = PoseStamped([4.4, -0.5, 0], [0, 0, 0, 1]) +#image_switch = ImageSwitchPublisher() +start_signal_waiter = StartSignalWaiter() def move_vel(speed, distance, isForward, angle=0): # Starts a new node @@ -82,20 +85,32 @@ def set_parameters(new_parameters): rospy.logerr("Service call failed: %s" % e) +def pub_fake_pose(fake_pose: PoseStamped): + toya_pose_pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=100) + msg = PoseWithCovarianceStamped() + msg.pose.pose.position = fake_pose.pose.position + msg.pose.pose.orientation = fake_pose.pose.orientation + msg.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.06853892326654787] + toya_pose_pub.publish(msg) + + def yeet_into_arena(): + global image_switch try: - image_switch.pub_now(ImageEnum.HI.value) # hi im toya + #image_switch.pub_now(ImageEnum.HI.value) # hi im toya sing_my_angel_of_music("Push down my Hand, when you are Ready.") - image_switch.pub_now(ImageEnum.PUSHBUTTONS.value) + #image_switch.pub_now(ImageEnum.PUSHBUTTONS.value) plan = Code(lambda: rospy.sleep(1)) * 999999 >> Monitor(monitor_func) plan.perform() except SensorMonitoringCondition: - sing_my_angel_of_music("Starting Storing Grocery.") + sing_my_angel_of_music("Starting GPSR.") # load everything world giskard robokudo.... # Wait for the start signal - image_switch.pub_now(ImageEnum.HI.value) + #image_switch.pub_now(ImageEnum.HI.value) start_signal_waiter.wait_for_startsignal() # Once the start signal is received, continue with the rest of the script @@ -103,45 +118,44 @@ def yeet_into_arena(): move_vel(speed=2, distance=4, isForward=True) rospy.sleep(2) new_parameters = { - 'forbid_radius': 0.2, - 'obstacle_occupancy': 5, - 'obstacle_radius': 0.2 + 'forbid_radius': 0.35, + 'obstacle_occupancy': 10, + 'obstacle_radius': 0.35 } set_parameters(new_parameters) # Once the start signal is received, continue with the rest of the script rospy.sleep(1) - fake_pose_2 = Pose([3, 0.3, 0]) - move.pub_fake_pose(fake_pose_2) - move_vel(0.2, 2, False, 0.06) + fake_pose_2 = Pose([2.88, 0.3, 0]) + pub_fake_pose(fake_pose_2) + move_vel(0.2, 2, False, 0.02) sing_my_angel_of_music("Driving.") - move_123 = Pose([4, -0.2, 0], [0, 0, 0.7, 0.7]) - move.pub_now(move_123) - move_145 = Pose([4.8, 0.8, 0], [0, 0, 0.7, 0.7]) - move.pub_now(move_145) + move_123 = Pose([4, -0.4, 0], [0, 0, 0, 1]) + navi.go_to_pose(move_123) + #move_145 = Pose([4.8, 0.8, 0], [0, 0, 0.7, 0.7]) + #navi.go_to_pose(move_145) + # --- main control --- # TODO: test on real robot def gpsr(): with real_robot: - yeet_into_arena() plan_list = utils.get_plans(high_level_plans) - # plan_list = utils.get_plans(high_level_plans) # get list of all available plans - # init params - # go to instruction point, look at human, etc. - # TODO add Vanessa's run into arena part + yeet_into_arena() + #sound_pub = SoundRequestPublisher() + #sound_pub.publish_sound_request() sing_my_angel_of_music("Going to the instruction point") - navi.go_to_pose(PoseStamped([3.0, 0.5, 0.0], [0, 0, 0, 1])) # in door - navi.go_to_room_entry_or_exit('living_room', 'entry') + navi.go_to_pose(PoseStamped([5.44, 0.2, 0.0], [0, 0, 0, 1])) # in door + #navi.go_to_room_entry_or_exit('office', 'exit') navi.go_to_pose(instruction_point) # look at a person when listening to command # high_level_plans.prepare_for_commands() instruction_list = [] - for i in range(5): # test + for i in range(10): # test # TODO add this to plan. # DetectAction(technique='human').resolve().perform() # giskardpy.move_head_to_human() @@ -163,111 +177,12 @@ def gpsr(): # match instruction to plan utils.call_plan_by_name(plan_list, snakecase(instruction['intent']), instruction) # if plan was successful, remove it from the list + instruction_list.remove(instruction) instruction_list = [] sing_my_angel_of_music("navigating to the instruction point") navi.go_to_pose(instruction_point) -data = { - "sentence": "Please bring the red cup from the kitchen table to the living room table", - "intent": "Transporting", - "Item": { - "value": "cup", - "entity": "Transportable", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - }, - "Source": { - "value": "couch table", - "entity": "DesignedFurniture", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - }, - "SourceRoom": { - "value": "living room", - "entity": "Room", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - }, - "DestinationRoom": { - "value": "dining room", - "entity": "Room", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - }, - "Destination": { - "value": "shelf", - "entity": "DesignedFurniture", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - } -} - -data2 = { - "sentence": " Bring me the cup from the living room table .", - "intent": "Transporting", - "BeneficiaryRole": { - "value": "me", - "entity": "NaturalPerson", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - }, - "Item": { - "value": "cup", - "entity": "Transportable", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - }, - "SourceRoom": { - "value": "living room", - "entity": "Room", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - }, - "Source": { - "value": "couch table", - "entity": "DesignedFurniture", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - } -} - -data3 = { - "sentence": "Please bring the red cup from the kitchen table to the living room table", - "intent": "LookingFor", - "Item": { - "value": "cup", - "entity": "Transportable", - "propertyAttribute": [], - "actionAttribute": [], - "numberAttribute": [] - } #, - # "Source": { - # "value": "coffee table", - # "entity": "DesignedFurniture", - # "propertyAttribute": [], - # "actionAttribute": [], - # "numberAttribute": [] - # }, - # "SourceRoom": { - # "value": "living room", - # "entity": "Room", - # "propertyAttribute": [], - # "actionAttribute": [], - # "numberAttribute": [] - # } -} - - # CHANGE CARE THIS STUFF GETS ACTUALLY EXECUTED def demo_plan(data): with real_robot: @@ -275,7 +190,9 @@ def demo_plan(data): print('--------------stahp----------------') return -#setup() -#gpsr() +setup() +#fake_pose_2 = Pose([2.88, 0.3, 0]) +#pub_fake_pose(fake_pose_2) +gpsr() #demo_plan(data2) #setup_demo.gripper.pub_now('open') diff --git a/demos/pycram_gpsr_demo/setup_demo.py b/demos/pycram_gpsr_demo/setup_demo.py index 73afa1160..ae873c016 100755 --- a/demos/pycram_gpsr_demo/setup_demo.py +++ b/demos/pycram_gpsr_demo/setup_demo.py @@ -14,8 +14,9 @@ import tf import pycram.utilities.gpsr_utils as plans from demos.pycram_gpsr_demo import tf_l +import demos.pycram_gpsr_demo.perception_interface as perception_interface -with_real_robot = False # CHANGE set to TRUE for real robot +with_real_robot = True # CHANGE set to TRUE for real robot # initialize interfaces #instruction_point = PoseStamped([1.45, 4.5, 0], [0, 0, 1, 0]) world = None @@ -65,6 +66,8 @@ def setup(): robot.set_color([0.5, 0.0, 0.2, 1]) robot_desig = ObjectDesignatorDescription(names=["hsrb"]) + perception_interface.init_robokudo() + # sync to kitchen and robot RobotStateUpdater("/tf", "/hsrb/robot_state/joint_states") rospy.sleep(2) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index dc53bcfb2..870c08733 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -455,7 +455,7 @@ def ground(self) -> Action: def monitor_func(): der: WrenchStamped() = fts.get_last_value() print(abs(der.wrench.force.y)) - if abs(der.wrench.force.y) > 0.45: + if abs(der.wrench.force.y) > 2.7: print(abs(der.wrench.force.y)) print(abs(der.wrench.torque.y)) return SensorMonitoringCondition