coverage: skip unreachable waypoints fast (no-progress watchdog)

The reported "semi-stuck at a wall for tens of minutes" was Nav2 grinding on an
unreachable waypoint: the planner repeatedly "Failed to create plan", the
collision-averse backup/spin recoveries aborted ("Collision Ahead") because the
footprint sits in costmap-lethal cells in the wedge, and each goal burned the
full 30 s goal_timeout (minutes of wall time under a slow GUI) before skipping.

Add a no-progress watchdog: while a goal is in flight, if the robot doesn't get
meaningfully closer to it (>5 cm) for no_progress_sec (6 s), it's stuck/blocked —
cancel and skip immediately (one strike, not a retry: a genuinely unreachable
waypoint won't become reachable, and gap-fill revisits later). This short-
circuits Nav2's recovery ladder, which can't help in a wedge anyway. Legitimate
long transits keep making progress, so they're untouched; goal_timeout stays as
a slow absolute backstop for a robot that creeps but never arrives.

Verified: no false skips during normal driving, coverage unaffected. (The exact
tens-of-minutes stall needs an unreachable waypoint to reproduce, which the test
runs didn't hit; the watchdog fires by construction when it recurs.)

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Author: kaiaai

src/oomwoo_coverage/oomwoo_coverage/coverage_planner_node.py

@@ -139,6 +139,15 @@ def __init__(self) -> None:
         self.goal_deadline = None      # per-waypoint watchdog start
         self.declare_parameter('goal_timeout_sec', 30.0)
         self.goal_timeout = self.get_parameter('goal_timeout_sec').value
+        # No-progress skip: if the robot doesn't get meaningfully closer to the
+        # current waypoint for this long it's stuck/blocked — give up FAST rather
+        # than waiting out Nav2's collision-averse recovery ladder (backup/spin/
+        # wait can't move in a costmap-lethal wedge) and the full goal timeout.
+        # Legitimate long transits keep making progress, so they're untouched.
+        self.declare_parameter('no_progress_sec', 6.0)
+        self.no_progress_sec = self.get_parameter('no_progress_sec').value
+        self._goal_best_dist = None     # closest we've gotten to the current goal
+        self._goal_progress_t = None    # last time that distance improved
         self.declare_parameter('max_retries', 3)
         self.max_retries = self.get_parameter('max_retries').value
         self.awaiting = False
@@ -586,6 +595,8 @@ def _send_next(self) -> None:
         goal.pose = p
         self.awaiting = True
         self.goal_deadline = self.get_clock().now()
+        self._goal_best_dist = None     # reset no-progress tracking for this goal
+        self._goal_progress_t = self.get_clock().now()
         self.nav_client.send_goal_async(goal).add_done_callback(
             self._on_goal_response)
 
@@ -815,15 +826,39 @@ def _tick(self) -> None:
                 + rclpy.duration.Duration(seconds=self.escape_sec)
             return
 
-        # per-goal watchdog: cancel a waypoint Nav2 is grinding on
-        if self.awaiting and self.goal_deadline is not None \
-                and self._elapsed(self.goal_deadline) >= self.goal_timeout:
-            self.get_logger().warn(f'waypoint {self.wp_index} timed out')
-            if self.goal_handle is not None:
-                self.goal_handle.cancel_goal_async()  # -> _on_result(CANCELED)
-            else:
-                self.awaiting = False
-                self.wp_retries = self.max_retries    # force skip next result
+        # per-goal watchdog: give up on a waypoint FAST. Primary trigger is
+        # NO-PROGRESS — if the robot isn't getting closer, it's stuck/blocked and
+        # Nav2's recovery ladder can't help, so don't wait it out; goal_timeout is
+        # only a slow backstop for a robot that creeps but never arrives. Either
+        # way it's a one-strike skip (not a retry): a genuinely stuck waypoint
+        # won't become reachable by trying again, and gap-fill revisits later.
+        if self.awaiting and self.goal_deadline is not None:
+            stuck = False
+            if self.robot_xy is not None \
+                    and self.wp_index < len(self.cached_poses):
+                g = self.cached_poses[self.wp_index].pose.position
+                dist = ((g.x - self.robot_xy[0]) ** 2
+                        + (g.y - self.robot_xy[1]) ** 2) ** 0.5
+                if dist <= 0.3:                 # basically arrived; let Nav2 finish
+                    self._goal_progress_t = self.get_clock().now()
+                elif self._goal_best_dist is None \
+                        or dist < self._goal_best_dist - 0.05:
+                    self._goal_best_dist = dist  # got closer: progress
+                    self._goal_progress_t = self.get_clock().now()
+                elif self._goal_progress_t is not None \
+                        and self._elapsed(self._goal_progress_t) \
+                        >= self.no_progress_sec:
+                    stuck = True
+            if stuck or self._elapsed(self.goal_deadline) >= self.goal_timeout:
+                self.get_logger().warn(
+                    f'waypoint {self.wp_index} '
+                    + ('stuck (no progress)' if stuck else 'timed out')
+                    + '; skipping')
+                self.wp_retries = self.max_retries    # one-strike: skip, not retry
+                if self.goal_handle is not None:
+                    self.goal_handle.cancel_goal_async()  # -> _on_result -> skip
+                else:
+                    self.awaiting = False
         # several skips in a row = wedged in a costmap-lethal pocket; Nav2's
         # own recoveries refuse to move there, so recover ourselves
         elif not self.awaiting and self.consecutive_skips >= self.escape_after: