[ext-int] Added init methods for giskard and robokudo

Author: Tigul

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:

src/pycram/external_interfaces/robokudo.py

@@ -1,20 +1,28 @@
 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
 from ..local_transformer import LocalTransformer
 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():