diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 85d090499..becb17920 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -9,11 +9,12 @@ from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) from ..datastructures.pose import Pose -from ..external_interfaces.robokudo import query +from ..external_interfaces.robokudo import * if TYPE_CHECKING: import owlready2 + class BelieveObject(ObjectDesignatorDescription): """ Description for Objects that are only believed in. @@ -138,6 +139,8 @@ def __init__(self, names: List[str], types: List[str], self.timestamps: List[float] = timestamps +@DeprecationWarning +# Depricated class this will be done differently class RealObject(ObjectDesignatorDescription): """ Object designator representing an object in the real world, when resolving this object designator description ] @@ -155,11 +158,11 @@ class Object(ObjectDesignatorDescription.Object): def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] = None, world_object: WorldObject = None, resolver: Optional[Callable] = None): """ - - :param names: - :param types: + + :param names: + :param types: :param world_object: - :param resolver: + :param resolver: """ super().__init__(resolver) self.types: Optional[List[str]] = types diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index a1a7d618a..d257b7fa2 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -1,135 +1,167 @@ import sys -from typing_extensions import Callable +from threading import Lock, RLock +from typing import Any -import rospy import actionlib import rosnode +import rospy +from geometry_msgs.msg import PointStamped +from typing_extensions import List, Callable, Optional -from ..designator import ObjectDesignatorDescription from ..datastructures.pose import Pose -from ..local_transformer import LocalTransformer -from ..datastructures.world import World -from ..datastructures.enums import ObjectType +from ..designator import ObjectDesignatorDescription try: - from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjetDesignator + from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjectDesignator from robokudo_msgs.msg import QueryAction, QueryGoal, QueryResult except ModuleNotFoundError as e: - rospy.logwarn(f"Could not import RoboKudo messages, RoboKudo interface could not be initialized") + rospy.logwarn("Failed to import Robokudo messages, the real robot will not be available") + +is_init = False + +number_of_par_goals = 0 +robokudo_lock = Lock() +robokudo_rlock = RLock() +with robokudo_rlock: + par_threads = {} + par_motion_goal = {} -robokudo_action_client = None + +def thread_safe(func: Callable) -> Callable: + """ + Adds thread safety to a function via a decorator. This uses the robokudo_lock + + :param func: Function that should be thread safe + :return: A function with thread safety + """ + + def wrapper(*args, **kwargs): + with robokudo_rlock: + return func(*args, **kwargs) + + return wrapper def init_robokudo_interface(func: Callable) -> Callable: """ - Tries to import the RoboKudo messages and with that initialize the RoboKudo interface. + Checks if the ROS messages are available and if Robokudo is running, if that is the case the interface will be + initialized. + + :param func: Function this decorator should be wrapping + :return: A callable function which initializes the interface and then calls the wrapped function """ + def wrapper(*args, **kwargs): - global robokudo_action_client - topics = list(map(lambda x: x[0], rospy.get_published_topics())) + global is_init + if is_init and "/robokudo" in rosnode.get_node_names(): + return func(*args, **kwargs) + elif is_init and "/robokudo" not in rosnode.get_node_names(): + rospy.logwarn("Robokudo node is not available anymore, could not initialize robokudo interface") + is_init = False + giskard_wrapper = None + return + if "robokudo_msgs" not in sys.modules: - rospy.logwarn("Could not initialize the RoboKudo interface since the robokudo_msgs are not imported") + rospy.logwarn("Could not initialize the Robokudo interface since the robokudo_msgs are not imported") return if "/robokudo" in rosnode.get_node_names(): - robokudo_action_client = create_robokudo_action_client() - rospy.loginfo("Successfully initialized robokudo interface") + rospy.loginfo_once("Successfully initialized Robokudo interface") + is_init = True else: - rospy.logwarn("RoboKudo is not running, could not initialize RoboKudo interface") + rospy.logwarn("Robokudo is not running, could not initialize Robokudo interface") return - return func(*args, **kwargs) + return wrapper -def create_robokudo_action_client() -> Callable: - """ - Creates a new action client for the RoboKudo query interface and returns a function encapsulating the action client. - The returned function can be called with an ObjectDesigantor as parameter and returns the result of the action client. +@init_robokudo_interface +def send_query(obj_type: Optional[str] = None, region: Optional[str] = None, + attributes: Optional[List[str]] = None) -> Any: + """Generic function to send a query to RoboKudo.""" + goal = QueryGoal() + + if obj_type: + goal.obj.type = obj_type + if region: + goal.obj.location = region + if attributes: + goal.obj.attribute = attributes - :return: A callable function encapsulating the action client - """ client = actionlib.SimpleActionClient('robokudo/query', QueryAction) rospy.loginfo("Waiting for action server") client.wait_for_server() - def action_client(object_desc): - global query_result + query_result = None - def active_callback(): - rospy.loginfo("Send query to Robokudo") + def done_callback(state, result): + nonlocal query_result + query_result = result + rospy.loginfo("Query completed") - def done_callback(state, result): - rospy.loginfo("Finished perceiving") - global query_result - query_result = result + client.send_goal(goal, done_cb=done_callback) + client.wait_for_result() + return query_result - def feedback_callback(msg): - pass - - object_goal = make_query_goal_msg(object_desc) - client.send_goal(object_goal, active_cb=active_callback, done_cb=done_callback, feedback_cb=feedback_callback) - wait = client.wait_for_result() - return query_result - - return action_client +@init_robokudo_interface +def query_object(obj_desc: ObjectDesignatorDescription) -> dict: + """Query RoboKudo for an object that fits the description.""" + goal = QueryGoal() + goal.obj.uid = str(id(obj_desc)) + goal.obj.type = str(obj_desc.types[0].name) -def msg_from_obj_desig(obj_desc: ObjectDesignatorDescription) -> 'robokudo_ObjetDesignator': - """ - Creates a RoboKudo Object designator from a PyCRAM Object Designator description + result = send_query(obj_type=goal.obj.type) - :param obj_desc: The PyCRAM Object designator that should be converted - :return: The RobotKudo Object Designator for the given PyCRAM designator - """ - obj_msg = robokudo_ObjetDesignator() - obj_msg.uid = str(id(obj_desc)) - obj_msg.obj_type = obj_desc.types[0] # For testing purposes + pose_candidates = {} + if result and result.res: + for i in range(len(result.res[0].pose)): + pose = Pose.from_pose_stamped(result.res[0].pose[i]) + source = result.res[0].pose_source[0] + pose_candidates[source] = pose + return pose_candidates - return obj_msg +@init_robokudo_interface +def query_human() -> PointStamped: + """Query RoboKudo for human detection and return the detected human's pose.""" + result = send_query(obj_type='human') + if result: + return result # Assuming result is of type PointStamped or similar. + return None -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. - :param obj_desc: The PyCRAM object designator description that should be converted - :return: The RoboKudo QueryGoal for the given object designator description - """ - goal_msg = QueryGoal() - goal_msg.obj.uid = str(id(obj_desc)) - goal_msg.obj.obj_type = str(obj_desc.types[0].name) # For testing purposes - if ObjectType.JEROEN_CUP == obj_desc.types[0]: - goal_msg.obj.color.append("blue") - elif ObjectType.BOWL == obj_desc.types[0]: - goal_msg.obj.color.append("red") - return goal_msg +@init_robokudo_interface +def stop_query(): + """Stop any ongoing query to RoboKudo.""" + init_robokudo_interface() + client = actionlib.SimpleActionClient('robokudo/query', QueryAction) + client.wait_for_server() + client.cancel_all_goals() + rospy.loginfo("Cancelled current RoboKudo query goal") @init_robokudo_interface -def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescription.Object: - """ - Sends a query to RoboKudo to look for an object that fits the description given by the Object designator description. - For sending the query to RoboKudo a simple action client will be created and the Object designator description is - sent as a goal. +def query_specific_region(region: str) -> Any: + """Query RoboKudo to scan a specific region.""" + return send_query(region=region) - :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. - """ - query_result = robokudo_action_client(object_desc) - pose_candidates = {} - if query_result.res == []: - rospy.logwarn("No suitable object could be found") - return - for i in range(0, len(query_result.res[0].pose)): - pose = Pose.from_pose_stamped(query_result.res[0].pose[i]) - pose.frame = World.current_world.robot.get_link_tf_frame(pose.frame) # TODO: pose.frame is a link name? - source = query_result.res[0].poseSource[i] - - lt = LocalTransformer() - pose = lt.transform_pose(pose, "map") +@init_robokudo_interface +def query_human_attributes() -> Any: + """Query RoboKudo for human attributes like brightness of clothes, headgear, and gender.""" + return send_query(obj_type='human', attributes=["attributes"]) - pose_candidates[source] = pose - return pose_candidates +@init_robokudo_interface +def query_waving_human() -> Pose: + """Query RoboKudo for detecting a waving human.""" + result = send_query(obj_type='human') + if result and result.res: + try: + pose = Pose.from_pose_stamped(result.res[0].pose[0]) + return pose + except IndexError: + pass + return None diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 261094ccc..7e7592cb7 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -21,7 +21,7 @@ from ..datastructures.pose import Pose from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType from ..external_interfaces import giskard -from ..external_interfaces.robokudo import query +from ..external_interfaces.robokudo import * try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py index 3dd46d33b..dac9bb24c 100644 --- a/src/pycram/process_modules/stretch_process_modules.py +++ b/src/pycram/process_modules/stretch_process_modules.py @@ -2,7 +2,7 @@ import rospy -from ..external_interfaces.robokudo import query +from ..external_interfaces.robokudo import * from ..utils import _apply_ik from ..external_interfaces import giskard from .default_process_modules import *