minghanchen
605 lines
22 KiB
Python
605 lines
22 KiB
Python
from __future__ import annotations
|
|
|
|
import tempfile
|
|
import unittest
|
|
from collections import deque
|
|
from pathlib import Path
|
|
from types import SimpleNamespace
|
|
from unittest.mock import patch
|
|
|
|
from cognitive_agent.application.mqtt_ota_seat_executor import (
|
|
MqttNativeSeatExecutor,
|
|
MqttOtaSeatExecutor,
|
|
)
|
|
from cognitive_agent.application.seat_service import SeatDemoService
|
|
from cognitive_agent.infrastructure.can.gateway import CanFrame
|
|
from seat_domain import parse_local_intent
|
|
|
|
|
|
REPOSITORY_ROOT = Path(__file__).resolve().parents[2]
|
|
MAX_HALL = {0: 480, 1: 374, 2: 917, 3: 647}
|
|
|
|
|
|
class _NativeFirmwareBus:
|
|
"""Deterministic firmware twin; it never opens or writes a physical bus."""
|
|
|
|
def __init__(self, matrix) -> None:
|
|
self.matrix = matrix
|
|
self.frames: deque[CanFrame] = deque()
|
|
self.sent: list[CanFrame] = []
|
|
self.debug_commands: list[str] = []
|
|
self._debug_command = ""
|
|
self._published_messages: list[dict[str, object]] = []
|
|
self.closed = False
|
|
|
|
@property
|
|
def published_messages(self) -> tuple[dict[str, object], ...]:
|
|
return tuple(self._published_messages)
|
|
|
|
def set_debug_command(self, command: str) -> None:
|
|
self._debug_command = command
|
|
self.debug_commands.append(command)
|
|
|
|
def _frame(self, message: str, values: dict[str, int]) -> CanFrame:
|
|
encoded = self.matrix.encode(message, values)
|
|
return CanFrame(
|
|
int(encoded["frame_id"], 16),
|
|
bytes.fromhex(encoded["data_hex"]),
|
|
channel=0,
|
|
)
|
|
|
|
def _system_ack(self, sequence: int, opcode: int, ack: int) -> CanFrame:
|
|
return self._frame(
|
|
"T2G_Seat_Move_System_Status_301",
|
|
{
|
|
"Ack": ack,
|
|
"System_State": 1,
|
|
"Reject_Reason": 0,
|
|
"Last_Sequence": sequence,
|
|
"Last_Opcode": opcode,
|
|
"Active_Mask": 0,
|
|
"Safety_Inhibit": 0,
|
|
"Calibration_Busy": 0,
|
|
"Fault_Summary": 0,
|
|
"Faulted_Mask": 0,
|
|
"Position_Valid_Mask": 0x0F,
|
|
"Alive": 1,
|
|
"Firmware_Revision": 1,
|
|
},
|
|
)
|
|
|
|
def _motor_status(
|
|
self,
|
|
motor: int,
|
|
*,
|
|
sequence: int = 0,
|
|
value_x100: int = 4_000,
|
|
complete: bool = False,
|
|
) -> CanFrame:
|
|
return self._frame(
|
|
"T2G_Seat_Move_Motor_Status_302",
|
|
{
|
|
"Motor_Sub_ID": motor,
|
|
"Task_State": 6 if complete else 0,
|
|
"Result": 2 if complete else 0,
|
|
"Direction": 0,
|
|
"Active": 0,
|
|
"Position_Valid": 1,
|
|
"Near_Endpoint": int(value_x100 == 0),
|
|
"Far_Endpoint": int(value_x100 == 10_000),
|
|
"Stall": 0,
|
|
"Driver_Fault": 0,
|
|
"Motor_Sequence": sequence,
|
|
"Value_X100": value_x100,
|
|
"Duty_Percent": 90,
|
|
},
|
|
)
|
|
|
|
def _diagnostic(self, motor: int, value_x100: int = 4_000) -> CanFrame:
|
|
maximum = MAX_HALL[motor]
|
|
hall = round(maximum * value_x100 / 10_000)
|
|
return self._frame(
|
|
"T2G_Seat_Move_Diagnostic_303",
|
|
{
|
|
"Motor_Sub_ID": motor,
|
|
"Last_Motor_Event": 1,
|
|
"Fault_Flags": 0,
|
|
"Hall_Count": hall,
|
|
"Max_Hall_Count": maximum,
|
|
"Hall_Speed_Half_Pps": 0,
|
|
"Retry_Count": 0,
|
|
"Diagnostic_Alive": 1,
|
|
},
|
|
)
|
|
|
|
def _queue_preflight(self) -> None:
|
|
for motor in range(4):
|
|
self.frames.append(self._motor_status(motor))
|
|
for motor in range(4):
|
|
self.frames.append(self._diagnostic(motor))
|
|
|
|
def send(
|
|
self,
|
|
arbitration_id: int,
|
|
data: bytes,
|
|
*,
|
|
is_fd: bool = False,
|
|
bitrate_switch: bool = False,
|
|
) -> CanFrame:
|
|
frame = CanFrame(
|
|
arbitration_id,
|
|
bytes(data),
|
|
channel=0,
|
|
is_fd=is_fd,
|
|
bitrate_switch=bitrate_switch,
|
|
)
|
|
self.sent.append(frame)
|
|
self._published_messages.append(
|
|
{
|
|
"topic": "n7/n7-headunit/can/command",
|
|
"qos": 1,
|
|
"retain": False,
|
|
"payload": {
|
|
"command": self._debug_command,
|
|
"channel": "can0",
|
|
"canId": f"0x{arbitration_id:03X}",
|
|
"frameType": "classic",
|
|
"brs": False,
|
|
"data": data.hex(" ").upper(),
|
|
},
|
|
}
|
|
)
|
|
if arbitration_id == 0x300:
|
|
opcode = data[0]
|
|
motor = data[1] & 0x03
|
|
sequence = int.from_bytes(data[2:4], "little")
|
|
self.frames.append(
|
|
self._system_ack(sequence, opcode, 4 if opcode == 0x09 else 1)
|
|
)
|
|
if opcode == 0x09:
|
|
self._queue_preflight()
|
|
elif opcode in {0x01, 0x0B}:
|
|
target = int.from_bytes(data[4:6], "little") if opcode == 0x01 else 0
|
|
self.frames.append(
|
|
self._motor_status(
|
|
motor,
|
|
sequence=sequence,
|
|
value_x100=target,
|
|
complete=True,
|
|
)
|
|
)
|
|
self.frames.append(self._diagnostic(motor, target))
|
|
elif opcode == 0x03:
|
|
direction = (data[1] >> 2) & 0x03
|
|
target = 5_000 if direction == 1 else 3_000
|
|
self.frames.append(
|
|
self._motor_status(
|
|
motor,
|
|
sequence=sequence,
|
|
value_x100=target,
|
|
complete=True,
|
|
)
|
|
)
|
|
self.frames.append(self._diagnostic(motor, target))
|
|
elif opcode in {0x0E, 0x0F}:
|
|
mode = int.from_bytes(data[4:6], "little") if opcode == 0x0E else 0
|
|
self.frames.append(
|
|
self._frame(
|
|
"T2G_Seat_Dynamic_Mode_Status_306",
|
|
{
|
|
"Dynamic_Mode_ID": mode,
|
|
"Dynamic_State": 1 if opcode == 0x0E else 0,
|
|
"Dynamic_Phase": 1 if opcode == 0x0E else 0,
|
|
"Dynamic_Active_Mask": 0x0F if opcode == 0x0E else 0,
|
|
"Dynamic_Fault_Mask": 0,
|
|
"Dynamic_Cycle_Index": 0,
|
|
"Dynamic_Phase_Elapsed_Ms": 10,
|
|
},
|
|
)
|
|
)
|
|
elif arbitration_id == 0x29E:
|
|
self.frames.append(
|
|
self._frame(
|
|
"T2G_Seat_Thermal_Status_4FA",
|
|
{
|
|
"Driver_Seat_Ventilation_Status": 1,
|
|
"Driver_Seat_Heating_Status": 2,
|
|
},
|
|
)
|
|
)
|
|
return frame
|
|
|
|
def receive(self, _timeout_ms: int, *, include_fd: bool = False) -> CanFrame | None:
|
|
del include_fd
|
|
return self.frames.popleft() if self.frames else None
|
|
|
|
def close(self) -> None:
|
|
self.closed = True
|
|
|
|
|
|
class _PublishOnlyNativeFirmwareBus(_NativeFirmwareBus):
|
|
"""Accept every Tx publication but expose no synchronous state feedback."""
|
|
|
|
def receive(self, _timeout_ms: int, *, include_fd: bool = False) -> CanFrame | None:
|
|
del include_fd
|
|
return None
|
|
|
|
|
|
def _function_config(
|
|
capability: str,
|
|
parameters: dict[str, object],
|
|
*,
|
|
motion: dict[str, object] | None = None,
|
|
) -> dict[str, object]:
|
|
operation = {
|
|
"id": "operation-1",
|
|
"capability": capability,
|
|
"target": {"zone": "front_right", "instance": "passenger"},
|
|
"parameters": parameters,
|
|
}
|
|
if motion is not None:
|
|
operation["motion"] = motion
|
|
return {
|
|
"metadata": {"request_id": "req-mqtt-native-seat"},
|
|
"spec": {"operations": [operation]},
|
|
}
|
|
|
|
|
|
def _motion_plan(
|
|
opcode: int,
|
|
motor: int,
|
|
param_a: int,
|
|
param_b: int,
|
|
*,
|
|
direction: int = 0,
|
|
target: int = 5_000,
|
|
) -> dict[str, object]:
|
|
return {
|
|
"spec": {
|
|
"commands": [
|
|
{
|
|
"id": "native-motion-1",
|
|
"operation_ref": "operation-1",
|
|
"message": "T2G_Seat_Move_Command_300",
|
|
"signals": {
|
|
"Opcode": opcode,
|
|
"Motor_Selector": motor,
|
|
"Direction": direction,
|
|
"Protocol_Version": 1,
|
|
"Sequence": 0,
|
|
"Param_A": param_a,
|
|
"Param_B": param_b,
|
|
},
|
|
"send_offset_ms": 0,
|
|
"completion": {
|
|
"value": target,
|
|
"tolerance": 100,
|
|
"timeout_ms": 1_000,
|
|
},
|
|
}
|
|
]
|
|
}
|
|
}
|
|
|
|
|
|
class MqttNativeSeatExecutorTests(unittest.TestCase):
|
|
def _executor(self) -> tuple[MqttNativeSeatExecutor, _NativeFirmwareBus]:
|
|
service = SeatDemoService(REPOSITORY_ROOT)
|
|
bus = _NativeFirmwareBus(service.hardware_matrix)
|
|
executor = service._new_hardware_executor(
|
|
lambda: bus,
|
|
remote_transport=True,
|
|
)
|
|
self.assertIsInstance(executor, MqttNativeSeatExecutor)
|
|
self.assertIs(MqttOtaSeatExecutor, MqttNativeSeatExecutor)
|
|
return executor, bus
|
|
|
|
def test_wireless_hardware_service_skips_local_profile_and_publishes_native_300(
|
|
self,
|
|
) -> None:
|
|
executor, bus = self._executor()
|
|
service = SeatDemoService(REPOSITORY_ROOT)
|
|
service.vehicle_mqtt_settings = SimpleNamespace(
|
|
enabled=True,
|
|
configured=True,
|
|
authorize=lambda _token: None,
|
|
)
|
|
validated_events: list[dict[str, object]] = []
|
|
|
|
with tempfile.TemporaryDirectory() as temporary_directory:
|
|
service.artifact_root = Path(temporary_directory)
|
|
with (
|
|
patch.object(
|
|
type(service.hardware_settings),
|
|
"validate_function_config",
|
|
side_effect=AssertionError(
|
|
"wireless CAN0 must not check the local combined profile"
|
|
),
|
|
),
|
|
patch.object(
|
|
service.hardware_execution,
|
|
"reserve",
|
|
side_effect=AssertionError(
|
|
"wireless CAN0 must not reserve the local physical bench"
|
|
),
|
|
),
|
|
patch.object(
|
|
service,
|
|
"_new_hardware_executor",
|
|
return_value=executor,
|
|
) as executor_factory,
|
|
):
|
|
result = service.process(
|
|
"把副驾座椅水平位置调到35%",
|
|
provider="local",
|
|
conversation_id="conv-wireless-native-can0",
|
|
execution_mode="hardware",
|
|
control_transport="wireless_mqtt",
|
|
_on_commands_validated=validated_events.append,
|
|
)
|
|
|
|
self.assertEqual(result["status"], "succeeded")
|
|
self.assertEqual(len(validated_events), 1)
|
|
self.assertTrue(executor_factory.call_args.kwargs["remote_transport"])
|
|
self.assertTrue(bus.sent)
|
|
self.assertTrue(all(frame.arbitration_id != 0x6A0 for frame in bus.sent))
|
|
move = next(frame for frame in bus.sent if frame.data[0] == 0x01)
|
|
self.assertEqual(move.arbitration_id, 0x300)
|
|
self.assertEqual(move.data[1] & 0x03, 0)
|
|
self.assertEqual(int.from_bytes(move.data[4:6], "little"), 3_500)
|
|
|
|
def test_publish_without_feedback_is_orchestration_success_not_vehicle_state(
|
|
self,
|
|
) -> None:
|
|
service = SeatDemoService(REPOSITORY_ROOT)
|
|
bus = _PublishOnlyNativeFirmwareBus(service.hardware_matrix)
|
|
executor = service._new_hardware_executor(
|
|
lambda: bus,
|
|
remote_transport=True,
|
|
)
|
|
service.vehicle_mqtt_settings = SimpleNamespace(
|
|
enabled=True,
|
|
configured=True,
|
|
authorize=lambda _token: None,
|
|
)
|
|
conversation_id = "conv-wireless-submit-no-feedback"
|
|
before = service.conversations.snapshot(conversation_id)["devices"][
|
|
"passenger"
|
|
]["ventilation"]
|
|
|
|
with tempfile.TemporaryDirectory() as temporary_directory:
|
|
service.artifact_root = Path(temporary_directory)
|
|
with patch.object(
|
|
service,
|
|
"_new_hardware_executor",
|
|
return_value=executor,
|
|
):
|
|
result = service.process(
|
|
"打开副驾座椅通风",
|
|
provider="local",
|
|
conversation_id=conversation_id,
|
|
execution_mode="hardware",
|
|
control_transport="wireless_mqtt",
|
|
)
|
|
|
|
self.assertEqual(result["status"], "succeeded")
|
|
self.assertEqual(result["execution_outcome"], "submitted_unverified")
|
|
self.assertEqual(
|
|
result["execution_report"]["spec"]["verification_status"],
|
|
"submitted_unverified",
|
|
)
|
|
operation = result["execution_report"]["spec"]["results"][-1]
|
|
self.assertEqual(operation["status"], "succeeded")
|
|
self.assertEqual(
|
|
operation["telemetry"]["verification_status"],
|
|
"submitted_unverified",
|
|
)
|
|
self.assertFalse(operation["telemetry"]["physical_effect_confirmed"])
|
|
self.assertEqual(
|
|
result["actuator_state"]["passenger"]["ventilation"],
|
|
before,
|
|
)
|
|
|
|
def test_publish_only_dynamic_sends_requested_frame_after_sequence_barrier(
|
|
self,
|
|
) -> None:
|
|
service = SeatDemoService(REPOSITORY_ROOT)
|
|
bus = _PublishOnlyNativeFirmwareBus(service.hardware_matrix)
|
|
executor = service._new_hardware_executor(
|
|
lambda: bus,
|
|
remote_transport=True,
|
|
)
|
|
plan = _motion_plan(0x0E, 0, 2, 0, target=2)
|
|
plan["spec"]["commands"][0]["completion"].update(
|
|
{"accepted_states": [1, 2], "timeout_ms": 1_000}
|
|
)
|
|
|
|
report = executor.execute(
|
|
plan,
|
|
_function_config("seat.start_dynamic_mode", {"mode_id": 2}),
|
|
)
|
|
|
|
self.assertEqual(report["spec"]["status"], "succeeded")
|
|
self.assertEqual(report["spec"]["verification_status"], "submitted_unverified")
|
|
self.assertEqual([frame.data[0] for frame in bus.sent], [0x09, 0x0E])
|
|
requested = bus.sent[-1]
|
|
self.assertEqual(requested.arbitration_id, 0x300)
|
|
self.assertEqual(int.from_bytes(requested.data[4:6], "little"), 2)
|
|
result = report["spec"]["results"][-1]
|
|
self.assertFalse(result["telemetry"]["feedback_required"])
|
|
self.assertFalse(result["telemetry"]["physical_effect_confirmed"])
|
|
self.assertEqual(
|
|
[item["payload"]["canId"] for item in result["telemetry"]["mqtt_messages"]],
|
|
["0x300", "0x300"],
|
|
)
|
|
|
|
def test_absolute_move_uses_native_300_publish_only_contract(self) -> None:
|
|
executor, bus = self._executor()
|
|
report = executor.execute(
|
|
_motion_plan(0x01, 0, 5_000, 30_000),
|
|
_function_config(
|
|
"seat.set_longitudinal_position",
|
|
{"position_pct": 50},
|
|
),
|
|
)
|
|
|
|
self.assertEqual(report["spec"]["status"], "succeeded")
|
|
self.assertEqual([frame.data[0] for frame in bus.sent], [0x09, 0x01])
|
|
self.assertTrue(all(frame.arbitration_id == 0x300 for frame in bus.sent))
|
|
move = bus.sent[-1]
|
|
self.assertEqual(move.data[0], 0x01)
|
|
self.assertEqual(int.from_bytes(move.data[4:6], "little"), 5_000)
|
|
telemetry = report["spec"]["results"][0]["telemetry"]
|
|
self.assertEqual(telemetry["verification_status"], "submitted_unverified")
|
|
self.assertFalse(telemetry["feedback_required"])
|
|
self.assertEqual(
|
|
[item["payload"]["canId"] for item in telemetry["mqtt_messages"]],
|
|
["0x300", "0x300"],
|
|
)
|
|
self.assertEqual(report["metadata"]["labels"]["feedback"], "wireless_mqtt_ch0")
|
|
|
|
def test_zero_position_preserves_firmware_known_range_home_opcode(self) -> None:
|
|
executor, bus = self._executor()
|
|
report = executor.execute(
|
|
_motion_plan(0x0B, 3, 0x4B52, 120, target=0),
|
|
_function_config("seat.set_legrest_height", {"position_pct": 0}),
|
|
)
|
|
|
|
self.assertEqual(report["spec"]["status"], "succeeded")
|
|
home = bus.sent[-1]
|
|
self.assertEqual(home.arbitration_id, 0x300)
|
|
self.assertEqual(home.data[0], 0x0B)
|
|
self.assertEqual(int.from_bytes(home.data[4:6], "little"), 0x4B52)
|
|
self.assertEqual(int.from_bytes(home.data[6:8], "little"), 120)
|
|
|
|
def test_relative_fallback_uses_bounded_native_timed_move(self) -> None:
|
|
executor, bus = self._executor()
|
|
report = executor.execute(
|
|
_motion_plan(
|
|
0x03,
|
|
2,
|
|
500,
|
|
900,
|
|
direction=1,
|
|
target=6,
|
|
),
|
|
_function_config(
|
|
"seat.set_backrest_recline",
|
|
{"position_pct": 50},
|
|
motion={
|
|
"mode": "relative",
|
|
"direction": "increase",
|
|
"delta_position_pct": 10,
|
|
},
|
|
),
|
|
)
|
|
|
|
self.assertEqual(report["spec"]["status"], "succeeded")
|
|
move = bus.sent[-1]
|
|
self.assertEqual(move.arbitration_id, 0x300)
|
|
self.assertEqual(move.data[0], 0x03)
|
|
self.assertEqual((move.data[1] >> 2) & 0x03, 1)
|
|
self.assertEqual(int.from_bytes(move.data[4:6], "little"), 500)
|
|
self.assertEqual(int.from_bytes(move.data[6:8], "little"), 900)
|
|
|
|
def test_wireless_thermal_uses_native_3ce_29e(self) -> None:
|
|
executor, bus = self._executor()
|
|
plan = {
|
|
"spec": {
|
|
"commands": [
|
|
{
|
|
"id": "thermal-mode",
|
|
"operation_ref": "operation-1",
|
|
"message": "T2G_Seat_Thermal_Mode_3CE",
|
|
"signals": {"Driver_Seat_Thermal_Mode": 1},
|
|
"send_offset_ms": 0,
|
|
},
|
|
{
|
|
"id": "thermal-control",
|
|
"operation_ref": "operation-1",
|
|
"message": "T2G_Seat_Thermal_Control_29E",
|
|
"signals": {
|
|
"Driver_Seat_Ventilation_On": 2,
|
|
"Driver_Seat_Ventilation_Mode": 1,
|
|
"Driver_Seat_Heating_On": 1,
|
|
"Driver_Seat_Ventilation_Gear": 0,
|
|
"Driver_Seat_Heating_Gear": 1,
|
|
},
|
|
"send_offset_ms": 20,
|
|
},
|
|
]
|
|
}
|
|
}
|
|
with patch("seat_domain.hardware_executor.time.sleep"):
|
|
report = executor.execute(
|
|
plan,
|
|
_function_config("seat.set_heating", {"state": "on", "level": 1}),
|
|
)
|
|
|
|
self.assertEqual(report["spec"]["status"], "succeeded")
|
|
self.assertEqual([frame.arbitration_id for frame in bus.sent], [0x3CE, 0x29E])
|
|
self.assertEqual(bus.sent[0].data, bytes.fromhex("10 00 00 00 00 00 00 00"))
|
|
self.assertEqual(bus.sent[1].data, bytes.fromhex("16 00 10 00 00 00 00 00"))
|
|
result = next(
|
|
item for item in report["spec"]["results"] if item["ref"] == "operation-1"
|
|
)
|
|
self.assertEqual(
|
|
[item["payload"]["canId"] for item in result["telemetry"]["mqtt_messages"]],
|
|
["0x3CE", "0x29E"],
|
|
)
|
|
|
|
def test_dynamic_modes_use_native_300_mode_ids_without_ota_selector_swap(self) -> None:
|
|
for mode_id, mode_name in ((1, "rocking-horse"), (2, "cradle")):
|
|
with self.subTest(mode_id=mode_id):
|
|
executor, bus = self._executor()
|
|
plan = _motion_plan(0x0E, 0, mode_id, 0, target=mode_id)
|
|
plan["spec"]["commands"][0]["completion"].update(
|
|
{
|
|
"accepted_states": [1, 2],
|
|
"timeout_ms": 1_000,
|
|
}
|
|
)
|
|
report = executor.execute(
|
|
plan,
|
|
_function_config("seat.start_dynamic_mode", {"mode_id": mode_id}),
|
|
)
|
|
|
|
self.assertEqual(report["spec"]["status"], "succeeded")
|
|
dynamic = bus.sent[-1]
|
|
self.assertEqual(dynamic.arbitration_id, 0x300)
|
|
self.assertEqual(dynamic.data[0], 0x0E)
|
|
self.assertEqual(int.from_bytes(dynamic.data[4:6], "little"), mode_id)
|
|
self.assertIn(
|
|
f"seat passenger dynamic {mode_name}",
|
|
bus.debug_commands,
|
|
)
|
|
self.assertTrue(all(frame.arbitration_id != 0x6A0 for frame in bus.sent))
|
|
|
|
def test_dynamic_stop_uses_native_300_opcode_0f(self) -> None:
|
|
executor, bus = self._executor()
|
|
plan = _motion_plan(0x0F, 0, 0, 0, target=0)
|
|
plan["spec"]["commands"][0]["completion"].update(
|
|
{"accepted_states": [0], "timeout_ms": 1_000}
|
|
)
|
|
report = executor.execute(
|
|
plan,
|
|
_function_config("seat.stop_dynamic_mode", {}),
|
|
)
|
|
|
|
self.assertEqual(report["spec"]["status"], "succeeded")
|
|
self.assertEqual(len(bus.sent), 1)
|
|
self.assertEqual(bus.sent[0].arbitration_id, 0x300)
|
|
self.assertEqual(bus.sent[0].data[0], 0x0F)
|
|
self.assertEqual(bus.sent[0].data[4:8], bytes(4))
|
|
|
|
def test_seat_height_language_maps_to_internal_axis_one_capability(self) -> None:
|
|
parsed = parse_local_intent("副驾座椅高度调到60%")
|
|
|
|
self.assertEqual(parsed["ambiguities"], [])
|
|
self.assertEqual(parsed["actions"][0]["axis"], "cushion_tilt")
|
|
self.assertEqual(parsed["actions"][0]["position_pct"], 60)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|