[ext-int] fixed import issues and added stderr writing

src/pycram/external_interfaces/giskard.py

@@ -1,23 +1,28 @@
 import rospy
+import sys
 
 from ..pose import Pose
 from ..robot_descriptions import robot_description
 from ..bullet_world import BulletWorld, Object
 
 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 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)
+    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")
 
 
 # Believe state management between pycram and giskard

src/pycram/external_interfaces/robokudo.py

@@ -1,16 +1,22 @@
 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 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
 
 
-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 +30,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.
 
@@ -51,6 +57,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti
     :return: An object designator for the found object, if there was an object that fitted the description.
     """
     global query_result
+
     def active_callback():
         rospy.loginfo("Send query to Robokudo")
 
@@ -80,15 +87,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