Merge remote-tracking branch 'base/main' into HEAD

LASR-at-Home · Apr 11, 2024 · 091f179 · 091f179
2 parents 2191420 + ac66bb3
commit 091f179
Show file tree

Hide file tree

Showing 4 changed files with 57 additions and 18 deletions.
diff --git a/skills/config/motions.yaml b/skills/config/motions.yaml
@@ -13,10 +13,10 @@ play_motion:
     pre_navigation:
       joints: [torso_lift_joint]
       points:
-      - positions: [0.35]
+      - positions: [0.15]
         time_from_start: 0.0
     post_navigation:
-      joints: [torso_lift_joint]
+      joints: [torso_lift_joint, head_1_joint, head_2_joint]
       points:
-      - positions: [0.35]
+      - positions: [0.25, 0.0, 0.0]
         time_from_start: 0.0
diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py
@@ -5,12 +5,12 @@
 from .wait_for_person_in_area import WaitForPersonInArea
 from .describe_people import DescribePeople
 from .look_to_point import LookToPoint
+from .play_motion import PlayMotion
 from .go_to_location import GoToLocation
 from .go_to_semantic_location import GoToSemanticLocation
 from .say import Say
 from .listen import Listen
 from .listen_for import ListenFor
-from .play_motion import PlayMotion
 from .receive_object import ReceiveObject
 from .handover_object import HandoverObject
 from .ask_and_listen import AskAndListen
diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py
@@ -28,21 +28,23 @@ class GoToLocation(smach.StateMachine):
     def __init__(self, location: Union[Pose, None] = None):
         super(GoToLocation, self).__init__(outcomes=["succeeded", "failed"])
 
+        IS_SIMULATION = (
+            "/pal_startup_control/start" not in rosservice.get_service_list()
+        )
+
         with self:
             smach.StateMachine.add(
                 "LOWER_BASE",
                 PlayMotion("pre_navigation"),
                 transitions={
-                    "succeeded": "ENABLE_HEAD_MANAGER",
+                    "succeeded": (
+                        "ENABLE_HEAD_MANAGER" if not IS_SIMULATION else "GO_TO_LOCATION"
+                    ),
                     "aborted": "failed",
                     "preempted": "failed",
                 },
             )
 
-            IS_SIMULATION = (
-                "/pal_startup_control/start" in rosservice.get_service_list()
-            )
-
             if not IS_SIMULATION:
 
                 if PUBLIC_CONTAINER:
@@ -51,11 +53,17 @@ def __init__(self, location: Union[Pose, None] = None):
                     )
                 else:
                     smach.StateMachine.add(
-                        smach_ros.SimpleServiceState(
+                        "ENABLE_HEAD_MANAGER",
+                        smach_ros.ServiceState(
                             "/pal_startup_control/start",
                             StartupStart,
                             request=StartupStartRequest("head_manager", ""),
                         ),
+                        transitions={
+                            "succeeded": "GO_TO_LOCATION",
+                            "preempted": "failed",
+                            "aborted": "failed",
+                        },
                     )
 
             if location is not None:
@@ -71,7 +79,11 @@ def __init__(self, location: Union[Pose, None] = None):
                         ),
                     ),
                     transitions={
-                        "succeeded": "DISABLE_HEAD_MANAGER",
+                        "succeeded": (
+                            "DISABLE_HEAD_MANAGER"
+                            if not IS_SIMULATION
+                            else "RAISE_BASE"
+                        ),
                         "aborted": "failed",
                         "preempted": "failed",
                     },
@@ -90,7 +102,11 @@ def __init__(self, location: Union[Pose, None] = None):
                         input_keys=["location"],
                     ),
                     transitions={
-                        "succeeded": "DISABLE_HEAD_MANAGER",
+                        "succeeded": (
+                            "DISABLE_HEAD_MANAGER"
+                            if not IS_SIMULATION
+                            else "RAISE_BASE"
+                        ),
                         "aborted": "failed",
                         "preempted": "failed",
                     },
@@ -105,14 +121,26 @@ def __init__(self, location: Union[Pose, None] = None):
                 else:
                     smach.StateMachine.add(
                         "DISABLE_HEAD_MANAGER",
-                        smach_ros.SimpleServiceState(
+                        smach_ros.ServiceState(
                             "/pal_startup_control/stop",
                             StartupStop,
                             request=StartupStopRequest("head_manager"),
                         ),
                         transitions={
-                            "succeeded": "succeeded",
-                            "aborted": "succeeded",
-                            "preempted": "succeeded",
+                            "succeeded": "RAISE_BASE",
+                            "aborted": "failed",
+                            "preempted": "failed",
                         },
                     )
+
+            smach.StateMachine.add(
+                "RAISE_BASE",
+                PlayMotion("post_navigation"),
+                transitions={
+                    "succeeded": (
+                        "ENABLE_HEAD_MANAGER" if not IS_SIMULATION else "GO_TO_LOCATION"
+                    ),
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
diff --git a/skills/src/lasr_skills/play_motion.py b/skills/src/lasr_skills/play_motion.py
@@ -1,4 +1,5 @@
 import smach_ros
+import rospy
 
 from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal
 
@@ -11,14 +12,24 @@ def __init__(self, motion_name: Union[str, None] = None):
             super(PlayMotion, self).__init__(
                 "play_motion",
                 PlayMotionAction,
-                goal=PlayMotionGoal(motion_name=motion_name, skip_planning=False),
+                goal=PlayMotionGoal(
+                    motion_name=motion_name,
+                    skip_planning=rospy.get_param(
+                        f"/play_motion/motions/{motion_name}/joints"
+                    )
+                    == ["torso_lift_joint"],
+                ),
             )
         else:
             super(PlayMotion, self).__init__(
                 "play_motion",
                 PlayMotionAction,
                 goal_cb=lambda ud, _: PlayMotionGoal(
-                    motion_name=ud.motion_name, skip_planning=False
+                    motion_name=ud.motion_name,
+                    skip_planning=rospy.get_param(
+                        f"/play_motion/motions/{ud.motion_name}/joints"
+                        == ["torso_lift_joint"]
+                    ),
                 ),
                 input_keys=["motion_name"],
             )