[detectAction] fixed process modules for each robot to defaultDetecti…

…ng module

src/pycram/process_modules/donbot_process_modules.py

@@ -2,13 +2,14 @@
 
 import numpy as np
 
+from .default_process_modules import DefaultDetecting, DefaultDetectingReal
 from ..worlds.bullet_world import World
 from ..designators.motion_designator import MoveArmJointsMotion, WorldStateDetectingMotion
 from ..local_transformer import LocalTransformer
 from ..process_module import ProcessModule, ProcessModuleManager
 from ..robot_description import RobotDescription
 from ..process_modules.pr2_process_modules import Pr2Detecting as DonbotDetecting, _move_arm_tcp
-from ..datastructures.enums import Arms
+from ..datastructures.enums import Arms, ExecutionType
 
 
 def _park_arms(arm):
@@ -161,8 +162,10 @@ def looking(self):
             return DonbotMoveHead(self._looking_lock)
 
     def detecting(self):
-        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
-            return DonbotDetecting(self._detecting_lock)
+        if ProcessModuleManager.execution_type == ExecutionType.SIMULATED:
+            return DefaultDetecting(self._detecting_lock)
+        elif ProcessModuleManager.execution_type == ExecutionType.REAL:
+            return DefaultDetectingReal(self._detecting_lock)
 
     def move_tcp(self):
         if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:

src/pycram/process_modules/hsrb_process_modules.py

@@ -2,6 +2,7 @@
 from threading import Lock
 from typing_extensions import Any
 
+from .default_process_modules import DefaultDetecting, DefaultDetectingReal
 from ..datastructures.enums import ExecutionType
 from ..external_interfaces.tmc import tmc_gripper_control, tmc_talk
 from ..robot_description import RobotDescription
@@ -233,11 +234,9 @@ def looking(self):
 
     def detecting(self):
         if ProcessModuleManager.execution_type == ExecutionType.SIMULATED:
-            return HSRBDetecting(self._detecting_lock)
+            return DefaultDetecting(self._detecting_lock)
         elif ProcessModuleManager.execution_type == ExecutionType.REAL:
-            return HSRBDetectingReal(self._detecting_lock)
-        elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL:
-            return HSRBDetecting(self._detecting_lock)
+            return DefaultDetectingReal(self._detecting_lock)
 
     def move_tcp(self):
         if  ProcessModuleManager.execution_type == ExecutionType.REAL:

src/pycram/process_modules/pr2_process_modules.py

@@ -81,6 +81,7 @@ def _execute(self, desig: MoveGripperMotion):
             robot.set_joint_position(joint, state)
 
 
+@DeprecationWarning
 class Pr2Detecting(ProcessModule):
     """
     This process module tries to detect an object with the given type. To be detected the object has to be in
@@ -238,38 +239,39 @@ def _execute(self, desig: LookingMotion):
         giskard.achieve_joint_goal({"head_pan_joint": new_pan + current_pan,
                                     "head_tilt_joint": new_tilt + current_tilt})
 
-#
-# class Pr2DetectingReal(ProcessModule):
-#     """
-#     Process Module for the real Pr2 that tries to detect an object fitting the given object description. Uses Robokudo
-#     for perception of the environment.
-#     """
-#
-#     def _execute(self, designator: DetectingMotion) -> Any:
-#
-#
-#
-#         query_result = query_object(ObjectDesignatorDescription(types=[designator.object_type]))
-#         print(query_result)
-#         obj_pose = query_result["ClusterPoseBBAnnotator"]
-#
-#         lt = LocalTransformer()
-#         obj_pose = lt.transform_pose(obj_pose, World.robot.get_link_tf_frame("torso_lift_link"))
-#         obj_pose.orientation = [0, 0, 0, 1]
-#         obj_pose.position.x += 0.05
-#
-#         world_obj = World.current_world.get_object_by_type(designator.object_type)
-#         if world_obj:
-#             world_obj[0].set_pose(obj_pose)
-#             return world_obj[0]
-#         elif designator.object_type == ObjectType.JEROEN_CUP:
-#             cup = Object("cup", ObjectType.JEROEN_CUP, "jeroen_cup.stl", pose=obj_pose)
-#             return cup
-#         elif designator.object_type == ObjectType.BOWL:
-#             bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=obj_pose)
-#             return bowl
-#
-#         return world_obj[0]
+
+@DeprecationWarning
+class Pr2DetectingReal(ProcessModule):
+    """
+    Process Module for the real Pr2 that tries to detect an object fitting the given object description. Uses Robokudo
+    for perception of the environment.
+    """
+
+    def _execute(self, designator: DetectingMotion) -> Any:
+
+
+
+        query_result = query_object(ObjectDesignatorDescription(types=[designator.object_type]))
+        print(query_result)
+        obj_pose = query_result["ClusterPoseBBAnnotator"]
+
+        lt = LocalTransformer()
+        obj_pose = lt.transform_pose(obj_pose, World.robot.get_link_tf_frame("torso_lift_link"))
+        obj_pose.orientation = [0, 0, 0, 1]
+        obj_pose.position.x += 0.05
+
+        world_obj = World.current_world.get_object_by_type(designator.object_type)
+        if world_obj:
+            world_obj[0].set_pose(obj_pose)
+            return world_obj[0]
+        elif designator.object_type == ObjectType.JEROEN_CUP:
+            cup = Object("cup", ObjectType.JEROEN_CUP, "jeroen_cup.stl", pose=obj_pose)
+            return cup
+        elif designator.object_type == ObjectType.BOWL:
+            bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=obj_pose)
+            return bowl
+
+        return world_obj[0]
 
 
 class Pr2MoveTCPReal(ProcessModule):

src/pycram/process_modules/stretch_process_modules.py

@@ -306,10 +306,10 @@ def looking(self):
             return StretchMoveHeadReal(self._looking_lock)
 
     def detecting(self):
-        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
-            return StretchDetecting(self._detecting_lock)
-        elif ProcessModuleManager.execution_type ==  ExecutionType.REAL:
-            return StretchDetectingReal(self._detecting_lock)
+        if ProcessModuleManager.execution_type == ExecutionType.SIMULATED:
+            return DefaultDetecting(self._detecting_lock)
+        elif ProcessModuleManager.execution_type == ExecutionType.REAL:
+            return DefaultDetectingReal(self._detecting_lock)
 
     def move_tcp(self):
         if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:

src/pycram/process_modules/tiago_process_modules.py

@@ -10,7 +10,8 @@
 from ..local_transformer import LocalTransformer
 from ..process_module import ProcessModuleManager, ProcessModule
 from .default_process_modules import DefaultOpen, DefaultClose, DefaultMoveGripper, DefaultMoveJoints, DefaultMoveTCP, \
-    DefaultNavigation, DefaultMoveHead, DefaultDetecting, DefaultMoveArmJoints, DefaultWorldStateDetecting
+    DefaultNavigation, DefaultMoveHead, DefaultDetecting, DefaultMoveArmJoints, DefaultWorldStateDetecting, \
+    DefaultDetectingReal
 from ..robot_description import RobotDescription
 from ..ros.logging import logdebug
 from ..external_interfaces import giskard
@@ -149,7 +150,7 @@ def detecting(self):
         if ProcessModuleManager.execution_type == ExecutionType.SIMULATED:
             return DefaultDetecting(self._detecting_lock)
         elif ProcessModuleManager.execution_type == ExecutionType.REAL:
-            return TiagoDetectingReal(self._detecting_lock)
+            return DefaultDetectingReal(self._detecting_lock)
 
     def move_tcp(self):
         if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: