diff --git a/src/k1link/simulation/px4_rover_control.py b/src/k1link/simulation/px4_rover_control.py index c03ccae..748093b 100644 --- a/src/k1link/simulation/px4_rover_control.py +++ b/src/k1link/simulation/px4_rover_control.py @@ -12,7 +12,7 @@ from k1link.simulation.contracts import AckermannControlSetpoint OFFBOARD_HEARTBEAT_SECONDS: Final = 0.1 OFFBOARD_WARMUP_HEARTBEATS: Final = 12 -OFFBOARD_ADMISSION_TIMEOUT_SECONDS: Final = 12.0 +OFFBOARD_ADMISSION_TIMEOUT_SECONDS: Final = 25.0 VEHICLE_COMMAND_DO_SET_MODE: Final = 176 VEHICLE_COMMAND_COMPONENT_ARM_DISARM: Final = 400 diff --git a/src/k1link/simulation/run_store.py b/src/k1link/simulation/run_store.py index aafafbb..457f15a 100644 --- a/src/k1link/simulation/run_store.py +++ b/src/k1link/simulation/run_store.py @@ -229,19 +229,7 @@ class QualificationRunStore: self._require_writable() with self._lock: run = self.load(command.run_id) - if run.kind not in COMMANDABLE_RUN_KINDS: - raise QualificationRunTransitionError( - f"{run.kind.value} cannot causally accept control commands" - ) - if run.state is not RunState.RUNNING: - raise QualificationRunTransitionError( - "control commands are accepted only while the run is running" - ) - if command.authority_generation != run.authority.generation: - raise QualificationRunConflictError("command authority generation is stale") - ttl_ns = command.valid_until_sim_ns - command.issued_at_sim_ns - if ttl_ns > run.authority.command_ttl_max_ns: - raise QualificationRunTransitionError("command TTL exceeds the authority limit") + _validate_command_admission(command, run) directory = self._existing_run_path(command.run_id) / "commands" expected_sequence = _next_sequence(directory) if command.sequence != expected_sequence: @@ -252,6 +240,31 @@ class QualificationRunStore: ) return command + def submit_admitted_command( + self, + command: ControlSetpoint, + *, + admitted_run: QualificationRun, + ) -> ControlSetpoint: + """Fsync one command using the worker's single-writer admitted run snapshot.""" + + self._require_writable() + with self._lock: + if admitted_run.run_id != command.run_id: + raise QualificationRunConflictError("command belongs to another admitted run") + _validate_command_admission(command, admitted_run) + directory = self._existing_run_path(command.run_id) / "commands" + if command.sequence < 1: + raise QualificationRunConflictError("command sequence must be positive") + previous = directory / _sequence_name(command.sequence - 1) + if command.sequence > 1 and not previous.is_file(): + raise QualificationRunConflictError("command sequence would create a journal gap") + _append_json_exclusive( + directory / _sequence_name(command.sequence), + command.to_dict(), + ) + return command + def list_commands(self, run_id: str) -> tuple[ControlSetpoint, ...]: path = self._existing_run_path(run_id) with self._lock: @@ -536,6 +549,25 @@ def _next_sequence(directory: Path) -> int: return len(_read_sequence_directory(directory, "journal")) + 1 +def _validate_command_admission( + command: ControlSetpoint, + run: QualificationRun, +) -> None: + if run.kind not in COMMANDABLE_RUN_KINDS: + raise QualificationRunTransitionError( + f"{run.kind.value} cannot causally accept control commands" + ) + if run.state is not RunState.RUNNING: + raise QualificationRunTransitionError( + "control commands are accepted only while the run is running" + ) + if command.authority_generation != run.authority.generation: + raise QualificationRunConflictError("command authority generation is stale") + ttl_ns = command.valid_until_sim_ns - command.issued_at_sim_ns + if ttl_ns > run.authority.command_ttl_max_ns: + raise QualificationRunTransitionError("command TTL exceeds the authority limit") + + def _sequence_name(sequence: int) -> str: return f"{sequence:0{SEQUENCE_WIDTH}d}.json" diff --git a/src/k1link/simulation/worker_agent.py b/src/k1link/simulation/worker_agent.py index eba5a9a..2e4d81a 100644 --- a/src/k1link/simulation/worker_agent.py +++ b/src/k1link/simulation/worker_agent.py @@ -250,6 +250,10 @@ class SimulationWorkerAgent: observed_at_utc=_utc_now(), host_monotonic_ns=time.monotonic_ns(), ) + active = tuple(run for run in self.store.list_runs() if run.state in ACTIVE_STATES) + if len(active) > 1: + raise WorkerAgentError("multiple active qualification runs violate worker authority") + self._active_run_cache = active[0] if active else None def serve_forever(self) -> None: self.socket_path.parent.mkdir(mode=0o700, parents=True, exist_ok=True) @@ -300,6 +304,7 @@ class SimulationWorkerAgent: terminal_state=RunState.ABORTED, reason="worker-agent-shutdown", ) + self._active_run_cache = None except Exception: self.supervisor.stop() @@ -417,6 +422,7 @@ class SimulationWorkerAgent: ) if running.state is not RunState.RUNNING: raise WorkerAgentError(f"provider start ended in terminal state {running.state.value}") + self._active_run_cache = running try: self.command_controller.start(run_id) self.collector.collect(run_id) @@ -433,8 +439,14 @@ class SimulationWorkerAgent: terminal_state=RunState.ABORTED, reason="px4-command-controller-admission-failed", ) + self._active_run_cache = None finally: raise WorkerAgentError("PX4 command controller admission failed") from exc + self._command_history[run_id] = { + command.command_id: command + for command in self.store.list_commands(run_id) + if isinstance(command, AckermannControlSetpoint) + } return self.status() def stop(self, *, run_id: str, idempotency_key: str) -> dict[str, Any]: @@ -472,6 +484,7 @@ class SimulationWorkerAgent: ) from command_stop_error if terminal.state is not RunState.COMPLETED: raise WorkerAgentError(f"provider stop ended in terminal state {terminal.state.value}") + self._active_run_cache = None return self.status() def command( @@ -525,7 +538,7 @@ class SimulationWorkerAgent: steering_normalized=steering_normalized, source="mission-core-control-station", ) - self.store.submit_command(command) + self.store.submit_admitted_command(command, admitted_run=active) history[command.command_id] = command try: snapshot = self.command_controller.submit(command) @@ -624,10 +637,15 @@ class SimulationWorkerAgent: ) def _active_run(self) -> QualificationRun | None: + cached: QualificationRun | None = getattr(self, "_active_run_cache", None) + if cached is not None: + return cached active = tuple(run for run in self.store.list_runs() if run.state in ACTIVE_STATES) if len(active) > 1: raise WorkerAgentError("multiple active qualification runs violate worker authority") - return active[0] if active else None + result = active[0] if active else None + self._active_run_cache = result + return result def _preflight(self) -> None: required = (self.paths.px4_root, self.paths.agent_binary, self.paths.scenario_path)