Merge pull request #182 from sunava/hsrb-processmodules

[hsrb-processmodule] introducing hsrb process module that are working…

requirements.txt

@@ -18,4 +18,9 @@ psutil==5.9.7
 lxml==4.9.1
 typing_extensions==4.9.0
 owlready2>=0.45
-catkin_pkg
+catkin_pkg
+Pillow~=10.1.0
+pynput~=1.7.7
+playsound~=1.3.0
+pydub~=0.25.1
+gTTS~=2.5.3

src/pycram/datastructures/enums.py

@@ -2,6 +2,11 @@
 
 from enum import Enum, auto
 
+class ExecutionType(Enum):
+    """Enum for Execution Process Module types."""
+    REAL = auto()
+    SIMULATED = auto()
+    SEMI_REAL = auto()
 
 class Arms(Enum):
     """Enum for Arms."""
@@ -120,3 +125,36 @@ class GripperType(Enum):
     CUSTOM = auto()
 
 
+class PerceptionTechniques(Enum):
+    """
+    Enum for techniques for perception tasks.
+    """
+    ALL = auto()
+    HUMAN = auto()
+    TYPES = auto()
+
+
+class ImageEnum(Enum):
+    """
+    Enum for image switch view on hsrb display.
+    """
+    HI = 0
+    TALK = 1
+    DISH = 2
+    DONE = 3
+    DROP = 4
+    HANDOVER = 5
+    ORDER = 6
+    PICKING = 7
+    PLACING = 8
+    REPEAT = 9
+    SEARCH = 10
+    WAVING = 11
+    FOLLOWING = 12
+    DRIVINGBACK = 13
+    PUSHBUTTONS = 14
+    FOLLOWSTOP = 15
+    JREPEAT = 16
+    SOFA = 17
+    INSPECT = 18
+    CHAIR = 37

src/pycram/designators/motion_designator.py

@@ -12,13 +12,14 @@
                                      MoveGripperMotion as ORMMoveGripperMotion, DetectingMotion as ORMDetectingMotion,
                                      OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion,
                                      Motion as ORMMotionDesignator)
-from ..datastructures.enums import ObjectType, Arms, GripperState
+from ..datastructures.enums import ObjectType, Arms, GripperState, ExecutionType
 
 from typing_extensions import Dict, Optional, get_type_hints
 from ..datastructures.pose import Pose
 from ..tasktree import with_tree
 from ..designator import BaseMotion
 
+
 @dataclass
 class MoveMotion(BaseMotion):
     """
@@ -160,9 +161,9 @@ def perform(self):
         if not world_object:
             raise PerceptionObjectNotFound(
                 f"Could not find an object with the type {self.object_type} in the FOV of the robot")
-        if ProcessModuleManager.execution_type == "real":
+        if ProcessModuleManager.execution_type == ExecutionType.REAL:
             return RealObject.Object(world_object.name, world_object.obj_type,
-                                                  world_object, world_object.get_pose())
+                                     world_object, world_object.get_pose())
 
         return ObjectDesignatorDescription.Object(world_object.name, world_object.obj_type,
                                                   world_object)
@@ -313,3 +314,26 @@ def insert(self, session: Session, *args, **kwargs) -> ORMClosingMotion:
         session.add(motion)
 
         return motion
+
+
+@dataclass
+class TalkingMotion(BaseMotion):
+    """
+    Talking Motion, lets the robot say a sentence.
+    """
+
+    cmd: str
+    """
+    Talking Motion, let the robot say a sentence.
+    """
+
+    @with_tree
+    def perform(self):
+        pm_manager = ProcessModuleManager.get_manager()
+        return pm_manager.talk().execute(self)
+
+    def to_sql(self) -> ORMMotionDesignator:
+        pass
+
+    def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator:
+        pass

src/pycram/external_interfaces/giskard.py

@@ -22,7 +22,7 @@
 try:
     from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper
     from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry
-    from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse
+    #from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse
 except ModuleNotFoundError as e:
     rospy.logwarn("Failed to import Giskard messages, the real robot will not be available")
 

src/pycram/external_interfaces/tmc.py

@@ -0,0 +1,60 @@
+import rospy
+from typing_extensions import Optional
+
+from ..datastructures.enums import GripperState
+from ..designators.motion_designator import MoveGripperMotion, TalkingMotion
+
+is_init = False
+
+
+def init_tmc_interface():
+    global is_init
+    if is_init:
+        return
+    try:
+        from tmc_control_msgs.msg import GripperApplyEffortActionGoal
+        from tmc_msgs.msg import Voice
+        is_init = True
+        rospy.loginfo("Successfully initialized tmc interface")
+    except ModuleNotFoundError as e:
+        rospy.logwarn(f"Could not import TMC messages, tmc interface could not be initialized")
+
+
+def tmc_gripper_control(designator: MoveGripperMotion, topic_name: Optional[str] = '/hsrb/gripper_controller/grasp/goal'):
+    """
+    Publishes a message to the gripper controller to open or close the gripper for the HSR.
+
+    :param designator: The designator containing the motion to be executed
+    :param topic_name: The topic name to publish the message to
+    """
+    if (designator.motion == GripperState.OPEN):
+        pub_gripper = rospy.Publisher(topic_name, GripperApplyEffortActionGoal,
+                                      queue_size=10)
+        rate = rospy.Rate(10)
+        msg = GripperApplyEffortActionGoal()
+        msg.goal.effort = 0.8
+        pub_gripper.publish(msg)
+
+    elif (designator.motion == GripperState.CLOSE):
+        pub_gripper = rospy.Publisher(topic_name, GripperApplyEffortActionGoal,
+                                      queue_size=10)
+        rate = rospy.Rate(10)
+        msg = GripperApplyEffortActionGoal()
+        msg.goal.effort = -0.8
+        pub_gripper.publish(msg)
+
+
+def tmc_talk(designator: TalkingMotion, topic_name: Optional[str] = '/talk_request'):
+    """
+    Publishes a sentence to the talk_request topic of the HSRB robot
+
+    :param designator: The designator containing the sentence to be spoken
+    :param topic_name: The topic name to publish the sentence to
+    """
+    pub = rospy.Publisher(topic_name, Voice, queue_size=10)
+    texttospeech = Voice()
+    # language 1 = english (0 = japanese)
+    texttospeech.language = 1
+    texttospeech.sentence = designator.cmd
+
+    pub.publish(texttospeech)

src/pycram/process_module.py

@@ -16,6 +16,7 @@
 from .language import Language
 from .robot_description import RobotDescription
 from typing_extensions import TYPE_CHECKING
+from .datastructures.enums import ExecutionType
 
 if TYPE_CHECKING:
     from .designators.motion_designator import BaseMotion
@@ -26,7 +27,7 @@ class ProcessModule:
     Implementation of process modules. Process modules are the part that communicate with the outer world to execute
      designators.
     """
-    execution_delay = True
+    execution_delay = False
     """
     Adds a delay of 0.5 seconds after executing a process module, to make the execution in simulation more realistic
     """
@@ -89,7 +90,7 @@ def __enter__(self):
         sets it to 'real'
         """
         self.pre = ProcessModuleManager.execution_type
-        ProcessModuleManager.execution_type = "real"
+        ProcessModuleManager.execution_type = ExecutionType.REAL
         self.pre_delay = ProcessModule.execution_delay
         ProcessModule.execution_delay = False
 
@@ -127,7 +128,7 @@ def __enter__(self):
         sets it to 'simulated'
         """
         self.pre = ProcessModuleManager.execution_type
-        ProcessModuleManager.execution_type = "simulated"
+        ProcessModuleManager.execution_type = ExecutionType.SIMULATED
 
     def __exit__(self, _type, value, traceback):
         """
@@ -140,6 +141,41 @@ def __call__(self):
         return self
 
 
+class SemiRealRobot:
+    """
+    Management class for executing designators on the semi-real robot. This is intended to be used in a with environment.
+    When importing this class an instance is imported instead.
+
+    Example:
+
+    .. code-block:: python
+
+        with semi_real_robot:
+            some designators
+    """
+
+    def __init__(self):
+        self.pre: str = ""
+
+    def __enter__(self):
+        """
+        Entering function for 'with' scope, saves the previously set :py:attr:`~ProcessModuleManager.execution_type` and
+        sets it to 'semi_real'
+        """
+        self.pre = ProcessModuleManager.execution_type
+        ProcessModuleManager.execution_type = ExecutionType.SEMI_REAL
+
+    def __exit__(self, type, value, traceback):
+        """
+        Exit method for the 'with' scope, sets the :py:attr:`~ProcessModuleManager.execution_type` to the previously
+        used one.
+        """
+        ProcessModuleManager.execution_type = self.pre
+
+    def __call__(self):
+        return self
+
+
 def with_real_robot(func: Callable) -> Callable:
     """
     Decorator to execute designators in the decorated class on the real robot.
@@ -158,7 +194,7 @@ def plan():
 
     def wrapper(*args, **kwargs):
         pre = ProcessModuleManager.execution_type
-        ProcessModuleManager.execution_type = "real"
+        ProcessModuleManager.execution_type = ExecutionType.REAL
         ret = func(*args, **kwargs)
         ProcessModuleManager.execution_type = pre
         return ret
@@ -184,7 +220,7 @@ def plan():
 
     def wrapper(*args, **kwargs):
         pre = ProcessModuleManager.execution_type
-        ProcessModuleManager.execution_type = "simulated"
+        ProcessModuleManager.execution_type = ExecutionType.Simulated
         ret = func(*args, **kwargs)
         ProcessModuleManager.execution_type = pre
         return ret
@@ -195,6 +231,7 @@ def wrapper(*args, **kwargs):
 # These are imported, so they don't have to be initialized when executing with
 simulated_robot = SimulatedRobot()
 real_robot = RealRobot()
+semi_real_robot = SemiRealRobot()
 
 
 class ProcessModuleManager(ABC):

src/pycram/process_modules/boxy_process_modules.py

@@ -200,29 +200,29 @@ def __init__(self):
         self._close_lock = Lock()
 
     def navigate(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return BoxyNavigation(self._navigate_lock)
 
     def looking(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return BoxyMoveHead(self._looking_lock)
 
     def detecting(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return BoxyDetecting(self._detecting_lock)
 
     def move_tcp(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return BoxyMoveTCP(self._move_tcp_lock)
 
     def move_arm_joints(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return BoxyMoveArmJoints(self._move_arm_joints_lock)
 
     def world_state_detecting(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return BoxyWorldStateDetecting(self._world_state_detecting_lock)
 
     def move_gripper(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return BoxyMoveGripper(self._move_gripper_lock)

src/pycram/process_modules/default_process_modules.py

@@ -196,41 +196,41 @@ def __init__(self):
         self._close_lock = Lock()
 
     def navigate(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultNavigation(self._navigate_lock)
 
     def looking(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultMoveHead(self._looking_lock)
 
     def detecting(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultDetecting(self._detecting_lock)
 
     def move_tcp(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultMoveTCP(self._move_tcp_lock)
 
     def move_arm_joints(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultMoveArmJoints(self._move_arm_joints_lock)
 
     def world_state_detecting(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultWorldStateDetecting(self._world_state_detecting_lock)
 
     def move_joints(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultMoveJoints(self._move_joints_lock)
 
     def move_gripper(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultMoveGripper(self._move_gripper_lock)
 
     def open(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultOpen(self._open_lock)
 
     def close(self):
-        if ProcessModuleManager.execution_type == "simulated":
+        if ProcessModuleManager.execution_type ==  ExecutionType.SIMULATED:
             return DefaultClose(self._close_lock)