Merge pull request cram2#183 from sunava/robokudo-interface

[robokudo interface] new queries for the newest robokudo mainly suturo

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

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

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

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 *