diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 763004689..1cac12bd5 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/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 diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index 87dee1c5f..5f844c496 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -1,8 +1,14 @@ 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 @@ -10,7 +16,7 @@ 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