Merge pull request #109 from Tigul/fix-interface-imports

Fix interface imports

src/pycram/external_interfaces/giskard.py

@@ -5,23 +5,38 @@
 from ..bullet_world import BulletWorld, Object
 
 from typing import List, Tuple, Dict
-
-topics = list(map(lambda x: x[0], rospy.get_published_topics()))
-try:
-    from giskardpy.python_interface import GiskardWrapper
-    from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Vector3Stamped
-    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)
-except ModuleNotFoundError as e:
-    rospy.logwarn("No Giskard topic available")
-
+from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Vector3Stamped
+
+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.
@@ -53,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,16 +1,30 @@
 import rospy
 import actionlib
 
-from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjetDesignator
-from robokudo_msgs.msg import QueryAction, QueryGoal, QueryResult
+
 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:
+def msg_from_obj_desig(obj_desc: ObjectDesignatorDescription) -> 'robokudo_ObjetDesignator':
     """
     Creates a RoboKudo Object designator from a PyCRAM Object Designator description
 
@@ -24,7 +38,7 @@ def msg_from_obj_desig(obj_desc: ObjectDesignatorDescription) -> robokudo_ObjetD
     return obj_msg
 
 
-def make_query_goal_msg(obj_desc: ObjectDesignatorDescription) -> QueryGoal:
+def make_query_goal_msg(obj_desc: ObjectDesignatorDescription) -> 'QueryGoal':
     """
     Creates a QueryGoal message from a PyCRAM Object designator description for the use of Querying RobotKudo.
 
@@ -50,7 +64,9 @@ 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():
         rospy.loginfo("Send query to Robokudo")
 
@@ -80,15 +96,5 @@ def feedback_callback(msg):
         pose = lt.transform_pose(pose, "map")
 
         pose_candidates[source] = pose
-        # pose_candidates.append(pose)
-
-    # for possible_pose in query_result.res[0].pose:
-    #     pose = Pose.from_pose_stamped(possible_pose)
-    #     pose.frame = BulletWorld.current_bullet_world.robot.get_link_tf_frame(pose.frame)
-
-    #     lt = LocalTransformer()
-    #     pose = lt.transform_pose(pose, "map")
-
-    #     pose_candidates.append(pose)
 
     return pose_candidates