From 0718e039ebc7bb21a54bfc265553ccee734fdc68 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 13 Nov 2023 16:57:20 +0100 Subject: [PATCH 1/2] [ext-int] fixed import issues and added stderr writing --- src/pycram/external_interfaces/giskard.py | 7 +++++- src/pycram/external_interfaces/robokudo.py | 25 ++++++++++------------ 2 files changed, 17 insertions(+), 15 deletions(-) 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 From aa613b33a2ec3aef571fcc17a49252798d3140db Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 14 Nov 2023 13:22:21 +0100 Subject: [PATCH 2/2] [ext-int] Added init methods for giskard and robokudo --- src/pycram/external_interfaces/giskard.py | 45 ++++++++++++++-------- src/pycram/external_interfaces/robokudo.py | 25 ++++++++---- 2 files changed, 45 insertions(+), 25 deletions(-) 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():