SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(simulation): drive stock rover through PX4 offboard

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-24 21:47:40 +03:00
parent 53fd1e3bfc
commit 101707d54c
12 changed files with 1329 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/simulation/px4_rover_control.py
+368
View File
@@ -0,0 +1,368 @@
from __future__ import annotations
import importlib
import math
import threading
import time
from contextlib import suppress
from dataclasses import dataclass
from typing import Any, Final
from k1link.simulation.contracts import AckermannControlSetpoint
OFFBOARD_HEARTBEAT_SECONDS: Final = 0.1
OFFBOARD_WARMUP_HEARTBEATS: Final = 12
OFFBOARD_ADMISSION_TIMEOUT_SECONDS: Final = 12.0
VEHICLE_COMMAND_DO_SET_MODE: Final = 176
VEHICLE_COMMAND_COMPONENT_ARM_DISARM: Final = 400
class Px4RoverControlError(RuntimeError):
"""The virtual-only PX4 rover command boundary failed closed."""
@dataclass(frozen=True, slots=True)
class Px4RoverControlSnapshot:
run_id: str
armed: bool
offboard: bool
ttl_expired_count: int
class Ros2Px4AckermannControl:
"""Publish canonical speed+steering through the PX4 ROS 2 offboard boundary."""
def __init__(
self,
*,
heartbeat_seconds: float = OFFBOARD_HEARTBEAT_SECONDS,
admission_timeout_seconds: float = OFFBOARD_ADMISSION_TIMEOUT_SECONDS,
) -> None:
if not 0.05 <= heartbeat_seconds <= 0.25:
raise ValueError("PX4 heartbeat period must remain within 4..20 Hz")
if admission_timeout_seconds <= 0:
raise ValueError("PX4 admission timeout must be positive")
self.heartbeat_seconds = heartbeat_seconds
self.admission_timeout_seconds = admission_timeout_seconds
self._lock = threading.RLock()
self._started = threading.Event()
self._control_ready = threading.Event()
self._disarmed = threading.Event()
self._stop = threading.Event()
self._thread: threading.Thread | None = None
self._run_id: str | None = None
self._latest_speed_mps = 0.0
self._latest_steering_normalized = 0.0
self._valid_until_monotonic_ns = 0
self._ttl_expired_count = 0
self._ttl_expired_for_latest = False
self._armed = False
self._offboard = False
self._disarm_requested = False
self._failure: BaseException | None = None
def start(self, run_id: str) -> Px4RoverControlSnapshot:
with self._lock:
if self._thread is not None:
raise Px4RoverControlError("PX4 command controller already owns a run")
self._run_id = run_id
self._latest_speed_mps = 0.0
self._latest_steering_normalized = 0.0
self._valid_until_monotonic_ns = 0
self._ttl_expired_for_latest = False
self._failure = None
self._stop.clear()
self._started.clear()
self._control_ready.clear()
self._disarmed.clear()
self._disarm_requested = False
self._thread = threading.Thread(
target=self._run,
name=f"px4-rover-control:{run_id}",
daemon=True,
)
self._thread.start()
if not self._started.wait(timeout=3):
self._abort_thread()
raise Px4RoverControlError("PX4 ROS 2 command publisher did not start")
self._raise_failure()
if not self._control_ready.wait(timeout=self.admission_timeout_seconds):
self._abort_thread()
self._raise_failure()
raise Px4RoverControlError("PX4 did not confirm armed Offboard rover control")
self._raise_failure()
return self.snapshot(run_id)
def submit(self, command: AckermannControlSetpoint) -> Px4RoverControlSnapshot:
with self._lock:
if self._run_id != command.run_id or self._thread is None:
raise Px4RoverControlError("PX4 command belongs to another or inactive run")
self._raise_failure_locked()
ttl_ns = command.valid_until_sim_ns - command.issued_at_sim_ns
self._latest_speed_mps = command.speed_mps
self._latest_steering_normalized = command.steering_normalized
self._valid_until_monotonic_ns = time.monotonic_ns() + ttl_ns
self._ttl_expired_for_latest = False
return self.snapshot(command.run_id)
def stop(self, run_id: str) -> Px4RoverControlSnapshot:
with self._lock:
if self._run_id != run_id or self._thread is None:
raise Px4RoverControlError("PX4 stop belongs to another or inactive run")
self._latest_speed_mps = 0.0
self._latest_steering_normalized = 0.0
self._valid_until_monotonic_ns = 0
self._disarm_requested = True
self._disarmed.clear()
self._disarmed.wait(timeout=2)
snapshot = self.snapshot(run_id)
self._stop.set()
thread = self._thread
thread.join(timeout=3)
if thread.is_alive():
raise Px4RoverControlError("PX4 command publisher did not stop")
self._raise_failure()
with self._lock:
self._thread = None
self._run_id = None
return snapshot
def snapshot(self, run_id: str) -> Px4RoverControlSnapshot:
with self._lock:
if self._run_id != run_id:
raise Px4RoverControlError("PX4 snapshot belongs to another run")
return Px4RoverControlSnapshot(
run_id=run_id,
armed=self._armed,
offboard=self._offboard,
ttl_expired_count=self._ttl_expired_count,
)
def _abort_thread(self) -> None:
self._stop.set()
thread = self._thread
if thread is not None:
thread.join(timeout=3)
with self._lock:
self._thread = None
self._run_id = None
def _raise_failure(self) -> None:
with self._lock:
self._raise_failure_locked()
def _raise_failure_locked(self) -> None:
if self._failure is not None:
raise Px4RoverControlError(
f"PX4 ROS 2 command publisher failed: {type(self._failure).__name__}"
) from self._failure
def _run(self) -> None:
rclpy: Any | None = None
node: Any | None = None
try:
rclpy = importlib.import_module("rclpy")
qos_module = importlib.import_module("rclpy.qos")
messages = importlib.import_module("px4_msgs.msg")
rclpy.init(args=None)
node = rclpy.create_node("missioncore_px4_ackermann_control")
qos = qos_module.QoSProfile(
reliability=qos_module.ReliabilityPolicy.BEST_EFFORT,
durability=qos_module.DurabilityPolicy.TRANSIENT_LOCAL,
history=qos_module.HistoryPolicy.KEEP_LAST,
depth=1,
)
offboard_publisher = node.create_publisher(
messages.OffboardControlMode,
"/fmu/in/offboard_control_mode",
qos,
)
speed_publisher = node.create_publisher(
messages.RoverSpeedSetpoint,
"/fmu/in/rover_speed_setpoint",
qos,
)
steering_publisher = node.create_publisher(
messages.RoverSteeringSetpoint,
"/fmu/in/rover_steering_setpoint",
qos,
)
attitude_publisher = node.create_publisher(
messages.RoverAttitudeSetpoint,
"/fmu/in/rover_attitude_setpoint",
qos,
)
rate_publisher = node.create_publisher(
messages.RoverRateSetpoint,
"/fmu/in/rover_rate_setpoint",
qos,
)
command_publisher = node.create_publisher(
messages.VehicleCommand,
"/fmu/in/vehicle_command",
qos,
)
def receive_status(status: Any) -> None:
with self._lock:
self._armed = status.arming_state == messages.VehicleStatus.ARMING_STATE_ARMED
self._offboard = (
status.nav_state == messages.VehicleStatus.NAVIGATION_STATE_OFFBOARD
)
if self._armed and self._offboard:
self._control_ready.set()
node.create_subscription(
messages.VehicleStatus,
"/fmu/out/vehicle_status_v1",
receive_status,
qos,
)
self._started.set()
heartbeat_count = 0
last_mode_request_monotonic = 0.0
while not self._stop.is_set():
now_monotonic = time.monotonic()
now_monotonic_ns = time.monotonic_ns()
rclpy.spin_once(node, timeout_sec=0)
with self._lock:
expired = (
self._valid_until_monotonic_ns > 0
and now_monotonic_ns >= self._valid_until_monotonic_ns
)
if expired:
self._latest_speed_mps = 0.0
self._latest_steering_normalized = 0.0
self._valid_until_monotonic_ns = 0
if not self._ttl_expired_for_latest:
self._ttl_expired_count += 1
self._ttl_expired_for_latest = True
speed_mps = self._latest_speed_mps
steering_normalized = self._latest_steering_normalized
disarm_requested = self._disarm_requested
timestamp_us = int(node.get_clock().now().nanoseconds // 1_000)
self._publish_setpoint(
messages,
offboard_publisher,
speed_publisher,
steering_publisher,
attitude_publisher,
rate_publisher,
timestamp_us,
speed_mps,
steering_normalized,
)
heartbeat_count += 1
if (
heartbeat_count >= OFFBOARD_WARMUP_HEARTBEATS
and not self._control_ready.is_set()
and now_monotonic - last_mode_request_monotonic >= 1.0
):
self._publish_vehicle_command(
messages,
command_publisher,
timestamp_us,
VEHICLE_COMMAND_DO_SET_MODE,
param1=1.0,
param2=6.0,
)
self._publish_vehicle_command(
messages,
command_publisher,
timestamp_us,
VEHICLE_COMMAND_COMPONENT_ARM_DISARM,
param1=1.0,
)
last_mode_request_monotonic = now_monotonic
if disarm_requested:
self._publish_vehicle_command(
messages,
command_publisher,
timestamp_us,
VEHICLE_COMMAND_COMPONENT_ARM_DISARM,
param1=0.0,
)
with self._lock:
self._armed = False
self._offboard = False
self._disarm_requested = False
self._disarmed.set()
self._stop.wait(self.heartbeat_seconds)
except BaseException as exc:
with self._lock:
self._failure = exc
self._started.set()
self._control_ready.set()
self._disarmed.set()
finally:
if node is not None:
node.destroy_node()
if rclpy is not None:
with suppress(Exception):
rclpy.shutdown()
@staticmethod
def _publish_setpoint(
messages: Any,
offboard_publisher: Any,
speed_publisher: Any,
steering_publisher: Any,
attitude_publisher: Any,
rate_publisher: Any,
timestamp_us: int,
speed_mps: float,
steering_normalized: float,
) -> None:
offboard = messages.OffboardControlMode()
offboard.timestamp = timestamp_us
offboard.position = False
offboard.velocity = True
offboard.acceleration = False
offboard.attitude = False
offboard.body_rate = False
offboard.thrust_and_torque = False
offboard.direct_actuator = False
offboard_publisher.publish(offboard)
speed = messages.RoverSpeedSetpoint()
speed.timestamp = timestamp_us
speed.speed_body_x = float(speed_mps)
speed.speed_body_y = math.nan
speed_publisher.publish(speed)
steering = messages.RoverSteeringSetpoint()
steering.timestamp = timestamp_us
steering.normalized_steering_setpoint = float(steering_normalized)
steering_publisher.publish(steering)
attitude = messages.RoverAttitudeSetpoint()
attitude.timestamp = timestamp_us
attitude.yaw_setpoint = math.nan
attitude_publisher.publish(attitude)
rate = messages.RoverRateSetpoint()
rate.timestamp = timestamp_us
rate.yaw_rate_setpoint = math.nan
rate_publisher.publish(rate)
@staticmethod
def _publish_vehicle_command(
messages: Any,
publisher: Any,
timestamp_us: int,
command: int,
*,
param1: float,
param2: float = 0.0,
) -> None:
message = messages.VehicleCommand()
message.timestamp = timestamp_us
message.param1 = param1
message.param2 = param2
message.command = command
message.target_system = 1
message.target_component = 1
message.source_system = 1
message.source_component = 1
message.from_external = True
publisher.publish(message)
src/k1link/simulation/worker_agent.py
+173 -6
View File
@@ -11,12 +11,14 @@ import subprocess
import sys
import threading
import time
from contextlib import suppress
from datetime import UTC, datetime
from pathlib import Path
from types import FrameType
from typing import Any, Final
from k1link.simulation.contracts import (
AckermannControlSetpoint,
AuthorityProfile,
ProviderPin,
QualificationRun,
@@ -26,6 +28,10 @@ from k1link.simulation.contracts import (
)
from k1link.simulation.orchestrator import SimulationApplicationService
from k1link.simulation.process_supervisor import PosixProcessSupervisor
from k1link.simulation.px4_rover_control import (
Px4RoverControlSnapshot,
Ros2Px4AckermannControl,
)
from k1link.simulation.run_store import (
ACTIVE_RECOVERY_STATES,
QualificationRunNotFoundError,
@@ -44,6 +50,7 @@ from k1link.simulation.worker import (
SimulationWorldControl,
)
from k1link.simulation.worker_gateway import (
COMMAND_ACCEPTANCE_SCHEMA,
MAX_MESSAGE_BYTES,
OPERATIONS,
REQUEST_SCHEMA,
@@ -213,6 +220,8 @@ class SimulationWorkerAgent:
)
self.service = SimulationApplicationService(self.store, self.worker)
self.collector = GazeboPoseCollector(stock_rover_process_environment(self.paths))
self.command_controller = Ros2Px4AckermannControl()
self._command_lock = threading.RLock()
self._shutdown = threading.Event()
self._server_socket: socket.socket | None = None
self._connection_threads: set[threading.Thread] = set()
@@ -258,6 +267,8 @@ class SimulationWorkerAgent:
self._shutdown.set()
active = self._active_run()
if active is not None:
with suppress(Exception):
self.command_controller.stop(active.run_id)
try:
self.service.stop(
active.run_id,
@@ -309,6 +320,23 @@ class SimulationWorkerAgent:
mission_core_commit=_string(payload["mission_core_commit"], "commit", 40),
idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160),
)
if operation == "command":
_exact_keys(
payload,
{"run_id", "speed_mps", "steering_normalized", "idempotency_key"},
"command payload",
)
return self.command(
run_id=_safe_id(payload["run_id"], "run id"),
speed_mps=_bounded_number(payload["speed_mps"], "speed", -1.5, 1.5),
steering_normalized=_bounded_number(
payload["steering_normalized"],
"steering",
-1.0,
1.0,
),
idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160),
)
_exact_keys(payload, {"run_id", "idempotency_key"}, "stop payload")
return self.stop(
run_id=_safe_id(payload["run_id"], "run id"),
@@ -367,9 +395,22 @@ class SimulationWorkerAgent:
host_monotonic_ns=time.monotonic_ns(),
)
if running.state is not RunState.RUNNING:
raise WorkerAgentError(
f"provider start ended in terminal state {running.state.value}"
)
raise WorkerAgentError(f"provider start ended in terminal state {running.state.value}")
try:
self.command_controller.start(run_id)
except Exception as exc:
try:
self.service.stop(
run_id,
idempotency_key=f"{run_id}:command-controller-admission-failed",
observed_at_utc=_utc_now(),
host_monotonic_ns=time.monotonic_ns(),
sim_time_ns=None,
terminal_state=RunState.ABORTED,
reason="px4-command-controller-admission-failed",
)
finally:
raise WorkerAgentError("PX4 command controller admission failed") from exc
return self.status()
def stop(self, *, run_id: str, idempotency_key: str) -> dict[str, Any]:
@@ -381,19 +422,109 @@ class SimulationWorkerAgent:
sim_time_ns = int(self.collector.collect(run_id)["sim_time_ns"])
except Exception:
sim_time_ns = None
command_stop_error: Exception | None = None
try:
self.command_controller.stop(run_id)
except Exception as exc:
command_stop_error = exc
terminal = self.service.stop(
run_id,
idempotency_key=idempotency_key,
observed_at_utc=_utc_now(),
host_monotonic_ns=time.monotonic_ns(),
sim_time_ns=sim_time_ns,
terminal_state=(
RunState.ABORTED if command_stop_error is not None else RunState.COMPLETED
),
reason=(
"px4-command-controller-stop-failed"
if command_stop_error is not None
else "operator-stop-clean"
),
)
if terminal.state is not RunState.COMPLETED:
if command_stop_error is not None:
raise WorkerAgentError(
f"provider stop ended in terminal state {terminal.state.value}"
)
"PX4 command controller did not stop cleanly"
) from command_stop_error
if terminal.state is not RunState.COMPLETED:
raise WorkerAgentError(f"provider stop ended in terminal state {terminal.state.value}")
return self.status()
def command(
self,
*,
run_id: str,
speed_mps: float,
steering_normalized: float,
idempotency_key: str,
) -> dict[str, Any]:
with self._command_lock:
return self._command_locked(
run_id=run_id,
speed_mps=speed_mps,
steering_normalized=steering_normalized,
idempotency_key=idempotency_key,
)
def _command_locked(
self,
*,
run_id: str,
speed_mps: float,
steering_normalized: float,
idempotency_key: str,
) -> dict[str, Any]:
active = self._active_run()
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,
)
if existing is not None:
if not isinstance(existing, AckermannControlSetpoint) or (
existing.speed_mps != speed_mps
or existing.steering_normalized != steering_normalized
):
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"])
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,
issued_at_sim_ns=issued_at_sim_ns,
valid_until_sim_ns=issued_at_sim_ns + ttl_ns,
authority_generation=active.authority.generation,
speed_mps=speed_mps,
steering_normalized=steering_normalized,
source="mission-core-control-station",
)
self.store.submit_command(command)
try:
snapshot = self.command_controller.submit(command)
except Exception as exc:
current = self.store.load(run_id)
self.store.append_event(
run_id,
event_type="command.delivery-failed",
observed_at_utc=_utc_now(),
host_monotonic_ns=time.monotonic_ns(),
sim_time_ns=issued_at_sim_ns,
payload={
"command_id": command.command_id,
"provider": "px4-ros2-offboard",
"detail": type(exc).__name__,
},
expected_revision=current.revision,
)
raise WorkerAgentError("PX4 rejected the canonical rover setpoint") from exc
return _command_acceptance(command, snapshot)
def _handle_connection(self, connection: socket.socket) -> None:
request_id: str | None = None
try:
@@ -561,6 +692,18 @@ def _number(value: object, label: str) -> float:
return result
def _bounded_number(
value: object,
label: str,
minimum: float,
maximum: float,
) -> float:
result = _number(value, label)
if not minimum <= result <= maximum:
raise WorkerAgentError(f"{label} is outside the admitted range")
return result
def _integer(value: object, label: str) -> int:
if isinstance(value, bool) or not isinstance(value, int | str):
raise WorkerAgentError(f"{label} must be an integer")
@@ -592,6 +735,30 @@ def _safe_error(exc: Exception) -> str:
return f"{type(exc).__name__}: worker operation failed"
def _command_acceptance(
command: AckermannControlSetpoint,
snapshot: Px4RoverControlSnapshot,
) -> dict[str, Any]:
return {
"schema_version": COMMAND_ACCEPTANCE_SCHEMA,
"run_id": command.run_id,
"command_id": command.command_id,
"sequence": command.sequence,
"issued_at_sim_ns": command.issued_at_sim_ns,
"valid_until_sim_ns": command.valid_until_sim_ns,
"speed_mps": command.speed_mps,
"steering_normalized": command.steering_normalized,
"authority_scope": command.authority_scope.value,
"delivery": {
"provider": "px4-ros2-offboard",
"mode": "speed-steering",
"armed": snapshot.armed,
"offboard": snapshot.offboard,
"ttl_expired_count": snapshot.ttl_expired_count,
},
}
def main() -> int:
parser = argparse.ArgumentParser(description="Serve the Mission Core Simulation Worker agent.")
parser.add_argument("--socket", type=Path, required=True)
src/k1link/simulation/worker_gateway.py
+85 -1
View File
@@ -1,6 +1,7 @@
from __future__ import annotations
import json
import math
import socket
from collections.abc import Mapping
from pathlib import Path
@@ -11,8 +12,9 @@ REQUEST_SCHEMA: Final = "missioncore.simulation-worker-request/v1"
RESPONSE_SCHEMA: Final = "missioncore.simulation-worker-response/v1"
STATUS_SCHEMA: Final = "missioncore.simulation-worker-status/v1"
VEHICLE_STATE_SCHEMA: Final = "missioncore.vehicle-state/v1"
COMMAND_ACCEPTANCE_SCHEMA: Final = "missioncore.command-acceptance/v1"
MAX_MESSAGE_BYTES: Final = 64 * 1024
OPERATIONS: Final = frozenset({"status", "live", "start", "stop"})
OPERATIONS: Final = frozenset({"status", "live", "start", "command", "stop"})
class SimulationWorkerGatewayError(RuntimeError):
@@ -47,6 +49,15 @@ class PolygonWorkerGateway(Protocol):
idempotency_key: str,
) -> dict[str, Any]: ...
def command(
self,
*,
run_id: str,
speed_mps: float,
steering_normalized: float,
idempotency_key: str,
) -> dict[str, Any]: ...
class UnixSocketWorkerGateway:
"""Bounded request/response transport between Mission Core and one local worker."""
@@ -113,6 +124,27 @@ class UnixSocketWorkerGateway:
_validate_status(result)
return result
def command(
self,
*,
run_id: str,
speed_mps: float,
steering_normalized: float,
idempotency_key: str,
) -> dict[str, Any]:
result = self._request(
"command",
{
"run_id": run_id,
"speed_mps": speed_mps,
"steering_normalized": steering_normalized,
"idempotency_key": idempotency_key,
},
timeout_seconds=self.request_timeout_seconds,
)
_validate_command_acceptance(result)
return result
def _request(
self,
operation: str,
@@ -258,3 +290,55 @@ def _validate_vehicle_state(value: Mapping[str, Any]) -> None:
or not isinstance(value.get("safety"), dict)
):
raise SimulationWorkerGatewayError("vehicle state contains invalid values")
def _validate_command_acceptance(value: Mapping[str, Any]) -> None:
expected = {
"schema_version",
"run_id",
"command_id",
"sequence",
"issued_at_sim_ns",
"valid_until_sim_ns",
"speed_mps",
"steering_normalized",
"authority_scope",
"delivery",
}
if set(value) != expected or value.get("schema_version") != COMMAND_ACCEPTANCE_SCHEMA:
raise SimulationWorkerGatewayError("command acceptance does not match v1")
delivery = value.get("delivery")
numeric_fields = (
"sequence",
"issued_at_sim_ns",
"valid_until_sim_ns",
"speed_mps",
"steering_normalized",
)
if (
not isinstance(value.get("run_id"), str)
or not isinstance(value.get("command_id"), str)
or any(
isinstance(value.get(field), bool)
or not isinstance(value.get(field), int | float)
or not math.isfinite(value[field])
for field in numeric_fields
)
or value.get("authority_scope") != "virtual-only"
or not isinstance(delivery, dict)
or set(delivery)
!= {
"provider",
"mode",
"armed",
"offboard",
"ttl_expired_count",
}
or delivery.get("provider") != "px4-ros2-offboard"
or delivery.get("mode") != "speed-steering"
or not isinstance(delivery.get("armed"), bool)
or not isinstance(delivery.get("offboard"), bool)
or isinstance(delivery.get("ttl_expired_count"), bool)
or not isinstance(delivery.get("ttl_expired_count"), int)
):
raise SimulationWorkerGatewayError("command acceptance contains invalid values")
src/k1link/web/polygon_api.py
+42 -1
View File
@@ -10,7 +10,7 @@ from typing import Annotated, Any, Final
from fastapi import APIRouter, Header, HTTPException, Query
from fastapi import Path as PathParameter
from pydantic import BaseModel, ConfigDict
from pydantic import BaseModel, ConfigDict, Field
from k1link.simulation import (
QualificationRun,
@@ -54,6 +54,13 @@ class StartStockRoverRequest(BaseModel):
scenario_id: str
class AckermannCommandRequest(BaseModel):
model_config = ConfigDict(extra="forbid", frozen=True)
speed_mps: float = Field(ge=-1.5, le=1.5, allow_inf_nan=False)
steering_normalized: float = Field(ge=-1.0, le=1.0, allow_inf_nan=False)
def configured_polygon_runs_root() -> Path | None:
raw = os.environ.get(POLYGON_RUNS_ROOT_ENV)
if raw is None or not raw.strip():
@@ -250,6 +257,40 @@ def build_polygon_router(
detail="Simulation Worker не подтвердил остановку.",
) from exc
@router.post("/worker/runs/{run_id}/commands")
def command_polygon_worker_run(
run_id: Annotated[
str,
PathParameter(pattern=r"^[a-z0-9][a-z0-9-]{0,63}$"),
],
request: AckermannCommandRequest,
idempotency_key: Annotated[
str,
Header(alias="Idempotency-Key", min_length=1, max_length=160),
],
) -> dict[str, Any]:
gateway, _ = _control_context(
worker_provider,
control_provider,
commit_provider,
)
try:
return gateway.command(
run_id=run_id,
speed_mps=request.speed_mps,
steering_normalized=request.steering_normalized,
idempotency_key=idempotency_key,
)
except SimulationWorkerUnavailableError as exc:
raise HTTPException(status_code=503, detail="Simulation Worker недоступен.") from exc
except SimulationWorkerRejectedError as exc:
raise HTTPException(status_code=409, detail=str(exc)) from exc
except SimulationWorkerGatewayError as exc:
raise HTTPException(
status_code=502,
detail="Simulation Worker не подтвердил команду ровера.",
) from exc
return router