diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 1cac12bd5..eef4eebf2 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -1,5 +1,4 @@ import rospy -import sys from ..pose import Pose from ..robot_descriptions import robot_description @@ -8,25 +7,36 @@ from typing import List, Tuple, Dict from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Vector3Stamped -topics = list(map(lambda x: x[0], rospy.get_published_topics())) -try: - from giskardpy.python_interface import GiskardWrapper - from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry - from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse - - if "/giskard/command/goal" in topics: - giskard_wrapper = GiskardWrapper() - giskard_update_service = rospy.ServiceProxy("/giskard/update_world", UpdateWorld) - else: - sys.stderr.writelines(f'\033[93m' + "[Warning] Giskard is not running, could not initialize Giskard interface ") - sys.stderr.write("Please launch giskard before starting PyCRAM" + '\033[0m' ) -except ModuleNotFoundError as e: - rospy.logwarn("No Giskard topic available") - sys.stderr.write(f'\033[93m' + "[Warning] Could not import giskard messages, giskard will not be available") - +giskard_wrapper = None +giskard_update_service = None +is_init = False + + +def init_giskard_interface(): + global giskard_wrapper + global giskard_update_service + global is_init + if is_init: + return + topics = list(map(lambda x: x[0], rospy.get_published_topics())) + try: + from giskardpy.python_interface import GiskardWrapper + from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry + from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse + + if "/giskard/command/goal" in topics: + giskard_wrapper = GiskardWrapper() + giskard_update_service = rospy.ServiceProxy("/giskard/update_world", UpdateWorld) + is_init = True + rospy.loginfo("Successfully initialized Giskard interface") + else: + rospy.logwarn("Giskard is not running, could not initialize Giskard interface") + except ModuleNotFoundError as e: + rospy.logwarn("Failed to import Giskard messages, giskard interface could not be initialized") # Believe state management between pycram and giskard + def initial_adding_objects() -> None: """ Adds object that are loaded in the BulletWorld to the Giskard belief state, if they are not present at the moment. @@ -58,6 +68,7 @@ def sync_worlds() -> None: belief state such that it matches the objects present in the BulletWorld and moving the robot to the position it is currently at in the BulletWorld. """ + init_giskard_interface() add_gripper_groups() bullet_object_names = set() for obj in BulletWorld.current_bullet_world.objects: diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index 5f844c496..925a7a721 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -1,13 +1,6 @@ import rospy import actionlib -import sys -import os -try: - from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjetDesignator - from robokudo_msgs.msg import QueryAction, QueryGoal, QueryResult -except ModuleNotFoundError as e: - rospy.logwarn(f"Could not import RoboKudo messages, RoboKudo will not be available") - sys.stderr.write(f'\033[93m' + f"[Warning] Could not import RoboKudo messages, RoboKudo will not be available" + '\033[0m') + from ..designator import ObjectDesignatorDescription from ..pose import Pose @@ -15,6 +8,21 @@ from ..bullet_world import BulletWorld from ..enums import ObjectType +is_init = False + + +def init_robokudo_interface(): + global is_init + if is_init: + return + try: + from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjetDesignator + from robokudo_msgs.msg import QueryAction, QueryGoal, QueryResult + is_init = True + rospy.loginfo("Successfully initialized robokudo interface") + except ModuleNotFoundError as e: + rospy.logwarn(f"Could not import RoboKudo messages, RoboKudo interface could not be initialized") + def msg_from_obj_desig(obj_desc: ObjectDesignatorDescription) -> 'robokudo_ObjetDesignator': """ @@ -56,6 +64,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti :param object_desc: The object designator description which describes the object that should be perceived :return: An object designator for the found object, if there was an object that fitted the description. """ + init_robokudo_interface() global query_result def active_callback():