diff --git a/src/k1link/simulation/worker_agent.py b/src/k1link/simulation/worker_agent.py index c699bf4..eba5a9a 100644 --- a/src/k1link/simulation/worker_agent.py +++ b/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: diff --git a/tests/test_simulation_worker_agent_command.py b/tests/test_simulation_worker_agent_command.py index 08e845f..a9be998 100644 --- a/tests/test_simulation_worker_agent_command.py +++ b/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: