feat: HandoverObject skill, and update dunder init.

jws-1 · Mar 8, 2024 · a934e9d · a934e9d
1 parent bbdffdc
commit a934e9d
Show file tree

Hide file tree

Showing 3 changed files with 72 additions and 1 deletion.
diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py
@@ -9,3 +9,5 @@
 from .go_to_location import GoToLocation
 from .go_to_semantic_location import GoToSemanticLocation
 from .listen_wake_word import ListenWakeWord
+from .handover_object import HandoverObject
+from .receive_object import ReceiveObject
diff --git a/skills/src/lasr_skills/handover_object.py b/skills/src/lasr_skills/handover_object.py
@@ -0,0 +1,69 @@
+import smach
+from smach_ros import SimpleActionState
+
+from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal
+from pal_interaction_msgs.msg import TtsGoal, TtsAction, TtsText
+
+from lasr_skills import ListenWakeWord
+
+
+class HandoverObject(smach.StateMachine):
+
+    def __init__(self):
+        smach.StateMachine.__init__(
+            self, outcomes=["succeeded", "failed"], input_keys=["object_name"]
+        )
+
+        with self:
+            # TODO: clear octomap and look around
+            smach.StateMachine.add(
+                "HANDOVER_OBJECT",
+                SimpleActionState(
+                    "play_motion",
+                    PlayMotionAction,
+                    goal=PlayMotionGoal(
+                        motion_name="handover_object", skip_planning=False
+                    ),
+                ),
+                transitions={"succeeded": "succeeded", "aborted": "failed"},
+            )
+            smach.StateMachine.add(
+                "SAY",
+                SimpleActionState(
+                    "tts",
+                    TtsAction,
+                    goal_cb=lambda ud, _: TtsGoal(
+                        rawtext=TtsText(
+                            text=f"Please grab the {ud.object_name} in my hand, and say 'done' when you are ready for me to release it.",
+                            lang_id="en_GB",
+                        )
+                    ),
+                    input_keys=["object_name"],
+                ),
+            )
+            smach.StateMachine.add(
+                "WAIT_FOR_SPEECH",
+                ListenWakeWord("done"),
+                transitions={"succeeded": "OPEN_GRIPPER", "failed": "failed"},
+            )
+
+            smach.StateMachine.add(
+                "OPEN_GRIPPER",
+                SimpleActionState(
+                    "play_motion",
+                    PlayMotionAction,
+                    goal=PlayMotionGoal(
+                        motion_name="open_gripper", skip_planning=False
+                    ),
+                ),
+                transitions={"succeeded": "succeeded", "aborted": "failed"},
+            )
+            smach.StateMachine.add(
+                "FOLD_ARM",
+                SimpleActionState(
+                    "play_motion",
+                    PlayMotionAction,
+                    goal=PlayMotionGoal(motion_name="fold_arm", skip_planning=False),
+                ),
+                transitions={"succeeded": "succeeded", "aborted": "failed"},
+            )
diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py
@@ -44,7 +44,7 @@ def __init__(self):
             smach.StateMachine.add(
                 "WAIT_FOR_SPEECH",
                 ListenWakeWord("done"),
-                transitions={"succeeded": "CLOSE_GRIPPER", "failed": "WAIT_FOR_SPEECH"},
+                transitions={"succeeded": "CLOSE_GRIPPER", "failed": "failed"},
             )
 
             smach.StateMachine.add(