From 9121821836992688b0bb7abfd5d0e18fb45dcde3 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Mon, 2 Dec 2024 15:31:34 +0100 Subject: [PATCH] [detectAction] fixed process modules for each robot to defaultDetecting module --- .../process_modules/donbot_process_modules.py | 9 ++- .../process_modules/hsrb_process_modules.py | 7 +- .../process_modules/pr2_process_modules.py | 66 ++++++++++--------- .../stretch_process_modules.py | 8 +-- .../process_modules/tiago_process_modules.py | 5 +- 5 files changed, 50 insertions(+), 45 deletions(-) diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index e39b5bcb3..862c2c69f 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/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: diff --git a/src/pycram/process_modules/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py index ffd1f4287..b6407e151 100644 --- a/src/pycram/process_modules/hsrb_process_modules.py +++ b/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: diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index e55371f86..e95d996ea 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/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): diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py index 028b8f333..8f8d1306a 100644 --- a/src/pycram/process_modules/stretch_process_modules.py +++ b/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: diff --git a/src/pycram/process_modules/tiago_process_modules.py b/src/pycram/process_modules/tiago_process_modules.py index 37fa694fd..4966f5572 100644 --- a/src/pycram/process_modules/tiago_process_modules.py +++ b/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: