265 lines
10 KiB
Python
265 lines
10 KiB
Python
from __future__ import annotations
|
|
|
|
import re
|
|
from dataclasses import dataclass
|
|
from enum import StrEnum
|
|
from typing import Any, Final
|
|
|
|
from k1link.simulation.contracts import ControlProfile
|
|
|
|
PROVIDER_PROFILE_SCHEMA: Final = "missioncore.simulation-provider-profile/v1"
|
|
IDENTIFIER_PATTERN: Final = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._:-]{0,127}$")
|
|
CAPABILITY_PATTERN: Final = re.compile(r"^[a-z0-9][a-z0-9._/-]{0,127}$")
|
|
CLOCK_DOMAIN_PATTERN: Final = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._:/-]{0,127}$")
|
|
COMMAND_CAPABILITY_BY_PROFILE: Final = {
|
|
ControlProfile.ROVER_SPEED_STEERING_V1: "command.rover-speed-steering/v1",
|
|
ControlProfile.ROVER_SPEED_YAW_RATE_V1: "command.rover-speed-yaw-rate/v1",
|
|
}
|
|
|
|
|
|
class SimulationProviderContractError(ValueError):
|
|
"""A simulation provider profile violates the admitted v1 contract."""
|
|
|
|
|
|
class ProviderRole(StrEnum):
|
|
WORLD = "world"
|
|
PHYSICS = "physics"
|
|
STATE = "state"
|
|
CONTROLLER = "controller"
|
|
TRANSPORT = "transport"
|
|
SENSOR = "sensor"
|
|
TRAFFIC = "traffic"
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class SimulationProviderDescriptor:
|
|
provider_id: str
|
|
roles: tuple[ProviderRole, ...]
|
|
capabilities: tuple[str, ...]
|
|
|
|
def __post_init__(self) -> None:
|
|
_identifier(self.provider_id, "provider id")
|
|
if not self.roles:
|
|
raise SimulationProviderContractError("provider roles must not be empty")
|
|
if any(not isinstance(role, ProviderRole) for role in self.roles):
|
|
raise SimulationProviderContractError("provider role is unknown")
|
|
if len(self.roles) != len(set(self.roles)):
|
|
raise SimulationProviderContractError("provider roles must be unique")
|
|
if not self.capabilities:
|
|
raise SimulationProviderContractError("provider capabilities must not be empty")
|
|
if len(self.capabilities) != len(set(self.capabilities)):
|
|
raise SimulationProviderContractError("provider capabilities must be unique")
|
|
for capability in self.capabilities:
|
|
_capability(capability)
|
|
|
|
def to_dict(self) -> dict[str, object]:
|
|
return {
|
|
"provider_id": self.provider_id,
|
|
"roles": [role.value for role in self.roles],
|
|
"capabilities": list(self.capabilities),
|
|
}
|
|
|
|
@classmethod
|
|
def from_dict(cls, value: object) -> SimulationProviderDescriptor:
|
|
document = _object(value, "provider descriptor")
|
|
_exact_keys(document, {"provider_id", "roles", "capabilities"}, "provider descriptor")
|
|
roles = _array(document, "roles")
|
|
capabilities = _array(document, "capabilities")
|
|
try:
|
|
parsed_roles = tuple(
|
|
ProviderRole(_string_value(role, "provider role")) for role in roles
|
|
)
|
|
except ValueError as exc:
|
|
raise SimulationProviderContractError("provider role is unknown") from exc
|
|
return cls(
|
|
provider_id=_string(document, "provider_id"),
|
|
roles=parsed_roles,
|
|
capabilities=tuple(
|
|
_string_value(capability, "provider capability") for capability in capabilities
|
|
),
|
|
)
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class SimulationClockDescriptor:
|
|
provider_id: str
|
|
domain: str
|
|
unit: str = "nanoseconds"
|
|
mode: str = "simulation"
|
|
|
|
def __post_init__(self) -> None:
|
|
_identifier(self.provider_id, "clock provider id")
|
|
if not CLOCK_DOMAIN_PATTERN.fullmatch(self.domain):
|
|
raise SimulationProviderContractError("clock domain is not safe")
|
|
if self.unit != "nanoseconds" or self.mode != "simulation":
|
|
raise SimulationProviderContractError(
|
|
"v1 provider clocks must use simulation nanoseconds"
|
|
)
|
|
|
|
def to_dict(self) -> dict[str, object]:
|
|
return {
|
|
"provider_id": self.provider_id,
|
|
"domain": self.domain,
|
|
"unit": self.unit,
|
|
"mode": self.mode,
|
|
}
|
|
|
|
@classmethod
|
|
def from_dict(cls, value: object) -> SimulationClockDescriptor:
|
|
document = _object(value, "simulation clock")
|
|
_exact_keys(
|
|
document,
|
|
{"provider_id", "domain", "unit", "mode"},
|
|
"simulation clock",
|
|
)
|
|
return cls(
|
|
provider_id=_string(document, "provider_id"),
|
|
domain=_string(document, "domain"),
|
|
unit=_string(document, "unit"),
|
|
mode=_string(document, "mode"),
|
|
)
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class SimulationProviderProfile:
|
|
profile_id: str
|
|
providers: tuple[SimulationProviderDescriptor, ...]
|
|
clock: SimulationClockDescriptor
|
|
control_profiles: tuple[ControlProfile, ...]
|
|
world_frame: str = "map_enu"
|
|
body_frame: str = "base_link_flu"
|
|
|
|
def __post_init__(self) -> None:
|
|
_identifier(self.profile_id, "provider profile id")
|
|
if not self.providers:
|
|
raise SimulationProviderContractError("provider profile must declare providers")
|
|
provider_ids = [provider.provider_id for provider in self.providers]
|
|
if len(provider_ids) != len(set(provider_ids)):
|
|
raise SimulationProviderContractError("provider ids must be unique")
|
|
providers_by_id = {provider.provider_id: provider for provider in self.providers}
|
|
clock_provider = providers_by_id.get(self.clock.provider_id)
|
|
if clock_provider is None or "clock.simulation" not in clock_provider.capabilities:
|
|
raise SimulationProviderContractError("clock provider must declare clock.simulation")
|
|
if not any(
|
|
ProviderRole.STATE in provider.roles and "state.vehicle-pose" in provider.capabilities
|
|
for provider in self.providers
|
|
):
|
|
raise SimulationProviderContractError(
|
|
"provider profile must expose canonical vehicle pose"
|
|
)
|
|
if not self.control_profiles:
|
|
raise SimulationProviderContractError("control profiles must not be empty")
|
|
if any(not isinstance(profile, ControlProfile) for profile in self.control_profiles):
|
|
raise SimulationProviderContractError("control profile is unknown")
|
|
if len(self.control_profiles) != len(set(self.control_profiles)):
|
|
raise SimulationProviderContractError("control profiles must be unique")
|
|
controller_capabilities = {
|
|
capability
|
|
for provider in self.providers
|
|
if ProviderRole.CONTROLLER in provider.roles
|
|
for capability in provider.capabilities
|
|
}
|
|
if any(
|
|
COMMAND_CAPABILITY_BY_PROFILE[profile] not in controller_capabilities
|
|
for profile in self.control_profiles
|
|
):
|
|
raise SimulationProviderContractError(
|
|
"controller providers do not satisfy the declared control profiles"
|
|
)
|
|
if self.world_frame != "map_enu" or self.body_frame != "base_link_flu":
|
|
raise SimulationProviderContractError(
|
|
"v1 provider profiles must expose map_enu and base_link_flu"
|
|
)
|
|
|
|
def to_dict(self) -> dict[str, object]:
|
|
return {
|
|
"schema_version": PROVIDER_PROFILE_SCHEMA,
|
|
"profile_id": self.profile_id,
|
|
"providers": [provider.to_dict() for provider in self.providers],
|
|
"clock": self.clock.to_dict(),
|
|
"control_profiles": [profile.value for profile in self.control_profiles],
|
|
"canonical_frames": {
|
|
"world": self.world_frame,
|
|
"body": self.body_frame,
|
|
},
|
|
}
|
|
|
|
@classmethod
|
|
def from_dict(cls, value: object) -> SimulationProviderProfile:
|
|
document = _object(value, "simulation provider profile")
|
|
_exact_keys(
|
|
document,
|
|
{
|
|
"schema_version",
|
|
"profile_id",
|
|
"providers",
|
|
"clock",
|
|
"control_profiles",
|
|
"canonical_frames",
|
|
},
|
|
"simulation provider profile",
|
|
)
|
|
if document.get("schema_version") != PROVIDER_PROFILE_SCHEMA:
|
|
raise SimulationProviderContractError("provider profile schema is incompatible")
|
|
providers = _array(document, "providers")
|
|
control_profiles = _array(document, "control_profiles")
|
|
frames = _object(document.get("canonical_frames"), "canonical frames")
|
|
_exact_keys(frames, {"world", "body"}, "canonical frames")
|
|
try:
|
|
parsed_control_profiles = tuple(
|
|
ControlProfile(_string_value(profile, "control profile"))
|
|
for profile in control_profiles
|
|
)
|
|
except ValueError as exc:
|
|
raise SimulationProviderContractError("control profile is unknown") from exc
|
|
return cls(
|
|
profile_id=_string(document, "profile_id"),
|
|
providers=tuple(
|
|
SimulationProviderDescriptor.from_dict(provider) for provider in providers
|
|
),
|
|
clock=SimulationClockDescriptor.from_dict(document.get("clock")),
|
|
control_profiles=parsed_control_profiles,
|
|
world_frame=_string(frames, "world"),
|
|
body_frame=_string(frames, "body"),
|
|
)
|
|
|
|
|
|
def _identifier(value: str, label: str) -> str:
|
|
if not IDENTIFIER_PATTERN.fullmatch(value):
|
|
raise SimulationProviderContractError(f"{label} is not a safe identifier")
|
|
return value
|
|
|
|
|
|
def _capability(value: str) -> str:
|
|
if not CAPABILITY_PATTERN.fullmatch(value):
|
|
raise SimulationProviderContractError("provider capability is not safe")
|
|
return value
|
|
|
|
|
|
def _object(value: object, label: str) -> dict[str, Any]:
|
|
if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
|
|
raise SimulationProviderContractError(f"{label} must be an object")
|
|
return value
|
|
|
|
|
|
def _array(document: dict[str, Any], key: str) -> list[object]:
|
|
value = document.get(key)
|
|
if not isinstance(value, list):
|
|
raise SimulationProviderContractError(f"{key} must be an array")
|
|
return value
|
|
|
|
|
|
def _string(document: dict[str, Any], key: str) -> str:
|
|
return _string_value(document.get(key), key)
|
|
|
|
|
|
def _string_value(value: object, label: str) -> str:
|
|
if not isinstance(value, str) or not value:
|
|
raise SimulationProviderContractError(f"{label} must be a nonempty string")
|
|
return value
|
|
|
|
|
|
def _exact_keys(value: dict[str, Any], expected: set[str], label: str) -> None:
|
|
if set(value) != expected:
|
|
raise SimulationProviderContractError(f"{label} keys do not match v1")
|