robot_node.py

#!/usr/bin/env python3
# SPDX-FileCopyrightText: 2026 Real-Time Innovations, Inc.
# SPDX-License-Identifier: Apache-2.0
"""
Robot Node — the DDS approach: Pure DDS (Data-Centric Everything)

Uses RTI Connext DDS for all communication — pub-sub for
state/intent/telemetry/video, rti.rpc Requester / SimpleReplier for
commands and charging-station slot negotiation.

DDS architecture:
  • Types generated by rtiddsgen from robot_types.idl.
  • QoS loaded from robot_qos.xml (Pattern-based profiles).
  • writer.write() → middleware delivers via multicast; no per-subscriber threads.
  • SimpleReplier for commands (DDS-native request-reply).
  • Requester for slot negotiation — fan-out to all stations in one call.
  • DDS liveliness for peer health monitoring.
  • DDS SPDP for fleet discovery.
  • DDS SampleInfo provides source_timestamp and reception_timestamp
    with nanosecond precision.
  • rti.rpc correlates via SampleIdentity (writer GUID + sequence number).

See README §1 (DDS-Natural design philosophy) and §8 (Thread Model).
"""

import rti.connextdds as dds
import rti.idl as idl
import rti.rpc as rpc
import time
import threading
import argparse
import random
import math
import os
import sys
from datetime import datetime
from typing import Dict, List, Optional
from enum import IntEnum

sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'shared'))
from video_renderer import VideoRenderer

# Types generated by:  rtiddsgen -language python robot_types.idl
from robot_types import (
    RobotStatus, RobotIntentType, Command,
    Position, Velocity3, Waypoint,
    KinematicState, OperationalState, Intent, Telemetry, VideoFrame,
    StationStatus, CoveragePoint,
    CommandRequest, CommandResponse,
    SlotReserve, SlotAssignment, SlotRelease, SlotReleaseAck,
)


# Convenience set for charge-related intents
_CHARGE_INTENTS = (RobotIntentType.INTENT_CHARGE_QUEUE,
                   RobotIntentType.INTENT_CHARGE_DOCK)


# ═════════════════════════════════════════════════════════════════════════════
# Compact log timestamp
# ═════════════════════════════════════════════════════════════════════════════

def _ts():
    """Return a compact HH:MM:SS timestamp for log lines."""
    return datetime.now().strftime("%H:%M:%S")


# ═════════════════════════════════════════════════════════════════════════════
# RobotNode — Pure DDS robot participant
# ═════════════════════════════════════════════════════════════════════════════

class RobotNode:
    """
    A robot node using pure DDS for all communications.

    Pub-sub topics:
      KinematicState (10 Hz), OperationalState (on change), Intent (on change),
      Telemetry (1 Hz), VideoFrame (~5 fps).

    Request-reply (via rti.rpc):
      SimpleReplier: CommandRequest → CommandResponse (operator commands).
      Requester: SlotReserve → SlotAssignment (reserve a charging slot),
                 SlotRelease → SlotReleaseAck (release when done).
      Queue monitoring is pub-sub driven via StationStatus.

    Threading:
      ~9 threads total regardless of fleet size (see README §8).
    """

    def __init__(self, robot_id: str, domain_id: int = 0):
        self.robot_id = robot_id
        self.domain_id = domain_id
        self.running = False

        # ── Robot state ───────────────────────────────────────────────────
        self.position = {"x": random.uniform(0, 100),
                         "y": random.uniform(0, 100), "z": 0.0}
        self.status = RobotStatus.STATUS_MOVING
        self.intent = RobotIntentType.INTENT_FOLLOW_PATH
        self.battery_level = 100.0
        self.goal: Optional[dict] = None
        self.hold_position: Optional[dict] = None
        self.speed = 2.0
        self.heading = 0.0
        self.velocity_x = 0.0
        self.velocity_y = 0.0
        self.velocity_z = 0.0
        self.signal_strength = 95.0

        # Path route — a loop of waypoints the robot cycles through
        self.path_waypoints = self._generate_path_route()
        self.path_index = 0

        # ── Peer tracking (from DDS readers) ──────────────────────────────
        self.other_robots_state: Dict[str, KinematicState] = {}
        self.other_robots_intent: Dict[str, Intent] = {}
        self.other_robots_operational: Dict[str, OperationalState] = {}
        self._nearby_peers: set = set()

        # ── Charging state ────────────────────────────────────────────────
        self._charge_slot_id: Optional[str] = None
        self._charge_station_id: Optional[str] = None
        self._charge_dock: Optional[dict] = None
        self._charge_wait_pos: Optional[dict] = None
        self._charge_queue_rank: int = -1
        self._charge_requested: bool = False
        self._pre_charge_intent: Optional[RobotIntentType] = None
        self._pre_charge_status: Optional[RobotStatus] = None
        self._pre_charge_speed: float = 2.0
        self._pre_charge_goal: Optional[dict] = None
        self._pre_charge_hold: Optional[dict] = None
        # Station dock positions learned from StationStatus topic
        self._station_docks: Dict[str, dict] = {}  # station_id → {"x","y"}
        self.CHARGE_THRESHOLD = 20.0
        self.CHARGE_FULL = 95.0
        self.CHARGE_RATE = 2.0   # %/s while docked

        # ── DDS entities ──────────────────────────────────────────────────
        self.participant: Optional[dds.DomainParticipant] = None
        self.writers: Dict[str, dds.DataWriter] = {}
        self.readers: Dict[str, dds.DataReader] = {}

        # ── Video rendering ───────────────────────────────────────────────
        # fleet_robot_ids starts empty; peers are added dynamically from
        # DDS discovery (other_robots_state keys).  VideoRenderer uses
        # the list only for consistent colour assignment.
        self._fleet_robot_ids: List[str] = []
        self.video_renderer = VideoRenderer(robot_id, self._fleet_robot_ids)
        self._video_active = False   # True while at least one UI streams

        # ── RPC entities (rti.rpc) ────────────────────────────────────────
        self.command_replier: Optional[rpc.SimpleReplier] = None
        self.reserve_requester: Optional[rpc.Requester] = None
        self.release_requester: Optional[rpc.Requester] = None

        # ── OperationalState change tracking ──────────────────────────────
        self._last_published_status: Optional[RobotStatus] = None
        self._last_published_battery: Optional[float] = None

        # ── Intent change tracking ────────────────────────────────────────
        self._last_published_intent: Optional[RobotIntentType] = None
        self._last_published_target: Optional[tuple] = None
        self._last_published_path_index: Optional[int] = None
        self._path_version: int = 0          # bumped on SET_PATH
        self._last_published_path_version: int = -1

        # ── Coverage tracking ─────────────────────────────────────────────
        self._last_cov_x: float = float('nan')
        self._last_cov_y: float = float('nan')
        self._cov_working: bool = False      # True once path_index >= 1 after CMD_FOLLOW_PATH

        # ── Pre-idle state (saved by CMD_STOP, restored by CMD_RESUME) ────
        self._pre_idle_status: Optional[RobotStatus] = None

        # Locks
        self.data_lock = threading.Lock()

    # ── Path route generation ─────────────────────────────────────────────

    def _generate_path_route(self) -> List[dict]:
        """Generate a path route — a loop of waypoints covering an area.

        Each robot gets a slightly different route seeded by its id so
        they naturally spread out across the arena.
        """
        seed = hash(self.robot_id) & 0xFFFFFFFF
        rng = random.Random(seed)
        cx = rng.uniform(20, 80)
        cy = rng.uniform(20, 80)
        radius = rng.uniform(15, 30)
        n_pts = rng.randint(5, 8)
        waypoints = []
        for i in range(n_pts):
            angle = 2 * math.pi * i / n_pts
            jx = rng.uniform(-5, 5)
            jy = rng.uniform(-5, 5)
            wx = max(5, min(95, cx + radius * math.cos(angle) + jx))
            wy = max(5, min(95, cy + radius * math.sin(angle) + jy))
            waypoints.append({"x": wx, "y": wy})
        return waypoints

    @property
    def path_target(self):
        """The waypoint the robot is currently heading toward."""
        return self.path_waypoints[self.path_index]

    # ═════════════════════════════════════════════════════════════════════
    # DDS initialisation
    # ═════════════════════════════════════════════════════════════════════

    def initialize_dds(self):
        """Create the DDS DomainParticipant, pub-sub entities, and RPC objects.

        Pub-sub: 5 writers (kinematic, operational, intent, telemetry, video)
                 + 4 readers (kinematic, operational, intent, station) with listeners.
        RPC:     1 SimpleReplier (commands) + 3 Requesters (slot negotiation).

        QoS profiles are loaded from robot_qos.xml (Pattern-based).
        Types are generated by rtiddsgen from robot_types.idl.
        """
        print(f"[{self.robot_id} {_ts()}] Initializing DDS on domain {self.domain_id}...")

        # Resolve the path to robot_qos.xml relative to this script
        script_dir = os.path.dirname(os.path.abspath(__file__))
        qos_xml = os.path.join(script_dir, "robot_qos.xml")
        qos_provider = dds.QosProvider(qos_xml)

        participant_qos = qos_provider.participant_qos_from_profile(
            "RobotQosLibrary::ParticipantProfile")
        participant_qos.participant_name.name = self.robot_id
        participant_qos.participant_name.role_name = "Tractor"
        self.participant = dds.DomainParticipant(self.domain_id, participant_qos)

        # ── Pub-Sub Topics (6 — command/slot handled by rti.rpc) ──
        kin_topic       = dds.Topic(self.participant, "KinematicState",   KinematicState)
        op_topic        = dds.Topic(self.participant, "OperationalState", OperationalState)
        intent_topic    = dds.Topic(self.participant, "Intent",           Intent)
        telem_topic     = dds.Topic(self.participant, "Telemetry",        Telemetry)
        video_topic     = dds.Topic(self.participant, "VideoFrame",       VideoFrame)
        cov_topic       = dds.Topic(self.participant, "CoveragePoint",   CoveragePoint)
        station_topic   = dds.Topic(self.participant, "StationStatus",    StationStatus)

        publisher  = dds.Publisher(self.participant)
        subscriber = dds.Subscriber(self.participant)

        # ── Pub-Sub Writers (6) ───────────────────────────────────────────
        kin_w_qos = qos_provider.datawriter_qos_from_profile(
            "RobotQosLibrary::KinematicStateProfile")
        op_w_qos = qos_provider.datawriter_qos_from_profile(
            "RobotQosLibrary::OperationalStateProfile")
        intent_w_qos = qos_provider.datawriter_qos_from_profile(
            "RobotQosLibrary::IntentProfile")
        telem_w_qos = qos_provider.datawriter_qos_from_profile(
            "RobotQosLibrary::TelemetryProfile")
        video_w_qos = qos_provider.datawriter_qos_from_profile(
            "RobotQosLibrary::VideoProfile")
        cov_w_qos = qos_provider.datawriter_qos_from_profile(
            "RobotQosLibrary::CoverageProfile")

        self.writers['kinematic']    = dds.DataWriter(publisher, kin_topic,    kin_w_qos)
        self.writers['operational']  = dds.DataWriter(publisher, op_topic,     op_w_qos)
        self.writers['intent']       = dds.DataWriter(publisher, intent_topic, intent_w_qos)
        self.writers['telemetry']    = dds.DataWriter(publisher, telem_topic,  telem_w_qos)
        self.writers['coverage']     = dds.DataWriter(publisher, cov_topic,    cov_w_qos)

        # ── Video DataWriter on a dedicated Publisher with partition ───
        # The partition is set to this robot's ID.  The UI's video
        # Subscriber starts with partition '__NONE__' (matches nothing).
        # When the operator selects a robot, the UI changes its
        # Subscriber partition to that robot_id → DDS matches the
        # writer/reader pair and triggers publication_matched_status.
        # Only the matched robot renders; all others see current_count=0
        # and skip rendering.  No CFT, no property inspection, no
        # polling — just partition-based matching, the most efficient
        # DDS mechanism for selective data delivery.
        video_pub_qos = dds.PublisherQos()
        video_pub_qos.partition.name = [self.robot_id]
        video_publisher = dds.Publisher(self.participant, video_pub_qos)
        self.writers['video'] = dds.DataWriter(
            video_publisher, video_topic, video_w_qos)

        # ── Pub-Sub Readers (4) ───────────────────────────────────────────
        kin_r_qos = qos_provider.datareader_qos_from_profile(
            "RobotQosLibrary::KinematicStateProfile")
        op_r_qos = qos_provider.datareader_qos_from_profile(
            "RobotQosLibrary::OperationalStateProfile")
        intent_r_qos = qos_provider.datareader_qos_from_profile(
            "RobotQosLibrary::IntentProfile")

        self.readers['kinematic']    = dds.DataReader(subscriber, kin_topic,    kin_r_qos)
        self.readers['operational']  = dds.DataReader(subscriber, op_topic,     op_r_qos)
        self.readers['intent']       = dds.DataReader(subscriber, intent_topic, intent_r_qos)

        # ── StationStatus reader (learn station dock positions for 3D video) ─
        station_r_qos = qos_provider.datareader_qos_from_profile(
            "RobotQosLibrary::StationStatusProfile")
        self.readers['station'] = dds.DataReader(subscriber, station_topic, station_r_qos)

        # ── RPC: Command handling (robot = server) ────────────────────────
        # SimpleReplier: receives CommandRequest, calls _handle_command,
        # writes the returned CommandResponse back with SampleIdentity
        # correlation.
        self.command_replier = rpc.SimpleReplier(
            request_type=CommandRequest,
            reply_type=CommandResponse,
            participant=self.participant,
            service_name="RobotCommand",
            handler=self._handle_command,
        )

        # ── RPC: Slot negotiation (robot = client) ────────────────────────
        # Two Requester objects — one per request-reply pair.
        self.reserve_requester = rpc.Requester(
            request_type=SlotReserve,
            reply_type=SlotAssignment,
            participant=self.participant,
            service_name="ChargingSlotReserve",
        )
        self.release_requester = rpc.Requester(
            request_type=SlotRelease,
            reply_type=SlotReleaseAck,
            participant=self.participant,
            service_name="ChargingSlotRelease",
        )

        # ── Pub-Sub Listeners ─────────────────────────────────────────────
        self._setup_listeners()

        print(f"[{self.robot_id} {_ts()}] DDS initialized — "
              f"5 pub-sub + 1 SimpleReplier + 2 Requesters")

    # ═════════════════════════════════════════════════════════════════════
    # Listeners — DDS callbacks for received data
    # ═════════════════════════════════════════════════════════════════════

    def _setup_listeners(self):
        """Attach DDS DataReaderListeners for pub-sub data reception.

        Only pub-sub readers (kinematic, operational, intent, station) get
        listeners.
        Command reception is handled by the SimpleReplier (§6.6 in spec).
        Slot negotiation replies are read by Requester objects (§6.7).

        DDS delivers to these callbacks automatically for every matched
        writer in the domain.
        """
        robot = self

        # ── KinematicState listener ───────────────────────────────────────
        class KinematicListener(dds.DataReaderListener):
            def on_data_available(self, reader):
                for sample in reader.take():
                    if sample.info.valid:
                        data = sample.data
                        if data.robot_id != robot.robot_id:
                            with robot.data_lock:
                                robot.other_robots_state[data.robot_id] = data

            def on_liveliness_changed(self, reader, status):
                """Handle DDS liveliness changes for peer robots.

                The middleware fires this callback within ~10 s of
                lease expiry (30 s lease).
                """
                if status.alive_count_change < 0:
                    print(f"[{robot.robot_id} {_ts()}] ⚠  Robot DEAD "
                          f"(alive={status.alive_count})")
                elif status.alive_count_change > 0:
                    print(f"[{robot.robot_id} {_ts()}] ✓  Robot ALIVE "
                          f"(alive={status.alive_count})")

        self.readers['kinematic'].set_listener(
            KinematicListener(),
            dds.StatusMask.DATA_AVAILABLE | dds.StatusMask.LIVELINESS_CHANGED)

        # ── OperationalState listener ─────────────────────────────────────
        class OperationalListener(dds.DataReaderListener):
            def on_data_available(self, reader):
                for sample in reader.take():
                    if sample.info.valid:
                        data = sample.data
                        if data.robot_id != robot.robot_id:
                            with robot.data_lock:
                                robot.other_robots_operational[data.robot_id] = data

        self.readers['operational'].set_listener(
            OperationalListener(), dds.StatusMask.DATA_AVAILABLE)

        # ── Intent listener ───────────────────────────────────────────────
        class IntentListener(dds.DataReaderListener):
            def on_data_available(self, reader):
                for sample in reader.take():
                    if sample.info.valid:
                        data = sample.data
                        if data.robot_id != robot.robot_id:
                            with robot.data_lock:
                                robot.other_robots_intent[data.robot_id] = data

        self.readers['intent'].set_listener(
            IntentListener(), dds.StatusMask.DATA_AVAILABLE)

        # ── StationStatus listener (queue monitoring + dock positions) ────
        # This listener serves two purposes:
        # 1. Learn station dock positions for 3D rendering.
        # 2. Monitor queue rank changes — when a queued robot sees its
        #    rank drop to 0 it navigates to the dock.
        class StationStatusListener(dds.DataReaderListener):
            def on_data_available(self, reader):
                for sample in reader.take():
                    if sample.info.valid:
                        data = sample.data
                        with robot.data_lock:
                            robot._station_docks[data.station_id] = {
                                "x": data.dock_x,
                                "y": data.dock_y,
                                "queue_len": len(data.queue),
                                "is_occupied": data.is_occupied,
                            }
                        # Queue-rank monitoring (outside lock — may
                        # trigger RPC calls in _abort_charge).
                        if (robot._charge_station_id == data.station_id
                                and robot.intent in _CHARGE_INTENTS):
                            robot._on_station_status_update(data)

        self.readers['station'].set_listener(
            StationStatusListener(), dds.StatusMask.DATA_AVAILABLE)

    # ═════════════════════════════════════════════════════════════════════
    # Command handling
    # ═════════════════════════════════════════════════════════════════════

    def _handle_command(self, cmd: CommandRequest) -> CommandResponse:
        """Process an incoming command and return a response.

        Called by the SimpleReplier for each incoming CommandRequest.
        The return value is automatically written back with SampleIdentity
        correlation.
        """
        # Filter: only process commands addressed to this robot.
        # All robots share the same service_name, so every robot's
        # SimpleReplier receives every command.
        if cmd.robot_id != self.robot_id:
            return CommandResponse(
                robot_id=self.robot_id,
                success=True,
                description="not addressed to me",
                resulting_status=self.status,
                resulting_intent=self.intent,
            )

        cmd_name = cmd.command.name
        print(f"[{self.robot_id} {_ts()}] Received command: {cmd_name}")

        success = True
        description = ""
        resulting_status = self.status
        resulting_intent = self.intent

        if cmd.command == Command.CMD_STOP:
            self._pre_idle_status = self.status
            self.status = RobotStatus.STATUS_IDLE
            self.velocity_x = 0.0
            self.velocity_y = 0.0
            description = "Idle (paused)"

        elif cmd.command == Command.CMD_FOLLOW_PATH:
            if self._charge_slot_id:
                self._abort_charge()
            self.goal = None
            self.hold_position = None
            self.intent = RobotIntentType.INTENT_FOLLOW_PATH
            self.status = RobotStatus.STATUS_MOVING
            self._cov_working = False  # reset — must reach waypoint[1] again
            self._last_cov_x = float('nan')
            self._last_cov_y = float('nan')
            if cmd.goto_params is not None and cmd.goto_params.speed > 0:
                self.speed = cmd.goto_params.speed
            description = f"Following path at speed {self.speed:.1f}"

        elif cmd.command == Command.CMD_GOTO:
            if self._charge_slot_id:
                self._abort_charge()
            gp = cmd.goto_params
            gx = gp.x if gp is not None else self.position["x"]
            gy = gp.y if gp is not None else self.position["y"]
            spd = gp.speed if gp is not None else 0
            if spd > 0:
                self.speed = spd
            self.goal = {"x": gx, "y": gy}
            self.hold_position = None
            self.intent = RobotIntentType.INTENT_GOTO
            self.status = RobotStatus.STATUS_MOVING
            description = f"Moving to ({gx:.0f}, {gy:.0f}) at speed {self.speed:.1f}"

        elif cmd.command == Command.CMD_RESUME:
            if cmd.goto_params is not None and cmd.goto_params.speed > 0:
                self.speed = cmd.goto_params.speed
            if self._pre_idle_status is not None:
                # Restore whatever status we had before STOP
                self.status = self._pre_idle_status
                self._pre_idle_status = None
            elif self.intent == RobotIntentType.INTENT_FOLLOW_PATH:
                self.hold_position = None
                self.status = RobotStatus.STATUS_MOVING
            elif self.intent == RobotIntentType.INTENT_GOTO:
                if self.goal is not None:
                    self.hold_position = None
                    self.status = RobotStatus.STATUS_MOVING
                elif self.hold_position is not None:
                    self.goal = dict(self.hold_position)
                    self.hold_position = None
                    self.status = RobotStatus.STATUS_MOVING
                else:
                    self.hold_position = {
                        "x": self.position["x"],
                        "y": self.position["y"],
                    }
                    self.status = RobotStatus.STATUS_HOLDING
            elif self.intent in _CHARGE_INTENTS:
                if self.goal is not None:
                    self.status = RobotStatus.STATUS_MOVING
                elif self.hold_position is not None:
                    self.status = RobotStatus.STATUS_HOLDING
                else:
                    self.status = RobotStatus.STATUS_MOVING
            else:
                self.hold_position = {
                    "x": self.position["x"],
                    "y": self.position["y"],
                }
                self.status = RobotStatus.STATUS_HOLDING
            description = f"Resumed ({self.intent.name}) at speed {self.speed:.1f}"

        elif cmd.command == Command.CMD_SET_PATH:
            wps = (cmd.set_path_params.waypoints
                   if cmd.set_path_params is not None else [])
            if len(wps) < 2:
                success = False
                description = "Need at least 2 waypoints"
            else:
                self.path_waypoints = [{"x": w.x, "y": w.y} for w in wps]
                self.path_index = self.path_index % len(self.path_waypoints)
                self._path_version += 1
                description = f"Path route set ({len(wps)} waypoints)"
                print(f"[{self.robot_id} {_ts()}] Path route updated: {len(wps)} waypoints")

        elif cmd.command == Command.CMD_CHARGE:
            if self.status == RobotStatus.STATUS_CHARGING:
                success = False
                description = "Already charging"
            elif self._charge_requested:
                success = False
                description = "Charge already in progress"
            else:
                self._charge_requested = True
                threading.Thread(target=self._initiate_charge, daemon=True).start()
                description = "Heading to charging station"

        else:
            success = False
            description = f"Unknown command: {cmd_name}"

        resulting_status = self.status
        resulting_intent = self.intent

        response = CommandResponse(
            robot_id=self.robot_id,
            success=success,
            description=description,
            resulting_status=resulting_status,
            resulting_intent=resulting_intent,
        )
        print(f"[{self.robot_id} {_ts()}] → {description}")
        return response

    # ═════════════════════════════════════════════════════════════════════
    # Charging station interaction (DDS request-reply)
    # ═════════════════════════════════════════════════════════════════════

    def _initiate_charge(self):
        """Begin the charging workflow with a single directed SlotReserve.

        The robot already knows every station's queue length and dock
        position from the StationStatus pub-sub topic.  It picks the
        best station locally (shortest queue, then closest) and sends
        one SlotReserve.  No offer/accept round-trip needed.

        Queue monitoring is handled by StationStatusListener — when the
        station publishes an updated queue, the listener calls
        _on_station_status_update() which detects rank changes.
        """
        if self.intent in _CHARGE_INTENTS:
            self._charge_requested = False
            return

        # Stash current state
        self._pre_charge_intent = self.intent
        self._pre_charge_status = self.status
        self._pre_charge_speed = self.speed
        self._pre_charge_goal = dict(self.goal) if self.goal else None
        self._pre_charge_hold = dict(self.hold_position) if self.hold_position else None
        self._cov_working = False  # stop recording coverage during charge cycle

        print(f"[{self.robot_id} {_ts()}] ⚡ Battery {self.battery_level:.0f}% — requesting charge slot")

        # Pick the best station from cached StationStatus data
        best_sid = None
        best_qlen = float('inf')
        best_dist = float('inf')
        for sid, info in self._station_docks.items():
            qlen = info.get("queue_len", 0)
            dist = math.hypot(info["x"] - self.position["x"],
                              info["y"] - self.position["y"])
            if (qlen < best_qlen
                    or (qlen == best_qlen and dist < best_dist)):
                best_sid = sid
                best_qlen = qlen
                best_dist = dist

        if best_sid is None:
            print(f"[{self.robot_id} {_ts()}] ✗ No charging stations known — staying on task")
            self._charge_requested = False
            return

        dock = self._station_docks[best_sid]
        print(f"[{self.robot_id} {_ts()}] ⚡ Best station: {best_sid} "
              f"queue={best_qlen} dist={best_dist:.0f} "
              f"dock=({dock['x']:.0f},{dock['y']:.0f})")

        # Send SlotReserve — station_id makes it directed
        request_id = self.reserve_requester.send_request(SlotReserve(
            robot_id=self.robot_id,
            station_id=best_sid,
            battery_level=self.battery_level,
        ))

        # Collect reply (may arrive from multiple stations due to fan-out)
        try:
            replies = self.reserve_requester.receive_replies(
                dds.Duration.from_seconds(5),
                related_request_id=request_id)
        except dds.TimeoutError:
            replies = []

        assignment = None
        for reply in replies:
            if reply.info.valid:
                d = reply.data
                if d.granted and d.station_id == best_sid:
                    assignment = d
                    break

        if assignment is None:
            print(f"[{self.robot_id} {_ts()}] ✗ Slot not granted — staying on task")
            self._charge_requested = False
            return

        # Guard against a command that changed intent while we were
        # waiting for the RPC reply (e.g. rapid CHARGE → GOTO).  If
        # the handler already moved the robot to a non-charge intent,
        # release the slot we just reserved and bail out.
        if self.intent not in (self._pre_charge_intent, *_CHARGE_INTENTS):
            print(f"[{self.robot_id} {_ts()}] ⚡ Intent changed to "
                  f"{self.intent.name} during reserve — releasing slot")
            try:
                self.release_requester.send_request(SlotRelease(
                    robot_id=self.robot_id,
                    slot_id=assignment.slot_id,
                    station_id=best_sid,
                ))
            except Exception:
                pass
            self._charge_requested = False
            return

        print(f"[{self.robot_id} {_ts()}] ⚡ Reserved slot {assignment.slot_id} "
              f"rank={assignment.queue_rank}")

        # Store charge state
        self._charge_slot_id = assignment.slot_id
        self._charge_station_id = best_sid
        self._charge_dock = {"x": assignment.dock_x, "y": assignment.dock_y}
        self._charge_queue_rank = assignment.queue_rank

        # Navigate to station
        if assignment.queue_rank == 0:
            self.goal = dict(self._charge_dock)
            self.intent = RobotIntentType.INTENT_CHARGE_DOCK
            print(f"[{self.robot_id} {_ts()}] ⚡ Heading to dock "
                  f"({self.goal['x']:.0f},{self.goal['y']:.0f})")
        else:
            wx, wy = self._wait_position(
                self._charge_dock["x"], self._charge_dock["y"],
                assignment.queue_rank)
            self._charge_wait_pos = {"x": wx, "y": wy}
            self.goal = dict(self._charge_wait_pos)
            self.intent = RobotIntentType.INTENT_CHARGE_QUEUE
            print(f"[{self.robot_id} {_ts()}] ⚡ Heading to wait position "
                  f"rank={assignment.queue_rank} "
                  f"goal=({self.goal['x']:.1f},{self.goal['y']:.1f})")

        self.status = RobotStatus.STATUS_MOVING
        self.hold_position = None
        self.speed = 3.0

    def _on_station_status_update(self, status: StationStatus):
        """React to a StationStatus update for our charging station.

        Called from StationStatusListener when the station publishes
        an updated queue.

        If our rank dropped to 0 → navigate to dock.
        If our rank changed → update wait position.
        If we disappeared from the queue → abort charge.
        """
        if not self._charge_slot_id:
            return

        # Find our rank in the published queue
        my_rank = -1
        for entry in status.queue:
            if entry.robot_id == self.robot_id:
                my_rank = entry.rank
                break

        if my_rank < 0:
            # We're not in the queue — station may have expired us
            if self.intent == RobotIntentType.INTENT_CHARGE_QUEUE:
                print(f"[{self.robot_id} {_ts()}] ⚡ Disappeared from "
                      f"{status.station_id} queue — aborting charge")
                self._abort_charge()
                self.intent = self._pre_charge_intent or RobotIntentType.INTENT_FOLLOW_PATH
                self.status = RobotStatus.STATUS_MOVING
                self.speed = self._pre_charge_speed
                self.goal = self._pre_charge_goal
                self.hold_position = None
            return

        old_rank = self._charge_queue_rank
        self._charge_queue_rank = my_rank

        if my_rank == 0 and old_rank != 0:
            # Promoted to dock!
            self.goal = dict(self._charge_dock)
            self.hold_position = None
            self.intent = RobotIntentType.INTENT_CHARGE_DOCK
            self.status = RobotStatus.STATUS_MOVING
            print(f"[{self.robot_id} {_ts()}] ⚡ Queue advanced → heading to dock")
        elif my_rank != old_rank and my_rank > 0:
            wx, wy = self._wait_position(
                self._charge_dock["x"], self._charge_dock["y"], my_rank)
            self._charge_wait_pos = {"x": wx, "y": wy}
            self.goal = dict(self._charge_wait_pos)
            self.hold_position = None
            self.status = RobotStatus.STATUS_MOVING
            print(f"[{self.robot_id} {_ts()}] ⚡ Queue rank {old_rank} → {my_rank}")

    @staticmethod
    def _wait_position(dock_x: float, dock_y: float, rank: int) -> tuple:
        """Compute a waiting position near a station for a given queue rank.

        Robots line up in a column extending toward the centre of the arena
        (50, 50), each separated by ~8 units so collision avoidance doesn't
        kick in.
        """
        spacing = 8
        offset = 10 + rank * spacing
        dx = 1 if dock_x < 50 else -1
        dy = 1 if dock_y < 50 else -1
        if abs(50 - dock_x) >= abs(50 - dock_y):
            return (dock_x + dx * offset, dock_y)
        else:
            return (dock_x, dock_y + dy * offset)

    def _abort_charge(self):
        """Cancel an in-progress charge.  Release the slot via Requester.

        Fire-and-forget: we send the SlotRelease but do NOT wait for
        the reply.  This method is called from DDS callback threads
        (SimpleReplier handler, StationStatusListener) where blocking
        would stall the middleware's internal dispatch.
        """
        if self._charge_slot_id:
            try:
                self.release_requester.send_request(SlotRelease(
                    robot_id=self.robot_id,
                    slot_id=self._charge_slot_id,
                    station_id=self._charge_station_id or "",
                ))
            except Exception as e:
                print(f"[{self.robot_id} {_ts()}] ⚠ Release failed: {e}")
            print(f"[{self.robot_id} {_ts()}] ⚡ Charge aborted (slot released)")
        self._clear_charge_state()

    def _release_charge_slot(self):
        """Release the slot and resume prior task.

        Fire-and-forget like _abort_charge — called from the
        update_position thread; blocking it would pause the
        simulation for up to 2 seconds.
        """
        if self._charge_slot_id:
            try:
                self.release_requester.send_request(SlotRelease(
                    robot_id=self.robot_id,
                    slot_id=self._charge_slot_id,
                    station_id=self._charge_station_id or "",
                ))
            except Exception as e:
                print(f"[{self.robot_id} {_ts()}] ⚠ Release failed: {e}")

        print(f"[{self.robot_id} {_ts()}] ⚡ Charging complete "
              f"({self.battery_level:.0f}%) — resuming")

        # Restore pre-charge state
        if (self._pre_charge_intent == RobotIntentType.INTENT_GOTO
                and self._pre_charge_goal is None
                and self._pre_charge_hold is not None):
            self.intent = RobotIntentType.INTENT_GOTO
            self.goal = dict(self._pre_charge_hold)
            self.hold_position = None
            self.status = RobotStatus.STATUS_MOVING
            self.speed = self._pre_charge_speed
        else:
            self.intent = self._pre_charge_intent or RobotIntentType.INTENT_FOLLOW_PATH
            self.status = RobotStatus.STATUS_MOVING
            self.speed = self._pre_charge_speed
            self.goal = self._pre_charge_goal
            self.hold_position = None

        self._clear_charge_state()

    def _clear_charge_state(self):
        """Reset all charge-related fields."""
        self._charge_slot_id = None
        self._charge_station_id = None
        self._charge_dock = None
        self._charge_wait_pos = None
        self._charge_queue_rank = -1
        self._charge_requested = False
        self._pre_charge_intent = None
        self._pre_charge_status = None
        self._pre_charge_speed = 2.0
        self._pre_charge_goal = None
        self._pre_charge_hold = None
        # Coverage must re-earn _cov_working by reaching a waypoint
        self._cov_working = False
        self._last_cov_x = float('nan')
        self._last_cov_y = float('nan')

    # ═════════════════════════════════════════════════════════════════════
    # Collision avoidance
    # ═════════════════════════════════════════════════════════════════════

    AVOIDANCE_RADIUS = 12.0
    AVOIDANCE_STRENGTH = 4.0
    MIN_SEPARATION = 4.0
    MAX_TURN_RATE = 2.0          # rad/s — non-holonomic steering limit

    @property
    def _is_docking(self) -> bool:
        return (self.intent == RobotIntentType.INTENT_CHARGE_DOCK
                and self.goal is not None)

    def _compute_avoidance(self):
        """Return (dx, dy, hard_override) repulsion from nearby peers.

        Inverse-square repulsion algorithm; see _compute_avoidance()
        docstring for details.
        """
        ax, ay = 0.0, 0.0
        hard_override = False
        mx, my = self.position["x"], self.position["y"]
        docking = self._is_docking
        near_station = (self.intent in _CHARGE_INTENTS
                        and self._charge_dock is not None)

        if docking:
            eff_radius = self.AVOIDANCE_RADIUS * 0.4
            eff_strength = self.AVOIDANCE_STRENGTH * 0.3
        elif near_station:
            eff_radius = self.AVOIDANCE_RADIUS * 0.5
            eff_strength = self.AVOIDANCE_STRENGTH * 0.25
        else:
            eff_radius = self.AVOIDANCE_RADIUS
            eff_strength = self.AVOIDANCE_STRENGTH

        with self.data_lock:
            peers = {
                rid: (s.position.x, s.position.y)
                for rid, s in self.other_robots_state.items()
                if rid != self.robot_id
            }
            docking_peers: set = set()
            if near_station and not docking:
                for rid in peers:
                    intent_msg = self.other_robots_intent.get(rid)
                    if (intent_msg
                            and intent_msg.intent_type == RobotIntentType.INTENT_CHARGE_DOCK):
                        docking_peers.add(rid)

        for rid, (px, py) in peers.items():
            dx = mx - px
            dy = my - py
            dist = math.sqrt(dx * dx + dy * dy)

            was_nearby = rid in self._nearby_peers
            is_nearby = dist < eff_radius
            # Hysteresis: enter at eff_radius, clear at 115% to avoid
            # log flicker when peers hover near the boundary.
            is_clear = dist > eff_radius * 1.15

            if is_nearby and not was_nearby:
                print(f"[{self.robot_id} {_ts()}] ⚠ Peer {rid} nearby "
                      f"({dist:.1f} units) — avoiding")
                self._nearby_peers.add(rid)
            elif is_clear and was_nearby:
                print(f"[{self.robot_id} {_ts()}] ✓ Peer {rid} clear "
                      f"({dist:.1f} units)")
                self._nearby_peers.discard(rid)

            if is_nearby and dist > 0.01:
                ratio = eff_radius / dist
                strength = eff_strength * ratio * ratio
                if near_station and rid in docking_peers:
                    strength *= 3.0
                ax += (dx / dist) * strength
                ay += (dy / dist) * strength
                if dist < self.MIN_SEPARATION and not near_station and not docking:
                    hard_override = True

        return ax, ay, hard_override

    def _steer_heading(self, desired_vx, desired_vy, dt):
        """Turn self.heading toward the desired velocity, clamped by MAX_TURN_RATE.

        Returns (vx, vy) along the updated heading.  Speed is reduced
        when the heading error is large so the tractor decelerates into
        sharp turns.
        """
        desired_speed = math.sqrt(desired_vx**2 + desired_vy**2)
        if desired_speed < 0.01:
            return 0.0, 0.0

        desired_heading = math.atan2(desired_vy, desired_vx)
        diff = (desired_heading - self.heading + math.pi) % (2 * math.pi) - math.pi
        max_turn = self.MAX_TURN_RATE * dt
        self.heading += max(-max_turn, min(max_turn, diff))
        self.heading = (self.heading + math.pi) % (2 * math.pi) - math.pi

        # Slow down when turning hard — tractors decelerate into sharp turns
        turn_factor = max(0.2, math.cos(min(abs(diff), math.pi / 2)))
        speed = desired_speed * turn_factor

        return math.cos(self.heading) * speed, math.sin(self.heading) * speed

    # ═════════════════════════════════════════════════════════════════════
    # Position update (movement tick)
    # ═════════════════════════════════════════════════════════════════════

    def update_position(self):
        """Simulate robot movement — 10 Hz tick loop.

        Handles path following, GOTO, collision avoidance, battery drain,
        signal strength simulation, and auto-charge.
        """
        dt = 0.1
        while self.running:
            if self.status == RobotStatus.STATUS_MOVING:
                vx, vy = 0.0, 0.0

                if (self.intent in (RobotIntentType.INTENT_GOTO, *_CHARGE_INTENTS)
                        and self.goal is not None):
                    dx = self.goal["x"] - self.position["x"]
                    dy = self.goal["y"] - self.position["y"]
                    dist = (dx**2 + dy**2) ** 0.5

                    if dist <= self.speed * dt:
                        # Arrived
                        self.position["x"] = self.goal["x"]
                        self.position["y"] = self.goal["y"]
                        if dist > 0.01:
                            self.heading = math.atan2(dy, dx)
                        self.velocity_x = 0.0
                        self.velocity_y = 0.0

                        if self.intent == RobotIntentType.INTENT_CHARGE_DOCK:
                            print(f"[{self.robot_id} {_ts()}] Arrived at charging dock → CHARGING")
                            self.goal = None
                            self.status = RobotStatus.STATUS_CHARGING
                        elif self.intent == RobotIntentType.INTENT_CHARGE_QUEUE:
                            print(f"[{self.robot_id} {_ts()}] Arrived at charge wait pos "
                                  f"(rank {self._charge_queue_rank})")
                            self.hold_position = {"x": self.goal["x"], "y": self.goal["y"]}
                            self.goal = None
                            self.status = RobotStatus.STATUS_HOLDING
                        else:
                            print(f"[{self.robot_id} {_ts()}] Reached goal "
                                  f"({self.goal['x']:.1f}, {self.goal['y']:.1f}) → HOLDING")
                            self.hold_position = {"x": self.goal["x"], "y": self.goal["y"]}
                            self.goal = None
                            self.status = RobotStatus.STATUS_HOLDING
                    else:
                        vx = dx / dist * self.speed
                        vy = dy / dist * self.speed

                elif self.intent == RobotIntentType.INTENT_FOLLOW_PATH:
                    wp = self.path_target
                    dx = wp["x"] - self.position["x"]
                    dy = wp["y"] - self.position["y"]
                    dist = (dx**2 + dy**2) ** 0.5

                    if dist <= self.speed * dt:
                        self.position["x"] = wp["x"]
                        self.position["y"] = wp["y"]
                        self.path_index = (self.path_index + 1) % len(self.path_waypoints)
                        self._cov_working = True  # reached a waypoint — coverage starts
                        nwp = self.path_target
                        ndx = nwp["x"] - self.position["x"]
                        ndy = nwp["y"] - self.position["y"]
                        ndist = (ndx**2 + ndy**2) ** 0.5
                        if ndist > 0.01:
                            vx = ndx / ndist * self.speed
                            vy = ndy / ndist * self.speed
                    else:
                        vx = dx / dist * self.speed
                        vy = dy / dist * self.speed

                # Collision avoidance + non-holonomic steering
                if self.status == RobotStatus.STATUS_MOVING:
                    avoid_x, avoid_y, hard_override = self._compute_avoidance()
                    if hard_override:
                        dvx, dvy = avoid_x, avoid_y
                    else:
                        dvx, dvy = vx + avoid_x, vy + avoid_y

                    vx, vy = self._steer_heading(dvx, dvy, dt)
                    self.position["x"] += vx * dt
                    self.position["y"] += vy * dt
                    self.velocity_x = vx
                    self.velocity_y = vy

                    # ── Coverage point (active path work only) ────────
                    if self.intent == RobotIntentType.INTENT_FOLLOW_PATH:
                        if self._cov_working and not self._charge_requested:
                            cx, cy = self.position["x"], self.position["y"]
                            dx_c = cx - self._last_cov_x
                            dy_c = cy - self._last_cov_y
                            # NaN comparison is always False → first point always publishes
                            if not (dx_c * dx_c + dy_c * dy_c < 1.0):
                                self.writers['coverage'].write(CoveragePoint(
                                    robot_id=self.robot_id,
                                    x=cx, y=cy))
                                self._last_cov_x = cx
                                self._last_cov_y = cy

            elif self.status == RobotStatus.STATUS_HOLDING:
                vx, vy = 0.0, 0.0
                if self.hold_position:
                    dx = self.hold_position["x"] - self.position["x"]
                    dy = self.hold_position["y"] - self.position["y"]
                    dist = math.sqrt(dx * dx + dy * dy)
                    if dist > 0.05:
                        spring_speed = min(0.5, dist)
                        vx = dx / dist * spring_speed
                        vy = dy / dist * spring_speed

                avoid_x, avoid_y, hard_override = self._compute_avoidance()
                if hard_override:
                    dvx, dvy = avoid_x, avoid_y
                else:
                    dvx, dvy = vx + avoid_x, vy + avoid_y

                vx, vy = self._steer_heading(dvx, dvy, dt)
                self.position["x"] += vx * dt
                self.position["y"] += vy * dt
                self.velocity_x = vx
                self.velocity_y = vy

            elif self.status == RobotStatus.STATUS_CHARGING:
                self.velocity_x = 0.0
                self.velocity_y = 0.0
                self.battery_level = min(100.0, self.battery_level + self.CHARGE_RATE * dt)
                if self.battery_level >= self.CHARGE_FULL:
                    self._release_charge_slot()

            # Clamp to arena
            self.position["x"] = max(0, min(100, self.position["x"]))
            self.position["y"] = max(0, min(100, self.position["y"]))

            # Battery drain
            if self.status != RobotStatus.STATUS_CHARGING:
                actual_speed = math.sqrt(self.velocity_x**2 + self.velocity_y**2)
                drain = 0.01 + 0.01 * actual_speed
                self.battery_level = max(0, self.battery_level - drain)

            # Signal strength — weaker near edges, stronger near centre
            cx = abs(self.position["x"] - 50.0)
            cy = abs(self.position["y"] - 50.0)
            edge_dist = max(cx, cy)
            base_signal = max(10.0, 100.0 - edge_dist * 1.6)
            self.signal_strength = max(0.0, min(100.0,
                base_signal + random.uniform(-3.0, 3.0)))

            # Auto-charge trigger
            if (self.battery_level < self.CHARGE_THRESHOLD
                    and not self._charge_requested
                    and self.status != RobotStatus.STATUS_CHARGING):
                self._charge_requested = True
                threading.Thread(target=self._initiate_charge, daemon=True).start()

            time.sleep(dt)

    # ═════════════════════════════════════════════════════════════════════
    # Publishers — periodic data publishing threads
    # ═════════════════════════════════════════════════════════════════════

    def publish_kinematic_state(self):
        """Publish KinematicState at 10 Hz.

        One writer.write() call; the middleware delivers to all matched
        DataReaders via multicast.
        """
        while self.running:
            sample = KinematicState(
                robot_id=self.robot_id,
                position=Position(
                    x=self.position['x'],
                    y=self.position['y'],
                    z=self.position['z'],
                ),
                velocity=Velocity3(
                    x=self.velocity_x,
                    y=self.velocity_y,
                    z=self.velocity_z,
                ),
                heading=self.heading,
            )
            self.writers['kinematic'].write(sample)
            time.sleep(0.1)  # 10 Hz

    def publish_operational_state(self):
        """Publish OperationalState on change (checked at 10 Hz).

        Only publishes when status or battery_level changes.
        """
        while self.running:
            current_status = self.status
            current_battery = round(self.battery_level, 1)

            if (current_status != self._last_published_status
                    or current_battery != self._last_published_battery):
                sample = OperationalState(
                    robot_id=self.robot_id,
                    status=current_status,
                    battery_level=self.battery_level,
                )
                self.writers['operational'].write(sample)
                self._last_published_status = current_status
                self._last_published_battery = current_battery

            time.sleep(0.1)  # Check 10 Hz, publish on change

    def publish_intent(self):
        """Publish Intent on change (checked at 10 Hz).

        Publishes whenever intent_type, target, or path_index changes.
        TRANSIENT_LOCAL ensures late joiners get the last sample.
        """
        while self.running:
            # Compute current target
            if self.intent == RobotIntentType.INTENT_GOTO:
                if self.goal is not None:
                    tx, ty = self.goal["x"], self.goal["y"]
                elif self.hold_position is not None:
                    tx, ty = self.hold_position["x"], self.hold_position["y"]
                else:
                    tx, ty = self.position["x"], self.position["y"]
            elif self.intent in _CHARGE_INTENTS:
                if self._charge_dock:
                    tx, ty = self._charge_dock["x"], self._charge_dock["y"]
                elif self.goal:
                    tx, ty = self.goal["x"], self.goal["y"]
                else:
                    tx, ty = self.position["x"], self.position["y"]
            elif self.intent == RobotIntentType.INTENT_FOLLOW_PATH:
                wp = self.path_target
                tx, ty = wp["x"], wp["y"]
            else:
                tx, ty = self.position["x"], self.position["y"]

            current_target = (round(tx, 1), round(ty, 1))

            if (self.intent != self._last_published_intent
                    or current_target != self._last_published_target
                    or self.path_index != self._last_published_path_index
                    or self._path_version != self._last_published_path_version):

                # Build waypoints list
                path_wps = []
                if self.intent == RobotIntentType.INTENT_FOLLOW_PATH:
                    path_wps = [Waypoint(x=w["x"], y=w["y"])
                                for w in self.path_waypoints]

                sample = Intent(
                    robot_id=self.robot_id,
                    intent_type=self.intent,
                    target_position=Position(x=tx, y=ty, z=0.0),
                    path_waypoints=path_wps,
                    path_index=self.path_index,
                )
                self.writers['intent'].write(sample)
                self._last_published_intent = self.intent
                self._last_published_target = current_target
                self._last_published_path_index = self.path_index
                self._last_published_path_version = self._path_version

            time.sleep(0.1)  # Check 10 Hz, publish on change

    def publish_telemetry(self):
        """Publish Telemetry at 1 Hz."""
        while self.running:
            sample = Telemetry(
                robot_id=self.robot_id,
                cpu_usage=random.uniform(20, 80),
                memory_usage=random.uniform(30, 70),
                temperature=random.uniform(40, 70),
                signal_strength=self.signal_strength,
            )
            self.writers['telemetry'].write(sample)
            time.sleep(1.0)  # 1 Hz

    def _is_video_requested(self) -> bool:
        """Check whether any UI is viewing this robot's onboard camera."""
        try:
            count = (self.writers['video']
                     .publication_matched_status.current_count)
            # Log changes in match count
            prev = getattr(self, '_last_match_count', -1)
            if count != prev:
                print(f"[{self.robot_id} {_ts()}] 📷 publication_matched "
                      f"current_count: {prev} → {count}")
                self._last_match_count = count
            return count > 0
        except Exception as e:
            print(f"[{self.robot_id} {_ts()}] 📷 _is_video_requested error: {e}")
            return False

    def publish_video(self):
        """Publish VideoFrame at ~5 fps — only when the UI selects this robot.

        DDS-natural demand detection using **partitions**:

        Each robot's VideoFrame DataWriter lives on a dedicated Publisher
        with its partition set to the robot's own ID.  The UI's video
        DataReader lives on a Subscriber whose partition starts as
        ``'__NONE__'`` (no match).  When the operator selects a robot,
        the UI simply changes its Subscriber partition to that robot_id.

        DDS matching is partition-based:
          • Only the selected robot's writer matches → it renders and
            publishes JPEG frames.
          • All other robots see ``publication_matched_status.current_count == 0``
            → they skip PyBullet rendering entirely (zero CPU).
          • No content filter evaluation, no property inspection, no
            polling — partitions are the most efficient DDS mechanism
            for selective data delivery.

        Peer list for the 3-D renderer is built dynamically from
        other_robots_state (discovered via DDS, no static config needed).
        """
        was_active = False
        _vid_frame_count = 0
        _vid_error_count = 0

        while self.running:
            # ── Check if the UI is requesting this robot's camera ─────
            requested = self._is_video_requested()

            if not requested:
                if was_active:
                    print(f"[{self.robot_id} {_ts()}] 📷 Video: "
                          f"no longer selected — pausing render "
                          f"(published {_vid_frame_count} frames, "
                          f"{_vid_error_count} errors)")
                    was_active = False
                    _vid_frame_count = 0
                    _vid_error_count = 0
                time.sleep(0.2)
                continue

            if not was_active:
                print(f"[{self.robot_id} {_ts()}] 📷 Video: "
                      f"selected by UI — starting render")
                was_active = True
                _vid_frame_count = 0
                _vid_error_count = 0

            # ── Gather scene data under lock, render, and publish ─────
            try:
                with self.data_lock:
                    # Keep fleet_robot_ids in sync with discovered peers
                    for rid in self.other_robots_state:
                        if rid not in self._fleet_robot_ids and rid != self.robot_id:
                            self._fleet_robot_ids.append(rid)

                    peers = {}
                    for rid, ks in self.other_robots_state.items():
                        if rid != self.robot_id:
                            os_msg = self.other_robots_operational.get(rid)
                            status_name = (os_msg.status.name
                                           if os_msg else "STATUS_MOVING")
                            peers[rid] = {
                                "x": ks.position.x,
                                "y": ks.position.y,
                                "heading": ks.heading,
                                "status": status_name,
                            }

                    snap_pos = dict(self.position)
                    snap_heading = self.heading
                    snap_wps = list(self.path_waypoints)

                    # Charging station positions for 3-D rendering
                    stations = {}
                    for sid, dock in self._station_docks.items():
                        is_active = (self.status == RobotStatus.STATUS_CHARGING
                                     and self._charge_station_id == sid)
                        stations[sid] = {
                            "x": dock["x"],
                            "y": dock["y"],
                            "active": is_active,
                        }

                # ── Render & publish ──────────────────────────────────
                jpeg_bytes = self.video_renderer.render_frame(
                    my_pos=snap_pos,
                    my_heading=snap_heading,
                    peers=peers,
                    my_waypoints=snap_wps,
                    charging_stations=stations,
                )

                frame = VideoFrame(
                    robot_id=self.robot_id,
                    frame_data=jpeg_bytes,
                )
                self.writers['video'].write(frame)
                _vid_frame_count += 1
                if _vid_frame_count % 25 == 0:  # every ~5 seconds
                    print(f"[{self.robot_id} {_ts()}] 📷 Video heartbeat: "
                          f"{_vid_frame_count} frames published")
            except Exception as e:
                _vid_error_count += 1
                if self.running:
                    import traceback
                    print(f"[{self.robot_id} {_ts()}] 📷 Video render error "
                          f"(#{_vid_error_count}): {e}")
                    traceback.print_exc()

            time.sleep(0.2)  # ~5 fps

    # ═════════════════════════════════════════════════════════════════════
    # Status reporting
    # ═════════════════════════════════════════════════════════════════════

    def print_status(self):
        """Periodically print a one-line system status summary."""
        prev_status = None
        prev_intent = None
        ticks = 0
        while self.running:
            time.sleep(5)
            ticks += 1
            with self.data_lock:
                n_peers = len(self.other_robots_state)
            status_name = self.status.name
            intent_name = self.intent.name
            changed = (status_name != prev_status or intent_name != prev_intent)
            prev_status = status_name
            prev_intent = intent_name
            if not changed and ticks < 6:
                continue
            ticks = 0
            x, y = self.position['x'], self.position['y']
            batt = self.battery_level
            sig = self.signal_strength
            print(f"[{self.robot_id} {_ts()}] pos=({x:.1f},{y:.1f}) "
                  f"{status_name}/{intent_name} batt={batt:.0f}% "
                  f"sig={sig:.0f}% peers={n_peers}")

    # ═════════════════════════════════════════════════════════════════════
    # Main run loop
    # ═════════════════════════════════════════════════════════════════════

    def run(self):
        """Initialise DDS and start all background threads."""
        self.initialize_dds()
        self.running = True

        threads = [
            threading.Thread(target=self.update_position,          daemon=True, name="position"),
            threading.Thread(target=self.publish_kinematic_state,  daemon=True, name="kinematic"),
            threading.Thread(target=self.publish_operational_state, daemon=True, name="operational"),
            threading.Thread(target=self.publish_intent,           daemon=True, name="intent"),
            threading.Thread(target=self.publish_telemetry,        daemon=True, name="telemetry"),
            threading.Thread(target=self.publish_video,            daemon=True, name="video"),
            threading.Thread(target=self.print_status,             daemon=True, name="status"),
        ]

        for t in threads:
            t.start()

        print(f"[{self.robot_id} {_ts()}] Robot is running "
              f"({len(threads)} app threads + DDS internal threads). "
              f"Press Ctrl+C to stop.")

        try:
            while self.running:
                time.sleep(1)
        except KeyboardInterrupt:
            print(f"\n[{self.robot_id} {_ts()}] Shutting down...")
            self.running = False
            for t in threads:
                t.join(timeout=1)
            if self.participant:
                self.participant.close()


# ═════════════════════════════════════════════════════════════════════════════
# CLI entry point
# ═════════════════════════════════════════════════════════════════════════════

def main():
    parser = argparse.ArgumentParser(
        description='Robot Node — the DDS approach: Pure DDS')
    parser.add_argument('--id', required=True, help='Robot ID (e.g. tractor1)')
    parser.add_argument('--domain', type=int, default=0,
                        help='DDS Domain ID (default: 0)')
    args = parser.parse_args()

    robot = RobotNode(args.id, args.domain)
    robot.run()


if __name__ == '__main__':
    main()