Feature/improve lease takeover

spot_tools/src/spot_executor/spot_executor.py

@@ -62,6 +62,12 @@ def monitor_lease():
                 self.owner = leases[0].lease_owner
                 self.owner_name = self.owner.client_name
 
+                # If someone else owns the lease, the current plan is invalid
+                # since the robot may be moved.
+                if not self.owner_name.startswith("understanding"):
+                    if self.feedback is not None:
+                        self.feedback.plan_valid = False
+
                 # If nobody owns the lease, then the owner string is empty.
                 # We should try to take the lease back in that case.
                 if self.owner_name == "":
@@ -115,7 +121,7 @@ def monitor_lease():
                     if self.feedback is not None:
                         self.feedback.break_out_of_waiting_loop = False
                     self.taking_back_lease = False
-                time.sleep(0.5)
+                time.sleep(0.1)
 
         self.monitoring_thread = threading.Thread(target=monitor_lease, daemon=False)
         self.monitoring_thread.start()
@@ -214,6 +220,7 @@ def process_action_sequence(self, sequence, feedback):
                 feedback.print("INFO", command)
 
                 success = False
+                feedback.plan_valid = True
                 try:
                     if type(command) is Follow:
                         success = self.execute_follow(command, feedback)

spot_tools/src/spot_skills/navigation_utils.py

@@ -143,7 +143,6 @@ def follow_trajectory_continuous(
                 return False
 
             path = shapely.LineString(waypoints_list[:, :2])
-            continue
 
         if time.time() - t0 > timeout:
             # TODO: I think we need to tell Spot to stop?
@@ -187,6 +186,10 @@ def follow_trajectory_continuous(
         )
         feedback.print("INFO", f"Navigating to waypoint {current_waypoint}")
 
+        if feedback is not None and not feedback.plan_valid:
+            feedback.print("INFO", "Plan invalidated by lease change, exiting follow")
+            return False
+
         navigate_to_absolute_pose(spot, current_waypoint, frame_name, stairs=stairs)
         time.sleep(1 / rate)
     return True

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

@@ -87,6 +87,7 @@ def __init__(self, odom_frame: str, output_dir: str):
         self.pick_confirmation_image_index = 0
 
         self.break_out_of_waiting_loop = False
+        self.plan_valid = True
         self.odom_frame = odom_frame
 
         self.output_dir = output_dir
@@ -548,6 +549,7 @@ def process_sequence():
             self.spot_executor.terminate_sequence(self.feedback_collector)
 
         self.feedback_collector.break_out_of_waiting_loop = False
+        self.feedback_collector.plan_valid = True
         self.background_thread = threading.Thread(target=process_sequence, daemon=False)
         self.background_thread.start()