diff --git a/controls/sae_2025_ws/src/uav/test/test_comms_diagnostics.py b/controls/sae_2025_ws/src/uav/test/test_comms_diagnostics.py new file mode 100644 index 000000000..06a572ca1 --- /dev/null +++ b/controls/sae_2025_ws/src/uav/test/test_comms_diagnostics.py @@ -0,0 +1,87 @@ +from __future__ import annotations + +from types import SimpleNamespace + +from uav.runtime.comms_diagnostics import ( + log_bootstrap_diagnostics, + log_comms_diag, + topic_graph_counts, +) + + +class _FakeLogger: + def __init__(self) -> None: + self.messages: list[str] = [] + + def info(self, message: str) -> None: + self.messages.append(message) + + +def test_log_comms_diag_prefixes_and_serializes_fields() -> None: + logger = _FakeLogger() + + log_comms_diag( + logger, + "MODE", + "runtime comm snapshot", + connection_status={"payload_1": False}, + peer_vehicle_names=("payload_1",), + ready=False, + ) + + assert len(logger.messages) == 1 + assert logger.messages[0].startswith("COMMS DIAG | MODE | runtime comm snapshot") + assert 'connection_status={"payload_1": false}' in logger.messages[0] + assert 'peer_vehicle_names=["payload_1"]' in logger.messages[0] + assert "ready=false" in logger.messages[0] + + +def test_log_bootstrap_diagnostics_emits_boot_and_network_snapshots( + monkeypatch, +) -> None: + logger = _FakeLogger() + + monkeypatch.setattr("uav.runtime.comms_diagnostics.socket.gethostname", lambda: "test-host") + monkeypatch.setattr("uav.runtime.comms_diagnostics.os.getpid", lambda: 4242) + monkeypatch.setattr( + "uav.runtime.comms_diagnostics._run_snapshot_command", + lambda command, timeout_sec=1.5: ( + 0, + "snapshot for " + " ".join(command), + ), + ) + + log_bootstrap_diagnostics( + logger, + runtime_kind="payload_mission", + vehicle_name="payload_0", + mission_path="/tmp/mission.yaml", + mission_target="payload", + auto_launch=True, + peer_heartbeat_hz=10.0, + peer_stale_timeout_s=0.5, + peer_vehicle_names=("payload_1",), + vision_nodes=(), + ) + + assert len(logger.messages) == 5 + assert logger.messages[0].startswith("COMMS DIAG | BOOT |") + assert "vehicle_name=payload_0" in logger.messages[0] + assert "hostname=test-host" in logger.messages[0] + assert any("COMMS DIAG | NET | snapshot=ip_addr" in message for message in logger.messages) + assert any( + "snapshot for nmcli -t -f NAME,TYPE,DEVICE,STATE con show --active" in message + for message in logger.messages + ) + + +def test_topic_graph_counts_handles_missing_methods_and_errors() -> None: + node = SimpleNamespace( + count_publishers=lambda _topic: 2, + count_subscribers=lambda _topic: (_ for _ in ()).throw(RuntimeError("boom")), + ) + + counts = topic_graph_counts(node, "/payload_1/mode_manager/heartbeat") + + assert counts["publishers"] == 2 + assert counts["subscribers"] == "error: boom" diff --git a/controls/sae_2025_ws/src/uav/test/test_runtime_behavior.py b/controls/sae_2025_ws/src/uav/test/test_runtime_behavior.py index 7ae15f961..3737cbda5 100644 --- a/controls/sae_2025_ws/src/uav/test/test_runtime_behavior.py +++ b/controls/sae_2025_ws/src/uav/test/test_runtime_behavior.py @@ -694,6 +694,47 @@ def test_run_active_mode_handles_update_error(): assert handled == ["error"] +def test_run_active_mode_logs_connectivity_transition_once(): + _require_runtime_support() + + start_mode = _TrackingMode("start") + start_mode.peer_vehicle_names = ("payload_1",) + start_mode.connection_ready = lambda _connection_status: False + manager = _make_mode_manager() + manager.modes = {"start": start_mode} + manager.active_mode = "start" + manager._peer_connections = SimpleNamespace( + status=lambda: {"payload_1": False}, + peer_names=("payload_1",), + debug_snapshot=lambda: { + "local_heartbeat_topic": "/payload_0/mode_manager/heartbeat", + "remote_heartbeat_topics": { + "payload_1": "/payload_1/mode_manager/heartbeat" + }, + "peer_vehicle_names": ("payload_1",), + "connection_status": {"payload_1": False}, + "last_seen_age_s": {"payload_1": None}, + }, + close=lambda: None, + ) + manager.count_publishers = lambda _topic: 0 + manager.count_subscribers = lambda _topic: 1 + + ModeManager._run_active_mode(manager, 1.0) + ModeManager._run_active_mode(manager, 2.0) + + diag_messages = [ + msg + for level, msg in manager._logger.messages + if level == "info" + and "COMMS DIAG | MODE | runtime comm snapshot" in msg + and "reason=mode_connectivity_transition" in msg + ] + assert len(diag_messages) == 1 + assert "connection_ready=false" in diag_messages[0] + assert start_mode.disconnects == [(1.0, {"payload_1": False}), (1.0, {"payload_1": False})] + + def test_handle_mode_state_requires_exact_transition_label(): _require_runtime_support() diff --git a/controls/sae_2025_ws/src/uav/uav/modes/payload/PayloadPeerFleetTestMode.py b/controls/sae_2025_ws/src/uav/uav/modes/payload/PayloadPeerFleetTestMode.py index 945d72655..8f712d17c 100644 --- a/controls/sae_2025_ws/src/uav/uav/modes/payload/PayloadPeerFleetTestMode.py +++ b/controls/sae_2025_ws/src/uav/uav/modes/payload/PayloadPeerFleetTestMode.py @@ -88,6 +88,13 @@ def __init__( peer_name: 0 for peer_name in self._peer_names } self._last_disconnect_signature: tuple[str, ...] = () + self._peer_state_topics = { + peer_name: vehicle.namespaced_path(local_topic, namespace=f"/{peer_name}") + for peer_name in self._peer_names + } + self._last_operating_state: str | None = None + self._logged_first_peer_receipt: set[str] = set() + self._logged_first_shared_receipt: set[str] = set() def _now(self) -> float: return self.node.get_clock().now().nanoseconds * 1e-9 @@ -108,6 +115,12 @@ def _on_peer_message(self, peer_name: str, message: String) -> None: self._peer_message_counts[peer_name] = ( self._peer_message_counts.get(peer_name, 0) + 1 ) + if peer_name not in self._logged_first_peer_receipt: + self._logged_first_peer_receipt.add(peer_name) + self.log( + f"COMMS DIAG | PEER_TEST | first peer-local message received " + f"peer_name={peer_name} topic={self._peer_state_topics.get(peer_name, '')}" + ) def _on_shared_message(self, message: String) -> None: self._shared_message_count += 1 @@ -119,6 +132,25 @@ def _on_shared_message(self, message: String) -> None: self._shared_remote_message_counts[sender] = ( self._shared_remote_message_counts.get(sender, 0) + 1 ) + if sender not in self._logged_first_shared_receipt: + self._logged_first_shared_receipt.add(sender) + self.log( + f"COMMS DIAG | PEER_TEST | first shared remote message received " + f"peer_name={sender} topic={self._shared_topic}" + ) + + def _log_state_transition( + self, *, state: str, disconnected_peers: tuple[str, ...] = () + ) -> None: + if state == self._last_operating_state: + return + self._last_operating_state = state + self.log( + "COMMS DIAG | PEER_TEST | state transition " + f"state={state} disconnected_peers={list(disconnected_peers)} " + f"peer_total={sum(self._peer_message_counts.values())} " + f"shared_remote_total={sum(self._shared_remote_message_counts.values())}" + ) def _status_payload( self, *, state: str, disconnected_peers: tuple[str, ...] @@ -183,13 +215,22 @@ def on_enter(self) -> None: self._peer_message_counts[peer_name] = 0 self._shared_remote_message_counts[peer_name] = 0 self._last_disconnect_signature = () + self._last_operating_state = None + self._logged_first_peer_receipt.clear() + self._logged_first_shared_receipt.clear() self.log( f"starting peer fleet test for {self.vehicle.name}; expecting peers {self._peer_names}" ) + self.log( + "COMMS DIAG | PEER_TEST | topics " + f"local_topic={self._local_topic} shared_topic={self._shared_topic} " + f"peer_topics={self._peer_state_topics}" + ) def on_update(self, time_delta: float) -> None: del time_delta now = self._now() + self._log_state_transition(state="connected") self._publish_status(now=now, state="connected") self._log_status(now=now, state="connected") @@ -207,6 +248,9 @@ def on_disconnect( if disconnect_signature != self._last_disconnect_signature: self.log("waiting for peers: " + ", ".join(disconnect_signature)) self._last_disconnect_signature = disconnect_signature + self._log_state_transition( + state="waiting", disconnected_peers=disconnect_signature + ) self._publish_status( now=now, state="waiting", diff --git a/controls/sae_2025_ws/src/uav/uav/runtime/ModeManager.py b/controls/sae_2025_ws/src/uav/uav/runtime/ModeManager.py index 9b9ecf1dd..338aacf53 100644 --- a/controls/sae_2025_ws/src/uav/uav/runtime/ModeManager.py +++ b/controls/sae_2025_ws/src/uav/uav/runtime/ModeManager.py @@ -10,6 +10,7 @@ from uav.vehicles.Vehicle import Vehicle from uav.modes.Mode import Mode +from .comms_diagnostics import log_comms_diag, topic_graph_counts from .comm_naming import ManagedCommKind, resolve_managed_target from .comm_policy import ManagedCommLifetime, managed_creation_phase_error from .managed_comms import ModeCommBuilder @@ -76,6 +77,7 @@ def __init__( self._shared_mode_state = {} self._current_comm_builder = None self._runtime_closed = False + self._last_connectivity_diag_signature = None self._initialize_comms_runtime(vehicle_name=vehicle_name) self.start_mission_service = self.create_service( Trigger, "mode_manager/start_mission", self._start_mission_callback @@ -94,12 +96,71 @@ def shared_state_for(self, mode_or_class: object) -> dict[str, Any]: return self._shared_mode_state.setdefault(key, {}) def _comm_debug_snapshot(self) -> dict[str, Any]: + peer_snapshot = None + if hasattr(self._peer_connections, "debug_snapshot"): + peer_snapshot = self._peer_connections.debug_snapshot() return { "peer_vehicle_names": tuple(self._peer_connections.peer_names), "connection_status": self._peer_connections.status(), "descriptors": self._managed_registry.debug_descriptors(), + "peer_snapshot": peer_snapshot, } + def _diag_log(self, category: str, message: str | None = None, **fields) -> None: + log_comms_diag(self.get_logger(), category, message=message, **fields) + + def _heartbeat_graph_snapshot(self) -> dict[str, dict[str, object]]: + peer_snapshot = getattr(self._peer_connections, "debug_snapshot", lambda: {})() + topics: dict[str, str] = {} + local_topic = peer_snapshot.get("local_heartbeat_topic") + if local_topic: + topics["local"] = str(local_topic) + for peer_name, topic in sorted( + dict(peer_snapshot.get("remote_heartbeat_topics", {})).items() + ): + topics[peer_name] = str(topic) + return { + label: {"topic": topic, **topic_graph_counts(self, topic)} + for label, topic in topics.items() + } + + def _log_comm_snapshot( + self, + *, + reason: str, + include_descriptors: bool = False, + **fields, + ) -> None: + snapshot = self._comm_debug_snapshot() + descriptor_payload = None + if include_descriptors: + descriptor_payload = [ + vars(descriptor).copy() for descriptor in snapshot["descriptors"] + ] + payload = { + "reason": reason, + "peer_vehicle_names": snapshot["peer_vehicle_names"], + "connection_status": snapshot["connection_status"], + "peer_snapshot": snapshot["peer_snapshot"], + "heartbeat_graph": self._heartbeat_graph_snapshot(), + "descriptor_count": len(snapshot["descriptors"]), + "bound_descriptor_count": sum( + 1 for descriptor in snapshot["descriptors"] if descriptor.bound + ), + "descriptors": descriptor_payload, + } + payload.update(fields) + self._diag_log("MODE", "runtime comm snapshot", **payload) + + def _handle_peer_connection_change(self, peer_name: str, *, connected: bool) -> None: + self._managed_registry.on_peer_connection_change(peer_name, connected=connected) + self._log_comm_snapshot( + reason="peer_connection_change", + peer_name=peer_name, + connected=connected, + active_mode=self.active_mode, + ) + def _initialize_comms_runtime(self, *, vehicle_name: str) -> None: self._runtime_vehicle_name = normalize_vehicle_name(vehicle_name) raw_node = super(ModeManager, self) @@ -122,13 +183,15 @@ def _initialize_comms_runtime(self, *, vehicle_name: str) -> None: raw_destroy_publisher=self._raw_node_api.destroy_publisher, raw_destroy_subscription=self._raw_node_api.destroy_subscription, raw_destroy_client=self._raw_node_api.destroy_client, + diagnostic_logger=self._diag_log, ) self._peer_connections = PeerConnectionTracker( runtime_vehicle_name=self._runtime_vehicle_name, peer_heartbeat_hz=self.peer_heartbeat_hz, peer_stale_timeout_s=self.peer_stale_timeout_s, now_seconds=self._now_seconds, - on_connection_change=self._managed_registry.on_peer_connection_change, + on_connection_change=self._handle_peer_connection_change, + diagnostic_logger=self._diag_log, raw_create_publisher=self._raw_node_api.create_publisher, raw_create_subscription=self._raw_node_api.create_subscription, raw_destroy_publisher=self._raw_node_api.destroy_publisher, @@ -141,6 +204,7 @@ def configure_peer_vehicle_names( self, peer_vehicle_names: tuple[str, ...] | list[str] | set[str] ) -> None: self._peer_connections.configure(peer_vehicle_names) + self._log_comm_snapshot(reason="after_peer_config", include_descriptors=True) def setup_vision(self, vision_nodes: list[str]) -> None: nodes_to_setup = [node for node in vision_nodes if node] @@ -376,6 +440,7 @@ def switch_mode(self, mode_name: str) -> None: if mode_name in self.modes: self.active_mode = mode_name + self._last_connectivity_diag_signature = None mode = self.get_active_mode() builder = self._make_comm_builder( owner=mode, @@ -389,6 +454,11 @@ def switch_mode(self, mode_name: str) -> None: except Exception: self._managed_registry.destroy_for_owner(mode, lifetime="active") raise + self._log_comm_snapshot( + reason="mode_activated", + include_descriptors=True, + active_mode=mode_name, + ) else: self.get_logger().error(f"Mode {mode_name} not found.") @@ -400,8 +470,25 @@ def _run_active_mode(self, current_time: float) -> None: self.last_update_time = current_time mode = self.get_active_mode() connection_status = self._mode_connection_status(mode) + connection_ready = mode.connection_ready(connection_status) + connectivity_signature = ( + self.active_mode, + bool(connection_ready), + tuple(sorted(connection_status.items())), + ) + last_connectivity_diag_signature = getattr( + self, "_last_connectivity_diag_signature", None + ) + if connectivity_signature != last_connectivity_diag_signature: + self._last_connectivity_diag_signature = connectivity_signature + self._log_comm_snapshot( + reason="mode_connectivity_transition", + active_mode=self.active_mode, + connection_ready=connection_ready, + connection_status=connection_status, + ) try: - if mode.connection_ready(connection_status): + if connection_ready: mode.update(time_delta) else: mode.disconnect(time_delta, connection_status) @@ -420,6 +507,7 @@ def _deactivate_active_mode(self) -> None: mode_name = self.active_mode mode = self.modes.get(mode_name) self.active_mode = None + self._last_connectivity_diag_signature = None if mode is None or not getattr(mode, "active", False): return diff --git a/controls/sae_2025_ws/src/uav/uav/runtime/comms_diagnostics.py b/controls/sae_2025_ws/src/uav/uav/runtime/comms_diagnostics.py new file mode 100644 index 000000000..63236b8cc --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/runtime/comms_diagnostics.py @@ -0,0 +1,154 @@ +from __future__ import annotations + +import json +import os +import shlex +import socket +import subprocess +from typing import Any, Mapping, Sequence + +_DIAG_PREFIX = "COMMS DIAG" +_ENV_KEYS = ( + "ROS_DOMAIN_ID", + "ROS_LOCALHOST_ONLY", + "RMW_IMPLEMENTATION", + "ROS_DISCOVERY_SERVER", + "CYCLONEDDS_URI", + "CYCLONEDDS_HOME", + "FASTRTPS_DEFAULT_PROFILES_FILE", + "FASTDDS_DEFAULT_PROFILES_FILE", +) +_NETWORK_COMMANDS = ( + ("ip_addr", ("ip", "-br", "addr")), + ("ip_route", ("ip", "route")), + ("nmcli_dev_status", ("nmcli", "-t", "-f", "DEVICE,TYPE,STATE,CONNECTION", "dev", "status")), + ( + "nmcli_active_connections", + ("nmcli", "-t", "-f", "NAME,TYPE,DEVICE,STATE", "con", "show", "--active"), + ), +) + + +def _single_line(value: object) -> str: + return " ".join(str(value).split()) + + +def _json_ready(value: object) -> object: + if isinstance(value, Mapping): + return { + str(key): _json_ready(inner_value) + for key, inner_value in sorted(value.items(), key=lambda item: str(item[0])) + } + if isinstance(value, (list, tuple)): + return [_json_ready(item) for item in value] + if isinstance(value, set): + return [_json_ready(item) for item in sorted(value, key=str)] + return value + + +def _serialize(value: object) -> str: + if isinstance(value, str): + return _single_line(value) + if isinstance(value, (dict, list, tuple, set)): + return json.dumps(_json_ready(value), sort_keys=True, default=str) + if isinstance(value, bool): + return "true" if value else "false" + return _single_line(value) + + +def log_comms_diag(logger, category: str, message: str | None = None, **fields) -> None: + parts: list[str] = [] + if message: + parts.append(_single_line(message)) + for key, value in fields.items(): + if value is None: + continue + parts.append(f"{key}={_serialize(value)}") + payload = " ".join(parts) + if payload: + logger.info(f"{_DIAG_PREFIX} | {category} | {payload}") + return + logger.info(f"{_DIAG_PREFIX} | {category}") + + +def _run_snapshot_command( + command: Sequence[str], *, timeout_sec: float = 1.5 +) -> tuple[int | None, str]: + try: + result = subprocess.run( + list(command), + capture_output=True, + text=True, + check=False, + timeout=timeout_sec, + ) + except FileNotFoundError: + return None, "unavailable" + except subprocess.TimeoutExpired: + return None, "timeout" + except Exception as exc: # pragma: no cover - defensive + return None, f"error: {exc}" + + output = _single_line( + result.stdout.strip() or result.stderr.strip() or "" + ) + return int(result.returncode), output + + +def log_bootstrap_diagnostics( + logger, + *, + runtime_kind: str, + vehicle_name: str, + mission_path: str, + mission_target: str, + auto_launch: bool, + peer_heartbeat_hz: float, + peer_stale_timeout_s: float, + peer_vehicle_names: Sequence[str], + vision_nodes: Sequence[str] = (), +) -> None: + log_comms_diag( + logger, + "BOOT", + runtime_kind=runtime_kind, + vehicle_name=vehicle_name, + mission_path=mission_path, + mission_target=mission_target, + auto_launch=auto_launch, + peer_heartbeat_hz=peer_heartbeat_hz, + peer_stale_timeout_s=peer_stale_timeout_s, + peer_vehicle_names=tuple(peer_vehicle_names), + vision_nodes=tuple(vision_nodes), + pid=os.getpid(), + hostname=socket.gethostname(), + env={key: os.environ.get(key, "") for key in _ENV_KEYS}, + ) + for label, command in _NETWORK_COMMANDS: + returncode, output = _run_snapshot_command(command) + log_comms_diag( + logger, + "NET", + snapshot=label, + command=shlex.join(command), + returncode=returncode, + output=output, + ) + + +def topic_graph_counts(node, topic_name: str) -> dict[str, object]: + counts: dict[str, object] = {} + for label, method_name in ( + ("publishers", "count_publishers"), + ("subscribers", "count_subscribers"), + ): + method = getattr(node, method_name, None) + if not callable(method): + counts[label] = None + continue + try: + counts[label] = int(method(topic_name)) + except Exception as exc: # pragma: no cover - defensive + counts[label] = f"error: {_single_line(exc)}" + return counts + diff --git a/controls/sae_2025_ws/src/uav/uav/runtime/managed_registry.py b/controls/sae_2025_ws/src/uav/uav/runtime/managed_registry.py index a2e8ecd7b..66ccc926d 100644 --- a/controls/sae_2025_ws/src/uav/uav/runtime/managed_registry.py +++ b/controls/sae_2025_ws/src/uav/uav/runtime/managed_registry.py @@ -19,6 +19,7 @@ class ManagedCommDescriptor: interface_name: str name: str peers: tuple[str, ...] + bound: bool = False @dataclass @@ -34,7 +35,7 @@ class ManagedCommSpec: kwargs: dict[str, Any] peers: tuple[str, ...] - def descriptor(self) -> ManagedCommDescriptor: + def descriptor(self, *, bound: bool = False) -> ManagedCommDescriptor: return ManagedCommDescriptor( kind=self.kind, scope=self.scope, @@ -45,6 +46,7 @@ def descriptor(self) -> ManagedCommDescriptor: ), name=self.name, peers=self.peers, + bound=bound, ) @@ -59,6 +61,7 @@ def __init__( raw_destroy_publisher: Callable[[object], bool | None], raw_destroy_subscription: Callable[[object], bool | None], raw_destroy_client: Callable[[object], bool | None], + diagnostic_logger: Callable[..., None] | None = None, ) -> None: self._connection_status = connection_status self._raw_create_publisher = raw_create_publisher @@ -67,10 +70,36 @@ def __init__( self._raw_destroy_publisher = raw_destroy_publisher self._raw_destroy_subscription = raw_destroy_subscription self._raw_destroy_client = raw_destroy_client + self._diagnostic_logger = diagnostic_logger self._entities: dict[int, ManagedEntity] = {} def debug_descriptors(self) -> tuple[ManagedCommDescriptor, ...]: - return tuple(wrapper.spec.descriptor() for wrapper in self._entities.values()) + return tuple( + wrapper.spec.descriptor( + bound=wrapper.get_underlying() is not None and not wrapper.destroyed + ) + for wrapper in self._entities.values() + ) + + def _log_diag(self, category: str, message: str, **fields) -> None: + if self._diagnostic_logger is None: + return + self._diagnostic_logger(category, message=message, **fields) + + def _wrapper_fields(self, wrapper: ManagedEntity) -> dict[str, object]: + descriptor = wrapper.spec.descriptor( + bound=wrapper.get_underlying() is not None and not wrapper.destroyed + ) + return { + "kind": descriptor.kind, + "scope": descriptor.scope, + "lifetime": descriptor.lifetime, + "owner_label": descriptor.owner_label, + "interface_name": descriptor.interface_name, + "name": descriptor.name, + "peers": descriptor.peers, + "bound": descriptor.bound, + } def register( self, @@ -100,7 +129,12 @@ def register( ) wrapper = self._make_wrapper(spec) self._entities[id(wrapper)] = wrapper - self._ensure_managed_underlying(wrapper) + self._log_diag( + "REGISTRY", + "registered managed comm entity", + **self._wrapper_fields(wrapper), + ) + self._ensure_managed_underlying(wrapper, reason="register") return wrapper def destroy_for_owner( @@ -114,11 +148,11 @@ def destroy_for_owner( continue if lifetime is not None and wrapper.spec.lifetime != lifetime: continue - self._destroy_managed_entity(wrapper) + self._destroy_managed_entity(wrapper, reason="destroy_for_owner") def destroy_publisher(self, publisher) -> bool: if isinstance(publisher, ManagedEntity) and publisher.spec.kind == "publisher": - return self._destroy_managed_entity(publisher) + return self._destroy_managed_entity(publisher, reason="destroy_publisher") return bool(self._raw_destroy_publisher(publisher)) def destroy_subscription(self, subscription) -> bool: @@ -126,32 +160,43 @@ def destroy_subscription(self, subscription) -> bool: isinstance(subscription, ManagedEntity) and subscription.spec.kind == "subscription" ): - return self._destroy_managed_entity(subscription) + return self._destroy_managed_entity( + subscription, reason="destroy_subscription" + ) return bool(self._raw_destroy_subscription(subscription)) def destroy_client(self, client) -> bool: if isinstance(client, ManagedEntity) and client.spec.kind == "client": - return self._destroy_managed_entity(client) + return self._destroy_managed_entity(client, reason="destroy_client") return bool(self._raw_destroy_client(client)) def on_peer_connection_change(self, peer_name: str, *, connected: bool) -> None: + affected = 0 for wrapper in list(self._entities.values()): if wrapper.destroyed or peer_name not in wrapper.spec.peers: continue + affected += 1 if wrapper.spec.scope == "peer": if connected: - self._ensure_managed_underlying(wrapper) + self._ensure_managed_underlying(wrapper, reason="peer_connected") else: - self._destroy_managed_underlying(wrapper) + self._destroy_managed_underlying(wrapper, reason="peer_disconnected") continue if connected and wrapper.spec.scope == "shared": # Hardware testing showed shared endpoints need explicit rebind # when a remote unit rejoins the network. - self._refresh_managed_underlying(wrapper) + self._refresh_managed_underlying(wrapper, reason="peer_reconnected") + self._log_diag( + "REGISTRY", + "processed peer connection change", + peer_name=peer_name, + connected=connected, + affected_entities=affected, + ) def close(self) -> None: for wrapper in list(self._entities.values()): - self._destroy_managed_entity(wrapper) + self._destroy_managed_entity(wrapper, reason="close") def _make_wrapper(self, spec: ManagedCommSpec) -> ManagedEntity: if spec.kind == "publisher": @@ -183,7 +228,9 @@ def _create_managed_underlying(self, wrapper: ManagedEntity): **spec.kwargs, ) - def _destroy_managed_underlying(self, wrapper: ManagedEntity) -> None: + def _destroy_managed_underlying( + self, wrapper: ManagedEntity, *, reason: str = "" + ) -> None: underlying = wrapper.detach() if underlying is None: return @@ -193,6 +240,12 @@ def _destroy_managed_underlying(self, wrapper: ManagedEntity) -> None: self._raw_destroy_subscription(underlying) else: self._raw_destroy_client(underlying) + self._log_diag( + "REGISTRY", + "destroyed underlying entity", + reason=reason, + **self._wrapper_fields(wrapper), + ) def _should_have_managed_underlying(self, wrapper: ManagedEntity) -> bool: if wrapper.destroyed: @@ -205,23 +258,55 @@ def _should_have_managed_underlying(self, wrapper: ManagedEntity) -> bool: for peer_name in wrapper.spec.peers ) - def _ensure_managed_underlying(self, wrapper: ManagedEntity) -> None: + def _ensure_managed_underlying( + self, wrapper: ManagedEntity, *, reason: str = "" + ) -> None: if not self._should_have_managed_underlying(wrapper): + self._log_diag( + "REGISTRY", + "deferred underlying entity creation", + reason=reason, + connection_status=dict(self._connection_status()), + **self._wrapper_fields(wrapper), + ) return if wrapper.get_underlying() is not None: return wrapper.attach(self._create_managed_underlying(wrapper)) + self._log_diag( + "REGISTRY", + "created underlying entity", + reason=reason, + **self._wrapper_fields(wrapper), + ) - def _refresh_managed_underlying(self, wrapper: ManagedEntity) -> None: + def _refresh_managed_underlying( + self, wrapper: ManagedEntity, *, reason: str = "" + ) -> None: if wrapper.destroyed: return - self._destroy_managed_underlying(wrapper) - self._ensure_managed_underlying(wrapper) + self._log_diag( + "REGISTRY", + "refreshing underlying entity", + reason=reason, + **self._wrapper_fields(wrapper), + ) + self._destroy_managed_underlying(wrapper, reason=reason) + self._ensure_managed_underlying(wrapper, reason=reason) - def _destroy_managed_entity(self, wrapper: ManagedEntity) -> bool: + def _destroy_managed_entity( + self, wrapper: ManagedEntity, *, reason: str = "" + ) -> bool: if id(wrapper) not in self._entities: return False - self._destroy_managed_underlying(wrapper) + self._destroy_managed_underlying(wrapper, reason=reason) + descriptor_fields = self._wrapper_fields(wrapper) wrapper.mark_destroyed() self._entities.pop(id(wrapper), None) + self._log_diag( + "REGISTRY", + "destroyed managed entity wrapper", + reason=reason, + **descriptor_fields, + ) return True diff --git a/controls/sae_2025_ws/src/uav/uav/runtime/payload_mission.py b/controls/sae_2025_ws/src/uav/uav/runtime/payload_mission.py index 0e9221262..16c4d5761 100644 --- a/controls/sae_2025_ws/src/uav/uav/runtime/payload_mission.py +++ b/controls/sae_2025_ws/src/uav/uav/runtime/payload_mission.py @@ -4,6 +4,7 @@ from rclpy.executors import ExternalShutdownException from rclpy.node import Node +from .comms_diagnostics import log_bootstrap_diagnostics from .PayloadModeManager import PayloadModeManager from .mission_spec import MissionSpec, mission_path_for_name @@ -65,6 +66,19 @@ def main(args=None) -> None: try: manager_kwargs = bootstrap.manager_kwargs() + mission_spec = manager_kwargs["mission_spec"] + log_bootstrap_diagnostics( + bootstrap.get_logger(), + runtime_kind="payload_mission", + vehicle_name=manager_kwargs["vehicle_name"], + mission_path=str(bootstrap.get_parameter("mode_map").value), + mission_target=mission_spec.target, + auto_launch=bool(manager_kwargs["auto_launch"]), + peer_heartbeat_hz=float(manager_kwargs["peer_heartbeat_hz"]), + peer_stale_timeout_s=float(manager_kwargs["peer_stale_timeout_s"]), + peer_vehicle_names=getattr(mission_spec, "peer_vehicle_names", ()), + vision_nodes=getattr(mission_spec, "vision_nodes", ()), + ) bootstrap.destroy_node() bootstrap = None mission_node = PayloadModeManager(**manager_kwargs) diff --git a/controls/sae_2025_ws/src/uav/uav/runtime/peer_connections.py b/controls/sae_2025_ws/src/uav/uav/runtime/peer_connections.py index ca09bc0d3..bc56ed3ad 100644 --- a/controls/sae_2025_ws/src/uav/uav/runtime/peer_connections.py +++ b/controls/sae_2025_ws/src/uav/uav/runtime/peer_connections.py @@ -1,7 +1,7 @@ from __future__ import annotations from collections.abc import Iterable -from typing import Callable, Mapping +from typing import Any, Callable, Mapping from std_msgs.msg import Empty @@ -65,6 +65,7 @@ def __init__( peer_stale_timeout_s: float = 0.5, now_seconds: Callable[[], float], on_connection_change: Callable[[str], None] | Callable[..., None], + diagnostic_logger: Callable[..., None] | None = None, raw_create_publisher: Callable[..., object], raw_create_subscription: Callable[..., object], raw_destroy_publisher: Callable[[object], bool | None], @@ -77,6 +78,7 @@ def __init__( self.peer_stale_timeout_s = float(peer_stale_timeout_s) self._now_seconds = now_seconds self._on_connection_change = on_connection_change + self._diagnostic_logger = diagnostic_logger self._raw_create_publisher = raw_create_publisher self._raw_create_subscription = raw_create_subscription self._raw_destroy_publisher = raw_destroy_publisher @@ -101,6 +103,13 @@ def __init__( 1.0 / self.peer_heartbeat_hz, self._peer_timer_callback, ) + self._log_diag( + "HEARTBEAT", + "initialized local heartbeat publisher", + local_topic=self.local_heartbeat_topic(), + peer_heartbeat_hz=self.peer_heartbeat_hz, + peer_stale_timeout_s=self.peer_stale_timeout_s, + ) @property def peer_names(self) -> tuple[str, ...]: @@ -109,9 +118,41 @@ def peer_names(self) -> tuple[str, ...]: def status(self) -> dict[str, bool]: return dict(self._connected) + def local_heartbeat_topic(self) -> str | None: + if not self._runtime_vehicle_name: + return None + return self._heartbeat_topic_for(self._runtime_vehicle_name) + def _heartbeat_topic_for(self, vehicle_name: str) -> str: return f"/{vehicle_name}/{HEARTBEAT_TOPIC_SUFFIX}" + def remote_heartbeat_topics(self) -> dict[str, str]: + return { + peer_name: self._heartbeat_topic_for(peer_name) for peer_name in self._peer_names + } + + def debug_snapshot(self) -> dict[str, Any]: + current_time = self._now_seconds() + return { + "local_heartbeat_topic": self.local_heartbeat_topic(), + "remote_heartbeat_topics": self.remote_heartbeat_topics(), + "peer_vehicle_names": self.peer_names, + "connection_status": self.status(), + "last_seen_age_s": { + peer_name: ( + None + if last_seen is None + else max(0.0, current_time - float(last_seen)) + ) + for peer_name, last_seen in sorted(self._last_seen.items()) + }, + } + + def _log_diag(self, category: str, message: str, **fields) -> None: + if self._diagnostic_logger is None: + return + self._diagnostic_logger(category, message=message, **fields) + def configure( self, peer_vehicle_names: tuple[str, ...] | list[str] | set[str] ) -> None: @@ -145,12 +186,26 @@ def configure( ) self._peer_names = tuple(sorted(next_peer_names)) + self._log_diag( + "HEARTBEAT", + "configured peer heartbeat subscriptions", + local_topic=self.local_heartbeat_topic(), + peer_vehicle_names=self._peer_names, + remote_heartbeat_topics=self.remote_heartbeat_topics(), + removed_peer_vehicle_names=tuple(sorted(current_peer_names - next_peer_names)), + ) def _peer_heartbeat_callback(self, peer_name: str) -> None: self._last_seen[peer_name] = self._now_seconds() if self._connected.get(peer_name, False): return self._connected[peer_name] = True + self._log_diag( + "HEARTBEAT", + "observed first heartbeat from peer", + peer_name=peer_name, + topic=self._heartbeat_topic_for(peer_name), + ) self._on_connection_change(peer_name, connected=True) def _peer_timer_callback(self) -> None: @@ -167,6 +222,14 @@ def _peer_timer_callback(self) -> None: if not self._connected.get(peer_name, False): continue self._connected[peer_name] = False + self._log_diag( + "HEARTBEAT", + "peer heartbeat went stale", + peer_name=peer_name, + topic=self._heartbeat_topic_for(peer_name), + seconds_since_last_seen=max(0.0, current_time - float(last_seen)), + peer_stale_timeout_s=self.peer_stale_timeout_s, + ) self._on_connection_change(peer_name, connected=False) def close(self) -> None: diff --git a/controls/sae_2025_ws/src/uav/uav/runtime/uav_mission.py b/controls/sae_2025_ws/src/uav/uav/runtime/uav_mission.py index c64f7d4c6..839582581 100644 --- a/controls/sae_2025_ws/src/uav/uav/runtime/uav_mission.py +++ b/controls/sae_2025_ws/src/uav/uav/runtime/uav_mission.py @@ -5,6 +5,7 @@ from rclpy.node import Node from uav.vehicles.AirframeClass import AirframeClass +from .comms_diagnostics import log_bootstrap_diagnostics from .UAVModeManager import UAVModeManager from .mission_spec import MissionSpec, mission_path_for_name @@ -80,6 +81,19 @@ def main(args=None) -> None: try: manager_kwargs = bootstrap.manager_kwargs() + mission_spec = manager_kwargs["mission_spec"] + log_bootstrap_diagnostics( + bootstrap.get_logger(), + runtime_kind="uav_mission", + vehicle_name=manager_kwargs["vehicle_name"], + mission_path=str(bootstrap.get_parameter("mode_map").value), + mission_target=mission_spec.target, + auto_launch=bool(manager_kwargs["auto_launch"]), + peer_heartbeat_hz=float(manager_kwargs["peer_heartbeat_hz"]), + peer_stale_timeout_s=float(manager_kwargs["peer_stale_timeout_s"]), + peer_vehicle_names=getattr(mission_spec, "peer_vehicle_names", ()), + vision_nodes=getattr(mission_spec, "vision_nodes", ()), + ) bootstrap.destroy_node() bootstrap = None mission_node = UAVModeManager(**manager_kwargs)