diff --git a/src/pycram/external_interfaces/knowrob.py b/src/pycram/external_interfaces/knowrob.py index f58a2556e..6e9affd16 100644 --- a/src/pycram/external_interfaces/knowrob.py +++ b/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 \ No newline at end of file + 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