Merge pull request #17 from SUTURO/gpsr

Gpsr

demos/pycram_gpsr_demo/knowrob_interface.py

@@ -1,20 +1,28 @@
 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
 kitchen = 'http://www.ease-crc.org/ont/SOMA.owl#Kitchen'
 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 +112,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 +143,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,30 +171,65 @@ def check_existence_of_class(nlp_name):
         return tmp
 
 
-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), "
+# get iri from objects.py mapping
+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:
+        poses_list = utils.knowrob_poses_result_to_list_dict(knowrob_poses_list)
+    else:
+        rospy.logerr("[KnowRob] query returned empty :(")
+    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):
-    knowrob_poses_list = kb.prolog_client.all_solutions(f"subclass_of(Obj, {items_iri}), "
-                                                        f""f"predefined_destination_location(Obj, 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:
         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 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 = []
@@ -203,6 +244,53 @@ def test_predefined_locations():
     return {'source': source_list, 'destination': destination_list}
 
 
+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']
+
+
+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 = []
+    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 +343,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.")

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:

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

demos/pycram_gpsr_demo/nlp_processing.py

@@ -17,33 +17,22 @@
 todo_plans = []
 currentSpeech = ""
 stoppedSpeaking = Condition()
-canSpeak = True
-canListen = True
+canSpeak = True  # CHANGE set to TRUE for real robot
+canListen = True  # CHANGE set to TRUE when NLP and Whisper enabled
 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):
     global tts, canSpeak, stoppedSpeaking
-    rospy.loginfo("SPEAKING: " + text)
+    rospy.loginfo("[SPEAKING:] " + text)
     #tts.pub_now(text)
     # handled on other side now
     if canSpeak:
@@ -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
@@ -114,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
@@ -123,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:
@@ -146,7 +137,7 @@ def confirm_nlp_output(received_output):
 
 # --- THIS IS THE MAIN FUNCTION FOR NLP ---
 def listen_to_commands():
-    nlp_subscribe()
+    nlp_subscribe()  # CHANGE  !!! done during setup !!! ENABLE FOR TESTING ONLY. DISABLE FOR REAL ROBOT
     global response, todo_plans
     while True:
         if canDisplay:
@@ -161,4 +152,3 @@ def listen_to_commands():
     rospy.loginfo("[CRAM] do stuff")
     nlp_unsubscribe()
     return todo_plans
-