Feature/graceful lease handoff

spot_tools/src/spot_executor/spot.py

@@ -64,6 +64,7 @@ def __init__(
             logging.warning("Spot does not have an arm, manipulation disabled!")
             self.manipulation_api_client = None
 
+        self.power_client = self.robot.ensure_client("power")
         self.estop_client = self.robot.ensure_client("estop")
         self.estop_keep_alive = None
         self.lease_client = self.robot.ensure_client("lease")

spot_tools/src/spot_executor/spot_executor.py

@@ -1,7 +1,11 @@
+import threading
 import time
 
 import numpy as np
 import skimage as ski
+from bosdyn.api.robot_state_pb2 import BehaviorFault
+from bosdyn.client.exceptions import LeaseUseError
+from bosdyn.client.robot_command import BehaviorFaultError
 from robot_executor_interface.action_descriptions import (
     Follow,
     Gaze,
@@ -34,6 +38,86 @@ def transform_command_frame(tf_trans, tf_q, command, feedback=None):
     return command
 
 
+# The lease manager should run in a separate thread to handle the exchange
+# of the lease e.g., when the tablet takes control of the robot.
+class LeaseManager:
+    def __init__(self, spot_interface, feedback=None):
+        self.spot_interface = spot_interface
+        self.monitoring_thread = None
+        self.feedback = feedback
+
+        self.initialize_thread()
+        self.taking_back_lease = False
+
+    def initialize_thread(self):
+        def monitor_lease():
+            while True:
+                leases = self.spot_interface.lease_client.list_leases()
+
+                # owner of the full lease
+                owner = leases[0].lease_owner
+
+                # print("Current lease owner: ", owner)
+                # if self.feedback is not None:
+                #     self.feedback.print(
+                #         "INFO",
+                #         f"LEASE MANAGER THREAD: Current lease owner: {owner.client_name}",
+                #     )
+
+                # If nobody owns the lease, then the owner string is empty.
+                # We should try to take the lease back in that case.
+                if owner.client_name == "":
+                    # We should set the feedback's break_out_of_waiting_loop to True
+                    # so that the pick skill gets immediately cancelled if it is running.
+                    if self.feedback is not None:
+                        self.feedback.break_out_of_waiting_loop = True
+
+                    self.taking_back_lease = True
+                    if self.feedback is not None:
+                        self.feedback.print(
+                            "INFO",
+                            "LEASE MANAGER THREAD: Trying to take lease back, since nobody owns it.",
+                        )
+                    self.spot_interface.take_lease()
+                    try:
+                        self.spot_interface.stand()
+                    except BehaviorFaultError:
+                        fault_ids = []
+                        for fault in (
+                            self.spot_interface.get_state().behavior_fault_state.faults
+                        ):
+                            if fault.cause == BehaviorFault.CAUSE_LEASE_TIMEOUT:
+                                fault_ids.append(fault.behavior_fault_id)
+                        for fault_id in fault_ids:
+                            if self.feedback is not None:
+                                self.feedback.print(
+                                    "INFO",
+                                    f"LEASE MANAGER THREAD: Clearing behavior fault {fault_id}",
+                                )
+                            self.spot_interface.command_client.clear_behavior_fault(
+                                fault_id
+                            )
+
+                        if (
+                            len(
+                                self.spot_interface.get_state().behavior_fault_state.faults
+                            )
+                            == 0
+                        ):
+                            self.spot_interface.stand()
+                        else:
+                            if self.feedback is not None:
+                                self.feedback.print(
+                                    "WARN",
+                                    "LEASE MANAGER THREAD: Could not clear all behavior faults, cannot stand.",
+                                )
+                    self.taking_back_lease = False
+                time.sleep(0.5)
+
+        self.monitoring_thread = threading.Thread(target=monitor_lease, daemon=False)
+        self.monitoring_thread.start()
+
+
 class SpotExecutor:
     def __init__(
         self,
@@ -52,6 +136,11 @@ def __init__(
         self.keep_going = True
         self.processing_action_sequence = False
 
+        self.lease_manager = None
+
+    def initialize_lease_manager(self, feedback):
+        self.lease_manager = LeaseManager(self.spot_interface, feedback)
+
     def terminate_sequence(self, feedback):
         # Tell the actions sequence to break
         self.keep_going = False
@@ -68,6 +157,7 @@ def terminate_sequence(self, feedback):
     def process_action_sequence(self, sequence, feedback):
         self.processing_action_sequence = True
         self.keep_going = True
+
         try:
             feedback.print("INFO", "Would like to execute: ")
             for command in sequence.actions:
@@ -76,7 +166,23 @@ def process_action_sequence(self, sequence, feedback):
             self.spot_interface.robot.time_sync.wait_for_sync()
             self.spot_interface.take_lease()
 
-            for ix, command in enumerate(sequence.actions):
+            ix = 0
+            while ix < len(sequence.actions):
+                # If the lease manager is actively taking back the lease and getting the
+                # robot to stand back up, we don't want to send it any commands. It will break.
+                if (
+                    self.lease_manager is not None
+                    and self.lease_manager.taking_back_lease
+                ):
+                    feedback.print(
+                        "INFO",
+                        "Waiting for lease manager to finish taking back lease...",
+                    )
+                    time.sleep(0.5)
+                    continue
+
+                command = sequence.actions[ix]
+
                 if not self.keep_going:
                     feedback.print("INFO", "Action sequence was pre-empted.")
                     break
@@ -85,22 +191,31 @@ def process_action_sequence(self, sequence, feedback):
                     pick_next = type(sequence.actions[ix + 1]) is Pick
                 feedback.print("INFO", "Spot executor executing command: ")
                 feedback.print("INFO", command)
-                if type(command) is Follow:
-                    self.execute_follow(command, feedback)
 
-                elif type(command) is Gaze:
-                    self.execute_gaze(command, feedback, pick_next=pick_next)
+                try:
+                    if type(command) is Follow:
+                        self.execute_follow(command, feedback)
 
-                elif type(command) is Pick:
-                    self.execute_pick(command, feedback)
+                    elif type(command) is Gaze:
+                        self.execute_gaze(command, feedback, pick_next=pick_next)
 
-                elif type(command) is Place:
-                    self.execute_place(command, feedback)
+                    elif type(command) is Pick:
+                        self.execute_pick(command, feedback)
+
+                    elif type(command) is Place:
+                        self.execute_place(command, feedback)
+
+                    else:
+                        raise Exception(
+                            f"SpotExecutor received unknown command type {type(command)}"
+                        )
+                    ix += 1
+
+                except LeaseUseError:
+                    # feedback.print("INFO", "Lost lease, stopping action sequence.")
+                    # Wait until the lease manager has taken the lease back
+                    time.sleep(1)
 
-                else:
-                    raise Exception(
-                        f"SpotExecutor received unknown command type {type(command)}"
-                    )
         except Exception as ex:
             self.processing_action_sequence = False
             raise ex

spot_tools/src/spot_skills/grasp_utils.py

@@ -38,7 +38,6 @@
     stow_arm,
 )
 from spot_skills.detection_utils import Detector
-from spot_skills.primitives import execute_recovery_action
 
 g_image_click = None
 g_image_display = None
@@ -203,11 +202,17 @@ def object_grasp(
             print("Found object centroid:", xy)
 
     if xy is None:
-        execute_recovery_action(spot, recover_arm=True)
-        spot.sit()
-        raise Exception(
-            "Failed to find an object in any cameras after 2 attempts. Please check the detector or user input."
-        )
+        if feedback is not None:
+            feedback.print(
+                "INFO",
+                "Failed to find an object in any cameras after 2 attempts. Please check the detector or user input.",
+            )
+        return False
+        # execute_recovery_action(spot, recover_arm=True)
+        # spot.sit()
+        # raise Exception(
+        #     "Failed to find an object in any cameras after 2 attempts. Please check the detector or user input."
+        # )
 
     # If xy is not None, then display the annotated image
     else:
@@ -222,9 +227,8 @@ def object_grasp(
             )
 
             if response is not None and not response:
-                raise Exception(
-                    "The user determined that the detection was invalid. Aborting."
-                )
+                feedback.print("INFO", "User requested abort.")
+                return False
 
     pick_vec = geometry_pb2.Vec2(x=xy[0], y=xy[1])
     stow_arm(spot)

spot_tools_ros/examples/test_spot_executor_ros.py

@@ -5,12 +5,7 @@
 from robot_executor_msgs.msg import ActionSequenceMsg
 from visualization_msgs.msg import MarkerArray
 
-from robot_executor_interface.action_descriptions import (
-    ActionSequence,
-    Follow,
-    Gaze,
-    Pick,
-)
+from robot_executor_interface.action_descriptions import ActionSequence, Follow, Pick
 
 
 class Tester(Node):
@@ -31,26 +26,26 @@ def __init__(self):
         path = np.array(
             [
                 [0.0, 0],
-                [1.0, 0],
-                [3.0, 5],
-                [5.0, 5],
+                [3.8, 0],
+                # [3.0, 5],
+                # [5.0, 5],
             ]
         )
 
         follow_cmd = Follow("hamilton/odom", path)
 
-        gaze_cmd = Gaze(
-            "hamilton/odom",
-            np.array([5.0, 5, 0]),
-            np.array([7.0, 7, 0]),
-            stow_after=True,
-        )
+        # gaze_cmd = Gaze(
+        #    "hamilton/odom",
+        #    np.array([5.0, 5, 0]),
+        #    np.array([7.0, 7, 0]),
+        #    stow_after=True,
+        # )
 
         pick_cmd = Pick(
             "hamilton/odom", "bag", np.array([5.0, 5, 0]), np.array([7.0, 7, 0])
         )
 
-        seq = ActionSequence("id0", "spot", [follow_cmd, gaze_cmd, pick_cmd])
+        seq = ActionSequence("id0", "spot", [follow_cmd, pick_cmd])
 
         publisher.publish(to_msg(seq))
         viz_publisher.publish(to_viz_msg(seq, "planner_ns"))

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

@@ -266,6 +266,7 @@ def __init__(self):
         self.declare_parameter("goal_tolerance", 0.0)
         goal_tolerance = self.get_parameter("goal_tolerance").value
         assert goal_tolerance > 0
+        self.get_logger().info(f"{goal_tolerance=}")
 
         # Pick/Inspect Skill
         self.declare_parameter("semantic_model_path", "")
@@ -379,6 +380,7 @@ def tf_lookup_fn(parent, child):
             follower_lookahead,
             goal_tolerance,
         )
+        self.spot_executor.initialize_lease_manager(self.feedback_collector)
 
         self.action_sequence_sub = self.create_subscription(
             ActionSequenceMsg,
@@ -407,9 +409,11 @@ def process_sequence():
             self.status_str = "Processing action sequence"
             self.get_logger().info("Starting action sequence")
             sequence = from_msg(msg)
+
             self.spot_executor.process_action_sequence(
                 sequence, self.feedback_collector
             )
+
             self.get_logger().info("Finished execution action sequence.")
             self.status_str = "Idle"