[Knowrob] added functions from robocup branch

src/pycram/external_interfaces/knowrob.py

@@ -1,9 +1,16 @@
 import logging
 import os
 import sys
+from std_msgs.msg import String
+from knowledge_msgs.srv import IsKnown, ObjectPose
+import rospy
 import rosservice
 
-from typing_extensions import Dict, List, Union
+
+from typing import Dict, List, Union
+
+interf = None
+is_init = False
 
 SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__))
 sys.path.append(os.path.join(SCRIPT_DIR, os.pardir, os.pardir, "neem-interface", "src"))
@@ -27,6 +34,26 @@
 logger.setLevel(logging.DEBUG)
 
 
+def init_knowrob_interface():
+    """
+    Initializes the knowrob interface.
+    Can not be used at the moment.
+    """
+    global interf
+    global is_init
+    if is_init:
+        return
+    try:
+        import rospy
+        # from suturo_knowledge import interf_q
+        # interf = interf_q.InterfacePlanningKnowledge()
+        is_init = True
+        rospy.loginfo("Successfully initialized Knowrob interface")
+
+    except ModuleNotFoundError as e:
+        rospy.logwarn("Failed to import Knowrob messages, knowrob interface could not be initialized")
+
+
 def all_solutions(q):
     logging.info(q)
     r = prolog.all_solutions(q)
@@ -123,4 +150,66 @@ def knowrob_string_to_pose(pose_as_string: str) -> List[float]:
     pos, ori = pose_as_string[1+i+2:-2].split("],[")
     xyz = list(map(float, pos.split(",")))
     qxyzw = list(map(float, ori.split(",")))
-    return xyz + qxyzw
+    return xyz + qxyzw
+
+def get_guest_info(id):
+    """
+    function that uses Knowledge Service to get Name and drink from new guest via ID
+    :param id: integer for person
+    :return: ["name", "drink"]
+    """
+
+    rospy.wait_for_service('info_server')
+    try:
+        info_service = rospy.ServiceProxy('info_server', IsKnown)
+        # guest_data = person_infos: "name,drink"
+        guest_data = info_service(id)
+        # result = ['person_infos: "name', 'drink"']
+        result = str(guest_data).split(',')
+        result[0] = result[0][13:]
+        return result
+    except rospy.ServiceException as e:
+        rospy.logerr("Service call failed")
+        pass
+
+def get_table_pose(table_name):
+    """
+    Get table pose from knowledge
+    :param table_name: predefined name for each table
+
+    :return: object pose of the given table
+    """
+    rospy.wait_for_service('pose_server')
+    try:
+        service = rospy.ServiceProxy('pose_server', ObjectPose)
+        table_pose = service(table_name)
+        return table_pose
+    except rospy.ServiceException:
+        rospy.logerr("Service call failed")
+        pass
+
+
+def get_location_pose(location_name):
+    """
+    Can not be used yet.
+    Gives the pose for a given location back for instance for perceiving, placing, opening etc.
+    """
+    try:
+        location_pos = interf.get_pose(location_name)
+        return location_pos
+    except:
+        rospy.logerr("Failed to contact knowrob")
+        pass
+
+
+def get_handle_pos(handle):
+    """
+      Can not be used yet.
+      Gives the pose for a given handle back for instance for opening a container.
+    """
+    try:
+        handle_pos = interf.get_pose_of_handle(handle)
+        return handle_pos
+    except:
+        rospy.logerr("Failed to contact knowrob")
+        pass