feat: implement mission executor with breadcrumb system and failsafe integration

- Full mission state machine: IDLE/RUNNING/PAUSED/COMPLETE/ABORTED
- Breadcrumb buffer: 1800 entries at 2s interval (1hr coverage)
- Failsafe integration: HOVER=pause, RETURN_TO_SAFE=abort+breadcrumb, EMERGENCY=immediate abort
- Waypoint loading from YAML mission file via LOAD command
- Arrival detection with configurable radius
- Image capture loiter with configurable delay
- Simple proportional navigation toward waypoints (Phase 3+ replaces with full planner)
- Entry point recorded at mission start for safe zone reference

src/rov_mission/rov_mission/mission_executor.py

@@ -1,118 +1,390 @@
 #!/usr/bin/env python3
 """
-Mission executor node.
+Mission Executor Node — Argonaut 3
-Manages inspection mission state machine.
+====================================
-Sequences waypoints, triggers image capture, handles replanning.
+Manages inspection mission state machine per ROV_Failsafe_Design_v2.0.
 
-States: IDLE → RUNNING → (PAUSED | COMPLETE | ABORTED)
+Sequences inspection waypoints, triggers image capture, maintains the
+breadcrumb position buffer used by the failsafe return-to-safe path,
+and responds correctly to each failsafe action level.
+
+States: IDLE -> RUNNING -> PAUSED | COMPLETE | ABORTED
+
+Failsafe integration:
+  ACTION_HOVER            -> pause mission, hold position
+  ACTION_RETURN_TO_SAFE   -> abort mission, return via breadcrumb path
+  ACTION_EMERGENCY_SURFACE -> abort mission, emergency surface immediately
+
+Breadcrumb system:
+  Position logged every 2s from mission start.
+  Circular buffer of 1800 entries (1 hour of operation).
+  Used by return-to-safe to reverse the entry path safely.
 
 Topics subscribed:
-  /rov/state         (nav_msgs/Odometry)              — current position
+  /rov/state              (nav_msgs/Odometry)              — current position
-  /rov/failsafe      (rov_interfaces/FailsafeStatus)  — safety override
+  /rov/failsafe           (rov_interfaces/FailsafeStatus)  — safety override
 
 Topics published:
-  /rov/cmd_vel       (geometry_msgs/Twist)            — velocity setpoints to controller
+  /rov/cmd_vel            (geometry_msgs/Twist)            — velocity setpoints
-  /rov/mission/status (rov_interfaces/MissionStatus)  — current mission state
+  /rov/mission/status     (rov_interfaces/MissionStatus)   — mission state
+  /rov/mission/waypoint   (rov_interfaces/InspectionWaypoint) — current target
 
 Services provided:
-  /rov/mission/command  (rov_interfaces/MissionCommand) — start/pause/abort/resume
+  /rov/mission/command    (rov_interfaces/MissionCommand)  — start/pause/abort/resume
 """
 
+import math
+import yaml
+import collections
+from dataclasses import dataclass, field
+from typing import List, Optional
+
 import rclpy
 from rclpy.node import Node
-from geometry_msgs.msg import Twist
+from geometry_msgs.msg import Twist, Point
 from nav_msgs.msg import Odometry
+from std_msgs.msg import Header
 from rov_interfaces.msg import MissionStatus, FailsafeStatus, InspectionWaypoint
 from rov_interfaces.srv import MissionCommand
 
 
-class MissionExecutor(Node):
+# ---------------------------------------------------------------------------
-    """Executes inspection missions from a waypoint list."""
+# Breadcrumb entry
+# ---------------------------------------------------------------------------
 
-    # Mission states
+@dataclass
-    STATE_IDLE = 'IDLE'
+class Breadcrumb:
-    STATE_RUNNING = 'RUNNING'
+    """Single position record for the return-to-safe path."""
-    STATE_PAUSED = 'PAUSED'
+    timestamp_s: float   # ROS time in seconds
+    x: float             # metres
+    y: float             # metres
+    z: float             # metres (negative = depth)
+    heading_deg: float   # degrees
+    confidence: float    # 0.0–1.0 (placeholder until full state estimation)
+
+
+# ---------------------------------------------------------------------------
+# Breadcrumb buffer
+# ---------------------------------------------------------------------------
+
+class BreadcrumbBuffer:
+    """
+    Circular buffer of position records logged during mission.
+    1800 entries at 2s interval = 1 hour of operation.
+    Older entries are discarded as new ones are added.
+    """
+
+    CAPACITY = 1800          # entries
+    LOG_INTERVAL_S = 2.0     # seconds between entries
+    MIN_CONFIDENCE = 0.3     # entries below this are flagged during reversal
+
+    def __init__(self):
+        self._buffer: collections.deque = collections.deque(maxlen=self.CAPACITY)
+        self._last_log_time: float = 0.0
+
+    def update(self, now_s: float, position: Point, heading_deg: float,
+               confidence: float = 1.0):
+        """Log a breadcrumb if the log interval has elapsed."""
+        if now_s - self._last_log_time >= self.LOG_INTERVAL_S:
+            self._buffer.append(Breadcrumb(
+                timestamp_s=now_s,
+                x=position.x,
+                y=position.y,
+                z=position.z,
+                heading_deg=heading_deg,
+                confidence=confidence,
+            ))
+            self._last_log_time = now_s
+
+    def get_return_path(self) -> List[Breadcrumb]:
+        """
+        Return the breadcrumb buffer in reverse order for path reversal.
+        Entries below MIN_CONFIDENCE are filtered — skipped in favour of
+        the next reliable entry to reduce navigation uncertainty.
+        """
+        reliable = [b for b in self._buffer if b.confidence >= self.MIN_CONFIDENCE]
+        return list(reversed(reliable))
+
+    def clear(self):
+        """Clear all entries — called at mission start."""
+        self._buffer.clear()
+        self._last_log_time = 0.0
+
+    @property
+    def entry_count(self) -> int:
+        """Number of entries currently in buffer."""
+        return len(self._buffer)
+
+
+# ---------------------------------------------------------------------------
+# Mission executor node
+# ---------------------------------------------------------------------------
+
+class MissionExecutor(Node):
+    """
+    Executes inspection missions from a waypoint sequence.
+    Integrates with the failsafe monitor and maintains breadcrumb buffer.
+    """
+
+    # Internal mission states
+    STATE_IDLE     = 'IDLE'
+    STATE_RUNNING  = 'RUNNING'
+    STATE_PAUSED   = 'PAUSED'
     STATE_COMPLETE = 'COMPLETE'
-    STATE_ABORTED = 'ABORTED'
+    STATE_ABORTED  = 'ABORTED'
 
     def __init__(self):
         super().__init__('mission_executor')
 
-        self.state = self.STATE_IDLE
+        # ------------------------------------------------------------------
-        self.waypoints = []          # List of InspectionWaypoint
+        # Parameters (from dev.yaml / field.yaml)
-        self.current_wp_index = 0
+        # ------------------------------------------------------------------
-        self.mission_id = ''
-        self.current_position = None
-        self.failsafe_active = False
 
-        # --- Subscribers ---
+        self.declare_parameter('waypoint_arrival_radius_m', 0.5)
-        self.state_sub = self.create_subscription(
+        self.declare_parameter('waypoint_timeout_s', 60.0)
-            Odometry, '/rov/state', self.state_callback, 10)
+        self.declare_parameter('image_capture_delay_s', 1.0)
-        self.failsafe_sub = self.create_subscription(
-            FailsafeStatus, '/rov/failsafe', self.failsafe_callback, 10)
 
-        # --- Publishers ---
+        self._arrival_radius  = self.get_parameter('waypoint_arrival_radius_m').value
-        self.cmd_pub = self.create_publisher(Twist, '/rov/cmd_vel', 10)
+        self._wp_timeout_s    = self.get_parameter('waypoint_timeout_s').value
-        self.status_pub = self.create_publisher(MissionStatus, '/rov/mission/status', 10)
+        self._capture_delay_s = self.get_parameter('image_capture_delay_s').value
 
-        # --- Services ---
+        # ------------------------------------------------------------------
-        self.cmd_srv = self.create_service(
+        # Mission state
-            MissionCommand, '/rov/mission/command', self.command_callback)
+        # ------------------------------------------------------------------
 
-        # --- Timer: mission loop at 10Hz ---
+        self.state              = self.STATE_IDLE
-        self.timer = self.create_timer(0.1, self.mission_loop)
+        self.mission_id         = ''
+        self.waypoints: List[InspectionWaypoint] = []
+        self.current_wp_index   = 0
 
-        self.get_logger().info('MissionExecutor node started')
+        # Entry point — set when mission starts, used for safe zone reference
+        self.entry_point: Optional[Point] = None
 
-    def state_callback(self, msg: Odometry):
+        # Operation mode — affects return-to-safe path choice
-        """Update current position."""
+        self.operation_mode = 'tethered'   # 'tethered' or 'untethered'
+
+        # ------------------------------------------------------------------
+        # Sensor state
+        # ------------------------------------------------------------------
+
+        self.current_position: Optional[Point]  = None
+        self.current_heading_deg: float          = 0.0
+
+        # ------------------------------------------------------------------
+        # Failsafe state
+        # ------------------------------------------------------------------
+
+        self.last_failsafe_action       = FailsafeStatus.ACTION_NONE
+        self.last_assessment_state      = FailsafeStatus.ASSESSMENT_GREEN
+
+        # ------------------------------------------------------------------
+        # Breadcrumb buffer
+        # ------------------------------------------------------------------
+
+        self.breadcrumbs = BreadcrumbBuffer()
+
+        # ------------------------------------------------------------------
+        # Waypoint timing
+        # ------------------------------------------------------------------
+
+        self.wp_start_time: Optional[float]    = None   # When current wp navigation began
+        self.capture_start_time: Optional[float] = None  # When image capture loiter began
+        self.capturing_image: bool              = False
+
+        # ------------------------------------------------------------------
+        # Subscribers
+        # ------------------------------------------------------------------
+
+        self.create_subscription(
+            Odometry, '/rov/state',
+            self._state_callback, 10)
+
+        self.create_subscription(
+            FailsafeStatus, '/rov/failsafe',
+            self._failsafe_callback, 10)
+
+        # ------------------------------------------------------------------
+        # Publishers
+        # ------------------------------------------------------------------
+
+        self.cmd_pub = self.create_publisher(
+            Twist, '/rov/cmd_vel', 10)
+
+        self.status_pub = self.create_publisher(
+            MissionStatus, '/rov/mission/status', 10)
+
+        self.waypoint_pub = self.create_publisher(
+            InspectionWaypoint, '/rov/mission/waypoint', 10)
+
+        # ------------------------------------------------------------------
+        # Service
+        # ------------------------------------------------------------------
+
+        self.create_service(
+            MissionCommand, '/rov/mission/command',
+            self._command_callback)
+
+        # ------------------------------------------------------------------
+        # Timers
+        # ------------------------------------------------------------------
+
+        # Mission loop at 10 Hz
+        self.create_timer(0.1, self._mission_loop)
+
+        # Status publisher at 2 Hz
+        self.create_timer(0.5, self._publish_status)
+
+        self.get_logger().info('MissionExecutor started')
+
+    # ------------------------------------------------------------------
+    # Subscriber callbacks
+    # ------------------------------------------------------------------
+
+    def _state_callback(self, msg: Odometry):
+        """Update current position and heading from state estimator."""
         self.current_position = msg.pose.pose.position
 
-    def failsafe_callback(self, msg: FailsafeStatus):
+        # Extract heading from quaternion (yaw only — simplified)
-        """Handle failsafe — abort mission if active."""
+        q = msg.pose.pose.orientation
-        was_active = self.failsafe_active
+        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
-        self.failsafe_active = (msg.action != FailsafeStatus.ACTION_NONE)
+        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
-        if self.failsafe_active and not was_active:
+        self.current_heading_deg = math.degrees(math.atan2(siny_cosp, cosy_cosp))
-            self.get_logger().warn('Failsafe triggered — aborting mission')
-            self.state = self.STATE_ABORTED
 
-    def command_callback(self, request, response):
+        # Log breadcrumb if mission is active
-        """Handle mission command service calls."""
+        if self.state == self.STATE_RUNNING and self.current_position is not None:
+            now_s = self.get_clock().now().nanoseconds / 1e9
+            self.breadcrumbs.update(
+                now_s,
+                self.current_position,
+                self.current_heading_deg,
+            )
+
+    def _failsafe_callback(self, msg: FailsafeStatus):
+        """
+        Respond to failsafe actions per design spec priority.
+        Different actions require different mission responses.
+        """
+        action           = msg.action
+        assessment_state = msg.assessment_state
+
+        # Store for use in mission loop
+        self.last_failsafe_action  = action
+        self.last_assessment_state = assessment_state
+
+        # --- Emergency surface: abort immediately, no controlled return ---
+        if action == FailsafeStatus.ACTION_EMERGENCY_SURFACE:
+            if self.state not in (self.STATE_ABORTED, self.STATE_COMPLETE):
+                self.get_logger().error(
+                    'EMERGENCY SURFACE commanded — aborting mission immediately')
+                self._abort_mission('Emergency surface — failsafe')
+            return
+
+        # --- Return to safe: abort mission, initiate breadcrumb reversal ---
+        if action == FailsafeStatus.ACTION_RETURN_TO_SAFE:
+            if self.state not in (self.STATE_ABORTED, self.STATE_COMPLETE):
+                self.get_logger().warn(
+                    f'RETURN TO SAFE commanded — aborting mission '
+                    f'(breadcrumbs: {self.breadcrumbs.entry_count})')
+                self._abort_mission('Return to safe — failsafe')
+                # Breadcrumb path handed off to motion controller here
+                # (Phase 3+ — motion controller implements path reversal)
+            return
+
+        # --- Hover: pause mission, hold position ---
+        if action == FailsafeStatus.ACTION_HOVER:
+            if self.state == self.STATE_RUNNING:
+                self.get_logger().warn('HOVER commanded — pausing mission')
+                self.state = self.STATE_PAUSED
+                self._stop_motion()
+            return
+
+        # --- No action: if paused by failsafe and now clear, resume ---
+        if action == FailsafeStatus.ACTION_NONE:
+            if (self.state == self.STATE_PAUSED and
+                    assessment_state == FailsafeStatus.ASSESSMENT_GREEN):
+                self.get_logger().info(
+                    'Failsafe cleared (GREEN) — resuming mission')
+                self.state = self.STATE_RUNNING
+
+    # ------------------------------------------------------------------
+    # Service callback — mission commands
+    # ------------------------------------------------------------------
+
+    def _command_callback(self, request, response):
+        """
+        Handle mission commands: START, PAUSE, RESUME, ABORT, LOAD.
+        LOAD accepts a YAML waypoint file path via parameters[0].
+        """
         cmd = request.command.upper()
 
         if cmd == 'START':
-            if self.state == self.STATE_IDLE:
+            if self.state != self.STATE_IDLE:
-                self.mission_id = request.mission_id
-                self.current_wp_index = 0
-                self.state = self.STATE_RUNNING
-                response.success = True
-                response.message = f'Mission {self.mission_id} started'
-            else:
                 response.success = False
-                response.message = f'Cannot start — current state: {self.state}'
+                response.message = f'Cannot START — current state: {self.state}'
+                return response
+
+            if not self.waypoints:
+                response.success = False
+                response.message = 'Cannot START — no waypoints loaded'
+                return response
+
+            # Record entry point at mission start
+            self.entry_point = (
+                self.current_position if self.current_position is not None
+                else Point()
+            )
+
+            # Set operation mode from parameters if provided
+            if request.parameters:
+                self.operation_mode = request.parameters[0]
+
+            self.breadcrumbs.clear()
+            self.current_wp_index = 0
+            self.mission_id       = request.mission_id
+            self.state            = self.STATE_RUNNING
+            self.wp_start_time    = self.get_clock().now().nanoseconds / 1e9
+
+            self.get_logger().info(
+                f'Mission {self.mission_id} started — '
+                f'{len(self.waypoints)} waypoints, '
+                f'mode={self.operation_mode}'
+            )
+            response.success = True
+            response.message = f'Mission {self.mission_id} started'
+
+        elif cmd == 'LOAD':
+            # Load waypoints from YAML file path in parameters[0]
+            if not request.parameters:
+                response.success = False
+                response.message = 'LOAD requires parameters[0] = YAML file path'
+                return response
+
+            loaded, message = self._load_waypoints(request.parameters[0])
+            response.success = loaded
+            response.message = message
 
         elif cmd == 'PAUSE':
-            if self.state == self.STATE_RUNNING:
+            if self.state != self.STATE_RUNNING:
-                self.state = self.STATE_PAUSED
-                response.success = True
-                response.message = 'Mission paused'
-            else:
                 response.success = False
-                response.message = f'Cannot pause — current state: {self.state}'
+                response.message = f'Cannot PAUSE — current state: {self.state}'
+                return response
+            self.state = self.STATE_PAUSED
+            self._stop_motion()
+            response.success = True
+            response.message = 'Mission paused'
 
         elif cmd == 'RESUME':
-            if self.state == self.STATE_PAUSED:
+            if self.state != self.STATE_PAUSED:
-                self.state = self.STATE_RUNNING
-                response.success = True
-                response.message = 'Mission resumed'
-            else:
                 response.success = False
-                response.message = f'Cannot resume — current state: {self.state}'
+                response.message = f'Cannot RESUME — current state: {self.state}'
+                return response
+            self.state = self.STATE_RUNNING
+            self.wp_start_time = self.get_clock().now().nanoseconds / 1e9
+            response.success = True
+            response.message = 'Mission resumed'
 
         elif cmd == 'ABORT':
-            self.state = self.STATE_ABORTED
+            self._abort_mission('Operator abort command')
-            self.stop_motion()
             response.success = True
             response.message = 'Mission aborted'
 
@@ -122,10 +394,12 @@ class MissionExecutor(Node):
 
         return response
 
-    def mission_loop(self):
+    # ------------------------------------------------------------------
-        """Main mission execution loop — called at 10Hz."""
+    # Mission execution loop — 10 Hz
-        self.publish_status()
+    # ------------------------------------------------------------------
 
+    def _mission_loop(self):
+        """Main mission execution loop."""
         if self.state != self.STATE_RUNNING:
             return
 
@@ -137,46 +411,229 @@ class MissionExecutor(Node):
         if self.current_wp_index >= len(self.waypoints):
             self.get_logger().info('All waypoints reached — mission complete')
             self.state = self.STATE_COMPLETE
+            self._stop_motion()
             return
 
-        # TODO: navigate to current waypoint
+        if self.current_position is None:
-        # TODO: check arrival condition
+            self.get_logger().warn('No position estimate — waiting')
-        # TODO: trigger image capture if required
+            return
-        # TODO: advance to next waypoint on arrival
 
-    def stop_motion(self):
+        current_wp = self.waypoints[self.current_wp_index]
-        """Publish zero velocity to stop the ROV."""
+        now_s      = self.get_clock().now().nanoseconds / 1e9
-        self.cmd_pub.publish(Twist())  # All zeros = stop
 
-    def publish_status(self):
+        # --- Publish current waypoint target ---
+        self.waypoint_pub.publish(current_wp)
+
+        # --- Check waypoint timeout ---
+        if (self.wp_start_time is not None and
+                now_s - self.wp_start_time > self._wp_timeout_s):
+            self.get_logger().warn(
+                f'Waypoint {current_wp.waypoint_id} timed out after '
+                f'{self._wp_timeout_s}s — advancing to next'
+            )
+            self._advance_waypoint(now_s)
+            return
+
+        # --- Check arrival at waypoint ---
+        distance = self._distance_to(current_wp.position)
+
+        if distance > self._arrival_radius:
+            # Not yet arrived — navigate toward waypoint
+            self._navigate_toward(current_wp)
+            return
+
+        # --- Arrived at waypoint ---
+        self._stop_motion()
+
+        if current_wp.capture_image and not self.capturing_image:
+            # Start image capture loiter
+            self.capturing_image    = True
+            self.capture_start_time = now_s
+            self.get_logger().info(
+                f'Arrived at {current_wp.waypoint_id} — '
+                f'capturing image (loiter {current_wp.loiter_time_s}s)'
+            )
+            return
+
+        if self.capturing_image:
+            # Wait for loiter time before advancing
+            elapsed_capture = now_s - self.capture_start_time
+            if elapsed_capture < current_wp.loiter_time_s + self._capture_delay_s:
+                return
+            self.capturing_image = False
+            self.get_logger().info(
+                f'Image capture complete at {current_wp.waypoint_id}')
+
+        # Advance to next waypoint
+        self._advance_waypoint(now_s)
+
+    # ------------------------------------------------------------------
+    # Navigation helpers
+    # ------------------------------------------------------------------
+
+    def _navigate_toward(self, waypoint: InspectionWaypoint):
+        """
+        Publish proportional velocity command toward the waypoint.
+        Simple P-controller — replaced by full motion planner in Phase 3+.
+        Max speed: 0.3 m/s in XY, 0.1 m/s in Z.
+        """
+        MAX_XY = 0.3   # m/s
+        MAX_Z  = 0.1   # m/s
+        KP     = 0.5   # proportional gain
+
+        dx = waypoint.position.x - self.current_position.x
+        dy = waypoint.position.y - self.current_position.y
+        dz = waypoint.position.z - self.current_position.z
+
+        # Clamp to max speeds
+        vx = max(-MAX_XY, min(MAX_XY, KP * dx))
+        vy = max(-MAX_XY, min(MAX_XY, KP * dy))
+        vz = max(-MAX_Z,  min(MAX_Z,  KP * dz))
+
+        cmd = Twist()
+        cmd.linear.x = vx
+        cmd.linear.y = vy
+        cmd.linear.z = vz
+        self.cmd_pub.publish(cmd)
+
+    def _stop_motion(self):
+        """Publish zero velocity to halt the ROV."""
+        self.cmd_pub.publish(Twist())  # All zeros
+
+    def _distance_to(self, target: Point) -> float:
+        """Euclidean distance from current position to target."""
+        if self.current_position is None:
+            return float('inf')
+        dx = target.x - self.current_position.x
+        dy = target.y - self.current_position.y
+        dz = target.z - self.current_position.z
+        return math.sqrt(dx*dx + dy*dy + dz*dz)
+
+    def _advance_waypoint(self, now_s: float):
+        """Move to the next waypoint in the sequence."""
+        self.current_wp_index += 1
+        self.wp_start_time     = now_s
+        self.capturing_image   = False
+
+        if self.current_wp_index < len(self.waypoints):
+            next_wp = self.waypoints[self.current_wp_index]
+            self.get_logger().info(
+                f'Advancing to waypoint {self.current_wp_index}: '
+                f'{next_wp.waypoint_id}'
+            )
+
+    def _abort_mission(self, reason: str):
+        """Abort the mission and stop motion."""
+        self.state = self.STATE_ABORTED
+        self._stop_motion()
+        self.get_logger().warn(f'Mission aborted: {reason}')
+
+    # ------------------------------------------------------------------
+    # Waypoint loading
+    # ------------------------------------------------------------------
+
+    def _load_waypoints(self, yaml_path: str):
+        """
+        Load InspectionWaypoint list from a YAML mission file.
+        Expected format:
+          waypoints:
+            - id: "WP1"
+              x: 1.0
+              y: 2.0
+              z: -5.0
+              heading_deg: 90.0
+              standoff_m: 0.5
+              loiter_time_s: 3.0
+              capture_image: true
+              panel_id: "P1"
+        """
+        try:
+            with open(yaml_path, 'r') as f:
+                data = yaml.safe_load(f)
+
+            if 'waypoints' not in data:
+                return False, f'No waypoints key in {yaml_path}'
+
+            self.waypoints = []
+            for entry in data['waypoints']:
+                wp = InspectionWaypoint()
+                wp.header.stamp    = self.get_clock().now().to_msg()
+                wp.waypoint_id     = str(entry.get('id', ''))
+                wp.position.x      = float(entry.get('x', 0.0))
+                wp.position.y      = float(entry.get('y', 0.0))
+                wp.position.z      = float(entry.get('z', 0.0))
+                wp.heading_deg     = float(entry.get('heading_deg', 0.0))
+                wp.standoff_m      = float(entry.get('standoff_m', 0.5))
+                wp.loiter_time_s   = float(entry.get('loiter_time_s', 2.0))
+                wp.capture_image   = bool(entry.get('capture_image', False))
+                wp.panel_id        = str(entry.get('panel_id', ''))
+                self.waypoints.append(wp)
+
+            msg = f'Loaded {len(self.waypoints)} waypoints from {yaml_path}'
+            self.get_logger().info(msg)
+            return True, msg
+
+        except FileNotFoundError:
+            return False, f'Mission file not found: {yaml_path}'
+        except Exception as e:
+            return False, f'Failed to load waypoints: {str(e)}'
+
+    # ------------------------------------------------------------------
+    # Status publishing — 2 Hz
+    # ------------------------------------------------------------------
+
+    def _publish_status(self):
         """Publish current mission status."""
-        msg = MissionStatus()
+        msg             = MissionStatus()
         msg.header.stamp = self.get_clock().now().to_msg()
-        msg.mission_id = self.mission_id
+        msg.mission_id  = self.mission_id
-        msg.current_task = f'Waypoint {self.current_wp_index}/{len(self.waypoints)}'
-        msg.message = self.state
-
-        if len(self.waypoints) > 0:
-            msg.progress_percent = (self.current_wp_index / len(self.waypoints)) * 100.0
 
         # Map state string to uint8
-        state_map = {
+        msg.state = {
-            self.STATE_IDLE: MissionStatus.STATE_IDLE,
+            self.STATE_IDLE:     MissionStatus.STATE_IDLE,
-            self.STATE_RUNNING: MissionStatus.STATE_RUNNING,
+            self.STATE_RUNNING:  MissionStatus.STATE_RUNNING,
-            self.STATE_PAUSED: MissionStatus.STATE_PAUSED,
+            self.STATE_PAUSED:   MissionStatus.STATE_PAUSED,
             self.STATE_COMPLETE: MissionStatus.STATE_COMPLETE,
-            self.STATE_ABORTED: MissionStatus.STATE_ABORTED,
+            self.STATE_ABORTED:  MissionStatus.STATE_ABORTED,
-        }
+        }.get(self.state, MissionStatus.STATE_IDLE)
-        msg.state = state_map.get(self.state, MissionStatus.STATE_IDLE)
+
+        # Progress
+        total = len(self.waypoints)
+        msg.progress_percent = (
+            (self.current_wp_index / total * 100.0) if total > 0 else 0.0
+        )
+
+        # Current task description
+        if self.state == self.STATE_RUNNING and total > 0:
+            wp_id = (self.waypoints[self.current_wp_index].waypoint_id
+                     if self.current_wp_index < total else 'complete')
+            msg.current_task = f'WP {self.current_wp_index + 1}/{total}: {wp_id}'
+        else:
+            msg.current_task = self.state
+
+        msg.message = (
+            f'{self.state} | '
+            f'WP {self.current_wp_index}/{total} | '
+            f'Breadcrumbs: {self.breadcrumbs.entry_count}'
+        )
         self.status_pub.publish(msg)
 
 
+# ---------------------------------------------------------------------------
+# Entry point
+# ---------------------------------------------------------------------------
+
 def main(args=None):
+    """ROS2 node entry point."""
     rclpy.init(args=args)
     node = MissionExecutor()
-    rclpy.spin(node)
+    try:
-    node.destroy_node()
+        rclpy.spin(node)
-    rclpy.shutdown()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
 
 
 if __name__ == '__main__':