diff --git a/application/backend/app/routers/robot.py b/application/backend/app/routers/robot.py
index 6565e92..625ead4 100644
--- a/application/backend/app/routers/robot.py
+++ b/application/backend/app/routers/robot.py
@@ -2,6 +2,7 @@
 from sqlalchemy.orm import Session
 from sqlalchemy import desc
 from datetime import datetime
+import time
 from app.database import get_db
 from app.models.robot import RobotStatus
 from app.models.scan import ScanRecord
@@ -68,7 +69,7 @@ class MissionConfig(BaseModel):
     sampling_precision_m: float = 5.0
     area_width: Optional[float] = None
     area_height: Optional[float] = None
-    row_spacing: float = 0.8
+    row_spacing: float = 0.5
     waypoint_spacing: float = 0.5
     origin_x: float = 0.0
     origin_y: float = 0.0
@@ -78,7 +79,7 @@ class MissionConfig(BaseModel):
 
 def _calc_total_waypoints(area_width: float, area_height: float,
                           row_spacing: float, waypoint_spacing: float,
-                          wall_margin: float = 0.20) -> int:
+                          wall_margin: float = 0.30) -> int:
     """Match the coverage_planner.py waypoint generation logic (centered grid)."""
     import math
     ew = area_width - 2 * wall_margin  # centered: same effective width
@@ -151,18 +152,17 @@ async def start_coverage_mission(config: MissionConfig = None, db: Session = Dep
         )
         cmd = ['bash', '-c', ros_cmd]
 
-        # Start the mission as a background process (may fail on non-ROS hosts; continue for capture)
-        try:
-            mission_process = subprocess.Popen(
-                cmd,
-                env=clean_env,
-                preexec_fn=os.setsid if hasattr(os, "setsid") else None,
-            )
-        except Exception:
-            mission_process = None  # noqa: PLW0602
+        mission_process = subprocess.Popen(
+            cmd,
+            env=clean_env,
+            preexec_fn=os.setsid if hasattr(os, "setsid") else None,
+        )
+        time.sleep(1.0)
+        if mission_process.poll() is not None:
+            raise RuntimeError(f"roslaunch exited immediately with return code {mission_process.returncode}")
 
         # Start waypoint capture loop (one sample per '/coverage/capture_ready'=true message).
-        start_capture_loop(scan_id, total_points=total_points)
+        start_capture_loop(scan_id, total_points=total_points, require_ros=True)
         progress = get_capture_progress()
 
         return {
@@ -177,6 +177,17 @@ async def start_coverage_mission(config: MissionConfig = None, db: Session = Dep
         }
 
     except Exception as e:
+        if mission_process is not None and mission_process.poll() is None:
+            try:
+                pgid = os.getpgid(mission_process.pid)
+                if pgid != os.getpgrp():
+                    os.killpg(pgid, signal.SIGTERM)
+                else:
+                    mission_process.terminate()
+                mission_process.wait(timeout=5)
+            except Exception:
+                pass
+            mission_process = None  # noqa: PLW0602
         raise HTTPException(
             status_code=status.HTTP_500_INTERNAL_SERVER_ERROR,
             detail=f"Failed to start mission: {str(e)}"
@@ -248,6 +259,18 @@ async def get_mission_status():
             "message": "No mission has been started",
         }
 
+    if active_scan_id and progress.get("status") == "completed":
+        return {
+            "status": "completed",
+            "running": False,
+            "pid": mission_process.pid if mission_process and mission_process.poll() is None else None,
+            "scan_id": active_scan_id,
+            "captured_points": progress["captured_points"],
+            "total_points": progress["total_points"],
+            "progress_percent": progress["progress_percent"],
+            "message": "Mission completed",
+        }
+
     if active_scan_id and progress.get("status") == "running":
         return {
             "status": "running",
diff --git a/application/backend/app/services/ros_sensor_bridge.py b/application/backend/app/services/ros_sensor_bridge.py
index deb9bfd..61aaca4 100644
--- a/application/backend/app/services/ros_sensor_bridge.py
+++ b/application/backend/app/services/ros_sensor_bridge.py
@@ -16,8 +16,10 @@
 import io
 import os
 import logging
+import socket
 import threading
 from typing import Optional, Dict, Any
+from urllib.parse import urlparse
 
 logger = logging.getLogger(__name__)
 
@@ -38,12 +40,15 @@
     "rgb_image_path": None,        # kept for waypoint capture compatibility
     "voltage": None,
     "battery_percent": None,
+    "coverage_total_points": None,
+    "coverage_complete": False,
     "capture_ready_queue": [],
     "lock": threading.Lock(),
     "capture_ready_condition": threading.Condition(),
 }
 _ros_thread: Optional[threading.Thread] = None
 _ros_stop = threading.Event()
+DEFAULT_LOCAL_ROS_MASTER_URI = "http://127.0.0.1:11311"
 
 
 def voltage_to_percent(voltage: float) -> int:
@@ -55,6 +60,32 @@ def voltage_to_percent(voltage: float) -> int:
     return max(0, min(100, int(round(percent))))
 
 
+def _can_reach_ros_master(uri: str, timeout_sec: float = 0.5) -> bool:
+    if not uri:
+        return False
+    try:
+        parsed = urlparse(uri)
+        host = parsed.hostname
+        port = parsed.port or 11311
+        if not host:
+            return False
+        with socket.create_connection((host, port), timeout=timeout_sec):
+            return True
+    except OSError:
+        return False
+
+
+def get_ros_master_uri() -> str:
+    from app.config import settings
+
+    configured = (settings.ROS_MASTER_URI or os.environ.get("ROS_MASTER_URI") or "").strip()
+    if configured:
+        return configured
+    if _can_reach_ros_master(DEFAULT_LOCAL_ROS_MASTER_URI):
+        return DEFAULT_LOCAL_ROS_MASTER_URI
+    return ""
+
+
 def get_live_rgb_bytes() -> Optional[bytes]:
     """Return latest RGB JPEG bytes from cache, or None."""
     with _ros_cache["lock"]:
@@ -97,7 +128,7 @@ def _ros_subscriber_thread(thermal_image_save_dir: str, rgb_image_save_dir: str)
     """Run rospy node and subscribe to sensor topics; update _ros_cache."""
     try:
         import rospy
-        from std_msgs.msg import Float64, Float32, Bool
+        from std_msgs.msg import Float64, Float32, Bool, Int32
         from sensor_msgs.msg import Image
     except ImportError as e:
         logger.warning("rospy not available, ROS sensor bridge disabled: %s", e)
@@ -233,6 +264,14 @@ def cb_capture_ready(msg):
             _ros_cache["capture_ready_queue"].append(True)
             _ros_cache["capture_ready_condition"].notify_all()
 
+    def cb_coverage_total(msg):
+        with _ros_cache["lock"]:
+            _ros_cache["coverage_total_points"] = int(msg.data)
+
+    def cb_coverage_complete(msg):
+        with _ros_cache["lock"]:
+            _ros_cache["coverage_complete"] = bool(msg.data)
+
     def _set_voltage(voltage_value):
         with _ros_cache["lock"]:
             _ros_cache["voltage"] = voltage_value
@@ -260,6 +299,8 @@ def cb_voltage_battery(msg):
     rospy.Subscriber("/sensors/sht40/humidity", Float64, cb_hum, queue_size=1)
     rospy.Subscriber("/sensors/thermal/mean", Float64, cb_thermal_mean, queue_size=1)
     rospy.Subscriber("/coverage/capture_ready", Bool, cb_capture_ready, queue_size=50)
+    rospy.Subscriber("/coverage/total_points", Int32, cb_coverage_total, queue_size=10)
+    rospy.Subscriber("/coverage/complete", Bool, cb_coverage_complete, queue_size=10)
     topic_types = {}
     try:
         topic_types = {name: t for name, t in rospy.get_published_topics()}
@@ -286,11 +327,15 @@ def cb_voltage_battery(msg):
 
 
 def start_ros_bridge(thermal_image_save_dir: str, rgb_image_save_dir: str) -> bool:
-    """Start the ROS subscriber thread if ROS_MASTER_URI is set. Return True if using ROS."""
+    """Start the ROS subscriber thread when a reachable ROS master is available."""
     global _ros_thread
     from app.config import settings
-    uri = (settings.ROS_MASTER_URI or os.environ.get("ROS_MASTER_URI") or "").strip()
+    uri = get_ros_master_uri()
     if not uri:
+        logger.warning("ROS bridge not started: no ROS master configured or reachable")
+        return False
+    if not _can_reach_ros_master(uri):
+        logger.warning("ROS bridge not started: ROS master %s is not reachable", uri)
         return False
     if _ros_thread is not None and _ros_thread.is_alive():
         return True
@@ -326,9 +371,25 @@ def get_latest_from_ros() -> Dict[str, Any]:
             "rgb_image_path": _ros_cache["rgb_image_path"],
             "voltage": _ros_cache["voltage"],
             "battery_percent": _ros_cache["battery_percent"],
+            "coverage_total_points": _ros_cache["coverage_total_points"],
+            "coverage_complete": bool(_ros_cache["coverage_complete"]),
+        }
+
+
+def get_coverage_state() -> Dict[str, Any]:
+    with _ros_cache["lock"]:
+        return {
+            "total_points": _ros_cache["coverage_total_points"],
+            "complete": bool(_ros_cache["coverage_complete"]),
         }
 
 
+def clear_coverage_state() -> None:
+    with _ros_cache["lock"]:
+        _ros_cache["coverage_total_points"] = None
+        _ros_cache["coverage_complete"] = False
+
+
 def wait_for_next_capture_ready(timeout_sec: float = 0.5) -> bool:
     """Wait and consume one capture trigger from /coverage/capture_ready."""
     condition = _ros_cache["capture_ready_condition"]
@@ -348,9 +409,7 @@ def clear_capture_ready_queue() -> None:
 
 
 def is_ros_configured() -> bool:
-    from app.config import settings
-    uri = (settings.ROS_MASTER_URI or os.environ.get("ROS_MASTER_URI") or "").strip()
-    return bool(uri)
+    return bool(get_ros_master_uri())
 
 
 def get_required_topics_status() -> Dict[str, Any]:
diff --git a/application/backend/app/services/waypoint_capture_service.py b/application/backend/app/services/waypoint_capture_service.py
index 999710f..c140ad1 100644
--- a/application/backend/app/services/waypoint_capture_service.py
+++ b/application/backend/app/services/waypoint_capture_service.py
@@ -24,8 +24,10 @@
     is_ros_configured,
     start_ros_bridge,
     get_latest_from_ros,
+    get_coverage_state,
     wait_for_next_capture_ready,
     clear_capture_ready_queue,
+    clear_coverage_state,
     save_live_rgb_to_file,
     save_live_thermal_to_file,
 )
@@ -112,6 +114,15 @@ def _mark_scan_completed(scan_id: int) -> None:
         db.close()
 
 
+def _sync_total_points_from_ros() -> Optional[int]:
+    metrics = get_coverage_state()
+    total_points = metrics.get("total_points")
+    if total_points is not None:
+        with _capture_state_lock:
+            _capture_state["total_points"] = int(total_points)
+    return total_points
+
+
 def _capture_loop_impl(scan_id: int):
     stop_event = _capture_state["stop_event"]
     if not stop_event:
@@ -128,6 +139,18 @@ def _capture_loop_impl(scan_id: int):
     simulate = not use_ros and not _sht40_script.exists()
 
     while not stop_event.is_set():
+        if use_ros:
+            total_points = _sync_total_points_from_ros()
+            coverage_state = get_coverage_state()
+            with _capture_state_lock:
+                captured_points = int(_capture_state.get("captured_points") or 0)
+            if coverage_state.get("complete") and total_points is not None and captured_points >= int(total_points):
+                with _capture_state_lock:
+                    _capture_state["status"] = "completed"
+                _mark_scan_completed(scan_id)
+                stop_event.set()
+                break
+
         capture_ready = wait_for_next_capture_ready(timeout_sec=0.5) if use_ros else False
         if stop_event.is_set():
             break
@@ -221,8 +244,11 @@ def _capture_loop_impl(scan_id: int):
         finally:
             db.close()
 
+        ros_total_points = _sync_total_points_from_ros() if use_ros else None
         with _capture_state_lock:
             _capture_state["captured_points"] = sequence_index + 1
+            if ros_total_points is not None:
+                _capture_state["total_points"] = int(ros_total_points)
             total_points = _capture_state["total_points"]
         sequence_index += 1
         if total_points and sequence_index >= total_points:
@@ -237,7 +263,7 @@ def _capture_loop_impl(scan_id: int):
             _capture_state["status"] = "stopped"
 
 
-def start_capture_loop(scan_id: int, total_points: Optional[int] = None) -> None:
+def start_capture_loop(scan_id: int, total_points: Optional[int] = None, require_ros: bool = False) -> None:
     """Start background thread that captures on each ROS '/coverage/capture_ready'=true event."""
     old_thread = _capture_state["thread"]
     if old_thread is not None and old_thread.is_alive():
@@ -249,8 +275,11 @@ def start_capture_loop(scan_id: int, total_points: Optional[int] = None) -> None
     rgb_dir = os.path.join(settings.UPLOAD_DIR, "realsense_latest")
     os.makedirs(thermal_dir, exist_ok=True)
     os.makedirs(rgb_dir, exist_ok=True)
-    use_ros = is_ros_configured() and start_ros_bridge(thermal_dir, rgb_dir)
+    use_ros = start_ros_bridge(thermal_dir, rgb_dir)
+    if require_ros and not use_ros:
+        raise RuntimeError("ROS bridge unavailable; refusing to simulate mission captures")
     clear_capture_ready_queue()
+    clear_coverage_state()
     logger.warning("Starting capture loop: scan_id=%d, total_points=%s, use_ros=%s", scan_id, total_points, use_ros)
     _capture_state["use_ros"] = use_ros
     _capture_state["stop_event"] = threading.Event()
@@ -295,6 +324,10 @@ def get_capture_progress() -> dict:
         total_points = _capture_state.get("total_points")
         status = _capture_state.get("status") or "idle"
         last_capture_ready = _capture_state.get("last_capture_ready")
+        use_ros = bool(_capture_state.get("use_ros"))
+    ros_total_points = get_coverage_state().get("total_points") if use_ros else None
+    if ros_total_points is not None:
+        total_points = int(ros_total_points)
     progress_percent = 0.0
     if total_points and total_points > 0:
         progress_percent = min(100.0, (captured_points / total_points) * 100.0)
diff --git a/application/src/App.jsx b/application/src/App.jsx
index bcae852..30e72ac 100644
--- a/application/src/App.jsx
+++ b/application/src/App.jsx
@@ -190,6 +190,11 @@ function App() {
 
         if (progress?.status === 'completed' && !finishHandledRef.current) {
           finishHandledRef.current = true
+          try {
+            await apiClient.stopCoverageMission()
+          } catch (stopError) {
+            console.error('Failed to stop completed mission cleanly:', stopError)
+          }
           setIsScanning(false)
           setRobotStatus(prev => ({ ...prev, operatingState: 'Idle' }))
           setScanPhase('Scan complete')
@@ -275,7 +280,7 @@ function App() {
 
   const handleConfirmStartScan = async ({
     areaSize, precision, totalPoints: configuredTotal,
-    originX = 0, originY = 0, rowSpacing = 0.8, dwellTime = 2.0, waypointTimeout = 30.0
+    originX = 0, originY = 0, rowSpacing = 0.5, dwellTime = 2.0, waypointTimeout = 30.0
   }) => {
     setShowScanConfigModal(false)
     try {
@@ -506,4 +511,3 @@ function App() {
 }
 
 export default App
-
diff --git a/application/src/components/ScanConfigModal.jsx b/application/src/components/ScanConfigModal.jsx
index 248b946..8c19ef9 100644
--- a/application/src/components/ScanConfigModal.jsx
+++ b/application/src/components/ScanConfigModal.jsx
@@ -7,7 +7,7 @@ const SCAN_SECONDS_PER_SQUARE_METER = 5
 const SCAN_SECONDS_PER_POINT = 8
 const FUEL_ESTIMATE_SECONDS_PER_POINT = 60
 
-const WALL_MARGIN = 0.20
+const WALL_MARGIN = 0.45
 
 function calcTotalPoints(areaSize, precision, rowSpacing) {
   const ew = areaSize - 2 * WALL_MARGIN
@@ -32,7 +32,7 @@ function ScanConfigModal({ open, onCancel, onConfirm }) {
   const [precision, setPrecision] = useState(0.5)
   const [originX, setOriginX] = useState(0)
   const [originY, setOriginY] = useState(0)
-  const [rowSpacing, setRowSpacing] = useState(0.8)
+  const [rowSpacing, setRowSpacing] = useState(0.5)
   const [dwellTime, setDwellTime] = useState(2.0)
   const [waypointTimeout, setWaypointTimeout] = useState(30.0)
 
@@ -86,7 +86,7 @@ function ScanConfigModal({ open, onCancel, onConfirm }) {
         </div>
 
         <div className="scan-config-group">
-          <p className="scan-config-label">Origin (robot start position in meters)</p>
+          <p className="scan-config-label">Area Center in Odom (meters)</p>
           <div className="scan-config-inputs">
             <label className="scan-config-input-label">
               X
@@ -121,7 +121,7 @@ function ScanConfigModal({ open, onCancel, onConfirm }) {
                 step="0.1"
                 min="0.1"
                 value={rowSpacing}
-                onChange={(e) => setRowSpacing(parseFloat(e.target.value) || 0.8)}
+                onChange={(e) => setRowSpacing(parseFloat(e.target.value) || 0.5)}
                 className="scan-config-input"
               />
             </label>
diff --git a/catkin_ws/src/pyroscope_navigation/COVERAGE_TUNING.md b/catkin_ws/src/pyroscope_navigation/COVERAGE_TUNING.md
new file mode 100644
index 0000000..d89af86
--- /dev/null
+++ b/catkin_ws/src/pyroscope_navigation/COVERAGE_TUNING.md
@@ -0,0 +1,269 @@
+# Coverage Mission Nav — Tunable Parameters Guide
+
+A complete reference for every parameter you can adjust when testing and tuning
+the `coverage_mission_nav.launch` deployment.  Values shown are the current
+defaults; you can override most of them on the `roslaunch` command line.
+
+---
+
+## Quick-start launch template
+
+```bash
+roslaunch pyroscope_navigation coverage_mission_nav.launch \
+  area_width:=10.0        \
+  area_height:=10.0       \
+  row_spacing:=0.5        \
+  waypoint_spacing:=0.5   \
+  origin_x:=0.0           \
+  origin_y:=0.0           \
+  wall_margin:=0.45       \
+  dwell_time:=3.0         \
+  waypoint_timeout:=60.0  \
+  stall_timeout:=12.0     \
+  target_cost_threshold:=253
+```
+
+---
+
+## 1 · Coverage Planner parameters (`coverage_planner.py`)
+
+These are forwarded through `coverage_mission_nav.launch` arguments and set on
+the ROS parameter server under the `coverage_planner` node namespace.
+
+### 1.1 Mission geometry
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `area_width` | `10.0` | m | Total width (X-axis extent) of the survey rectangle. Increase for a larger field; decrease for a smaller plot. |
+| `area_height` | `10.0` | m | Total height (Y-axis extent) of the survey rectangle. |
+| `row_spacing` | `0.5` | m | Distance between adjacent sweep rows. Smaller → denser coverage but more waypoints and longer mission. Typical range: 0.3 – 1.0 m. |
+| `waypoint_spacing` | `0.5` | m | Distance between consecutive waypoints along a single row. Smaller → finer sampling within a row. |
+| `origin_x` | `0.0` | m | X-coordinate of the centre of the survey area in the `odom` frame. Shift to place the grid over a different part of the field. |
+| `origin_y` | `0.0` | m | Y-coordinate of the centre of the survey area. |
+| `wall_margin` | `0.45` | m | Safety buffer inset from the area boundary before the first/last waypoints are placed. Keeps the robot away from fences or plot edges. Must be ≥ robot half-width + `inflation_radius`. |
+
+### 1.2 Timing
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `dwell_time` | `3.0` | s | How long the robot pauses at each waypoint after arriving (e.g., for a sensor capture). Set to `0` to skip pausing. |
+| `waypoint_timeout` | `60.0` | s | Maximum time allowed to reach one waypoint. If exceeded the planner records a failure and tries the next target. Increase if the robot legitimately needs longer to navigate around obstacles. |
+| `stall_timeout` | `12.0` | s | If the robot makes no progress toward the current goal for this many seconds, the target is abandoned. Increase for slow terrain; decrease for faster failure detection. |
+| `progress_log_interval` | `2.0` | s | How often the planner logs its progress percentage. Cosmetic only. |
+
+### 1.3 Target safety / costmap filtering
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `target_cost_threshold` | `253` | 0–255 | Costmap cost above which a target cell is considered occupied/unsafe and will be skipped. `253` is just below `LETHAL_OBSTACLE` (254), so only confirmed lethal obstacles are rejected while high-inflation cells are still treated as reachable. Lower values (e.g. `128`) make the planner more conservative and skip heavily inflated cells too. |
+| `target_check_radius` | `0.05` | m | Radius around a candidate target within which the costmap is sampled. Larger → the robot must find a bigger clear patch before committing to a target. |
+| `max_target_failures` | `4` | count | Number of consecutive navigation failures on a single target before it is permanently skipped. Increase if obstacles are transient; decrease if you want the planner to move on quickly. |
+
+### 1.4 Path planning service
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `make_plan_tolerance` | `0.25` | m | Goal tolerance passed to the `move_base/make_plan` service when checking reachability. Larger → the global planner accepts plans that land slightly off the exact target, reducing failures in cluttered areas. |
+| `progress_epsilon` | `0.05` | m | Minimum improvement in distance-to-goal that counts as "progress". If the robot closes the gap by less than this in one `progress_log_interval` the stall timer keeps running. |
+
+### 1.5 No-target retry behaviour
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `no_target_retry_limit` | `8` | count | How many times the planner retries finding any reachable target before declaring the mission stuck. Increase if the costmap clears slowly. |
+| `no_target_retry_sleep` | `1.5` | s | Pause between retry attempts when no reachable target can be found. Gives the costmap time to update. |
+
+---
+
+## 2 · DWA local planner (`config/dwa_planner.yaml`)
+
+The DWA planner handles reactive obstacle avoidance and generates velocity
+commands every control cycle.
+
+### 2.1 Velocity limits
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `max_vel_x` | `0.25` | m/s | Maximum forward speed. Increasing this makes the robot faster but leaves less reaction time for obstacle avoidance. Start with ≤ 0.3 m/s outdoors. |
+| `min_vel_x` | `0.05` | m/s | Minimum forward speed when moving (prevents the robot stopping mid-trajectory). |
+| `max_vel_theta` | `0.8` | rad/s | Maximum angular (turning) speed. ~46 °/s. Reduce if the robot's odometry diverges during fast turns. |
+| `min_vel_theta` | `-0.8` | rad/s | Maximum reverse angular speed (must be negative to allow turning both ways). |
+| `min_vel_trans` | `0.04` | m/s | Absolute minimum translational speed when any motion is commanded. |
+| `max_vel_y` | `0.0` | m/s | Lateral velocity — keep at 0 for a differential-drive robot. |
+| `min_vel_y` | `0.0` | m/s | Lateral velocity lower bound — keep at 0. |
+
+### 2.2 Acceleration limits
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `acc_lim_x` | `0.8` | m/s² | Maximum linear acceleration. Lower values produce gentler starts/stops and reduce wheel slip. |
+| `acc_lim_theta` | `1.5` | rad/s² | Maximum angular acceleration. |
+| `acc_lim_y` | `0.0` | m/s² | Lateral acceleration — keep at 0. |
+
+### 2.3 Goal tolerances
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `xy_goal_tolerance` | `0.15` | m | How close the robot must get to the waypoint position to consider it reached. Tighten for precise coverage; loosen if the robot oscillates trying to reach an exact point. |
+| `yaw_goal_tolerance` | `0.30` | rad | Final heading tolerance (~17 °). Coverage missions typically do not require a precise final heading, so this can be relaxed. |
+| `latch_xy_goal_tolerance` | `true` | bool | When `true` the robot stops reversing once it is within `xy_goal_tolerance`, even if it drifts out again. Prevents the robot from backing up to meet its goal. |
+
+### 2.4 Forward simulation
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `sim_time` | `2.0` | s | How far ahead the planner simulates candidate trajectories. Longer → smoother paths but higher CPU cost. |
+| `sim_granularity` | `0.05` | s | Time step used inside each simulated trajectory. Smaller → finer collision checking but higher CPU cost. |
+| `vx_samples` | `10` | count | Number of forward-velocity candidates evaluated per cycle. More → better velocity choice but slower. |
+| `vtheta_samples` | `12` | count | Number of angular-velocity candidates evaluated. |
+| `vy_samples` | `1` | count | Lateral samples — 1 is correct for a non-holonomic robot. |
+
+### 2.5 Trajectory scoring weights
+
+These three weights are the most impactful DWA tuning knobs.
+
+| Parameter | Default | What it does |
+|-----------|---------|--------------|
+| `path_distance_bias` | `8.0` | Reward for staying close to the global plan. Higher → robot tracks the planned path tightly; lower → robot cuts corners. |
+| `goal_distance_bias` | `16.0` | Reward for moving toward the goal. Higher → robot charges toward the goal even if it deviates from the plan; lower → robot follows the plan more patiently. |
+| `occdist_scale` | `0.3` | Penalty for trajectories that pass near obstacles. Higher → robot gives obstacles a wider berth (may get stuck in narrow spaces); lower → robot cuts close to obstacles (may clip them). |
+
+### 2.6 Oscillation and collision prevention
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `oscillation_reset_dist` | `0.20` | m | How far the robot must travel to reset the oscillation detector. |
+| `forward_point_distance` | `0.10` | m | Distance of the forward look-ahead point used for collision cost. |
+| `stop_time_buffer` | `0.25` | s | Extra time buffer added to deceleration calculations to guarantee a complete stop before an obstacle. |
+
+---
+
+## 3 · Costmap parameters
+
+### 3.1 Shared obstacle & inflation layer (`config/costmap_common.yaml`)
+
+Loaded into both the global and local costmap namespaces.
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `footprint` | `[[-0.125,-0.10],[-0.125,0.10],[0.125,0.10],[0.125,-0.10]]` | m | Robot collision polygon (25 cm × 20 cm rectangle). Update if you swap the chassis. |
+| `obstacle_layer/laser_scan/obstacle_range` | `4.0` | m | Maximum range at which LIDAR readings are used to **mark** obstacles. |
+| `obstacle_layer/laser_scan/raytrace_range` | `5.0` | m | Maximum range used to **clear** (raytrace through) free space. Should be ≥ `obstacle_range`. |
+| `obstacle_layer/laser_scan/max_obstacle_height` | `0.5` | m | Points above this height are ignored. |
+| `obstacle_layer/laser_scan/min_obstacle_height` | `0.0` | m | Points below this height are ignored. |
+| `inflation_layer/inflation_radius` | `0.20` | m | Distance to inflate obstacles outward. Effectively adds this buffer around every obstacle. Must be ≥ robot half-width for safe navigation (robot half-width ≈ 0.125 m). |
+| `inflation_layer/cost_scaling_factor` | `2.0` | — | Exponential decay rate of inflation cost with distance. Higher → cost drops off faster (narrower inflation band). |
+| `resolution` | `0.05` | m/cell | Costmap grid resolution (5 cm). |
+
+### 3.2 Global costmap (`config/global_costmap.yaml`)
+
+Used by the global planner (NavFn) for full path planning.
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `global_costmap/update_frequency` | `4.0` | Hz | How often the global costmap is rebuilt from sensor data. Lower → less CPU; higher → faster obstacle updates for global planning. |
+| `global_costmap/publish_frequency` | `2.0` | Hz | How often the costmap is published for visualisation/consumers. |
+| `global_costmap/transform_tolerance` | `3.0` | s | Maximum age of TF data before the costmap reports a TF error. |
+| `global_costmap/width` | `15.0` | m | Rolling-window width centred on the robot. Larger → more context for global planning; more RAM and CPU. |
+| `global_costmap/height` | `15.0` | m | Rolling-window height. |
+
+### 3.3 Local costmap (`config/local_costmap.yaml`)
+
+Used by the DWA planner for reactive obstacle avoidance.
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `local_costmap/update_frequency` | `5.0` | Hz | Should be ≥ `controller_frequency` so the DWA planner always has fresh obstacle data. |
+| `local_costmap/publish_frequency` | `2.0` | Hz | Publication rate for visualisation. |
+| `local_costmap/transform_tolerance` | `3.0` | s | TF staleness tolerance. |
+| `local_costmap/width` | `4.0` | m | Rolling-window width for immediate surroundings. |
+| `local_costmap/height` | `4.0` | m | Rolling-window height. |
+
+---
+
+## 4 · move_base core parameters (inline in `coverage_mission_nav.launch`)
+
+| Parameter | Default | Unit | What it does |
+|-----------|---------|------|--------------|
+| `planner_frequency` | `1.0` | Hz | How often the global plan is recomputed. Higher → plan updates more quickly around new obstacles; higher CPU cost. |
+| `controller_frequency` | `5.0` | Hz | How often the DWA planner computes and sends a velocity command. Increase for faster obstacle response; must be ≤ `local_costmap/update_frequency`. |
+| `planner_patience` | `8.0` | s | How long move_base waits for the global planner before triggering a recovery behaviour. |
+| `controller_patience` | `8.0` | s | How long move_base waits for the local planner to make progress before triggering a recovery behaviour. |
+| `oscillation_timeout` | `30.0` | s | Time the robot can oscillate (detected by `oscillation_distance`) before a recovery is triggered. |
+| `oscillation_distance` | `0.15` | m | Robot must move this far to reset the oscillation timer. |
+| `NavfnROS/allow_unknown` | `true` | bool | Allows the global planner to plan through unknown (grey) costmap cells. **Must stay `true`** for mapless operation — setting it to `false` prevents planning in unexplored space. |
+| `NavfnROS/default_tolerance` | `0.5` | m | Goal tolerance for the global planner. Increase if NavFn repeatedly fails to find a plan to exact goal positions. |
+| `recovery_behavior_enabled` | `false` | bool | Disabling recovery keeps behaviour deterministic in open outdoor spaces. Enable (`true`) if the robot frequently gets hard-stuck. |
+| `conservative_reset_dist` | `0.50` | m | Radius around the robot that is cleared during a conservative reset recovery. |
+
+---
+
+## 5 · Practical tuning scenarios
+
+### Robot navigates but misses many coverage points
+
+- Decrease `row_spacing` and `waypoint_spacing` for denser waypoints.
+- Decrease `target_cost_threshold` (e.g. to `200`) so the planner rejects unsafe
+  targets earlier, avoiding wasted navigation attempts.
+
+### Robot is too slow
+
+- Increase `max_vel_x` in `dwa_planner.yaml` (try `0.35`).
+- Increase `acc_lim_x` to reach top speed sooner (try `1.2`).
+- Lower `sim_time` slightly (e.g. to `1.5`) to reduce planning latency.
+
+### Robot frequently reports "stalled" or "timeout"
+
+- Increase `stall_timeout` to `20.0` and `waypoint_timeout` to `90.0`.
+- Increase `controller_patience` and `planner_patience` in the launch file.
+- Check that `max_vel_x` is not too low (minimum ~0.1 m/s for outdoor terrain).
+
+### Robot clips or collides with obstacles
+
+- Increase `inflation_radius` in `costmap_common.yaml` (try `0.30` m).
+- Increase `occdist_scale` in `dwa_planner.yaml` (try `0.6`).
+- Decrease `target_cost_threshold` so obstacle-adjacent targets are skipped.
+
+### Robot oscillates or overshoots waypoints
+
+- Increase `xy_goal_tolerance` in `dwa_planner.yaml` (try `0.25` m).
+- Reduce `goal_distance_bias` (try `10.0`) so the planner brakes earlier.
+- Reduce `max_vel_x` and `acc_lim_x` for gentler motion.
+
+### "No reachable target found" messages dominate the log
+
+- Increase `no_target_retry_limit` to `15` and `no_target_retry_sleep` to `3.0`.
+- Increase `global_costmap/update_frequency` to `6.0` Hz so the costmap clears
+  faster after the robot moves.
+- Lower `target_cost_threshold` to allow targets near (but not inside) inflated
+  obstacle zones.
+
+### Robot gets permanently stuck
+
+- Set `recovery_behavior_enabled` to `true` in the launch file.
+- Enable `clearing_rotation_allowed` to let move_base spin in place to clear
+  obstacle cells from the local costmap.
+
+---
+
+## 6 · Should parameter tuning alone be sufficient?
+
+**Yes, for most issues in an obstacle-free or lightly cluttered environment.**
+The navigation stack is fully functional: the costmap-aware target selector,
+DWA planner, and move_base recovery pipeline provide a solid foundation that
+responds well to parameter changes.
+
+**However, some failure modes require code changes, not just tuning:**
+
+| Symptom | Likely root cause | Fix |
+|---------|-------------------|-----|
+| Robot spins at the first waypoint indefinitely | Odometry not publishing or wrong frame name | Check `/odom` topic and TF tree |
+| Global planner always fails (`allow_unknown false` warning) | `NavfnROS/allow_unknown` was accidentally set to `false` | Set back to `true` |
+| Coverage grid is not centred on the physical field | `origin_x` / `origin_y` set to the wrong values | Measure field origin in `odom` coordinates |
+| Robot navigates into inflated obstacle zones | `target_cost_threshold` too permissive (253 accepts all non-lethal costs including high-inflation cells) | Lower to ≤ 200 for conservative operation |
+| Very large areas (>15 m) with no plan found | Global costmap window too small | Increase `global_costmap/width` and `height` |
+| Mission completes but field not fully covered | `row_spacing` too large relative to sensor FOV | Halve `row_spacing` |
+
+In summary: **start with the parameters in Section 5 above**, iterate one change
+at a time, and watch the `/coverage/progress` and `/move_base/status` topics to
+understand what the planner is doing.
diff --git a/catkin_ws/src/pyroscope_navigation/config/costmap_common.yaml b/catkin_ws/src/pyroscope_navigation/config/costmap_common.yaml
index 9b62936..acdc746 100644
--- a/catkin_ws/src/pyroscope_navigation/config/costmap_common.yaml
+++ b/catkin_ws/src/pyroscope_navigation/config/costmap_common.yaml
@@ -1,27 +1,30 @@
 # Shared costmap parameters for global and local costmaps
 # Tuned for outdoor field operation (larger ranges than indoor transbot_nav)
+# See COVERAGE_TUNING.md for full tuning guidance.
 
-# Robot footprint (Transbot is ~25cm x 20cm)
+# Robot footprint (Transbot is ~25cm x 20cm).
+# Update this polygon if the chassis changes.
 footprint: [[-0.125, -0.10], [-0.125, 0.10], [0.125, 0.10], [0.125, -0.10]]
 
-# Obstacle layer params (must be under obstacle_layer/ so the plugin finds them)
+# Obstacle layer — marks and clears obstacles from LIDAR returns
 obstacle_layer:
   observation_sources: laser_scan
   laser_scan:
     sensor_frame: laser
     data_type: LaserScan
     topic: /scan
-    marking: true
-    clearing: true
-    max_obstacle_height: 0.5
+    marking: true        # mark occupied cells when a return is seen
+    clearing: true       # raytrace through free space to clear stale marks
+    max_obstacle_height: 0.5   # ignore returns above 0.5 m (tall posts, etc.)
     min_obstacle_height: 0.0
-    obstacle_range: 4.0
-    raytrace_range: 5.0
+    obstacle_range: 4.0        # mark obstacles up to 4 m away
+    raytrace_range: 5.0        # clear free space up to 5 m (must be ≥ obstacle_range)
 
-# Inflation layer params
+# Inflation layer — expands each obstacle outward so the robot's centre stays
+# safely away. inflation_radius must be ≥ robot half-width (~0.125 m).
 inflation_layer:
-  inflation_radius: 0.12
-  cost_scaling_factor: 5.0
+  inflation_radius: 0.20        # metres to inflate every obstacle
+  cost_scaling_factor: 2.0      # exponential decay rate; higher = narrower band
 
-# Map resolution
+# Grid resolution shared between global and local costmaps (5 cm per cell)
 resolution: 0.05
diff --git a/catkin_ws/src/pyroscope_navigation/config/dwa_planner.yaml b/catkin_ws/src/pyroscope_navigation/config/dwa_planner.yaml
index 041d94b..9ced536 100644
--- a/catkin_ws/src/pyroscope_navigation/config/dwa_planner.yaml
+++ b/catkin_ws/src/pyroscope_navigation/config/dwa_planner.yaml
@@ -1,38 +1,73 @@
 # DWA local planner — tuned for outdoor field operation
 # Slower speeds, larger tolerances, longer simulation horizon
+# See COVERAGE_TUNING.md for a full description of every parameter.
 
 DWAPlannerROS:
-  # Robot velocity limits (conservative for uneven terrain)
-  max_vel_x: 0.45
-  min_vel_x: -0.10
-  max_vel_y: 0.0
+
+  # ── Velocity limits ──────────────────────────────────────────────────────────
+  # max_vel_x: fastest the robot can move forward (m/s). Increase carefully;
+  #            higher speed means less reaction time for obstacle avoidance.
+  max_vel_x: 0.25
+  # min_vel_x: slowest forward speed when moving. Prevents the robot stopping
+  #            mid-trajectory in the absence of an explicit stop command.
+  min_vel_x: 0.05
+  max_vel_y: 0.0           # lateral — keep 0 for differential-drive
   min_vel_y: 0.0
-  max_vel_theta: 1.2
-  min_vel_theta: -1.2
-  min_vel_trans: 0.08
-  acc_lim_x: 1.5
-  acc_lim_y: 0.0
-  acc_lim_theta: 3.0
-
-  # Goal tolerances (relaxed for outdoor GPS/odom drift)
-  yaw_goal_tolerance: 0.4
-  xy_goal_tolerance: 0.40
+  # max_vel_theta: max angular (turn) speed in rad/s (~46°/s). Reduce if
+  #               odometry drifts badly during fast turns.
+  max_vel_theta: 0.8
+  min_vel_theta: -0.8      # must be negative to allow turning both ways
+  min_vel_trans: 0.04      # absolute minimum translational speed when moving
+
+  # ── Acceleration limits ───────────────────────────────────────────────────────
+  # acc_lim_x: max linear acceleration (m/s²). Lower = gentler starts/stops,
+  #            less wheel slip on loose terrain.
+  acc_lim_x: 0.8
+  acc_lim_y: 0.0           # lateral — keep 0
+  # acc_lim_theta: max angular acceleration (rad/s²).
+  acc_lim_theta: 1.5
+
+  # ── Goal tolerances ───────────────────────────────────────────────────────────
+  # yaw_goal_tolerance: final heading tolerance (rad, ~17°). Coverage missions
+  #                     rarely require precise final heading — can be relaxed.
+  yaw_goal_tolerance: 0.30
+  # xy_goal_tolerance: position tolerance (m). Tighten for precise sampling
+  #                    points; loosen if the robot oscillates around the goal.
+  xy_goal_tolerance: 0.15
+  # latch_xy_goal_tolerance: when true, once inside tolerance the robot stops
+  #                          even if it drifts slightly out — prevents backing up.
   latch_xy_goal_tolerance: true
 
-  # Forward simulation (longer horizon for outdoor planning)
+  # ── Forward simulation ────────────────────────────────────────────────────────
+  # sim_time: how far ahead (seconds) candidate trajectories are simulated.
+  #           Longer = smoother paths; higher CPU cost.
   sim_time: 2.0
+  # sim_granularity: time step inside each simulated trajectory (s). Smaller =
+  #                  finer collision checking; higher CPU cost.
   sim_granularity: 0.05
-  vx_samples: 12
-  vy_samples: 1
-  vtheta_samples: 20
+  vx_samples: 10           # number of forward-velocity candidates evaluated
+  vy_samples: 1            # 1 = correct for non-holonomic robot
+  vtheta_samples: 12       # number of angular-velocity candidates evaluated
 
-  # Trajectory scoring -- balance reaching goals vs avoiding walls
-  path_distance_bias: 20.0
-  goal_distance_bias: 28.0
+  # ── Trajectory scoring weights ────────────────────────────────────────────────
+  # These three are the most impactful DWA tuning knobs.
+  # path_distance_bias: reward for staying near the global plan. Higher = robot
+  #                     tracks the planned path more tightly.
+  path_distance_bias: 8.0
+  # goal_distance_bias: reward for moving toward the goal. Higher = robot charges
+  #                     toward goal even if it deviates from the plan.
+  goal_distance_bias: 16.0
+  # occdist_scale: penalty for passing near obstacles. Higher = wider berth around
+  #               obstacles (may get stuck in narrow gaps); lower = cuts closer.
   occdist_scale: 0.3
 
-  # Oscillation prevention
-  oscillation_reset_dist: 0.10
+  # ── Oscillation prevention ────────────────────────────────────────────────────
+  # oscillation_reset_dist: robot must travel this far (m) to reset oscillation
+  #                         detection counter.
+  oscillation_reset_dist: 0.20
 
-  # Stop before hitting things
-  forward_point_distance: 0.15
+  # ── Collision prevention ──────────────────────────────────────────────────────
+  forward_point_distance: 0.10  # look-ahead distance for collision cost (m)
+  # stop_time_buffer: extra time buffer added to deceleration calculations so
+  #                   the robot is guaranteed to stop before hitting an obstacle.
+  stop_time_buffer: 0.25
diff --git a/catkin_ws/src/pyroscope_navigation/config/global_costmap.yaml b/catkin_ws/src/pyroscope_navigation/config/global_costmap.yaml
index 9be8d99..3019c46 100644
--- a/catkin_ws/src/pyroscope_navigation/config/global_costmap.yaml
+++ b/catkin_ws/src/pyroscope_navigation/config/global_costmap.yaml
@@ -1,17 +1,24 @@
 # Global costmap — rolling window, no static map
 # Operates in odom frame for mapless outdoor navigation
+# See COVERAGE_TUNING.md for full tuning guidance.
 
 global_costmap:
-  global_frame: odom
+  global_frame: odom          # odometry frame — no SLAM map required
   robot_base_frame: base_link
-  update_frequency: 2.0
-  publish_frequency: 1.0
+  # update_frequency: how often the costmap is rebuilt from sensor data (Hz).
+  # Increase for faster obstacle updates; trades off against CPU load.
+  update_frequency: 4.0
+  publish_frequency: 2.0      # publication rate for rviz / consumers
+  # transform_tolerance: max TF data age before the costmap raises a TF error.
   transform_tolerance: 3.0
-  static_map: false
-  rolling_window: true
+  static_map: false           # no pre-built map — must stay false for mapless operation
+  rolling_window: true        # window follows the robot as it moves
+  # width / height: rolling-window size in metres. Must be large enough for the
+  # global planner to see the next waypoint from the current position.
+  # Increase if global planning regularly fails on longer waypoint gaps.
   width: 15.0
   height: 15.0
-  resolution: 0.05
+  resolution: 0.05            # 5 cm grid cells — matches costmap_common.yaml
 
   plugins:
     - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
diff --git a/catkin_ws/src/pyroscope_navigation/config/local_costmap.yaml b/catkin_ws/src/pyroscope_navigation/config/local_costmap.yaml
index 45e9b3a..b53a1e0 100644
--- a/catkin_ws/src/pyroscope_navigation/config/local_costmap.yaml
+++ b/catkin_ws/src/pyroscope_navigation/config/local_costmap.yaml
@@ -1,13 +1,19 @@
 # Local costmap — rolling window for reactive obstacle avoidance
+# Smaller and faster than the global costmap; used only by the DWA planner.
+# See COVERAGE_TUNING.md for full tuning guidance.
 
 local_costmap:
   global_frame: odom
   robot_base_frame: base_link
+  # update_frequency: should be ≥ controller_frequency so the DWA planner
+  # always has fresh obstacle data (controller_frequency defaults to 5 Hz).
   update_frequency: 5.0
   publish_frequency: 2.0
   transform_tolerance: 3.0
   static_map: false
   rolling_window: true
+  # width / height: keep small (4 m) for fast update cycles; just large enough
+  # to see obstacles within the DWA sim_time horizon (2 s × 0.25 m/s ≈ 0.5 m).
   width: 4.0
   height: 4.0
   resolution: 0.05
diff --git a/catkin_ws/src/pyroscope_navigation/launch/coverage_mission_nav.launch b/catkin_ws/src/pyroscope_navigation/launch/coverage_mission_nav.launch
index 40ded62..343ec7d 100644
--- a/catkin_ws/src/pyroscope_navigation/launch/coverage_mission_nav.launch
+++ b/catkin_ws/src/pyroscope_navigation/launch/coverage_mission_nav.launch
@@ -1,57 +1,137 @@
 <launch>
-    <!-- Coverage planner arguments (can be passed from backend or command line) -->
-    <arg name="area_width" default="10.0"/>
+    <!-- ═══════════════════════════════════════════════════════════════════════
+         Coverage planner arguments — override on the command line, e.g.:
+           roslaunch pyroscope_navigation coverage_mission_nav.launch \
+             area_width:=15.0 row_spacing:=0.3 dwell_time:=5.0
+         See COVERAGE_TUNING.md for a full description of every parameter.
+         ═══════════════════════════════════════════════════════════════════════ -->
+
+    <!-- ── Mission geometry ─────────────────────────────────────────────────── -->
+    <!-- Total size of the survey rectangle (metres) -->
+    <arg name="area_width"  default="10.0"/>
     <arg name="area_height" default="10.0"/>
-    <arg name="row_spacing" default="1.0"/>
-    <arg name="waypoint_spacing" default="1.0"/>
+    <!-- row_spacing: distance between adjacent sweep rows (m).
+         Smaller = denser coverage; more waypoints, longer mission. -->
+    <arg name="row_spacing"       default="0.5"/>
+    <!-- waypoint_spacing: distance between waypoints along a row (m). -->
+    <arg name="waypoint_spacing"  default="0.5"/>
+    <!-- origin_x / origin_y: centre of the survey area in the odom frame (m). -->
     <arg name="origin_x" default="0.0"/>
     <arg name="origin_y" default="0.0"/>
+    <!-- wall_margin: safety inset from the area boundary before the first/last
+         waypoints are placed (m). Must be ≥ robot half-width + inflation_radius. -->
+    <arg name="wall_margin" default="0.45"/>
+
+    <!-- ── Path planning ────────────────────────────────────────────────────── -->
+    <!-- make_plan_tolerance: goal tolerance passed to move_base/make_plan (m).
+         Increase if the global planner often refuses plans near exact targets. -->
+    <arg name="make_plan_tolerance" default="0.25"/>
+
+    <!-- ── Target safety / costmap filtering ────────────────────────────────── -->
+    <!-- target_cost_threshold: costmap cost (0–255) above which a target cell
+         is considered occupied and skipped. 253 = just below LETHAL_OBSTACLE.
+         Lower (e.g. 128) for more conservative operation. -->
+    <arg name="target_cost_threshold" default="253"/>
+    <!-- target_check_radius: radius (m) around each candidate target sampled
+         to confirm it is obstacle-free. -->
+    <arg name="target_check_radius" default="0.05"/>
+    <!-- max_target_failures: consecutive navigation failures before a target is
+         permanently skipped. Increase for transient obstacles. -->
+    <arg name="max_target_failures" default="4"/>
+
+    <!-- ── Timing ────────────────────────────────────────────────────────────── -->
+    <!-- progress_log_interval: how often the planner logs progress (s). -->
+    <arg name="progress_log_interval" default="2.0"/>
+    <!-- progress_epsilon: minimum distance improvement (m) that counts as
+         progress for the stall detector. -->
+    <arg name="progress_epsilon" default="0.05"/>
+    <!-- stall_timeout: seconds without progress before the current target is
+         abandoned. Increase on slow terrain; decrease for faster failure detection. -->
+    <arg name="stall_timeout" default="12.0"/>
+    <!-- dwell_time: pause at each waypoint after arrival (s) for sensor capture.
+         Set to 0 to skip pausing. -->
     <arg name="dwell_time" default="3.0"/>
+    <!-- waypoint_timeout: max time to reach one waypoint before recording a
+         failure (s). Increase if the robot navigates around large obstacles. -->
     <arg name="waypoint_timeout" default="60.0"/>
 
+    <!-- ── No-target retry behaviour ─────────────────────────────────────────── -->
+    <!-- no_target_retry_limit: retries before declaring the mission stuck. -->
+    <arg name="no_target_retry_limit" default="8"/>
+    <!-- no_target_retry_sleep: pause between retries (s) — gives the costmap
+         time to clear after the robot moves. -->
+    <arg name="no_target_retry_sleep" default="1.5"/>
+
     <!-- Static transform: base_link -> laser (15cm above base) -->
     <node pkg="tf" type="static_transform_publisher" name="base_to_laser_tf"
           args="0 0 0.15 0 0 0 base_link laser 100"/>
 
-    <!-- move_base with mapless rolling-window costmaps and DWA planner -->
-    <node pkg="move_base" type="move_base" name="move_base" output="screen">
-        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>
+    <!-- ═══════════════════════════════════════════════════════════════════════
+         move_base — mapless rolling-window costmaps + DWA local planner.
+         Core timing/patience params are set inline below; costmap and DWA
+         params live in config/*.yaml (see COVERAGE_TUNING.md).
+         ═══════════════════════════════════════════════════════════════════════ -->
+    <node pkg="move_base" type="move_base" name="move_base" output="screen" required="true">
+        <param name="base_local_planner"  value="dwa_local_planner/DWAPlannerROS"/>
         <param name="base_global_planner" value="navfn/NavfnROS"/>
 
-        <!-- Global planner: allow planning through unknown space (no static map) -->
-        <param name="NavfnROS/allow_unknown" value="true"/>
-        <param name="NavfnROS/default_tolerance" value="0.5"/>
+        <!-- allow_unknown MUST stay true — required for mapless (no static map) operation.
+             Setting it false prevents NavFn from planning through unexplored space. -->
+        <param name="NavfnROS/allow_unknown"       value="true"/>
+        <!-- default_tolerance: global planner goal tolerance (m). Increase if
+             NavFn repeatedly fails to find plans to exact goal positions. -->
+        <param name="NavfnROS/default_tolerance"   value="0.5"/>
 
         <!-- Load costmap configs -->
         <rosparam file="$(find pyroscope_navigation)/config/costmap_common.yaml" command="load" ns="global_costmap"/>
         <rosparam file="$(find pyroscope_navigation)/config/costmap_common.yaml" command="load" ns="local_costmap"/>
         <rosparam file="$(find pyroscope_navigation)/config/global_costmap.yaml" command="load"/>
-        <rosparam file="$(find pyroscope_navigation)/config/local_costmap.yaml" command="load"/>
-        <rosparam file="$(find pyroscope_navigation)/config/dwa_planner.yaml" command="load"/>
+        <rosparam file="$(find pyroscope_navigation)/config/local_costmap.yaml"  command="load"/>
+        <rosparam file="$(find pyroscope_navigation)/config/dwa_planner.yaml"    command="load"/>
 
-        <!-- Planner frequency -->
-        <param name="planner_frequency" value="1.0"/>
+        <!-- planner_frequency: how often to recompute the global plan (Hz).
+             Increase for faster reaction to new obstacles; higher CPU cost. -->
+        <param name="planner_frequency"    value="1.0"/>
+        <!-- controller_frequency: how often to compute velocity commands (Hz).
+             Must be ≤ local_costmap/update_frequency (5 Hz). -->
         <param name="controller_frequency" value="5.0"/>
-        <param name="planner_patience" value="3.0"/>
-        <param name="controller_patience" value="3.0"/>
-        <param name="oscillation_timeout" value="4.0"/>
+        <!-- planner_patience / controller_patience: seconds before move_base
+             triggers a recovery behaviour when stuck. -->
+        <param name="planner_patience"     value="8.0"/>
+        <param name="controller_patience"  value="8.0"/>
+        <!-- oscillation_timeout / oscillation_distance: if the robot oscillates
+             (moves < oscillation_distance in oscillation_timeout seconds) a
+             recovery behaviour fires. -->
+        <param name="oscillation_timeout"  value="30.0"/>
         <param name="oscillation_distance" value="0.15"/>
-        <param name="recovery_behavior_enabled" value="true"/>
-
-        <!-- Recovery behaviors -->
-        <param name="clearing_rotation_allowed" value="true"/>
-        <param name="conservative_reset_dist" value="1.5"/>
+        <!-- Disable recovery for deterministic outdoor behaviour.
+             Set to true if the robot frequently gets hard-stuck. -->
+        <param name="recovery_behavior_enabled"  value="false"/>
+        <param name="clearing_rotation_allowed"  value="false"/>
+        <!-- conservative_reset_dist: radius (m) cleared around robot during a
+             conservative-reset recovery. -->
+        <param name="conservative_reset_dist" value="0.50"/>
     </node>
 
-    <!-- Coverage planner (lawnmower pattern via move_base) -->
+    <!-- Coverage planner (costmap-aware target selection via move_base) -->
     <node pkg="pyroscope_navigation" type="coverage_planner.py" name="coverage_planner" output="screen">
-        <param name="area_width" value="$(arg area_width)"/>
-        <param name="area_height" value="$(arg area_height)"/>
-        <param name="row_spacing" value="$(arg row_spacing)"/>
-        <param name="waypoint_spacing" value="$(arg waypoint_spacing)"/>
-        <param name="origin_x" value="$(arg origin_x)"/>
-        <param name="origin_y" value="$(arg origin_y)"/>
-        <param name="dwell_time" value="$(arg dwell_time)"/>
-        <param name="waypoint_timeout" value="$(arg waypoint_timeout)"/>
+        <param name="area_width"            value="$(arg area_width)"/>
+        <param name="area_height"           value="$(arg area_height)"/>
+        <param name="row_spacing"           value="$(arg row_spacing)"/>
+        <param name="waypoint_spacing"      value="$(arg waypoint_spacing)"/>
+        <param name="origin_x"             value="$(arg origin_x)"/>
+        <param name="origin_y"             value="$(arg origin_y)"/>
+        <param name="wall_margin"           value="$(arg wall_margin)"/>
+        <param name="make_plan_tolerance"   value="$(arg make_plan_tolerance)"/>
+        <param name="target_cost_threshold" value="$(arg target_cost_threshold)"/>
+        <param name="target_check_radius"   value="$(arg target_check_radius)"/>
+        <param name="max_target_failures"   value="$(arg max_target_failures)"/>
+        <param name="progress_log_interval" value="$(arg progress_log_interval)"/>
+        <param name="progress_epsilon"      value="$(arg progress_epsilon)"/>
+        <param name="stall_timeout"         value="$(arg stall_timeout)"/>
+        <param name="no_target_retry_limit" value="$(arg no_target_retry_limit)"/>
+        <param name="no_target_retry_sleep" value="$(arg no_target_retry_sleep)"/>
+        <param name="dwell_time"            value="$(arg dwell_time)"/>
+        <param name="waypoint_timeout"      value="$(arg waypoint_timeout)"/>
     </node>
 </launch>
diff --git a/catkin_ws/src/pyroscope_navigation/scripts/coverage_planner.py b/catkin_ws/src/pyroscope_navigation/scripts/coverage_planner.py
index 800f3f1..31a20a3 100755
--- a/catkin_ws/src/pyroscope_navigation/scripts/coverage_planner.py
+++ b/catkin_ws/src/pyroscope_navigation/scripts/coverage_planner.py
@@ -1,169 +1,145 @@
 #!/usr/bin/env python
 
 """
-Coverage Path Planner for Pyroscope
-Generates a boustrophedon (lawnmower) pattern over a rectangular area
-and sends waypoints to move_base for execution with obstacle avoidance.
-Pauses at each waypoint for thermal camera capture.
+Costmap-aware coverage planner for Pyroscope.
+
+This node does not precommit to a blind lawnmower list. It samples the live
+global costmap inside the requested square, keeps only safe free-space targets,
+and then repeatedly chooses the next uncovered target that has the shortest
+reachable plan from the robot's current pose.
 """
 
-import rospy
 import math
+
 import actionlib
+import rospy
 import tf
+from geometry_msgs.msg import PoseStamped
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
-from geometry_msgs.msg import Twist
 from nav_msgs.msg import OccupancyGrid
-from sensor_msgs.msg import LaserScan
-from std_msgs.msg import Bool, String
+from nav_msgs.srv import GetPlan
+from std_msgs.msg import Bool, Int32, String
+
+
+WALL_MARGIN = 0.30  # meters
 
 
-# Margin inset from area boundary so waypoints never land on walls.
-# Must be >= robot half-width (0.125m) + inflation_radius (0.15m).
-WALL_MARGIN = 0.20  # meters
+class CoverageTarget(object):
+    def __init__(self, target_id, x, y, row, col):
+        self.target_id = target_id
+        self.x = x
+        self.y = y
+        self.row = row
+        self.col = col
+        self.covered = False
+        self.skipped = False
+        self.failures = 0
+        self.currently_safe = True
 
 
-class CoveragePlanner:
+class CoveragePlanner(object):
     def __init__(self):
         rospy.init_node('coverage_planner', anonymous=False)
+        self.ready = False
 
-        # Area parameters
+        # Mission geometry
         self.area_width = rospy.get_param('~area_width', 10.0)
         self.area_height = rospy.get_param('~area_height', 10.0)
         self.row_spacing = rospy.get_param('~row_spacing', 1.0)
         self.waypoint_spacing = rospy.get_param('~waypoint_spacing', 1.0)
         self.origin_x = rospy.get_param('~origin_x', 0.0)
         self.origin_y = rospy.get_param('~origin_y', 0.0)
+        self.wall_margin = rospy.get_param('~wall_margin', WALL_MARGIN)
 
-        # Timing parameters
+        # Timing and planning
         self.dwell_time = rospy.get_param('~dwell_time', 3.0)
         self.waypoint_timeout = rospy.get_param('~waypoint_timeout', 60.0)
-        self.max_waypoint_failures = int(rospy.get_param('~max_waypoint_failures', 3))
-        self.pose_stale_timeout = rospy.get_param('~pose_stale_timeout', 0.75)
-        self.scan_stale_timeout = rospy.get_param('~scan_stale_timeout', 1.0)
-        self.costmap_stale_timeout = rospy.get_param('~costmap_stale_timeout', 2.0)
-
-        # Recovery parameters
-        self.recovery_turn_speed = rospy.get_param('~recovery_turn_speed', 0.7)
-        self.recovery_turn_angle = rospy.get_param('~recovery_turn_angle', 1.0)
-        self.recovery_backup_speed = rospy.get_param('~recovery_backup_speed', -0.12)
-        self.recovery_backup_distance = rospy.get_param('~recovery_backup_distance', 0.35)
-        self.recovery_clearance = rospy.get_param('~recovery_clearance', 0.30)
-        self.waypoint_validation_radius = rospy.get_param('~waypoint_validation_radius', 0.18)
-        self.waypoint_cost_threshold = int(rospy.get_param('~waypoint_cost_threshold', 80))
-        self.scan_blocked_margin = rospy.get_param('~scan_blocked_margin', 0.20)
-        self.scan_blocked_sector = rospy.get_param('~scan_blocked_sector', 0.20)
+        self.pose_stale_timeout = rospy.get_param('~pose_stale_timeout', 2.0)
+        self.costmap_stale_timeout = rospy.get_param('~costmap_stale_timeout', 5.0)
+        self.make_plan_tolerance = rospy.get_param('~make_plan_tolerance', 0.25)
+        self.progress_log_interval = rospy.get_param('~progress_log_interval', 2.0)
+        self.progress_epsilon = rospy.get_param('~progress_epsilon', 0.05)
+        self.stall_timeout = rospy.get_param('~stall_timeout', 30.0)
+        self.no_target_retry_limit = int(rospy.get_param('~no_target_retry_limit', 15))
+        self.no_target_retry_sleep = rospy.get_param('~no_target_retry_sleep', 3.0)
+
+        # Coverage target safety
+        self.target_cost_threshold = int(rospy.get_param('~target_cost_threshold', 85))
+        self.target_check_radius = rospy.get_param('~target_check_radius', 0.10)
+        self.max_target_failures = int(rospy.get_param('~max_target_failures', 8))
+        self.failure_penalty = rospy.get_param('~failure_penalty', 0.75)
+        self.row_change_penalty = rospy.get_param('~row_change_penalty', 0.10)
 
         # State
-        self.waypoints = []
-        self.current_index = 0
-        self.latest_scan = None
-        self.latest_scan_stamp = None
+        self.targets = []
+        self.target_lookup = {}
+        self.next_target_id = 0
+        self.sweep_target_ids = []
+        self.sweep_cursor = 0
+        self.sequence_initialized = False
         self.latest_costmap = None
         self.latest_costmap_stamp = None
+        self.last_selected_row = None
 
         # Publishers
-        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
         self.capture_ready_pub = rospy.Publisher('/coverage/capture_ready', Bool, queue_size=1)
-        self.progress_pub = rospy.Publisher('/coverage/progress', String, queue_size=1)
-        self.complete_pub = rospy.Publisher('/coverage/complete', Bool, queue_size=1)
-        self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback, queue_size=1)
-        self.costmap_sub = rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, self.costmap_callback, queue_size=1)
-        self.tf_listener = tf.TransformListener()
+        self.progress_pub = rospy.Publisher('/coverage/progress', String, queue_size=1, latch=True)
+        self.total_points_pub = rospy.Publisher('/coverage/total_points', Int32, queue_size=1, latch=True)
+        self.complete_pub = rospy.Publisher('/coverage/complete', Bool, queue_size=1, latch=True)
+
+        # Subscribers
+        self.costmap_sub = rospy.Subscriber(
+            '/move_base/global_costmap/costmap',
+            OccupancyGrid,
+            self.costmap_callback,
+            queue_size=1,
+        )
 
-        # move_base action client
+        # move_base interfaces
+        self.tf_listener = tf.TransformListener()
         self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
+        self.make_plan_srv = None
+        self.clear_costmaps_srv = None
+
+        self.complete_pub.publish(Bool(data=False))
+        self.total_points_pub.publish(Int32(data=0))
+        self.progress_pub.publish(String(data='Waiting for costmap...'))
+
         rospy.loginfo("Waiting for move_base action server...")
-        if not self.move_base_client.wait_for_server(rospy.Duration(30.0)):
-            rospy.logfatal("move_base action server not available -- aborting")
+        if not self.move_base_client.wait_for_server(rospy.Duration(60.0)):
+            rospy.logfatal("move_base action server not available -- inspect move_base startup logs")
             rospy.signal_shutdown("move_base unavailable")
             return
 
-        rospy.loginfo("Connected to move_base action server")
+        try:
+            rospy.wait_for_service('/move_base/make_plan', timeout=5.0)
+            self.make_plan_srv = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
+            rospy.loginfo("Connected to /move_base/make_plan")
+        except rospy.ROSException:
+            rospy.logwarn("/move_base/make_plan not available; planner will fall back to Euclidean ordering")
 
-        # Generate waypoints
-        self.generate_waypoints()
-        self.validate_waypoint_distances()
+        try:
+            from std_srvs.srv import Empty
+            rospy.wait_for_service('/move_base/clear_costmaps', timeout=2.0)
+            self.clear_costmaps_srv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
+        except Exception:
+            self.clear_costmaps_srv = None
 
-        rospy.loginfo("Coverage Planner initialized")
-        rospy.loginfo("  Area: %.1f x %.1f m (margin: %.2fm from walls)",
-                      self.area_width, self.area_height, WALL_MARGIN)
-        rospy.loginfo("  Row spacing: %.1f m, Waypoint spacing: %.1f m",
+        rospy.loginfo("Coverage planner configured")
+        rospy.loginfo("  Area: %.2f x %.2f m centered at (%.2f, %.2f)",
+                      self.area_width, self.area_height, self.origin_x, self.origin_y)
+        rospy.loginfo("  Sampling: row_spacing=%.2f m waypoint_spacing=%.2f m",
                       self.row_spacing, self.waypoint_spacing)
-        rospy.loginfo("  Origin: (%.1f, %.1f)", self.origin_x, self.origin_y)
-        rospy.loginfo("  Total waypoints: %d", len(self.waypoints))
-        rospy.loginfo("  Dwell time: %.1f s", self.dwell_time)
-        rospy.loginfo("  Max retries per waypoint: %d", self.max_waypoint_failures)
-
-    def generate_waypoints(self):
-        """Generate boustrophedon (lawnmower) waypoints with wall margin inset."""
-        self.waypoints = []
-
-        # Center the grid on the origin, then inset by wall margin
-        min_x = self.origin_x - self.area_width / 2.0 + WALL_MARGIN
-        max_x = self.origin_x + self.area_width / 2.0 - WALL_MARGIN
-        min_y = self.origin_y - self.area_height / 2.0 + WALL_MARGIN
-        max_y = self.origin_y + self.area_height / 2.0 - WALL_MARGIN
-
-        # If the area is too small for any margin, just use the center
-        if max_x <= min_x or max_y <= min_y:
-            cx = self.origin_x + self.area_width / 2.0
-            cy = self.origin_y + self.area_height / 2.0
-            self.waypoints.append((cx, cy))
-            rospy.logwarn("Area too small for margin -- using center point only (%.2f, %.2f)", cx, cy)
-            return
-
-        effective_width = max_x - min_x
-        effective_height = max_y - min_y
-
-        num_rows = max(1, int(math.ceil(effective_height / self.row_spacing)) + 1)
-        num_cols = max(1, int(math.ceil(effective_width / self.waypoint_spacing)) + 1)
-
-        for row in range(num_rows):
-            y = min_y + row * self.row_spacing
-            if y > max_y:
-                y = max_y
-
-            # Alternate direction for lawnmower pattern
-            if row % 2 == 0:
-                col_range = range(num_cols)
-            else:
-                col_range = range(num_cols - 1, -1, -1)
-
-            for col in col_range:
-                x = min_x + col * self.waypoint_spacing
-                if x > max_x:
-                    x = max_x
-
-                self.waypoints.append((x, y))
-
-        rospy.loginfo("Generated %d waypoints in %d rows (inset %.2fm from walls)",
-                      len(self.waypoints), num_rows, WALL_MARGIN)
-
-    def validate_waypoint_distances(self):
-        """Warn if consecutive waypoints exceed global costmap range"""
-        costmap_half = 7.5  # half of 15m rolling window
-        for i in range(1, len(self.waypoints)):
-            x0, y0 = self.waypoints[i - 1]
-            x1, y1 = self.waypoints[i]
-            dist = math.sqrt((x1 - x0) ** 2 + (y1 - y0) ** 2)
-            if dist > costmap_half:
-                rospy.logwarn(
-                    "Waypoints %d->%d are %.1fm apart (exceeds %.1fm costmap radius) "
-                    "-- move_base may fail to plan",
-                    i, i + 1, dist, costmap_half
-                )
-
-    def scan_callback(self, msg):
-        self.latest_scan = msg
-        self.latest_scan_stamp = msg.header.stamp if msg.header.stamp != rospy.Time() else rospy.Time.now()
+        rospy.loginfo("  Safety: wall_margin=%.2f m target_cost_threshold=%d target_check_radius=%.2f m",
+                      self.wall_margin, self.target_cost_threshold, self.target_check_radius)
+        self.ready = True
 
     def costmap_callback(self, msg):
         self.latest_costmap = msg
-        self.latest_costmap_stamp = msg.header.stamp if msg.header.stamp != rospy.Time() else rospy.Time.now()
-
-    def stop_robot(self):
-        self.cmd_vel_pub.publish(Twist())
+        if msg.header.stamp != rospy.Time():
+            self.latest_costmap_stamp = msg.header.stamp
+        else:
+            self.latest_costmap_stamp = rospy.Time.now()
 
     def get_robot_pose(self):
         try:
@@ -185,60 +161,54 @@ def wait_for_fresh_pose(self, timeout):
             rate.sleep()
         return False
 
-    def get_scan_age(self):
-        if self.latest_scan_stamp is None:
-            return None
-        return abs((rospy.Time.now() - self.latest_scan_stamp).to_sec())
-
     def get_costmap_age(self):
         if self.latest_costmap_stamp is None:
             return None
         return abs((rospy.Time.now() - self.latest_costmap_stamp).to_sec())
 
-    def normalize_angle(self, angle):
-        return math.atan2(math.sin(angle), math.cos(angle))
-
-    def angle_in_sector(self, angle, start, end):
-        angle = self.normalize_angle(angle)
-        start = self.normalize_angle(start)
-        end = self.normalize_angle(end)
-        if start <= end:
-            return start <= angle <= end
-        return angle >= start or angle <= end
-
-    def sector_clearance(self, start_angle, end_angle):
-        if self.latest_scan is None:
-            return None
+    def costmap_is_fresh(self):
+        costmap_age = self.get_costmap_age()
+        return costmap_age is not None and costmap_age <= self.costmap_stale_timeout
 
-        values = []
-        angle = self.latest_scan.angle_min
-        for distance in self.latest_scan.ranges:
-            if self.angle_in_sector(angle, start_angle, end_angle):
-                if math.isinf(distance):
-                    values.append(self.latest_scan.range_max)
-                elif not math.isnan(distance):
-                    values.append(distance)
-            angle += self.latest_scan.angle_increment
-
-        return min(values) if values else None
-
-    def choose_turn_direction(self):
-        left_clearance = self.sector_clearance(math.radians(20), math.radians(110))
-        right_clearance = self.sector_clearance(math.radians(-110), math.radians(-20))
-
-        if left_clearance is None and right_clearance is None:
-            return 1.0
-        if right_clearance is None:
-            return 1.0
-        if left_clearance is None:
-            return -1.0
-        return 1.0 if left_clearance >= right_clearance else -1.0
+    def wait_for_costmap(self, timeout):
+        deadline = rospy.Time.now() + rospy.Duration(timeout)
+        rate = rospy.Rate(10)
+        while rospy.Time.now() < deadline and not rospy.is_shutdown():
+            if self.latest_costmap is not None and self.costmap_is_fresh():
+                return True
+            rate.sleep()
+        return False
 
-    def costmap_cell(self, mx, my):
-        info = self.latest_costmap.info
-        if mx < 0 or my < 0 or mx >= info.width or my >= info.height:
-            return None
-        return self.latest_costmap.data[my * info.width + mx]
+    def build_pose(self, x, y, yaw):
+        pose = PoseStamped()
+        pose.header.frame_id = 'odom'
+        pose.header.stamp = rospy.Time.now()
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.position.z = 0.0
+        quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
+        pose.pose.orientation.x = quat[0]
+        pose.pose.orientation.y = quat[1]
+        pose.pose.orientation.z = quat[2]
+        pose.pose.orientation.w = quat[3]
+        return pose
+
+    def axis_points(self, min_value, max_value, spacing):
+        points = []
+        value = min_value
+        while value <= max_value + 1e-6:
+            points.append(min(value, max_value))
+            value += spacing
+        if not points:
+            points = [min_value]
+        elif abs(points[-1] - max_value) > 1e-6:
+            points.append(max_value)
+
+        deduped = []
+        for point in points:
+            if not deduped or abs(point - deduped[-1]) > 1e-6:
+                deduped.append(point)
+        return deduped
 
     def world_to_costmap(self, x, y):
         if self.latest_costmap is None:
@@ -247,267 +217,533 @@ def world_to_costmap(self, x, y):
         info = self.latest_costmap.info
         mx = int(math.floor((x - info.origin.position.x) / info.resolution))
         my = int(math.floor((y - info.origin.position.y) / info.resolution))
+
         if mx < 0 or my < 0 or mx >= info.width or my >= info.height:
             return None
-        return mx, my
+        return (mx, my)
 
-    def waypoint_cost_invalid(self, x, y):
-        costmap_age = self.get_costmap_age()
-        if self.latest_costmap is None or costmap_age is None or costmap_age > self.costmap_stale_timeout:
+    def costmap_cell(self, mx, my):
+        info = self.latest_costmap.info
+        if mx < 0 or my < 0 or mx >= info.width or my >= info.height:
+            return None
+        return self.latest_costmap.data[my * info.width + mx]
+
+    def is_target_safe(self, x, y):
+        if self.latest_costmap is None:
             return False
 
         center = self.world_to_costmap(x, y)
         if center is None:
             return False
 
+        info = self.latest_costmap.info
+        radius_cells = int(math.ceil(self.target_check_radius / info.resolution))
         mx, my = center
-        radius_cells = int(math.ceil(self.waypoint_validation_radius / self.latest_costmap.info.resolution))
+        has_known_cells = False
 
         for cx in range(mx - radius_cells, mx + radius_cells + 1):
             for cy in range(my - radius_cells, my + radius_cells + 1):
                 cost = self.costmap_cell(cx, cy)
                 if cost is None:
+                    return False
+                if cost < 0:
                     continue
-                if cost < 0 or cost >= self.waypoint_cost_threshold:
-                    rospy.logwarn(
-                        "Skipping waypoint (%.2f, %.2f): costmap cell %d near target exceeds threshold %d",
-                        x, y, cost, self.waypoint_cost_threshold
-                    )
-                    return True
+                has_known_cells = True
+                if cost >= self.target_cost_threshold:
+                    return False
+        return has_known_cells
 
-        return False
+    def refresh_targets_from_costmap(self):
+        previous_known = len(self.targets)
+        previous_total = self.count_total_targets()
+        previous_pending = self.count_pending_targets()
+
+        min_x = self.origin_x - self.area_width / 2.0 + self.wall_margin
+        max_x = self.origin_x + self.area_width / 2.0 - self.wall_margin
+        min_y = self.origin_y - self.area_height / 2.0 + self.wall_margin
+        max_y = self.origin_y + self.area_height / 2.0 - self.wall_margin
+
+        if max_x <= min_x or max_y <= min_y:
+            rospy.logwarn_throttle(5.0, "Area too small after wall margin; using center point fallback")
+            key = (0, 0)
+            target = self.target_lookup.get(key)
+            if target is None and self.is_target_safe(self.origin_x, self.origin_y):
+                target = CoverageTarget(self.next_target_id, self.origin_x, self.origin_y, 0, 0)
+                self.next_target_id += 1
+                self.target_lookup[key] = target
+            if target is not None:
+                target.currently_safe = self.is_target_safe(self.origin_x, self.origin_y)
+                target.x = self.origin_x
+                target.y = self.origin_y
+            self.targets = sorted(self.target_lookup.values(), key=lambda existing: (existing.row, existing.col))
+            return
+
+        x_points = self.axis_points(min_x, max_x, self.waypoint_spacing)
+        y_points = self.axis_points(min_y, max_y, self.row_spacing)
+
+        for row_index, y in enumerate(y_points):
+            for col_index, x in enumerate(x_points):
+                key = (row_index, col_index)
+                currently_safe = self.is_target_safe(x, y)
+                target = self.target_lookup.get(key)
+
+                if target is None:
+                    target = CoverageTarget(self.next_target_id, x, y, row_index, col_index)
+                    self.next_target_id += 1
+                    self.target_lookup[key] = target
+
+                target.x = x
+                target.y = y
+                target.currently_safe = currently_safe
+
+        self.targets = sorted(self.target_lookup.values(), key=lambda target: (target.row, target.col))
+        if len(self.targets) != previous_known:
+            self.sequence_initialized = False
+
+        current_total = self.count_total_targets()
+        current_pending = self.count_pending_targets()
+        if current_total != previous_total or current_pending != previous_pending:
+            rospy.loginfo(
+                "Coverage target set refreshed: total=%d pending=%d covered=%d skipped=%d",
+                current_total,
+                current_pending,
+                self.count_covered_targets(),
+                self.count_skipped_targets(),
+            )
+
+    def count_known_targets(self):
+        return len(self.targets)
+
+    def count_total_targets(self):
+        return len([target for target in self.targets if not target.skipped])
+
+    def count_covered_targets(self):
+        return len([target for target in self.targets if target.covered])
+
+    def count_pending_targets(self):
+        return len([target for target in self.targets if not target.covered and not target.skipped])
+
+    def count_active_targets(self):
+        return len([
+            target for target in self.targets
+            if not target.covered and not target.skipped and target.currently_safe
+        ])
+
+    def count_skipped_targets(self):
+        return len([target for target in self.targets if target.skipped])
+
+    def build_sweep_target_ids(self):
+        sequence = []
+        rows = sorted({target.row for target in self.targets})
+        for row in rows:
+            row_targets = [target for target in self.targets if target.row == row]
+            row_targets.sort(key=lambda target: target.col, reverse=bool(row % 2))
+            sequence.extend([target.target_id for target in row_targets])
+        self.sweep_target_ids = sequence
+
+    def initialize_sweep_sequence(self):
+        self.build_sweep_target_ids()
+        self.sequence_initialized = True
+        self.sweep_cursor = 0
+
+        if not self.sweep_target_ids:
+            return
 
-    def waypoint_scan_blocked(self, x, y):
         pose = self.get_robot_pose()
-        scan_age = self.get_scan_age()
-        if pose is None or scan_age is None or scan_age > self.scan_stale_timeout:
-            return False
+        if pose is None:
+            return
 
-        dx = x - pose[0]
-        dy = y - pose[1]
-        target_distance = math.sqrt(dx ** 2 + dy ** 2)
-        if target_distance <= 0.05:
-            return False
+        best_index = 0
+        best_distance = None
+        for index, target_id in enumerate(self.sweep_target_ids):
+            target = next((candidate for candidate in self.targets if candidate.target_id == target_id), None)
+            if target is None or target.covered or target.skipped:
+                continue
+            distance = math.sqrt((target.x - pose[0]) ** 2 + (target.y - pose[1]) ** 2)
+            if best_distance is None or distance < best_distance:
+                best_distance = distance
+                best_index = index
+        self.sweep_cursor = best_index
+
+    def ordered_pending_targets(self):
+        if not self.sequence_initialized:
+            self.initialize_sweep_sequence()
+
+        if not self.sweep_target_ids:
+            return []
+
+        target_by_id = {target.target_id: target for target in self.targets}
+        ordered = []
+        total = len(self.sweep_target_ids)
+        for offset in range(total):
+            index = (self.sweep_cursor + offset) % total
+            target = target_by_id.get(self.sweep_target_ids[index])
+            if target is None or target.covered or target.skipped:
+                continue
+            ordered.append(target)
+        return ordered
+
+    def advance_sweep_cursor(self, target):
+        if not self.sweep_target_ids:
+            return
+        try:
+            index = self.sweep_target_ids.index(target.target_id)
+        except ValueError:
+            return
+        self.sweep_cursor = (index + 1) % len(self.sweep_target_ids)
 
-        target_bearing = self.normalize_angle(math.atan2(dy, dx) - pose[2])
-        clearance = self.sector_clearance(
-            target_bearing - self.scan_blocked_sector,
-            target_bearing + self.scan_blocked_sector
+    def publish_total_points(self):
+        self.total_points_pub.publish(Int32(data=self.count_total_targets()))
+
+    def publish_progress(self):
+        covered = self.count_covered_targets()
+        total = self.count_total_targets()
+        skipped = self.count_skipped_targets()
+        active = self.count_active_targets()
+        if total > 0:
+            percent = int(round((100.0 * covered) / total))
+        else:
+            percent = 100
+        message = "%d/%d targets covered (%d%%, %d skipped, %d active)" % (
+            covered,
+            total,
+            percent,
+            skipped,
+            active,
         )
-        if clearance is None:
-            return False
+        self.progress_pub.publish(String(data=message))
+
+    def path_length(self, poses):
+        if not poses:
+            return None
+        total = 0.0
+        for i in range(1, len(poses)):
+            p0 = poses[i - 1].pose.position
+            p1 = poses[i].pose.position
+            total += math.sqrt((p1.x - p0.x) ** 2 + (p1.y - p0.y) ** 2)
+        return total
+
+    def plan_length_to_target(self, start_pose, target):
+        if self.make_plan_srv is None:
+            dx = target.x - start_pose.pose.position.x
+            dy = target.y - start_pose.pose.position.y
+            return math.sqrt(dx ** 2 + dy ** 2)
+
+        goal = self.build_pose(target.x, target.y, 0.0)
+        try:
+            response = self.make_plan_srv(start_pose, goal, self.make_plan_tolerance)
+        except rospy.ServiceException:
+            return None
+
+        if not response.plan.poses:
+            return None
+        return self.path_length(response.plan.poses)
+
+    def choose_next_target_once(self):
+        self.refresh_targets_from_costmap()
+        if not self.costmap_is_fresh():
+            rospy.logwarn("Global costmap is stale while selecting the next coverage target")
+            return None
+
+        pose = self.get_robot_pose()
+        if pose is None or pose[3] > self.pose_stale_timeout:
+            rospy.logwarn("Robot pose is stale while selecting the next coverage target")
+            return None
+
+        start_pose = self.build_pose(pose[0], pose[1], pose[2])
+        best_target = None
+        best_score = None
 
-        if clearance + self.scan_blocked_margin < target_distance:
+        pending = self.ordered_pending_targets()
+        fallback_target = None
+        planned_candidate_count = 0
+
+        for target in pending:
+            if not target.currently_safe:
+                continue
+
+            if fallback_target is None:
+                fallback_target = target
+
+            plan_length = self.plan_length_to_target(start_pose, target)
+            if plan_length is None:
+                continue
+            planned_candidate_count += 1
+
+            row_penalty = 0.0
+            if self.last_selected_row is not None and target.row != self.last_selected_row:
+                row_penalty = abs(target.row - self.last_selected_row) * self.row_change_penalty
+
+            score = plan_length + (target.failures * self.failure_penalty) + row_penalty
+            if best_score is None or score < best_score:
+                best_score = score
+                best_target = target
+
+        if best_target is not None:
+            return best_target
+
+        if fallback_target is not None:
             rospy.logwarn(
-                "Skipping waypoint (%.2f, %.2f): scan shows obstacle at %.2fm before target distance %.2fm",
-                x, y, clearance, target_distance
+                "make_plan found no path for ordered safe targets; falling back to sweep target %d at (%.2f, %.2f)",
+                fallback_target.target_id,
+                fallback_target.x,
+                fallback_target.y,
             )
-            return True
+            return fallback_target
 
-        return False
+        if self.count_pending_targets() > 0:
+            rospy.logwarn(
+                "No active safe targets remain in the live costmap (pending=%d active=%d planned=%d)",
+                self.count_pending_targets(),
+                self.count_active_targets(),
+                planned_candidate_count,
+            )
 
-    def waypoint_is_valid(self, x, y):
-        if self.waypoint_cost_invalid(x, y):
-            return False
-        if self.waypoint_scan_blocked(x, y):
-            return False
-        return True
+        return best_target
 
-    def rotate_for_recovery(self, turn_direction):
-        cmd = Twist()
-        cmd.angular.z = turn_direction * abs(self.recovery_turn_speed)
+    def choose_next_target(self):
+        target = self.choose_next_target_once()
+        if target is not None:
+            return target
 
+        if self.clear_costmaps_srv is not None:
+            try:
+                rospy.logwarn("No reachable target found; clearing costmaps and retrying once")
+                self.clear_costmaps_srv()
+                rospy.sleep(1.0)
+            except Exception:
+                pass
+            return self.choose_next_target_once()
+
+        return None
+
+    def send_move_base_goal(self, target):
         pose = self.get_robot_pose()
-        if pose is not None and pose[3] <= self.pose_stale_timeout:
-            start_yaw = pose[2]
-            timeout = rospy.Time.now() + rospy.Duration(4.0)
-            rate = rospy.Rate(15)
-            while not rospy.is_shutdown() and rospy.Time.now() < timeout:
-                pose = self.get_robot_pose()
-                if pose is None:
-                    break
-                yaw_delta = abs(self.normalize_angle(pose[2] - start_yaw))
-                if yaw_delta >= self.recovery_turn_angle:
-                    break
-                self.cmd_vel_pub.publish(cmd)
-                rate.sleep()
-        else:
-            duration = self.recovery_turn_angle / max(abs(self.recovery_turn_speed), 0.1)
-            end_time = rospy.Time.now() + rospy.Duration(duration)
-            rate = rospy.Rate(15)
-            while not rospy.is_shutdown() and rospy.Time.now() < end_time:
-                self.cmd_vel_pub.publish(cmd)
-                rate.sleep()
-
-        self.stop_robot()
-
-    def backup_for_recovery(self):
-        cmd = Twist()
-        cmd.linear.x = min(self.recovery_backup_speed, -0.05)
-        pose = self.get_robot_pose()
-        target_distance = abs(self.recovery_backup_distance)
-
-        if pose is not None and pose[3] <= self.pose_stale_timeout:
-            start_x = pose[0]
-            start_y = pose[1]
-            timeout = rospy.Time.now() + rospy.Duration(6.0)
-            rate = rospy.Rate(15)
-            while not rospy.is_shutdown() and rospy.Time.now() < timeout:
-                scan_age = self.get_scan_age()
-                rear_clearance = self.sector_clearance(math.radians(140), math.radians(-140))
-                if scan_age is not None and scan_age <= self.scan_stale_timeout and rear_clearance is not None and rear_clearance < self.recovery_clearance:
-                    rospy.logwarn("Stopping recovery backup early -- rear clearance only %.2fm", rear_clearance)
-                    break
-
-                pose = self.get_robot_pose()
-                if pose is None:
-                    break
-                distance = math.sqrt((pose[0] - start_x) ** 2 + (pose[1] - start_y) ** 2)
-                if distance >= target_distance:
-                    break
-                self.cmd_vel_pub.publish(cmd)
-                rate.sleep()
-        else:
-            duration = target_distance / max(abs(cmd.linear.x), 0.05)
-            end_time = rospy.Time.now() + rospy.Duration(duration)
-            rate = rospy.Rate(15)
-            while not rospy.is_shutdown() and rospy.Time.now() < end_time:
-                scan_age = self.get_scan_age()
-                rear_clearance = self.sector_clearance(math.radians(140), math.radians(-140))
-                if scan_age is not None and scan_age <= self.scan_stale_timeout and rear_clearance is not None and rear_clearance < self.recovery_clearance:
-                    rospy.logwarn("Stopping timed recovery backup early -- rear clearance only %.2fm", rear_clearance)
-                    break
-                self.cmd_vel_pub.publish(cmd)
-                rate.sleep()
-
-        self.stop_robot()
-
-    def perform_escape_recovery(self):
-        rospy.logwarn("Running escape recovery: cancel goal, clear costmaps, rotate toward open side, then back up")
-        self.move_base_client.cancel_goal()
-        self.stop_robot()
-        self.clear_costmaps()
-        rospy.sleep(0.5)
-
-        if self.get_scan_age() is None or self.get_scan_age() > self.scan_stale_timeout:
-            rospy.logwarn("Laser scan is stale during recovery; using timed turn")
-
-        turn_direction = self.choose_turn_direction()
-        self.rotate_for_recovery(turn_direction)
-        self.backup_for_recovery()
-        self.clear_costmaps()
-        rospy.sleep(1.0)
+        if pose is None or pose[3] > self.pose_stale_timeout:
+            rospy.logwarn("Robot pose is stale before goal dispatch")
+            return False
 
-    def send_move_base_goal(self, x, y):
-        """Send a goal to move_base and wait for result. Returns True on success."""
-        if not self.wait_for_fresh_pose(2.0):
-            rospy.logwarn("Robot pose is stale before goal dispatch; refusing to send goal until TF recovers")
+        if not self.costmap_is_fresh():
+            rospy.logwarn("Global costmap is stale before goal dispatch")
             return False
 
+        target.currently_safe = self.is_target_safe(target.x, target.y)
+        if not target.currently_safe:
+            rospy.logwarn("Target %d at (%.2f, %.2f) is no longer safe before dispatch",
+                          target.target_id, target.x, target.y)
+            return False
+
+        goal_yaw = pose[2]
+
         goal = MoveBaseGoal()
-        goal.target_pose.header.frame_id = "odom"
+        goal.target_pose.header.frame_id = 'odom'
         goal.target_pose.header.stamp = rospy.Time.now()
-        goal.target_pose.pose.position.x = x
-        goal.target_pose.pose.position.y = y
+        goal.target_pose.pose.position.x = target.x
+        goal.target_pose.pose.position.y = target.y
         goal.target_pose.pose.position.z = 0.0
-        goal.target_pose.pose.orientation.w = 1.0
+
+        quat = tf.transformations.quaternion_from_euler(0.0, 0.0, goal_yaw)
+        goal.target_pose.pose.orientation.x = quat[0]
+        goal.target_pose.pose.orientation.y = quat[1]
+        goal.target_pose.pose.orientation.z = quat[2]
+        goal.target_pose.pose.orientation.w = quat[3]
+
+        start_distance = math.sqrt((target.x - pose[0]) ** 2 + (target.y - pose[1]) ** 2)
+        best_distance = start_distance
+        start_time = rospy.Time.now()
+        last_progress_time = start_time
+        last_log_time = start_time
 
         self.move_base_client.send_goal(goal)
-        finished = self.move_base_client.wait_for_result(rospy.Duration(self.waypoint_timeout))
+        rospy.loginfo(
+            "Tracking target %d at (%.2f, %.2f): start_distance=%.2f m timeout=%.1f s stall_timeout=%.1f s",
+            target.target_id,
+            target.x,
+            target.y,
+            start_distance,
+            self.waypoint_timeout,
+            self.stall_timeout,
+        )
 
-        if not finished:
-            self.move_base_client.cancel_goal()
-            rospy.logwarn("move_base timed out reaching (%.2f, %.2f)", x, y)
-            return False
+        rate = rospy.Rate(4)
+        while not rospy.is_shutdown():
+            if self.move_base_client.wait_for_result(rospy.Duration(0.0)):
+                break
+
+            now = rospy.Time.now()
+            pose = self.get_robot_pose()
+            if pose is not None and pose[3] <= self.pose_stale_timeout:
+                current_distance = math.sqrt((target.x - pose[0]) ** 2 + (target.y - pose[1]) ** 2)
+                if current_distance < best_distance - self.progress_epsilon:
+                    improvement = best_distance - current_distance
+                    best_distance = current_distance
+                    last_progress_time = now
+                    rospy.loginfo(
+                        "Target %d progress: distance %.2f m (improved %.2f m, elapsed %.1f s)",
+                        target.target_id,
+                        current_distance,
+                        improvement,
+                        (now - start_time).to_sec(),
+                    )
+                elif (now - last_log_time).to_sec() >= self.progress_log_interval:
+                    rospy.loginfo(
+                        "Target %d status: distance %.2f m best %.2f m elapsed %.1f s stalled %.1f s",
+                        target.target_id,
+                        current_distance,
+                        best_distance,
+                        (now - start_time).to_sec(),
+                        (now - last_progress_time).to_sec(),
+                    )
+                    last_log_time = now
+
+            elapsed = (now - start_time).to_sec()
+            stalled = (now - last_progress_time).to_sec()
+            if elapsed >= self.waypoint_timeout:
+                self.move_base_client.cancel_goal()
+                rospy.logwarn(
+                    "move_base timed out reaching target %d at (%.2f, %.2f): best_distance=%.2f m elapsed=%.1f s",
+                    target.target_id,
+                    target.x,
+                    target.y,
+                    best_distance,
+                    elapsed,
+                )
+                return False
+
+            if stalled >= self.stall_timeout:
+                self.move_base_client.cancel_goal()
+                rospy.logwarn(
+                    "move_base stalled on target %d at (%.2f, %.2f): best_distance=%.2f m stalled=%.1f s",
+                    target.target_id,
+                    target.x,
+                    target.y,
+                    best_distance,
+                    stalled,
+                )
+                return False
+
+            rate.sleep()
 
         state = self.move_base_client.get_state()
         if state == actionlib.GoalStatus.SUCCEEDED:
+            rospy.loginfo(
+                "Reached target %d at (%.2f, %.2f): start_distance=%.2f m best_distance=%.2f m",
+                target.target_id,
+                target.x,
+                target.y,
+                start_distance,
+                best_distance,
+            )
             return True
-        else:
-            rospy.logwarn("move_base failed for (%.2f, %.2f) -- state %d", x, y, state)
-            return False
 
-    def clear_costmaps(self):
-        """Ask move_base to clear costmaps -- helps recover from stuck states."""
-        try:
-            from std_srvs.srv import Empty
-            rospy.wait_for_service('/move_base/clear_costmaps', timeout=2.0)
-            clear = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
-            clear()
-            rospy.loginfo("Costmaps cleared")
-        except Exception:
-            rospy.logwarn("Could not clear costmaps")
+        rospy.logwarn("move_base failed for target %d at (%.2f, %.2f) -- state %d",
+                      target.target_id, target.x, target.y, state)
+        return False
 
-    def publish_progress(self):
-        """Publish current progress"""
-        total = len(self.waypoints)
-        msg = "{}/{} waypoints ({}%)".format(
-            self.current_index, total,
-            int(100.0 * self.current_index / total) if total > 0 else 0
-        )
-        self.progress_pub.publish(String(data=msg))
+    def capture_target(self):
+        rospy.loginfo("Dwelling for %.1f s and triggering capture", self.dwell_time)
+        self.capture_ready_pub.publish(Bool(data=True))
+        rospy.sleep(self.dwell_time)
+        self.capture_ready_pub.publish(Bool(data=False))
+
+    def complete_mission(self):
+        covered = self.count_covered_targets()
+        skipped = self.count_skipped_targets()
+        total = self.count_total_targets()
+        rospy.loginfo("Coverage mission complete: covered=%d total=%d skipped=%d", covered, total, skipped)
+        self.publish_total_points()
+        self.publish_progress()
+        self.complete_pub.publish(Bool(data=True))
+        rospy.sleep(1.0)
 
     def run(self):
-        # Wait for subscribers to connect and costmap to populate
-        rospy.loginfo("Waiting 5s for costmap to populate from lidar...")
-        rospy.sleep(5.0)
-
-        rospy.loginfo("Starting coverage mission!")
-
-        while self.current_index < len(self.waypoints) and not rospy.is_shutdown():
-            x, y = self.waypoints[self.current_index]
-            waypoint_failures = 0
-
-            if not self.waypoint_is_valid(x, y):
-                rospy.logwarn("Skipping waypoint %d/%d because the target is invalid in the live environment",
-                              self.current_index + 1, len(self.waypoints))
-                self.current_index += 1
-                continue
+        if not self.ready or rospy.is_shutdown():
+            return
 
-            while waypoint_failures < self.max_waypoint_failures and not rospy.is_shutdown():
-                rospy.loginfo("Waypoint %d/%d attempt %d/%d: (%.2f, %.2f)",
-                              self.current_index + 1, len(self.waypoints),
-                              waypoint_failures + 1, self.max_waypoint_failures, x, y)
-                self.publish_progress()
+        rospy.loginfo("Waiting for a fresh robot pose...")
+        if not self.wait_for_fresh_pose(10.0):
+            rospy.logfatal("No fresh robot pose available; aborting coverage mission")
+            self.complete_pub.publish(Bool(data=True))
+            return
 
-                if not self.waypoint_is_valid(x, y):
-                    waypoint_failures = self.max_waypoint_failures
-                    break
+        rospy.loginfo("Waiting for the global costmap to populate...")
+        if not self.wait_for_costmap(10.0):
+            rospy.logfatal("No fresh global costmap available; aborting coverage mission")
+            self.complete_pub.publish(Bool(data=True))
+            return
 
-                success = self.send_move_base_goal(x, y)
-                if success:
-                    rospy.loginfo("Reached waypoint %d/%d",
-                                  self.current_index + 1, len(self.waypoints))
+        rospy.sleep(1.0)
+        self.refresh_targets_from_costmap()
+        self.publish_total_points()
+        self.publish_progress()
 
-                    # Only capture data at successfully reached waypoints
-                    rospy.loginfo("Dwelling for %.1f s (thermal capture)", self.dwell_time)
-                    self.capture_ready_pub.publish(Bool(data=True))
-                    rospy.sleep(self.dwell_time)
-                    self.capture_ready_pub.publish(Bool(data=False))
-                    break
+        if self.count_known_targets() == 0:
+            rospy.logwarn("No safe coverage targets found inside the requested square")
+            self.complete_mission()
+            return
 
-                waypoint_failures += 1
-                rospy.logwarn("Failed waypoint %d/%d on attempt %d/%d",
-                              self.current_index + 1, len(self.waypoints),
-                              waypoint_failures, self.max_waypoint_failures)
+        no_target_retries = 0
 
-                if waypoint_failures < self.max_waypoint_failures:
-                    self.perform_escape_recovery()
+        while not rospy.is_shutdown():
+            self.refresh_targets_from_costmap()
+            self.publish_total_points()
+            self.publish_progress()
 
-            if waypoint_failures >= self.max_waypoint_failures:
-                rospy.logwarn("Skipping waypoint %d/%d after %d failed attempts",
-                              self.current_index + 1, len(self.waypoints),
-                              self.max_waypoint_failures)
+            if self.count_pending_targets() == 0:
+                break
 
-            self.current_index += 1
+            target = self.choose_next_target()
+            if target is None:
+                pending_count = self.count_pending_targets()
+                active_count = self.count_active_targets()
+                if pending_count > 0 and no_target_retries < self.no_target_retry_limit:
+                    no_target_retries += 1
+                    rospy.logwarn(
+                        "No selectable target right now (pending=%d active=%d). Waiting %.1fs for costmap/planner recovery before retry %d/%d",
+                        pending_count,
+                        active_count,
+                        self.no_target_retry_sleep,
+                        no_target_retries,
+                        self.no_target_retry_limit,
+                    )
+                    rospy.sleep(self.no_target_retry_sleep)
+                    continue
 
-        # Mission complete
-        rospy.loginfo("Coverage mission complete! %d/%d waypoints visited",
-                      self.current_index, len(self.waypoints))
-        self.complete_pub.publish(Bool(data=True))
-        self.publish_progress()
+                rospy.logwarn(
+                    "No remaining reachable coverage targets; ending mission after %d retries (pending=%d active=%d)",
+                    no_target_retries,
+                    pending_count,
+                    active_count,
+                )
+                for pending in self.targets:
+                    if not pending.covered and not pending.skipped:
+                        pending.skipped = True
+                self.publish_total_points()
+                self.publish_progress()
+                break
+
+            no_target_retries = 0
+            rospy.loginfo("Selected target %d at (%.2f, %.2f) row=%d col=%d failures=%d",
+                          target.target_id, target.x, target.y,
+                          target.row, target.col, target.failures)
+            self.advance_sweep_cursor(target)
+
+            success = self.send_move_base_goal(target)
+            if success:
+                target.covered = True
+                self.last_selected_row = target.row
+                self.publish_progress()
+                self.capture_target()
+            else:
+                target.failures += 1
+                if target.failures >= self.max_target_failures:
+                    target.skipped = True
+                    self.publish_total_points()
+                    rospy.logwarn("Skipping target %d after %d failures",
+                                  target.target_id, self.max_target_failures)
+                self.publish_progress()
 
-        rospy.spin()
+        self.complete_mission()
 
 
 if __name__ == '__main__':