feat: safer navigation

skills/config/motions.yaml

@@ -9,4 +9,14 @@ play_motion:
       joints: [gripper_left_finger_joint, gripper_right_finger_joint]
       points:
       - positions: [0.04, 0.04]
+        time_from_start: 0.0
+    pre_navigation:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.15]
+        time_from_start: 0.0
+    post_navigation:
+      joints: [torso_lift_joint, head_1_joint, head_2_joint]
+      points:
+      - positions: [0.25, 0.0, 0.0]
         time_from_start: 0.0

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

skills/src/lasr_skills/go_to_location.py

@@ -1,18 +1,146 @@
 import smach_ros
-
+import smach
+import rospy
+import rosservice
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Pose, PoseStamped
 from std_msgs.msg import Header
 
+from typing import Union
+
+from lasr_skills import PlayMotion
+
+PUBLIC_CONTAINER = False
+
+try:
+    from pal_startup_msgs.srv import (
+        StartupStart,
+        StartupStop,
+        StartupStartRequest,
+        StartupStopRequest,
+    )
+except ModuleNotFoundError:
+    PUBLIC_CONTAINER = True
 
-class GoToLocation(smach_ros.SimpleActionState):
 
-    def __init__(self):
-        super(GoToLocation, self).__init__(
-            "move_base",
-            MoveBaseAction,
-            goal_cb=lambda ud, _: MoveBaseGoal(
-                target_pose=PoseStamped(pose=ud.location, header=Header(frame_id="map"))
-            ),
-            input_keys=["location"],
+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" if not IS_SIMULATION else "GO_TO_LOCATION"
+                    ),
+                    "aborted": "failed",
+                    "preempted": "failed",
+                },
+            )
+
+            if not IS_SIMULATION:
+
+                if PUBLIC_CONTAINER:
+                    rospy.logwarn(
+                        "You are using a public container. The head manager will not be stopped during navigation."
+                    )
+                else:
+                    smach.StateMachine.add(
+                        "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:
+                smach.StateMachine.add(
+                    "GO_TO_LOCATION",
+                    smach_ros.SimpleActionState(
+                        "move_base",
+                        MoveBaseAction,
+                        goal=MoveBaseGoal(
+                            target_pose=PoseStamped(
+                                pose=location, header=Header(frame_id="map")
+                            )
+                        ),
+                    ),
+                    transitions={
+                        "succeeded": (
+                            "DISABLE_HEAD_MANAGER"
+                            if not IS_SIMULATION
+                            else "RAISE_BASE"
+                        ),
+                        "aborted": "failed",
+                        "preempted": "failed",
+                    },
+                )
+            else:
+                smach.StateMachine.add(
+                    "GO_TO_LOCATION",
+                    smach_ros.SimpleActionState(
+                        "move_base",
+                        MoveBaseAction,
+                        goal_cb=lambda ud, _: MoveBaseGoal(
+                            target_pose=PoseStamped(
+                                pose=ud.location, header=Header(frame_id="map")
+                            )
+                        ),
+                        input_keys=["location"],
+                    ),
+                    transitions={
+                        "succeeded": (
+                            "DISABLE_HEAD_MANAGER"
+                            if not IS_SIMULATION
+                            else "RAISE_BASE"
+                        ),
+                        "aborted": "failed",
+                        "preempted": "failed",
+                    },
+                )
+
+            if not IS_SIMULATION:
+
+                if PUBLIC_CONTAINER:
+                    rospy.logwarn(
+                        "You are using a public container. The head manager will not be start following navigation."
+                    )
+                else:
+                    smach.StateMachine.add(
+                        "DISABLE_HEAD_MANAGER",
+                        smach_ros.ServiceState(
+                            "/pal_startup_control/stop",
+                            StartupStop,
+                            request=StartupStopRequest("head_manager"),
+                        ),
+                        transitions={
+                            "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",
+                },
+            )

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"],
             )