vee
349 lines
13 KiB
Python
349 lines
13 KiB
Python
"""Execute normal seat command plans through the shared CAN runtime.
|
|
|
|
ZLG's Windows driver gives one process exclusive ownership of a USBCAN device.
|
|
Self-repair and orchestration therefore exchange bounded, armed requests with
|
|
one transport-neutral runtime instead of opening the adapter independently.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from datetime import UTC, datetime
|
|
import json
|
|
import os
|
|
from pathlib import Path
|
|
import time
|
|
from typing import Any
|
|
import uuid
|
|
|
|
|
|
def _now() -> str:
|
|
return datetime.now(UTC).isoformat().replace("+00:00", "Z")
|
|
|
|
|
|
def _read_json(path: Path) -> dict[str, Any] | None:
|
|
try:
|
|
value = json.loads(path.read_text(encoding="utf-8"))
|
|
except (OSError, ValueError, TypeError):
|
|
return None
|
|
return value if isinstance(value, dict) else None
|
|
|
|
|
|
def _atomic_json(path: Path, value: dict[str, Any]) -> None:
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
temporary = path.with_name(
|
|
f".{path.name}.{os.getpid()}.{time.time_ns()}.tmp"
|
|
)
|
|
temporary.write_text(
|
|
json.dumps(value, ensure_ascii=False, indent=2), encoding="utf-8"
|
|
)
|
|
os.replace(temporary, path)
|
|
|
|
|
|
class ZcanBridgeSeatExecutor:
|
|
"""Hardware-seat executor compatible with ``HardwareSeatExecutor``."""
|
|
|
|
def __init__(
|
|
self,
|
|
matrix: Any,
|
|
bridge_manager: Any,
|
|
status_path: Path,
|
|
pending_dir: Path,
|
|
*,
|
|
required_version: str,
|
|
auto_home: bool,
|
|
startup_timeout_s: float = 12.0,
|
|
request_timeout_s: float = 660.0,
|
|
) -> None:
|
|
self.matrix = matrix
|
|
self.bridge_manager = bridge_manager
|
|
self.status_path = Path(status_path)
|
|
self.pending_dir = Path(pending_dir)
|
|
self.required_version = required_version
|
|
self.auto_home = bool(auto_home)
|
|
self.startup_timeout_s = float(startup_timeout_s)
|
|
self.request_timeout_s = float(request_timeout_s)
|
|
|
|
def _online_status(self, *, require_protocol: bool) -> dict[str, Any] | None:
|
|
value = _read_json(self.status_path)
|
|
if value is None or value.get("bridge_version") != self.required_version:
|
|
return None
|
|
try:
|
|
fresh = time.time() - float(value.get("heartbeat_epoch", 0)) < 3.0
|
|
except (TypeError, ValueError):
|
|
fresh = False
|
|
if not fresh:
|
|
return None
|
|
if require_protocol and not (value.get("system") and value.get("frame_epochs")):
|
|
return None
|
|
return value
|
|
|
|
def _wait_online(self, *, require_protocol: bool) -> dict[str, Any]:
|
|
if require_protocol and hasattr(
|
|
self.bridge_manager, "ensure_seat_protocol"
|
|
):
|
|
return self.bridge_manager.ensure_seat_protocol()
|
|
self.bridge_manager.ensure_async()
|
|
deadline = time.time() + self.startup_timeout_s
|
|
while time.time() < deadline:
|
|
value = self._online_status(require_protocol=require_protocol)
|
|
if value is not None:
|
|
return value
|
|
time.sleep(0.1)
|
|
qualifier = "及座椅ECU协议" if require_protocol else ""
|
|
raise RuntimeError(f"共享CAN Runtime{qualifier}未在线")
|
|
|
|
def _payload(
|
|
self,
|
|
command_plan: dict[str, Any],
|
|
function_config: dict[str, Any],
|
|
) -> dict[str, Any]:
|
|
commands = []
|
|
for command in command_plan["spec"]["commands"]:
|
|
encoded = self.matrix.encode(command["message"], command["signals"])
|
|
item = dict(command)
|
|
item["frame_id"] = int(encoded["frame_id"], 16)
|
|
item["data"] = list(bytes.fromhex(encoded["data_hex"]))
|
|
commands.append(item)
|
|
return {
|
|
"name": command_plan.get("metadata", {}).get(
|
|
"name", "自然语言座椅控制"
|
|
),
|
|
"commands": commands,
|
|
"operations": list(function_config["spec"]["operations"]),
|
|
"allow_auto_home": self.auto_home,
|
|
}
|
|
|
|
def _submit(self, payload: dict[str, Any]) -> dict[str, Any]:
|
|
status = self._wait_online(require_protocol=True)
|
|
idle_deadline = time.time() + self.startup_timeout_s
|
|
while status.get("current_request_id") and time.time() < idle_deadline:
|
|
time.sleep(0.1)
|
|
status = self._wait_online(require_protocol=True)
|
|
if status.get("current_request_id"):
|
|
raise RuntimeError(
|
|
"共享CAN Runtime正在执行其他台架任务：%s" %
|
|
status.get("current_request_id")
|
|
)
|
|
|
|
if (
|
|
hasattr(self.bridge_manager, "seat_position_sync_required")
|
|
and self.bridge_manager.seat_position_sync_required()
|
|
):
|
|
sync_id = "SEAT_SYNC_%d_%s" % (
|
|
int(time.time() * 1000), uuid.uuid4().hex[:6]
|
|
)
|
|
sync_result = self._submit_request(
|
|
sync_id,
|
|
action="initialize_known_ranges",
|
|
payload={"force_all_axes": True, "home_nonzero_axes": False},
|
|
timeout_s=self.request_timeout_s,
|
|
)
|
|
if sync_result.get("result") != "PASS":
|
|
detail = sync_result.get("details") or {}
|
|
raise RuntimeError(
|
|
detail.get("summary")
|
|
or sync_result.get("summary")
|
|
or "四轴位置同步失败，未执行座椅请求"
|
|
)
|
|
self.bridge_manager.mark_seat_position_synchronized()
|
|
|
|
request_id = "SEAT_ORCH_%d_%s" % (
|
|
int(time.time() * 1000), uuid.uuid4().hex[:6]
|
|
)
|
|
return self._submit_request(
|
|
request_id,
|
|
action="execute_command_plan",
|
|
payload=payload,
|
|
timeout_s=self.request_timeout_s,
|
|
)
|
|
|
|
def _submit_request(
|
|
self,
|
|
request_id: str,
|
|
*,
|
|
action: str,
|
|
payload: dict[str, Any],
|
|
timeout_s: float,
|
|
) -> dict[str, Any]:
|
|
request = {
|
|
"schema": "seat-function-request-v1",
|
|
"request_id": request_id,
|
|
"created_epoch": time.time(),
|
|
"armed_until_epoch": time.time() + timeout_s + 30.0,
|
|
"source": "cabin_cognitive_orchestrator",
|
|
"action": action,
|
|
"payload": payload,
|
|
}
|
|
_atomic_json(self.pending_dir / f"{request_id}.json", request)
|
|
|
|
deadline = time.time() + timeout_s
|
|
while time.time() < deadline:
|
|
status = self._online_status(require_protocol=False)
|
|
if status is None:
|
|
time.sleep(0.1)
|
|
continue
|
|
result = status.get("last_result") or {}
|
|
if result.get("request_id") == request_id:
|
|
return result
|
|
time.sleep(0.1)
|
|
raise RuntimeError("共享CAN Runtime执行座椅请求超时，已停止等待")
|
|
|
|
@staticmethod
|
|
def _physical_state_from_detail(detail: dict[str, Any]) -> dict[str, Any] | None:
|
|
motors = detail.get("motors") or []
|
|
axes = ("longitudinal", "cushion_tilt", "backrest_recline", "legrest_height")
|
|
positions: dict[str, Any] = {}
|
|
missing: list[str] = []
|
|
for motor, axis in enumerate(axes):
|
|
item = motors[motor] if motor < len(motors) else None
|
|
if not isinstance(item, dict):
|
|
missing.append(axis)
|
|
continue
|
|
positions[axis] = {
|
|
"position_pct": round(int(item.get("position", 0)) / 100),
|
|
"position_valid": bool(item.get("position_valid")),
|
|
"motion_state": "moving" if item.get("active") else "stopped",
|
|
}
|
|
thermal = detail.get("thermal") or []
|
|
if not positions and not thermal:
|
|
return None
|
|
thermal_byte = int(thermal[0]) if thermal else 0
|
|
ventilation_raw = thermal_byte & 0x07
|
|
heating_raw = (thermal_byte >> 3) & 0x07
|
|
return {
|
|
"position": positions,
|
|
"ventilation": {
|
|
"state": "off" if ventilation_raw <= 1 else "on",
|
|
"level": max(0, ventilation_raw - 1),
|
|
},
|
|
"heating": {
|
|
"state": "off" if heating_raw <= 1 else "on",
|
|
"level": max(0, heating_raw - 1),
|
|
},
|
|
"captured_at": _now(),
|
|
"source": "physical_can",
|
|
"channel": 0,
|
|
"target": {
|
|
"device": "seat", "zone": "front_right", "instance": "passenger"
|
|
},
|
|
"complete": not missing and bool(thermal),
|
|
"missing": missing + ([] if thermal else ["thermal"]),
|
|
}
|
|
|
|
@staticmethod
|
|
def _report(
|
|
function_config: dict[str, Any],
|
|
started_at: str,
|
|
bridge_result: dict[str, Any] | None,
|
|
error: Exception | None,
|
|
) -> dict[str, Any]:
|
|
request_id = function_config["metadata"]["request_id"]
|
|
passed = error is None and (bridge_result or {}).get("result") == "PASS"
|
|
detail = (bridge_result or {}).get("details") or {}
|
|
physical_state = (
|
|
ZcanBridgeSeatExecutor._physical_state_from_detail(detail)
|
|
if passed
|
|
else None
|
|
)
|
|
message = (
|
|
detail.get("summary")
|
|
or (bridge_result or {}).get("summary")
|
|
or (str(error) if error is not None else "共享CAN Runtime未返回通过结果")
|
|
)
|
|
results = [
|
|
{
|
|
"ref": operation["id"],
|
|
"domain": "seat",
|
|
"status": "succeeded" if passed else "failed",
|
|
"message": message,
|
|
"telemetry": {
|
|
"transport": "shared_can_runtime",
|
|
"channel": 0,
|
|
"hardware_feedback": passed,
|
|
"simulated": False,
|
|
"feedback_required": True,
|
|
"feedback_observed": physical_state is not None,
|
|
"feedback_match": physical_state is not None,
|
|
"physical_state": physical_state,
|
|
"bridge_result": bridge_result,
|
|
"error_type": type(error).__name__ if error is not None else None,
|
|
"stop_all_attempted": bool(detail.get("stop_all_attempted")),
|
|
},
|
|
}
|
|
for operation in function_config["spec"]["operations"]
|
|
]
|
|
spec = {
|
|
"mode": "hil",
|
|
"status": "succeeded" if passed else "failed",
|
|
"started_at": started_at,
|
|
"finished_at": _now(),
|
|
"results": results,
|
|
}
|
|
if physical_state is not None:
|
|
spec["physical_state"] = physical_state
|
|
return {
|
|
"apiVersion": "cognitive-agent/v1alpha1",
|
|
"kind": "ExecutionReport",
|
|
"metadata": {
|
|
"id": f"report-{request_id}",
|
|
"name": "T2G seat shared CAN runtime execution report",
|
|
"version": "1.0.0",
|
|
"request_id": request_id,
|
|
"labels": {"simulation": "false", "feedback": "physical_can"},
|
|
},
|
|
"spec": spec,
|
|
}
|
|
|
|
def execute(
|
|
self,
|
|
command_plan: dict[str, Any],
|
|
function_config: dict[str, Any],
|
|
actuator_state: dict[str, Any] | None = None,
|
|
) -> dict[str, Any]:
|
|
del actuator_state
|
|
started_at = _now()
|
|
try:
|
|
result = self._submit(self._payload(command_plan, function_config))
|
|
except Exception as exc: # normalize bridge/startup failures for the API
|
|
return self._report(function_config, started_at, None, exc)
|
|
return self._report(function_config, started_at, result, None)
|
|
|
|
def read_physical_state(self) -> dict[str, Any]:
|
|
status = self._wait_online(require_protocol=True)
|
|
axes = ("longitudinal", "cushion_tilt", "backrest_recline", "legrest_height")
|
|
positions: dict[str, Any] = {}
|
|
missing = []
|
|
motors = status.get("motors") or []
|
|
for motor, axis in enumerate(axes):
|
|
item = motors[motor] if motor < len(motors) else None
|
|
if not isinstance(item, dict):
|
|
missing.append(axis)
|
|
continue
|
|
positions[axis] = {
|
|
"position_pct": round(int(item.get("position", 0)) / 100),
|
|
"position_valid": bool(item.get("position_valid")),
|
|
"motion_state": "moving" if item.get("active") else "stopped",
|
|
}
|
|
thermal = status.get("thermal") or []
|
|
thermal_byte = int(thermal[0]) if thermal else 0
|
|
ventilation_raw = thermal_byte & 0x07
|
|
heating_raw = (thermal_byte >> 3) & 0x07
|
|
return {
|
|
"position": positions,
|
|
"ventilation": {
|
|
"state": "off" if ventilation_raw <= 1 else "on",
|
|
"level": max(0, ventilation_raw - 1),
|
|
},
|
|
"heating": {
|
|
"state": "off" if heating_raw <= 1 else "on",
|
|
"level": max(0, heating_raw - 1),
|
|
},
|
|
"captured_at": _now(),
|
|
"source": "physical_can",
|
|
"target": {
|
|
"device": "seat", "zone": "front_right", "instance": "passenger"
|
|
},
|
|
"complete": not missing,
|
|
"missing": missing,
|
|
}
|