Feature/debugging lease handoff

spot_tools/src/spot_executor/spot.py

@@ -233,12 +233,16 @@ def set_estop(self, name="my_estop", timeout=9.0):
 
     def aquire_lease(self):
         self.lease = self.lease_client.acquire()
-        self.lease_keep_alive = bosdyn.client.lease.LeaseKeepAlive(self.lease_client)
+        self.lease_keep_alive = bosdyn.client.lease.LeaseKeepAlive(
+            self.lease_client, warnings=False
+        )
         self.lease_client.list_leases()
 
     def take_lease(self):
         self.lease_client.take()
-        self.lease_keep_alive = bosdyn.client.lease.LeaseKeepAlive(self.lease_client)
+        self.lease_keep_alive = bosdyn.client.lease.LeaseKeepAlive(
+            self.lease_client, warnings=False
+        )
         self.lease_client.list_leases()
 
     def power_on(self):

spot_tools/src/spot_executor/spot_executor.py

@@ -15,7 +15,7 @@
 from scipy.spatial.transform import Rotation
 
 from spot_skills.arm_utils import gaze_at_vision_pose
-from spot_skills.grasp_utils import object_grasp, object_place
+from spot_skills.grasp_utils import object_grasp, object_place, stow_arm
 from spot_skills.navigation_utils import (
     follow_trajectory_continuous,
     turn_to_point,
@@ -49,24 +49,22 @@ def __init__(self, spot_interface, feedback=None):
         self.initialize_thread()
         self.taking_back_lease = False
 
+        leases = self.spot_interface.lease_client.list_leases()
+        self.owner = leases[0].lease_owner
+        self.owner_name = self.owner.client_name
+
     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}",
-                #     )
+                self.owner = leases[0].lease_owner
+                self.owner_name = self.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 == "":
+                if self.owner_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:
@@ -80,6 +78,7 @@ def monitor_lease():
                         )
                     self.spot_interface.take_lease()
                     try:
+                        stow_arm(self.spot_interface)
                         self.spot_interface.stand()
                     except BehaviorFaultError:
                         fault_ids = []
@@ -111,6 +110,9 @@ def monitor_lease():
                                     "WARN",
                                     "LEASE MANAGER THREAD: Could not clear all behavior faults, cannot stand.",
                                 )
+                    time.sleep(1)
+                    if self.feedback is not None:
+                        self.feedback.break_out_of_waiting_loop = False
                     self.taking_back_lease = False
                 time.sleep(0.5)
 
@@ -151,7 +153,10 @@ def terminate_sequence(self, feedback):
 
         # Block until action sequence is done executing
         while self.processing_action_sequence:
-            feedback.print("INFO", "Waiting for previous action sequence to terminate.")
+            feedback.print(
+                "INFO",
+                "Waiting for previous action sequence to terminate. You must release the lease!",
+            )
             time.sleep(1)
 
     def process_action_sequence(self, sequence, feedback):
@@ -167,6 +172,7 @@ def process_action_sequence(self, sequence, feedback):
             self.spot_interface.take_lease()
 
             ix = 0
+            inner_loop_attempts = 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.
@@ -178,6 +184,14 @@ def process_action_sequence(self, sequence, feedback):
                         "INFO",
                         "Waiting for lease manager to finish taking back lease...",
                     )
+                    time.sleep(1)
+                    continue
+
+                # If we don't own the lease, we don't try to take any actions
+                if (
+                    self.lease_manager is not None
+                    and not self.lease_manager.owner_name.startswith("understanding")
+                ):
                     time.sleep(0.5)
                     continue
 
@@ -189,32 +203,41 @@ def process_action_sequence(self, sequence, feedback):
                 pick_next = False
                 if ix < len(sequence.actions) - 1:
                     pick_next = type(sequence.actions[ix + 1]) is Pick
+                feedback.print("INFO", "\n")
                 feedback.print("INFO", "Spot executor executing command: ")
                 feedback.print("INFO", command)
 
+                success = False
                 try:
                     if type(command) is Follow:
-                        self.execute_follow(command, feedback)
+                        success = self.execute_follow(command, feedback)
 
                     elif type(command) is Gaze:
-                        self.execute_gaze(command, feedback, pick_next=pick_next)
+                        success = self.execute_gaze(
+                            command, feedback, pick_next=pick_next
+                        )
 
                     elif type(command) is Pick:
-                        self.execute_pick(command, feedback)
+                        success = self.execute_pick(command, feedback)
 
                     elif type(command) is Place:
-                        self.execute_place(command, feedback)
+                        success = self.execute_place(command, feedback)
 
                     else:
                         raise Exception(
                             f"SpotExecutor received unknown command type {type(command)}"
                         )
-                    ix += 1
+                    if success or inner_loop_attempts > 1:
+                        ix += 1
+                        inner_loop_attempts = 0
+                    else:
+                        inner_loop_attempts += 1
+                        time.sleep(1)
 
                 except LeaseUseError:
                     # feedback.print("INFO", "Lost lease, stopping action sequence.")
                     # Wait until the lease manager has taken the lease back
-                    time.sleep(1)
+                    time.sleep(2)
 
         except Exception as ex:
             self.processing_action_sequence = False
@@ -262,6 +285,7 @@ def execute_pick(self, command, feedback):
             feedback.pick_image_feedback(sem_img, outline_img)
 
         feedback.print("INFO", "Finished `pick` command")
+        feedback.print("INFO", f"Pick skill success: {success}")
         return success
 
     def execute_place(self, command, feedback):

spot_tools/src/spot_skills/detection_utils.py

@@ -19,7 +19,7 @@ def __init__(self, spot, yolo_world_path):
             raise ValueError("YOLOWorld model path must be provided.")
 
         self.yolo_model = YOLOWorld(yolo_world_path)
-        custom_classes = ["", "bag", "wood block", "pipe"]
+        custom_classes = ["", "bag", "cone", "pipe"]
         self.yolo_model.set_classes(custom_classes)
         print("Set classes for YOLOWorld model.")
 

spot_tools/src/spot_skills/grasp_utils.py

@@ -12,6 +12,7 @@
 import bosdyn.client.lease
 import bosdyn.client.util
 import cv2
+import numpy as np
 from bosdyn.api import (
     geometry_pb2,
     manipulation_api_pb2,
@@ -25,13 +26,13 @@
 from bosdyn.client.robot_command import RobotCommandBuilder, block_until_arm_arrives
 
 from spot_skills.arm_utils import (
-    arm_to_carry,
     arm_to_drop,
     close_gripper,
     open_gripper,
     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
@@ -113,8 +114,8 @@ def object_place(spot, semantic_class="bag", position=None):
         return True
 
     time.sleep(0.25)
-    arm_to_carry(spot)
-    stow_arm(spot)
+    # arm_to_carry(spot)
+    # stow_arm(spot)
     arm_to_drop(spot)
 
     # odom_T_task = get_root_T_ground_body(
@@ -137,7 +138,7 @@ def object_place(spot, semantic_class="bag", position=None):
     # cmd_id = spot.command_client.robot_command(robot_cmd)
     # spot.robot.logger.info("Impedance command issued")
     # block_until_arm_arrives(spot.command_client, cmd_id, 10.0)
-    input("Did impedance work")
+    # input("Did impedance work")
 
     open_gripper(spot)
     # drop_object(spot)
@@ -201,6 +202,14 @@ def object_grasp(
                 "INFO",
                 "Failed to find an object in any cameras after 2 attempts. Please check the detector or user input.",
             )
+        execute_recovery_action(
+            spot,
+            recover_arm=False,
+            relative_pose=math_helpers.SE2Pose(
+                x=0.0, y=0.0, angle=np.random.choice([-0.5, 0.5])
+            ),
+        )
+        time.sleep(1)
         return False
         # execute_recovery_action(spot, recover_arm=True)
         # spot.sit()
@@ -249,10 +258,13 @@ def object_grasp(
     )
 
     loop_timer = time.time()
+
+    # Reset success --> agent is successful only if it detects the object and picks it up
+    success = False
     # Get feedback from the robot
     while True:
         current_time = time.time()
-        if current_time - loop_timer > 20:
+        if current_time - loop_timer > 15:
             if feedback is not None:
                 feedback.print("INFO", "The pick skill timed out!")
             print("The pick skill timed out!")
@@ -279,7 +291,8 @@ def object_grasp(
         ]
 
         if response.current_state in failed_states:
-            print("Grasp failed.")
+            if feedback is not None:
+                feedback.print("INFO", "GRASP FAILED")
             break
 
         if response.current_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED:
@@ -318,6 +331,16 @@ def object_grasp(
 
     print("Finished grasp.")
 
+    # If the robot was not successful, send it back to the location it started the skill at
+    if not success:
+        execute_recovery_action(
+            spot,
+            recover_arm=False,
+            relative_pose=math_helpers.SE2Pose(
+                x=np.random.uniform(-1, -0.5), y=0.0, angle=0.0
+            ),
+        )
+
     if debug:
         return success, debug_images
     return success

spot_tools/src/spot_skills/primitives.py

@@ -17,11 +17,11 @@ def execute_recovery_action(
         stow_arm(spot)
         close_gripper(spot)
 
-    if absolute_pose:
+    if absolute_pose is not None:
         navigate_to_absolute_pose(spot, absolute_pose)
         return
 
-    elif relative_pose:
+    elif relative_pose is not None:
         navigate_to_relative_pose(spot, relative_pose)
         return
 

spot_tools_ros/examples/test_spot_executor_ros.py

@@ -5,7 +5,12 @@
 from robot_executor_msgs.msg import ActionSequenceMsg
 from visualization_msgs.msg import MarkerArray
 
-from robot_executor_interface.action_descriptions import ActionSequence, Follow, Pick
+from robot_executor_interface.action_descriptions import (
+    ActionSequence,
+    Follow,
+    Pick,
+    Place,
+)
 
 
 class Tester(Node):
@@ -42,10 +47,25 @@ def __init__(self):
         # )
 
         pick_cmd = Pick(
+            "hamilton/odom", "cone", np.array([5.0, 5, 0]), np.array([7.0, 7, 0])
+        )
+
+        path = np.array(
+            [
+                [3.8, 0],
+                [7.8, 0],
+            ]
+        )
+
+        second_follow_cmd = Follow("hamilton/odom", path)
+
+        place_cmd = Place(
             "hamilton/odom", "bag", np.array([5.0, 5, 0]), np.array([7.0, 7, 0])
         )
 
-        seq = ActionSequence("id0", "spot", [follow_cmd, pick_cmd])
+        seq = ActionSequence(
+            "id0", "spot", [follow_cmd, pick_cmd, second_follow_cmd, place_cmd]
+        )
 
         publisher.publish(to_msg(seq))
         viz_publisher.publish(to_viz_msg(seq, "planner_ns"))