Manipulation bypass skills (LASR-at-Home#144)

* feat: Say skill.

* feat: ListenFor skill.

* feat: ReceiveObject skill.

* feat: HandoverObject skill.

* fix: don't skip planning!

* feat/refactor: PlayMotion skill.

* chore: drop demo.yaml and add motions.yaml.

* fix: outcomes.

* feat: working Handover and Receive skills.

skills/config/motions.yaml

@@ -0,0 +1,12 @@
+play_motion:
+  motions:
+    reach_arm:
+      joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+      points:
+      - positions: [1.61, -0.93, -3.14, 1.83, -1.58, -0.53, 0.00]
+        time_from_start: 0.0
+    open_gripper:
+      joints: [gripper_left_finger_joint, gripper_right_finger_joint]
+      points:
+      - positions: [0.04, 0.04]
+        time_from_start: 0.0

skills/src/lasr_skills/__init__.py

@@ -7,5 +7,9 @@
 from .look_to_point import LookToPoint
 from .go_to_location import GoToLocation
 from .go_to_semantic_location import GoToSemanticLocation
+from .say import Say
 from .listen import Listen
-from .say import Say
+from .listen_for import ListenFor
+from .play_motion import PlayMotion
+from .receive_object import ReceiveObject
+from .handover_object import HandoverObject

skills/src/lasr_skills/handover_object.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+import smach
+import smach_ros
+
+from std_srvs.srv import Empty
+
+from lasr_skills import Say, ListenFor, PlayMotion
+
+import rospkg
+import rosparam
+import os
+
+
+class HandoverObject(smach.StateMachine):
+
+    def __init__(self):
+        smach.StateMachine.__init__(
+            self, outcomes=["succeeded", "failed"], input_keys=["object_name"]
+        )
+
+        r = rospkg.RosPack()
+        els = rosparam.load_file(
+            os.path.join(r.get_path("lasr_skills"), "config", "motions.yaml")
+        )
+        for param, ns in els:
+            rosparam.upload_params(ns, param)
+
+        with self:
+            smach.StateMachine.add(
+                "CLEAR_OCTOMAP",
+                smach_ros.ServiceState("clear_octomap", Empty),
+                transitions={
+                    "succeeded": "REACH_ARM",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "REACH_ARM",
+                PlayMotion(motion_name="reach_arm"),
+                transitions={
+                    "succeeded": "SAY_TAKE",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "SAY_TAKE",
+                Say(
+                    format_str="Please grab the {} from my end-effector, and say `I am done`.",
+                ),
+                transitions={
+                    "succeeded": "LISTEN_DONE",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+                remapping={"placeholders": "object_name"},
+            )
+            smach.StateMachine.add(
+                "LISTEN_DONE",
+                ListenFor("done"),
+                transitions={
+                    "succeeded": "OPEN_GRIPPER",
+                    "not_done": "LISTEN_DONE",
+                    "failed": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "OPEN_GRIPPER",
+                PlayMotion(motion_name="open_gripper"),
+                transitions={
+                    "succeeded": "FOLD_ARM",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "FOLD_ARM",
+                PlayMotion(motion_name="home"),
+                transitions={
+                    "succeeded": "succeeded",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+
+
+if __name__ == "__main__":
+    import rospy
+
+    rospy.init_node("handover_object")
+    sm = HandoverObject()
+    sm.userdata.object_name = "cola"
+    outcome = sm.execute()
+    rospy.loginfo("Outcome: " + outcome)
+    rospy.signal_shutdown("Done")

skills/src/lasr_skills/listen_for.py

@@ -0,0 +1,26 @@
+import smach_ros
+from lasr_speech_recognition_msgs.msg import (
+    TranscribeSpeechAction,
+    TranscribeSpeechGoal,
+)
+
+from actionlib_msgs.msg import GoalStatus
+
+
+class ListenFor(smach_ros.SimpleActionState):
+    def __init__(self, wake_word: str):
+
+        def speech_result_cb(userdata, status, result):
+            if status == GoalStatus.SUCCEEDED:
+                if wake_word in result.sequence.lower():
+                    return "succeeded"
+                return "not_done"
+            return "aborted"
+
+        smach_ros.SimpleActionState.__init__(
+            self,
+            "transcribe_speech",
+            TranscribeSpeechAction,
+            result_cb=speech_result_cb,
+            outcomes=["succeeded", "preempted", "aborted", "not_done"],
+        )

skills/src/lasr_skills/play_motion.py

@@ -0,0 +1,24 @@
+import smach_ros
+
+from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal
+
+from typing import Union
+
+
+class PlayMotion(smach_ros.SimpleActionState):
+    def __init__(self, motion_name: Union[str, None] = None):
+        if motion_name is not None:
+            super(PlayMotion, self).__init__(
+                "play_motion",
+                PlayMotionAction,
+                goal=PlayMotionGoal(motion_name=motion_name, skip_planning=False),
+            )
+        else:
+            super(PlayMotion, self).__init__(
+                "play_motion",
+                PlayMotionAction,
+                goal_cb=lambda ud, _: PlayMotionGoal(
+                    motion_name=ud.motion_name, skip_planning=False
+                ),
+                input_keys=["motion_name"],
+            )

skills/src/lasr_skills/receive_object.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python3
+import smach
+import smach_ros
+
+from std_srvs.srv import Empty
+
+from lasr_skills import Say, ListenFor, PlayMotion
+
+import rospkg
+import rosparam
+import os
+
+
+class ReceiveObject(smach.StateMachine):
+
+    def __init__(self):
+        smach.StateMachine.__init__(
+            self, outcomes=["succeeded", "failed"], input_keys=["object_name"]
+        )
+
+        r = rospkg.RosPack()
+        els = rosparam.load_file(
+            os.path.join(r.get_path("lasr_skills"), "config", "motions.yaml")
+        )
+        for param, ns in els:
+            rosparam.upload_params(ns, param)
+
+        with self:
+            smach.StateMachine.add(
+                "CLEAR_OCTOMAP",
+                smach_ros.ServiceState("clear_octomap", Empty),
+                transitions={
+                    "succeeded": "REACH_ARM",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "REACH_ARM",
+                PlayMotion(motion_name="reach_arm"),
+                transitions={
+                    "succeeded": "OPEN_GRIPPER",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "OPEN_GRIPPER",
+                PlayMotion(motion_name="open_gripper"),
+                transitions={
+                    "succeeded": "SAY_PLACE",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "SAY_PLACE",
+                Say(
+                    format_str="Please place the {} in my end-effector, and say `I am done`.",
+                ),
+                transitions={
+                    "succeeded": "LISTEN_DONE",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+                remapping={"placeholders": "object_name"},
+            )
+            smach.StateMachine.add(
+                "LISTEN_DONE",
+                ListenFor("done"),
+                transitions={
+                    "succeeded": "CLOSE_GRIPPER",
+                    "not_done": "LISTEN_DONE",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "CLOSE_GRIPPER",
+                smach_ros.ServiceState("parallel_gripper_controller/grasp", Empty),
+                transitions={
+                    "succeeded": "FOLD_ARM",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+            smach.StateMachine.add(
+                "FOLD_ARM",
+                PlayMotion(motion_name="home"),
+                transitions={
+                    "succeeded": "succeeded",
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+
+
+if __name__ == "__main__":
+    import rospy
+
+    rospy.init_node("receive_object")
+    sm = ReceiveObject()
+    sm.userdata.object_name = "cola"
+    outcome = sm.execute()
+    rospy.loginfo("Outcome: " + outcome)
+    rospy.signal_shutdown("Done")