perf(simulation): keep rover command path within TTL

src/k1link/simulation/worker_agent.py

@@ -70,6 +70,7 @@ PROVIDER_IDS: Final = (
 )
 ACTIVE_STATES: Final = frozenset(ACTIVE_RECOVERY_STATES)
 POSE_TOPIC: Final = "/world/rover/dynamic_pose/info"
+SIM_CLOCK_CACHE_MAX_AGE_NS: Final = 2_000_000_000
 
 
 class WorkerAgentError(RuntimeError):
@@ -94,6 +95,8 @@ class GazeboPoseCollector:
         self.environment = dict(environment)
         self._sequence = 0
         self._lock = threading.Lock()
+        self._latest_sim_time_ns: int | None = None
+        self._latest_host_monotonic_ns: int | None = None
 
     def collect(self, run_id: str) -> dict[str, Any]:
         try:
@@ -129,15 +132,18 @@ class GazeboPoseCollector:
             stamp.get("nsec"),
             "stamp.nsec",
         )
+        host_monotonic_ns = time.monotonic_ns()
         with self._lock:
             self._sequence += 1
             sequence = self._sequence
+            self._latest_sim_time_ns = sim_time_ns
+            self._latest_host_monotonic_ns = host_monotonic_ns
         return {
             "schema_version": VEHICLE_STATE_SCHEMA,
             "run_id": run_id,
             "sequence": sequence,
             "observed_at_utc": _utc_now(),
-            "host_monotonic_ns": time.monotonic_ns(),
+            "host_monotonic_ns": host_monotonic_ns,
             "sim_time_ns": sim_time_ns,
             "frame_id": "map_enu",
             "child_frame_id": "base_link_flu",
@@ -179,6 +185,20 @@ class GazeboPoseCollector:
             },
         }
 
+    def estimate_sim_time_ns(self) -> int:
+        """Return a bounded speed-factor-1 estimate anchored to a Gazebo sample."""
+
+        now_monotonic_ns = time.monotonic_ns()
+        with self._lock:
+            sim_time_ns = self._latest_sim_time_ns
+            sampled_at_ns = self._latest_host_monotonic_ns
+        if sim_time_ns is None or sampled_at_ns is None:
+            raise WorkerAgentError("Gazebo simulation clock has not been admitted")
+        age_ns = now_monotonic_ns - sampled_at_ns
+        if not 0 <= age_ns <= SIM_CLOCK_CACHE_MAX_AGE_NS:
+            raise WorkerAgentError("Gazebo simulation clock sample is stale")
+        return sim_time_ns + age_ns
+
 
 class SimulationWorkerAgent:
     """One loopback-only worker process owning provider lifecycle and live state."""
@@ -222,6 +242,7 @@ class SimulationWorkerAgent:
         self.collector = GazeboPoseCollector(stock_rover_process_environment(self.paths))
         self.command_controller = Ros2Px4AckermannControl()
         self._command_lock = threading.RLock()
+        self._command_history: dict[str, dict[str, AckermannControlSetpoint]] = {}
         self._shutdown = threading.Event()
         self._server_socket: socket.socket | None = None
         self._connection_threads: set[threading.Thread] = set()
@@ -398,7 +419,10 @@ class SimulationWorkerAgent:
             raise WorkerAgentError(f"provider start ended in terminal state {running.state.value}")
         try:
             self.command_controller.start(run_id)
+            self.collector.collect(run_id)
         except Exception as exc:
+            with suppress(Exception):
+                self.command_controller.stop(run_id)
             try:
                 self.service.stop(
                     run_id,
@@ -478,10 +502,8 @@ class SimulationWorkerAgent:
         if active is None or active.run_id != run_id or active.state is not RunState.RUNNING:
             raise WorkerAgentError("requested run does not own running command authority")
         command_id = f"cmd-{hashlib.sha256(idempotency_key.encode()).hexdigest()[:24]}"
-        existing = next(
-            (item for item in self.store.list_commands(run_id) if item.command_id == command_id),
-            None,
-        )
+        history = self._command_history_for(run_id)
+        existing = history.get(command_id)
         if existing is not None:
             if not isinstance(existing, AckermannControlSetpoint) or (
                 existing.speed_mps != speed_mps
@@ -490,13 +512,12 @@ class SimulationWorkerAgent:
                 raise WorkerAgentError("command idempotency key is bound to another setpoint")
             snapshot = self.command_controller.snapshot(run_id)
             return _command_acceptance(existing, snapshot)
-        sample = self.collector.collect(run_id)
-        issued_at_sim_ns = int(sample["sim_time_ns"])
+        issued_at_sim_ns = self.collector.estimate_sim_time_ns()
         ttl_ns = min(active.authority.command_ttl_max_ns, 250_000_000)
         command = AckermannControlSetpoint(
             run_id=run_id,
             command_id=command_id,
-            sequence=len(self.store.list_commands(run_id)) + 1,
+            sequence=len(history) + 1,
             issued_at_sim_ns=issued_at_sim_ns,
             valid_until_sim_ns=issued_at_sim_ns + ttl_ns,
             authority_generation=active.authority.generation,
@@ -505,6 +526,7 @@ class SimulationWorkerAgent:
             source="mission-core-control-station",
         )
         self.store.submit_command(command)
+        history[command.command_id] = command
         try:
             snapshot = self.command_controller.submit(command)
         except Exception as exc:
@@ -525,6 +547,27 @@ class SimulationWorkerAgent:
             raise WorkerAgentError("PX4 rejected the canonical rover setpoint") from exc
         return _command_acceptance(command, snapshot)
 
+    def _command_history_for(
+        self,
+        run_id: str,
+    ) -> dict[str, AckermannControlSetpoint]:
+        histories = getattr(self, "_command_history", None)
+        if histories is None:
+            histories = {}
+            self._command_history = histories
+        history = histories.get(run_id)
+        if history is None:
+            commands = self.store.list_commands(run_id)
+            history = {
+                command.command_id: command
+                for command in commands
+                if isinstance(command, AckermannControlSetpoint)
+            }
+            if len(history) != len(commands):
+                raise WorkerAgentError("running command history contains another command contract")
+            histories[run_id] = history
+        return history
+
     def _handle_connection(self, connection: socket.socket) -> None:
         request_id: str | None = None
         try:

tests/test_simulation_worker_agent_command.py

@@ -27,6 +27,9 @@ class _Collector:
     def collect(self, run_id: str) -> dict[str, object]:
         return {"run_id": run_id, "sim_time_ns": 1_000_000_000}
 
+    def estimate_sim_time_ns(self) -> int:
+        return 1_000_000_000
+
 
 class _Controller:
     def __init__(self) -> None: