vee
367 lines
14 KiB
Python
Executable File
367 lines
14 KiB
Python
Executable File
"""Virtual CAN executor and feedback simulator for the partial matrix."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from copy import deepcopy
|
|
from datetime import UTC, datetime
|
|
from typing import Any
|
|
|
|
from .matrix import SeatCanMatrix
|
|
|
|
|
|
COMMAND_TO_FEEDBACK = {
|
|
"Front_Passenger_Seat_Ventilation_S": "Copilot_Seat_Vtilat_Stat_4FA_S",
|
|
"Front_Passenger_Seat_Heating_S": "Copilot_Seat_Heat_Stat_4FA_S",
|
|
}
|
|
OPERATION_TO_COMMAND = {
|
|
("front_right", "seat.set_ventilation"): "Front_Passenger_Seat_Ventilation_S",
|
|
("front_right", "seat.set_heating"): "Front_Passenger_Seat_Heating_S",
|
|
}
|
|
MOTION_CAPABILITY_TO_AXIS = {
|
|
"seat.set_longitudinal_position": "longitudinal",
|
|
"seat.set_cushion_tilt": "cushion_tilt",
|
|
"seat.set_backrest_recline": "backrest_recline",
|
|
"seat.set_legrest_height": "legrest_height",
|
|
"seat.set_legrest_extension": "legrest_extension",
|
|
}
|
|
MOTION_AXIS_BITS = {
|
|
"longitudinal": 0,
|
|
"cushion_tilt": 1,
|
|
"backrest_recline": 2,
|
|
"legrest_height": 3,
|
|
"legrest_extension": 4,
|
|
}
|
|
|
|
|
|
def _now() -> str:
|
|
return datetime.now(UTC).isoformat().replace("+00:00", "Z")
|
|
|
|
|
|
class VirtualSeatExecutor:
|
|
def __init__(self, matrix: SeatCanMatrix):
|
|
self.matrix = matrix
|
|
|
|
def execute(
|
|
self,
|
|
command_plan: dict[str, Any],
|
|
function_config: dict[str, Any],
|
|
actuator_state: dict[str, Any] | None = None,
|
|
) -> dict[str, Any]:
|
|
started_at = _now()
|
|
operation_by_id = {
|
|
operation["id"]: operation
|
|
for operation in function_config["spec"]["operations"]
|
|
}
|
|
results = []
|
|
for command in command_plan["spec"]["commands"]:
|
|
operation_ids = command["operation_ref"].split(",")
|
|
operations = [operation_by_id[item] for item in operation_ids]
|
|
if command["message"] == "Seat_Thermal_Control_42A":
|
|
results.extend(self._execute_thermal(command, operations))
|
|
elif command["message"] in {
|
|
"Seat_Dynamic_Mode_Virtual",
|
|
"Seat_Dynamic_Mode_Stop_Virtual",
|
|
}:
|
|
if len(operations) != 1:
|
|
raise ValueError(
|
|
"A dynamic-mode command must reference exactly one operation"
|
|
)
|
|
results.append(self._execute_dynamic_mode(command, operations[0]))
|
|
else:
|
|
if len(operations) != 1:
|
|
raise ValueError("A motion command must reference exactly one operation")
|
|
results.append(
|
|
self._execute_motion(command, operations[0], actuator_state or {})
|
|
)
|
|
|
|
request_id = function_config["metadata"]["request_id"]
|
|
return {
|
|
"apiVersion": "cognitive-agent/v1alpha1",
|
|
"kind": "ExecutionReport",
|
|
"metadata": {
|
|
"id": f"report-{request_id}",
|
|
"name": "Seat virtual execution report",
|
|
"version": "1.0.0",
|
|
"request_id": request_id,
|
|
"labels": {"simulation": "true"},
|
|
},
|
|
"spec": {
|
|
"mode": "dry_run",
|
|
"status": "succeeded",
|
|
"started_at": started_at,
|
|
"finished_at": _now(),
|
|
"results": results,
|
|
},
|
|
}
|
|
def _execute_thermal(
|
|
self, command: dict[str, Any], operations: list[dict[str, Any]]
|
|
) -> list[dict[str, Any]]:
|
|
command_frame = self.matrix.encode(command["message"], command["signals"])
|
|
operation_by_signal = {
|
|
OPERATION_TO_COMMAND[(operation["target"]["zone"], operation["capability"])]: operation
|
|
for operation in operations
|
|
}
|
|
feedback_values = {}
|
|
for signal, value in command["signals"].items():
|
|
operation = operation_by_signal[signal]
|
|
level = int(operation["parameters"].get("level", 1))
|
|
feedback_values[COMMAND_TO_FEEDBACK[signal]] = level + 1 if value == 1 else 1
|
|
feedback_frame = self.matrix.encode("Seat_Thermal_Status_4FA", feedback_values)
|
|
|
|
results = []
|
|
for operation in operations:
|
|
command_signal = OPERATION_TO_COMMAND[
|
|
(operation["target"]["zone"], operation["capability"])
|
|
]
|
|
feedback_signal = COMMAND_TO_FEEDBACK[command_signal]
|
|
state = operation["parameters"]["state"]
|
|
requested_level = operation["parameters"].get("level")
|
|
effective_level = (requested_level or 1) if state == "on" else 0
|
|
results.append(
|
|
{
|
|
"ref": operation["id"],
|
|
"domain": "seat",
|
|
"status": "succeeded",
|
|
"message": (
|
|
f"虚拟回读确认 {operation['target']['instance']} "
|
|
f"{operation['capability']}={state}, level={effective_level}"
|
|
),
|
|
"telemetry": {
|
|
"requested_state": state,
|
|
"requested_level": requested_level,
|
|
"effective_level": effective_level,
|
|
"level_command_simulated": requested_level is not None,
|
|
"command_signal": command_signal,
|
|
"command_value": command["signals"][command_signal],
|
|
"feedback_signal": feedback_signal,
|
|
"feedback_value": feedback_values[feedback_signal],
|
|
"command_frame": command_frame,
|
|
"feedback_frame": feedback_frame,
|
|
"simulated": True,
|
|
},
|
|
}
|
|
)
|
|
return results
|
|
|
|
@staticmethod
|
|
def _execute_dynamic_mode(
|
|
command: dict[str, Any], operation: dict[str, Any]
|
|
) -> dict[str, Any]:
|
|
if operation["capability"] == "seat.stop_dynamic_mode":
|
|
return {
|
|
"ref": operation["id"],
|
|
"domain": "seat",
|
|
"status": "succeeded",
|
|
"message": "虚拟控制器已停止动态座椅模式",
|
|
"telemetry": {
|
|
"dynamic_mode_id": 0,
|
|
"dynamic_mode_name": "",
|
|
"dynamic_state": "idle",
|
|
"repeat": "none",
|
|
"max_runtime_ms": 600_000,
|
|
"command_frame": {
|
|
"frame_id": "virtual",
|
|
"data_hex": "dynamic-stop",
|
|
},
|
|
"feedback_frame": {
|
|
"frame_id": "virtual",
|
|
"data_hex": "dynamic-idle",
|
|
},
|
|
"simulated": True,
|
|
},
|
|
}
|
|
mode_id = int(operation["parameters"]["mode_id"])
|
|
cycle_ms = None
|
|
mode_name = "木马模式" if mode_id == 1 else "动态摇篮模式"
|
|
return {
|
|
"ref": operation["id"],
|
|
"domain": "seat",
|
|
"status": "succeeded",
|
|
"message": (
|
|
f"虚拟控制器已启动{mode_name}，持续循环至停止指令或十分钟上限"
|
|
),
|
|
"telemetry": {
|
|
"dynamic_mode_id": mode_id,
|
|
"dynamic_mode_name": mode_name,
|
|
"dynamic_state": "running",
|
|
"repeat": "until_stopped_or_timeout",
|
|
"max_runtime_ms": 600_000,
|
|
"cycle_duration_ms": cycle_ms,
|
|
"command_frame": {
|
|
"frame_id": "virtual",
|
|
"data_hex": f"dynamic-mode-{mode_id}",
|
|
},
|
|
"feedback_frame": {
|
|
"frame_id": "virtual",
|
|
"data_hex": f"dynamic-running-{mode_id}",
|
|
},
|
|
"simulated": True,
|
|
},
|
|
}
|
|
|
|
def _execute_motion(
|
|
self,
|
|
command: dict[str, Any],
|
|
operation: dict[str, Any],
|
|
actuator_state: dict[str, Any],
|
|
) -> dict[str, Any]:
|
|
capability = operation["capability"]
|
|
axis = MOTION_CAPABILITY_TO_AXIS[capability]
|
|
instance = operation["target"]["instance"]
|
|
target = int(operation["parameters"]["position_pct"])
|
|
target_angle = operation["parameters"].get("angle_deg")
|
|
start = int(
|
|
actuator_state.get(instance, {})
|
|
.get("position", {})
|
|
.get(axis, {})
|
|
.get("position_pct", target)
|
|
)
|
|
command_frame = self.matrix.encode(command["message"], command["signals"])
|
|
axis_mask = 1 << MOTION_AXIS_BITS[axis]
|
|
completion = command["completion"]
|
|
feedback_values = {
|
|
completion["signal"]: target,
|
|
"Moving_Mask": 0,
|
|
"Fault_Mask": 0,
|
|
"Position_Valid_Mask": axis_mask,
|
|
}
|
|
feedback_frame = self.matrix.encode(completion["message"], feedback_values)
|
|
midpoint = round((start + target) / 2)
|
|
samples = list(dict.fromkeys([start, midpoint, target]))
|
|
return {
|
|
"ref": operation["id"],
|
|
"domain": "seat",
|
|
"status": "succeeded",
|
|
"message": (
|
|
f"虚构电机反馈确认 {instance} {axis} 从 {start}% 到达 {target}%"
|
|
),
|
|
"telemetry": {
|
|
"motion_axis": axis,
|
|
"start_position_pct": start,
|
|
"requested_position_pct": target,
|
|
"effective_position_pct": target,
|
|
"requested_angle_deg": target_angle,
|
|
"effective_angle_deg": target_angle,
|
|
"position_tolerance_pct": completion.get("tolerance", 0),
|
|
"motion_samples_pct": samples,
|
|
"motion_state": "stopped",
|
|
"moving_mask": 0,
|
|
"fault_mask": 0,
|
|
"position_valid_mask": axis_mask,
|
|
"command_signal": next(
|
|
name for name in command["signals"] if name.endswith("_Target_Pct")
|
|
),
|
|
"feedback_signal": completion["signal"],
|
|
"command_frame": command_frame,
|
|
"feedback_frame": feedback_frame,
|
|
"matrix_source": "synthetic_demo",
|
|
"simulated": True,
|
|
},
|
|
}
|
|
|
|
|
|
SIMULATED_HARDWARE_FAULTS = (
|
|
"normal",
|
|
"feedback_timeout",
|
|
"adapter_disconnect",
|
|
"motor_stall",
|
|
"firmware_mismatch",
|
|
"partial_feedback",
|
|
)
|
|
|
|
|
|
class SimulatedHardwareSeatExecutor:
|
|
"""Development-only HIL facade that never opens a physical CAN adapter."""
|
|
|
|
_FAULT_DETAILS = {
|
|
"feedback_timeout": ("FeedbackTimeout", "模拟台架未在限定时间内返回目标反馈。"),
|
|
"adapter_disconnect": ("CanAdapterDisconnected", "模拟 CAN 适配器在执行前断开连接。"),
|
|
"motor_stall": ("MotorStall", "模拟电机堵转，已触发停止保护。"),
|
|
"firmware_mismatch": ("FirmwareMismatch", "模拟固件协议签名不匹配，控制帧未发送。"),
|
|
"partial_feedback": ("PartialFeedback", "模拟台架只返回了部分执行器反馈。"),
|
|
}
|
|
|
|
def __init__(self, matrix: SeatCanMatrix):
|
|
self.virtual = VirtualSeatExecutor(matrix)
|
|
|
|
def execute(
|
|
self,
|
|
command_plan: dict[str, Any],
|
|
function_config: dict[str, Any],
|
|
actuator_state: dict[str, Any] | None = None,
|
|
*,
|
|
fault_scenario: str = "normal",
|
|
) -> dict[str, Any]:
|
|
if fault_scenario not in SIMULATED_HARDWARE_FAULTS:
|
|
raise ValueError(
|
|
"simulation_fault must be one of "
|
|
+ ", ".join(SIMULATED_HARDWARE_FAULTS)
|
|
)
|
|
|
|
report = deepcopy(
|
|
self.virtual.execute(command_plan, function_config, actuator_state)
|
|
)
|
|
report["metadata"]["name"] = "Seat simulated hardware execution report"
|
|
report["metadata"]["labels"] = {
|
|
"simulation": "true",
|
|
"seat_feedback": "simulated_hardware",
|
|
"fault_scenario": fault_scenario,
|
|
}
|
|
report["spec"]["mode"] = "hil"
|
|
results = report["spec"]["results"]
|
|
for item in results:
|
|
item.setdefault("telemetry", {}).update(
|
|
{
|
|
"simulation": True,
|
|
"simulation_kind": "simulated_hardware",
|
|
"fault_scenario": fault_scenario,
|
|
"physical_can_opened": False,
|
|
}
|
|
)
|
|
if fault_scenario == "normal":
|
|
for item in results:
|
|
item["message"] = (
|
|
item["message"]
|
|
.replace("虚构", "模拟台架")
|
|
.replace("虚拟", "模拟台架")
|
|
)
|
|
return report
|
|
|
|
error_type, message = self._FAULT_DETAILS[fault_scenario]
|
|
affected = results
|
|
if fault_scenario == "motor_stall":
|
|
affected = [
|
|
next(
|
|
(item for item in results if item.get("telemetry", {}).get("motion_axis")),
|
|
results[0],
|
|
)
|
|
]
|
|
elif fault_scenario == "partial_feedback" and len(results) > 1:
|
|
affected = results[1:]
|
|
|
|
for item in affected:
|
|
telemetry = item["telemetry"]
|
|
item["status"] = "failed"
|
|
item["message"] = message
|
|
telemetry.update(
|
|
{
|
|
"error_type": error_type,
|
|
"failure_category": fault_scenario,
|
|
"feedback_received": False,
|
|
}
|
|
)
|
|
telemetry.pop("feedback_frame", None)
|
|
if fault_scenario == "motor_stall":
|
|
telemetry.update(
|
|
{"motion_state": "stalled", "fault_mask": 1, "stop_all_attempted": True}
|
|
)
|
|
if fault_scenario == "firmware_mismatch":
|
|
telemetry["command_sent"] = False
|
|
if fault_scenario == "adapter_disconnect":
|
|
telemetry["adapter_connected"] = False
|
|
|
|
succeeded = sum(item["status"] == "succeeded" for item in results)
|
|
report["spec"]["status"] = "partially_succeeded" if succeeded else "failed"
|
|
report["spec"]["finished_at"] = _now()
|
|
return report
|