diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index b8a1e98..ae0dda3 100644 --- a/spot_tools/src/spot_executor/spot_executor.py +++ b/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) diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index d76b264..599f956 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/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 diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index ba088eb..0db41ee 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/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()