diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/CMakeLists.txt new file mode 100644 index 0000000..fa25563 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.16) +project(vehicle_agent_windows CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# --------------------------------------------------------------------------- +# nlohmann/json —— 优先查找系统安装，不存在则 FetchContent 自动下载 +# --------------------------------------------------------------------------- +find_package(nlohmann_json 3.9 QUIET) +if(NOT nlohmann_json_FOUND) + message(STATUS "未找到系统 nlohmann/json，使用 FetchContent 下载") + include(FetchContent) + FetchContent_Declare( + nlohmann_json + GIT_REPOSITORY https://github.com/nlohmann/json.git + GIT_TAG v3.11.3 + GIT_SHALLOW TRUE + ) + FetchContent_MakeAvailable(nlohmann_json) +endif() + +# --------------------------------------------------------------------------- +# 可执行文件 +# --------------------------------------------------------------------------- +add_executable(vehicle_agent_windows + src/main.cpp + src/chassis_domain_server.cpp + src/control_domain_server.cpp +) + +target_include_directories(vehicle_agent_windows PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +target_link_libraries(vehicle_agent_windows PRIVATE nlohmann_json::nlohmann_json) + +# Windows 需要链接 Winsock +if(WIN32) + target_link_libraries(vehicle_agent_windows PRIVATE ws2_32) +endif() + +# --------------------------------------------------------------------------- +# 编译选项 +# --------------------------------------------------------------------------- +if(MSVC) + target_compile_options(vehicle_agent_windows PRIVATE /W4 /utf-8) +else() + target_compile_options(vehicle_agent_windows PRIVATE -Wall -Wextra) +endif() diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/docs/protocol.md b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/docs/protocol.md index 5b647ab..0debc66 100644 --- a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/docs/protocol.md +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/docs/protocol.md @@ -301,4 +301,281 @@ Windows 在动作完成后返回，包含质量评估结果供编排器做自动 | `validation_summary` | object | 验收关键指标（见上）| | `artifacts` | array | 关联数据文件列表 | -**自动验收规则**：编排器要求 `data_quality_passed == true && suitable_for_commit == true` \ No newline at end of file +**自动验收规则**：编排器要求 `data_quality_passed == true && suitable_for_commit == true` + +--- + +## 5. 运控域 payload 格式（端口 9002） + +### 5.1 CONTROL_GET_READINESS_REQ（type=11） + +```json +{ + "agent_name": "control", + "include_details": true, + "target_resource_ids": [] +} +``` + +### 5.2 CONTROL_GET_READINESS_RSP（type=12） + +```json +{ + "success": true, + "error_code": {"code": 0}, + "message": "运控就绪。", + "agent_ready": true, + "trajectory_executor_ready": true, + "vehicle_feedback_ready": true, + "control_output_ready": true, + "estop_released": true, + "vehicle_safe_to_move": true, + "checked_timestamp_us": 1711234567000000 +} +``` + +| 字段 | 类型 | 说明 | +|------|------|------| +| `trajectory_executor_ready` | bool | 轨迹执行器是否就绪 | +| `vehicle_feedback_ready` | bool | 车辆反馈链路是否就绪 | +| `control_output_ready` | bool | 控制输出链路是否就绪 | +| `estop_released` | bool | 急停是否释放（**必须为 true**）| +| `vehicle_safe_to_move` | bool | 是否允许移动（**必须为 true**）| + +### 5.3 CONTROL_EVALUATION_REQ（type=13） + +Ubuntu 发送，让车端按当前控制参数执行一次评估任务。`selected_task` 决定哪个子对象有效。 + +```json +{ + "header": { + "session_id": "workshop_v2_1711234567000000_1", + "task_id": "stage_control_lateral_001", + "vehicle_id": "AGV-001", + "request_id": "chassis_req_1711234568000000", + "client_send_timestamp_us": 1711234568000000, + "operator_id": "op_001", + "workshop_host": "ubuntu-workshop-pc" + }, + "test_case_id": "lateral_tracking_v1", + "task_purpose": 1, + "selected_task": 1, + "source_iteration_id": "iter_001", + "trajectory_tracking": { + "path": [ + {"x_m": 0.0, "y_m": 0.0, "yaw_rad": 0.0, "target_speed_ms": 0.5}, + {"x_m": 5.0, "y_m": 0.0, "yaw_rad": 0.0, "target_speed_ms": 0.5}, + {"x_m": 10.0, "y_m": 0.0, "yaw_rad": 0.0, "target_speed_ms": 0.0} + ], + "stop_at_end": true, + "timeout_sec": 60.0 + } +} +``` + +#### `selected_task` 枚举值 + +| 值 | 名称 | 有效子对象 | 说明 | +|----|------|----------|------| +| 0 | UNSPECIFIED | — | — | +| 1 | TRAJECTORY_TRACKING | `trajectory_tracking` | 跟踪给定轨迹，评估横向/航向误差 | +| 2 | VELOCITY_STEP | `velocity_step` | 速度阶跃，评估纵向动态响应 | +| 3 | ACCELERATION_DECELERATION | `accel_decel` | 加减速，评估纵向平顺性与 jerk | +| 4 | STOP_ACCURACY | `stop_accuracy` | 评估停车位置精度 | + +#### 各任务子对象格式 + +**trajectory_tracking** +```json +{ + "path": [ + {"x_m": 0.0, "y_m": 0.0, "yaw_rad": 0.0, "target_speed_ms": 0.5} + ], + "stop_at_end": true, + "timeout_sec": 60.0 +} +``` + +**velocity_step** +```json +{ + "target_velocity_ms": 1.0, + "hold_time_sec": 5.0, + "settle_before_step_sec": 2.0 +} +``` + +**accel_decel** +```json +{ + "start_velocity_ms": 0.0, + "target_velocity_ms": 1.5, + "target_accel_ms2": 0.5, + "hold_time_sec": 3.0 +} +``` + +**stop_accuracy** +```json +{ + "target_stop_x_m": 10.0, + "target_stop_y_m": 0.0, + "target_stop_yaw_rad": 0.0, + "timeout_sec": 30.0 +} +``` + +### 5.4 CONTROL_EVALUATION_RSP（type=14） + +```json +{ + "success": true, + "error_code": {"code": 0}, + "message": "轨迹跟踪评估完成。", + "job_id": "chassis_req_1711234568000000", + "data_quality_passed": true, + "suitable_for_commit": true, + "recommended_parameter_version": "control_lateral_pid_v2", + "validation_summary": { + "rms_lateral_error_m": 0.015, + "rms_heading_error_rad": 0.010, + "rms_speed_error_ms": 0.05, + "overshoot_ratio": 0.08, + "settle_time_sec": 1.2, + "stop_position_error_m": 0.02, + "max_jerk": 0.3, + "saturation_ratio": 0.05, + "auto_acceptance_passed": true + }, + "artifacts": [ + { + "file_name": "control_eval_001.csv", + "file_uri": "C:/calib_data/control_eval_001.csv", + "size_bytes": 204800, + "description": "控制评估误差时序数据" + } + ] +} +``` + +| 字段 | 类型 | 说明 | +|------|------|------| +| `data_quality_passed` | bool | 数据质量是否达标 | +| `suitable_for_commit` | bool | 参数是否适合写入 | +| `validation_summary` | object | 验收关键指标 | +| `validation_summary.rms_lateral_error_m` | float64 | 横向误差 RMS（m）| +| `validation_summary.rms_heading_error_rad` | float64 | 航向误差 RMS（rad）| +| `validation_summary.rms_speed_error_ms` | float64 | 速度误差 RMS（m/s）| +| `validation_summary.overshoot_ratio` | float64 | 超调比例（0~1）| +| `validation_summary.settle_time_sec` | float64 | 收敛时间（s）| +| `validation_summary.stop_position_error_m` | float64 | 停车位置误差（m）| +| `validation_summary.max_jerk` | float64 | 最大 jerk（m/s³）| +| `validation_summary.saturation_ratio` | float64 | 控制饱和占比（0~1）| +| `validation_summary.auto_acceptance_passed` | bool | 综合自动验收是否通过 | + +--- + +## 6. 交互时序 + +### 6.1 底盘标定完整流程 + +``` +Ubuntu (orchestrator) Ubuntu (gateway) Windows (vehicle_agent) + │ │ │ + │── execute_goal ───────────►│ │ + │ │── TCP connect :9001 ──────►│ + │ │── CHASSIS_GET_READINESS_REQ│ + │ │◄─ CHASSIS_GET_READINESS_RSP│ + │ │── TCP close ───────────────│ + │ │ │ + │ (readiness check passed) │ │ + │ │── TCP connect :9001 ──────►│ + │ │── CHASSIS_MOTION_PRIM_REQ ─│ (车辆开始运动) + │ │ ... 等待车辆完成 ... │ + │ │◄─ CHASSIS_MOTION_PRIM_RSP ─│ (含验收指标) + │ │── TCP close ───────────────│ + │◄── action result ─────────│ │ +``` + +### 6.2 注意事项 + +1. **每次请求建立新 TCP 连接**：Ubuntu 侧 `TcpClient` 每次 `send_recv` 都会新建、使用、关闭连接。Windows 侧不需要维护长连接状态。 +2. **超时控制**：Ubuntu 侧默认等待 5000ms（可通过 ROS2 参数 `chassis_timeout_ms` / `control_timeout_ms` 调整）。动作原语执行时间可能远超此值，Windows 侧应在动作完成后才回复 RSP 帧，不要提前响应。 +3. **急停处理**：Ubuntu 侧 orchestrator 在收到取消请求时会关闭 socket 连接。Windows 侧检测到连接断开即应停止当前动作并触发安全停车。 +4. **并发**：底盘域和运控域在不同端口，理论上可并发，但编排器保证同一时刻只有一个域的任务在执行。 + +--- + +## 7. Windows 侧实现最小要求 + +### 7.1 必须实现的接口 + +| 端口 | REQ type | 行为 | +|------|----------|------| +| 9001 | type=1 READINESS | 检查底盘状态，返回 type=2 | +| 9001 | type=3 MOTION_PRIMITIVE | 执行指定动作，完成后返回 type=4（含验收指标）| +| 9002 | type=11 READINESS | 检查运控状态，返回 type=12 | +| 9002 | type=13 EVALUATION | 执行控制评估，完成后返回 type=14（含验收指标）| + +### 7.2 错误响应格式 + +执行失败时，`success` 设为 `false`，`error_code.code` 填对应错误码，`message` 填人读错误说明： + +```json +{ + "success": false, + "error_code": {"code": 5}, + "message": "底盘驱动离线，无法执行动作。", + "job_id": "chassis_req_1711234567100000", + "data_quality_passed": false, + "suitable_for_commit": false +} +``` + +### 7.3 联调工具 + +Ubuntu 侧提供了 Python stub 服务器用于 Windows 侧联调前的自测： + +```bash +# 在 Ubuntu 上运行，模拟 Windows 车端（返回空 payload 的成功响应） +python3 src/win_ubuntu_bridge/vehicle_agent_gateway/scripts/windows_stub_server.py +``` + +--- + +## 附录 A：ErrorCode 值 + +| 值 | 名称 | 含义 | +|----|------|------| +| 0 | UNSPECIFIED | 未指定 | +| 1 | OK | 成功 | +| 2 | INVALID_ARGUMENT | 参数非法 | +| 3 | INVALID_STATE | 当前状态不允许此操作 | +| 4 | VEHICLE_BUSY | 车辆当前忙 | +| 5 | NOT_READY | 未准备好 | +| 6 | TIMEOUT | 超时 | +| 7 | NETWORK_LOSS | 网络断开 | +| 8 | SAFETY_TRIGGERED | 安全保护触发 | +| 9 | HARDWARE_FAULT | 硬件故障 | +| 10 | FILE_NOT_FOUND | 文件不存在 | +| 11 | CHECKSUM_MISMATCH | 校验失败 | +| 12 | INTERNAL_ERROR | 内部错误 | +| 13 | UNSUPPORTED_CAPABILITY | 当前车辆不支持该能力 | +| 14 | RESOURCE_LOCKED | 资源已被其他会话占用 | +| 15 | MANUAL_CONFIRM_REQUIRED | 需要人工确认 | +| 16 | APPROVAL_REQUIRED | 需要审批 | +| 17 | VALIDATION_FAILED | 验证失败 | +| 18 | ROLLBACK_REQUIRED | 需要回滚 | +| 19 | DATA_QUALITY_INSUFFICIENT | 数据质量不足 | + +--- + +## 附录 B：快速验证清单 + +Windows 侧自测时依次验证： + +- [ ] 监听 9001 端口，收到 type=1 帧，回复 type=2 帧（`success=true`, `estop_released=true`, `vehicle_safe_to_move=true`） +- [ ] 收到 type=3 帧（`selected_primitive=1` 直线），车辆执行，完成后回复 type=4（`success=true`, `data_quality_passed=true`, `suitable_for_commit=true`） +- [ ] 监听 9002 端口，收到 type=11 帧，回复 type=12 帧 +- [ ] 收到 type=13 帧（`selected_task=1` 轨迹跟踪），执行，回复 type=14 +- [ ] 模拟急停：收到 type=3 帧后主动关闭连接，验证车辆停车 diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/frame_codec.py b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/frame_codec.py new file mode 100644 index 0000000..d2c3e4a --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/frame_codec.py @@ -0,0 +1,311 @@ +"""frame_codec.py + +TCP 帧编解码层。 + +帧格式（与 Ubuntu 侧 proto_frame.cpp 完全一致）： + [4字节 LE uint32: msg_type][4字节 LE uint32: payload_len][payload_len字节: UTF-8 JSON] + +所有公共 JSON 字段的键名与 proto 字段名对齐，Ubuntu 侧 gateway_codec.hpp 是权威参考。 +""" + +import json +import socket +import struct +from enum import IntEnum +from typing import Any, Dict, Tuple + + +# --------------------------------------------------------------------------- +# 消息类型枚举（与 proto_frame.hpp MsgType 完全一致） +# --------------------------------------------------------------------------- +class MsgType(IntEnum): + # 底盘域 + CHASSIS_GET_READINESS_REQ = 1 + CHASSIS_GET_READINESS_RSP = 2 + CHASSIS_MOTION_PRIMITIVE_REQ = 3 + CHASSIS_MOTION_PRIMITIVE_RSP = 4 + CHASSIS_EMERGENCY_BRAKE_REQ = 5 + CHASSIS_EMERGENCY_BRAKE_RSP = 6 + # 运控域 + CONTROL_GET_READINESS_REQ = 11 + CONTROL_GET_READINESS_RSP = 12 + CONTROL_EVALUATION_REQ = 13 + CONTROL_EVALUATION_RSP = 14 + + +# --------------------------------------------------------------------------- +# 底盘动作原语类型（与 ChassisMotionPrimitiveType.msg 一致） +# --------------------------------------------------------------------------- +class ChassisMotionPrimitiveType(IntEnum): + UNSPECIFIED = 0 + STRAIGHT_LINE = 1 + ARC = 2 + IN_PLACE_ROTATION = 3 + STEERING_SWEEP = 4 + LATERAL_TRANSLATION = 5 + DIAGONAL_MOTION = 6 + MODULE_ALIGNMENT = 7 + COORDINATED_STEERING = 8 + + +# --------------------------------------------------------------------------- +# 运控评估任务类型（与 ControllerEvaluationTaskType.msg 一致） +# --------------------------------------------------------------------------- +class ControlEvaluationTaskType(IntEnum): + UNSPECIFIED = 0 + TRAJECTORY_TRACKING = 1 + VELOCITY_STEP = 2 + ACCELERATION_DECELERATION = 3 + STOP_ACCURACY = 4 + + +# --------------------------------------------------------------------------- +# ErrorCode（与 ErrorCode.msg 一致） +# --------------------------------------------------------------------------- +class ErrorCode(IntEnum): + UNSPECIFIED = 0 + OK = 1 + INVALID_ARGUMENT = 2 + INVALID_STATE = 3 + VEHICLE_BUSY = 4 + NOT_READY = 5 + TIMEOUT = 6 + NETWORK_LOSS = 7 + SAFETY_TRIGGERED = 8 + HARDWARE_FAULT = 9 + FILE_NOT_FOUND = 10 + CHECKSUM_MISMATCH = 11 + INTERNAL_ERROR = 12 + UNSUPPORTED_CAPABILITY = 13 + RESOURCE_LOCKED = 14 + MANUAL_CONFIRM_REQUIRED = 15 + APPROVAL_REQUIRED = 16 + VALIDATION_FAILED = 17 + ROLLBACK_REQUIRED = 18 + DATA_QUALITY_INSUFFICIENT = 19 + + +# --------------------------------------------------------------------------- +# 底层 I/O 工具 +# --------------------------------------------------------------------------- + +def _recv_exactly(conn: socket.socket, n: int) -> bytes: + """从 conn 读取恰好 n 字节，连接关闭时抛出 ConnectionError。""" + buf = bytearray() + while len(buf) < n: + chunk = conn.recv(n - len(buf)) + if not chunk: + raise ConnectionError("连接已关闭") + buf.extend(chunk) + return bytes(buf) + + +def recv_frame(conn: socket.socket) -> Tuple[MsgType, Dict[str, Any]]: + """从 conn 读一帧，返回 (msg_type, payload_dict)。 + + payload_dict 为空 dict 表示 payload_len=0 的帧。 + """ + header = _recv_exactly(conn, 8) + msg_type_val, payload_len = struct.unpack_from(" None: + """向 conn 发一帧。payload 为空 dict 时 payload_len=0。""" + if payload: + data = json.dumps(payload, ensure_ascii=False).encode("utf-8") + else: + data = b"" + header = struct.pack(" Dict[str, int]: + return {"code": int(code)} + + +def make_success_response(message: str = "") -> Dict[str, Any]: + return { + "success": True, + "error_code": make_error_code(ErrorCode.OK), + "message": message, + } + + +def make_failure_response(message: str, code: ErrorCode = ErrorCode.INTERNAL_ERROR) -> Dict[str, Any]: + return { + "success": False, + "error_code": make_error_code(code), + "message": message, + } + + +# --------------------------------------------------------------------------- +# Readiness 响应构造辅助 +# --------------------------------------------------------------------------- + +def make_chassis_readiness_response( + *, + success: bool, + message: str, + agent_ready: bool, + chassis_driver_online: bool, + motion_control_ready: bool, + estop_released: bool, + vehicle_safe_to_move: bool, + telemetry_available: bool, + checked_timestamp_us: int, + error_code: ErrorCode = ErrorCode.OK, +) -> Dict[str, Any]: + """构造底盘就绪响应 payload（对应 ChassisReadinessResponse.msg）。""" + return { + "success": success, + "error_code": make_error_code(error_code), + "message": message, + "agent_ready": agent_ready, + "chassis_driver_online": chassis_driver_online, + "motion_control_ready": motion_control_ready, + "estop_released": estop_released, + "vehicle_safe_to_move": vehicle_safe_to_move, + "telemetry_available": telemetry_available, + "checked_timestamp_us": checked_timestamp_us, + } + + +def make_control_readiness_response( + *, + success: bool, + message: str, + agent_ready: bool, + trajectory_executor_ready: bool, + vehicle_feedback_ready: bool, + control_output_ready: bool, + estop_released: bool, + vehicle_safe_to_move: bool, + checked_timestamp_us: int, + error_code: ErrorCode = ErrorCode.OK, +) -> Dict[str, Any]: + """构造运控就绪响应 payload（对应 ControlReadinessResponse.msg）。""" + return { + "success": success, + "error_code": make_error_code(error_code), + "message": message, + "agent_ready": agent_ready, + "trajectory_executor_ready": trajectory_executor_ready, + "vehicle_feedback_ready": vehicle_feedback_ready, + "control_output_ready": control_output_ready, + "estop_released": estop_released, + "vehicle_safe_to_move": vehicle_safe_to_move, + "checked_timestamp_us": checked_timestamp_us, + } + + +# --------------------------------------------------------------------------- +# 任务结果响应构造辅助 +# --------------------------------------------------------------------------- + +def make_chassis_job_result( + *, + success: bool, + message: str, + job_id: str, + data_quality_passed: bool, + suitable_for_commit: bool, + recommended_parameter_version: str = "", + estimated_straight_line_bias: float = 0.0, + max_lateral_error_m: float = 0.0, + max_yaw_error_rad: float = 0.0, + rms_lateral_error_m: float = 0.0, + rms_yaw_error_rad: float = 0.0, + repeatability_error_m: float = 0.0, + curvature_error: float = 0.0, + module_consistency_error: float = 0.0, + auto_acceptance_passed: bool = False, + artifacts: list = None, + error_code: ErrorCode = ErrorCode.OK, +) -> Dict[str, Any]: + """构造底盘任务结果 payload（对应 ChassisJobResult.msg）。""" + return { + "success": success, + "error_code": make_error_code(error_code if success else ErrorCode.INTERNAL_ERROR), + "message": message, + "job_id": job_id, + "data_quality_passed": data_quality_passed, + "suitable_for_commit": suitable_for_commit, + "recommended_parameter_version": recommended_parameter_version, + "estimated_straight_line_bias": estimated_straight_line_bias, + "validation_summary": { + "max_lateral_error_m": max_lateral_error_m, + "max_yaw_error_rad": max_yaw_error_rad, + "rms_lateral_error_m": rms_lateral_error_m, + "rms_yaw_error_rad": rms_yaw_error_rad, + "repeatability_error_m": repeatability_error_m, + "curvature_error": curvature_error, + "module_consistency_error": module_consistency_error, + "auto_acceptance_passed": auto_acceptance_passed, + }, + "artifacts": artifacts or [], + } + + +def make_control_job_result( + *, + success: bool, + message: str, + job_id: str, + data_quality_passed: bool, + suitable_for_commit: bool, + recommended_parameter_version: str = "", + rms_lateral_error_m: float = 0.0, + rms_heading_error_rad: float = 0.0, + rms_speed_error_ms: float = 0.0, + overshoot_ratio: float = 0.0, + settle_time_sec: float = 0.0, + stop_position_error_m: float = 0.0, + max_jerk: float = 0.0, + saturation_ratio: float = 0.0, + auto_acceptance_passed: bool = False, + artifacts: list = None, + error_code: ErrorCode = ErrorCode.OK, +) -> Dict[str, Any]: + """构造运控任务结果 payload（对应 ControlJobResult.msg）。""" + return { + "success": success, + "error_code": make_error_code(error_code if success else ErrorCode.INTERNAL_ERROR), + "message": message, + "job_id": job_id, + "data_quality_passed": data_quality_passed, + "suitable_for_commit": suitable_for_commit, + "recommended_parameter_version": recommended_parameter_version, + "validation_summary": { + "rms_lateral_error_m": rms_lateral_error_m, + "rms_heading_error_rad": rms_heading_error_rad, + "rms_speed_error_ms": rms_speed_error_ms, + "overshoot_ratio": overshoot_ratio, + "settle_time_sec": settle_time_sec, + "stop_position_error_m": stop_position_error_m, + "max_jerk": max_jerk, + "saturation_ratio": saturation_ratio, + "auto_acceptance_passed": auto_acceptance_passed, + }, + "artifacts": artifacts or [], + } + + +def make_artifact(file_name: str, file_uri: str, size_bytes: int = 0, description: str = "") -> Dict[str, Any]: + """构造文件引用条目（对应 FileReference.msg）。""" + return { + "file_name": file_name, + "file_uri": file_uri, + "size_bytes": size_bytes, + "description": description, + } diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/chassis_domain_server.hpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/chassis_domain_server.hpp new file mode 100644 index 0000000..5c6943b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/chassis_domain_server.hpp @@ -0,0 +1,31 @@ +#pragma once +// chassis_domain_server.hpp + +#include +#include +#include "frame_codec.hpp" +#include "chassis_handler.hpp" + +namespace vaw +{ + +class ChassisDomainServer +{ +public: + explicit ChassisDomainServer(uint16_t port, ChassisHandler* handler); + ~ChassisDomainServer(); + + // 阻塞运行，直到 stop() 被调用 + void run(); + void stop(); + +private: + void handle_connection(sock_t conn); + + uint16_t port_; + ChassisHandler* handler_; + std::atomic running_; + sock_t listen_sock_; +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/chassis_handler.hpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/chassis_handler.hpp new file mode 100644 index 0000000..c3742b8 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/chassis_handler.hpp @@ -0,0 +1,65 @@ +#pragma once +// chassis_handler.hpp +// 底盘处理器虚基类。 +// 子类覆盖各方法后即可接入真实底盘驱动；默认实现返回 stub 成功响应。 + +#include +#include "data_types.hpp" + +namespace vaw +{ + +class ChassisHandler +{ +public: + virtual ~ChassisHandler() = default; + + // 查询底盘就绪状态 + // req: CHASSIS_GET_READINESS_REQ 解析结果（AgentReadinessRequest 字段） + virtual ChassisReadinessResponse get_readiness(const nlohmann::json& req) + { + ChassisReadinessResponse rsp; + rsp.success = true; + rsp.error_code = ErrorCode::OK; + rsp.message = "底盘就绪（stub）"; + rsp.agent_ready = true; + rsp.chassis_driver_online = true; + rsp.motion_control_ready = true; + rsp.estop_released = true; + rsp.vehicle_safe_to_move = true; + rsp.telemetry_available = true; + rsp.checked_timestamp_us = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + return rsp; + } + + // 执行底盘动作原语 + // req: 已解析的 MotionPrimitiveRequest + virtual ChassisJobResult execute_motion_primitive(const MotionPrimitiveRequest& req) + { + (void)req; + ChassisJobResult result; + result.success = true; + result.error_code = ErrorCode::OK; + result.message = "动作原语执行完成（stub）"; + result.job_id = req.test_case_id; + result.data_quality_passed = true; + result.suitable_for_commit = true; + result.validation_summary.auto_acceptance_passed = true; + return result; + } + + // 紧急制动 + // req: CHASSIS_EMERGENCY_BRAKE_REQ payload + virtual nlohmann::json emergency_brake(const nlohmann::json& req) + { + (void)req; + return nlohmann::json{ + {"success", true}, + {"error_code", {"code", 0}}, + {"message", "紧急制动已执行（stub）"}, + }; + } +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/control_domain_server.hpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/control_domain_server.hpp new file mode 100644 index 0000000..2dd7962 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/control_domain_server.hpp @@ -0,0 +1,31 @@ +#pragma once +// control_domain_server.hpp + +#include +#include +#include "frame_codec.hpp" +#include "control_handler.hpp" + +namespace vaw +{ + +class ControlDomainServer +{ +public: + explicit ControlDomainServer(uint16_t port, ControlHandler* handler); + ~ControlDomainServer(); + + // 阻塞运行，直到 stop() 被调用 + void run(); + void stop(); + +private: + void handle_connection(sock_t conn); + + uint16_t port_; + ControlHandler* handler_; + std::atomic running_; + sock_t listen_sock_; +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/control_handler.hpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/control_handler.hpp new file mode 100644 index 0000000..367643b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/control_handler.hpp @@ -0,0 +1,53 @@ +#pragma once +// control_handler.hpp +// 运控处理器虚基类。 +// 子类覆盖各方法后即可接入真实运控驱动；默认实现返回 stub 成功响应。 + +#include +#include "data_types.hpp" + +namespace vaw +{ + +class ControlHandler +{ +public: + virtual ~ControlHandler() = default; + + // 查询运控就绪状态 + virtual ControlReadinessResponse get_readiness(const nlohmann::json& req) + { + (void)req; + ControlReadinessResponse rsp; + rsp.success = true; + rsp.error_code = ErrorCode::OK; + rsp.message = "运控就绪（stub）"; + rsp.agent_ready = true; + rsp.trajectory_executor_ready = true; + rsp.vehicle_feedback_ready = true; + rsp.control_output_ready = true; + rsp.estop_released = true; + rsp.vehicle_safe_to_move = true; + rsp.checked_timestamp_us = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + return rsp; + } + + // 执行控制评估任务 + // req: 已解析的 ControllerEvaluationRequest + virtual ControlJobResult execute_evaluation(const ControllerEvaluationRequest& req) + { + (void)req; + ControlJobResult result; + result.success = true; + result.error_code = ErrorCode::OK; + result.message = "控制评估完成（stub）"; + result.job_id = req.test_case_id; + result.data_quality_passed = true; + result.suitable_for_commit = true; + result.validation_summary.auto_acceptance_passed = true; + return result; + } +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/data_types.hpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/data_types.hpp new file mode 100644 index 0000000..6fb7c27 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/data_types.hpp @@ -0,0 +1,630 @@ +#pragma once +// data_types.hpp +// 所有消息数据结构定义，JSON key 与 proto 字段名及 gateway_codec.hpp 完全一致。 +// 使用 nlohmann/json ADL 钩子（from_json / to_json）实现序列化。 + +#include +#include +#include +#include +#include + +namespace vaw +{ + +// =========================================================================== +// 枚举 +// =========================================================================== + +enum class ErrorCode : uint32_t { + UNSPECIFIED = 0, + OK = 1, + INVALID_ARGUMENT = 2, + INVALID_STATE = 3, + VEHICLE_BUSY = 4, + NOT_READY = 5, + TIMEOUT = 6, + NETWORK_LOSS = 7, + SAFETY_TRIGGERED = 8, + HARDWARE_FAULT = 9, + FILE_NOT_FOUND = 10, + CHECKSUM_MISMATCH = 11, + INTERNAL_ERROR = 12, + UNSUPPORTED_CAPABILITY = 13, + RESOURCE_LOCKED = 14, + MANUAL_CONFIRM_REQUIRED = 15, + APPROVAL_REQUIRED = 16, + VALIDATION_FAILED = 17, + ROLLBACK_REQUIRED = 18, + DATA_QUALITY_INSUFFICIENT = 19, +}; + +enum class ChassisMotionPrimitiveType : uint32_t { + UNSPECIFIED = 0, + STRAIGHT_LINE = 1, + ARC = 2, + IN_PLACE_ROTATION = 3, + STEERING_SWEEP = 4, + LATERAL_TRANSLATION = 5, + DIAGONAL_MOTION = 6, + MODULE_ALIGNMENT = 7, + COORDINATED_STEERING = 8, +}; + +enum class ControlEvaluationTaskType : uint32_t { + UNSPECIFIED = 0, + TRAJECTORY_TRACKING = 1, + VELOCITY_STEP = 2, + ACCELERATION_DECELERATION = 3, + STOP_ACCURACY = 4, +}; + +// =========================================================================== +// 公共类型 +// =========================================================================== + +struct RequestHeader { + std::string session_id; + std::string task_id; + std::string vehicle_id; + std::string request_id; + int64_t client_send_timestamp_us{0}; + std::string operator_id; + std::string workshop_host; +}; + +inline void from_json(const nlohmann::json& j, RequestHeader& h) +{ + h.session_id = j.value("session_id", std::string{}); + h.task_id = j.value("task_id", std::string{}); + h.vehicle_id = j.value("vehicle_id", std::string{}); + h.request_id = j.value("request_id", std::string{}); + h.client_send_timestamp_us = j.value("client_send_timestamp_us", int64_t{0}); + h.operator_id = j.value("operator_id", std::string{}); + h.workshop_host = j.value("workshop_host", std::string{}); +} + +inline void to_json(nlohmann::json& j, const RequestHeader& h) +{ + j = { + {"session_id", h.session_id}, + {"task_id", h.task_id}, + {"vehicle_id", h.vehicle_id}, + {"request_id", h.request_id}, + {"client_send_timestamp_us", h.client_send_timestamp_us}, + {"operator_id", h.operator_id}, + {"workshop_host", h.workshop_host}, + }; +} + +struct FileReference { + std::string file_name; + std::string file_uri; + int64_t size_bytes{0}; + std::string description; +}; + +inline void from_json(const nlohmann::json& j, FileReference& r) +{ + r.file_name = j.value("file_name", std::string{}); + r.file_uri = j.value("file_uri", std::string{}); + r.size_bytes = j.value("size_bytes", int64_t{0}); + r.description = j.value("description", std::string{}); +} + +inline void to_json(nlohmann::json& j, const FileReference& r) +{ + j = { + {"file_name", r.file_name}, + {"file_uri", r.file_uri}, + {"size_bytes", r.size_bytes}, + {"description", r.description}, + }; +} + +// =========================================================================== +// 底盘命令原语 +// =========================================================================== + +struct StraightLineCommand { + double target_speed_ms{0.0}; + double target_distance_m{0.0}; + bool reverse{false}; +}; +inline void from_json(const nlohmann::json& j, StraightLineCommand& c) { + c.target_speed_ms = j.value("target_speed_ms", 0.0); + c.target_distance_m = j.value("target_distance_m", 0.0); + c.reverse = j.value("reverse", false); +} +inline void to_json(nlohmann::json& j, const StraightLineCommand& c) { + j = {{"target_speed_ms", c.target_speed_ms}, + {"target_distance_m", c.target_distance_m}, + {"reverse", c.reverse}}; +} + +struct ArcCommand { + double target_speed_ms{0.0}; + double radius_m{0.0}; + double sweep_angle_deg{0.0}; + bool clockwise{false}; +}; +inline void from_json(const nlohmann::json& j, ArcCommand& c) { + c.target_speed_ms = j.value("target_speed_ms", 0.0); + c.radius_m = j.value("radius_m", 0.0); + c.sweep_angle_deg = j.value("sweep_angle_deg", 0.0); + c.clockwise = j.value("clockwise", false); +} +inline void to_json(nlohmann::json& j, const ArcCommand& c) { + j = {{"target_speed_ms", c.target_speed_ms}, + {"radius_m", c.radius_m}, + {"sweep_angle_deg", c.sweep_angle_deg}, + {"clockwise", c.clockwise}}; +} + +struct InPlaceRotationCommand { + double target_yaw_deg{0.0}; + double target_angular_vel_deg_s{0.0}; +}; +inline void from_json(const nlohmann::json& j, InPlaceRotationCommand& c) { + c.target_yaw_deg = j.value("target_yaw_deg", 0.0); + c.target_angular_vel_deg_s = j.value("target_angular_vel_deg_s", 0.0); +} +inline void to_json(nlohmann::json& j, const InPlaceRotationCommand& c) { + j = {{"target_yaw_deg", c.target_yaw_deg}, + {"target_angular_vel_deg_s", c.target_angular_vel_deg_s}}; +} + +struct SteeringSweepCommand { + double target_angle_deg{0.0}; + double sweep_amplitude_deg{0.0}; + double sweep_frequency_hz{0.0}; + double duration_sec{0.0}; +}; +inline void from_json(const nlohmann::json& j, SteeringSweepCommand& c) { + c.target_angle_deg = j.value("target_angle_deg", 0.0); + c.sweep_amplitude_deg = j.value("sweep_amplitude_deg", 0.0); + c.sweep_frequency_hz = j.value("sweep_frequency_hz", 0.0); + c.duration_sec = j.value("duration_sec", 0.0); +} +inline void to_json(nlohmann::json& j, const SteeringSweepCommand& c) { + j = {{"target_angle_deg", c.target_angle_deg}, + {"sweep_amplitude_deg", c.sweep_amplitude_deg}, + {"sweep_frequency_hz", c.sweep_frequency_hz}, + {"duration_sec", c.duration_sec}}; +} + +struct LateralTranslationCommand { + double target_speed_ms{0.0}; + double target_distance_m{0.0}; + bool move_left{false}; +}; +inline void from_json(const nlohmann::json& j, LateralTranslationCommand& c) { + c.target_speed_ms = j.value("target_speed_ms", 0.0); + c.target_distance_m = j.value("target_distance_m", 0.0); + c.move_left = j.value("move_left", false); +} +inline void to_json(nlohmann::json& j, const LateralTranslationCommand& c) { + j = {{"target_speed_ms", c.target_speed_ms}, + {"target_distance_m", c.target_distance_m}, + {"move_left", c.move_left}}; +} + +struct DiagonalMotionCommand { + double target_speed_ms{0.0}; + double target_distance_m{0.0}; + double heading_deg{0.0}; +}; +inline void from_json(const nlohmann::json& j, DiagonalMotionCommand& c) { + c.target_speed_ms = j.value("target_speed_ms", 0.0); + c.target_distance_m = j.value("target_distance_m", 0.0); + c.heading_deg = j.value("heading_deg", 0.0); +} +inline void to_json(nlohmann::json& j, const DiagonalMotionCommand& c) { + j = {{"target_speed_ms", c.target_speed_ms}, + {"target_distance_m", c.target_distance_m}, + {"heading_deg", c.heading_deg}}; +} + +struct ModuleAlignmentCheckCommand { + std::vector module_ids; + double target_zero_deg{0.0}; + double tolerance_deg{0.0}; +}; +inline void from_json(const nlohmann::json& j, ModuleAlignmentCheckCommand& c) { + if (j.contains("module_ids") && j["module_ids"].is_array()) + c.module_ids = j["module_ids"].get>(); + c.target_zero_deg = j.value("target_zero_deg", 0.0); + c.tolerance_deg = j.value("tolerance_deg", 0.0); +} +inline void to_json(nlohmann::json& j, const ModuleAlignmentCheckCommand& c) { + j = {{"module_ids", c.module_ids}, + {"target_zero_deg", c.target_zero_deg}, + {"tolerance_deg", c.tolerance_deg}}; +} + +struct CoordinatedSteeringCommand { + std::vector module_ids; + double target_angle_deg{0.0}; + double hold_time_sec{0.0}; +}; +inline void from_json(const nlohmann::json& j, CoordinatedSteeringCommand& c) { + if (j.contains("module_ids") && j["module_ids"].is_array()) + c.module_ids = j["module_ids"].get>(); + c.target_angle_deg = j.value("target_angle_deg", 0.0); + c.hold_time_sec = j.value("hold_time_sec", 0.0); +} +inline void to_json(nlohmann::json& j, const CoordinatedSteeringCommand& c) { + j = {{"module_ids", c.module_ids}, + {"target_angle_deg", c.target_angle_deg}, + {"hold_time_sec", c.hold_time_sec}}; +} + +// =========================================================================== +// 底盘请求聚合 +// =========================================================================== + +// primitive 字段用 variant，index 对应 ChassisMotionPrimitiveType 枚举值 +using ChassisMotionPrimitive = std::variant< + std::monostate, // UNSPECIFIED (0) + StraightLineCommand, // 1 + ArcCommand, // 2 + InPlaceRotationCommand, // 3 + SteeringSweepCommand, // 4 + LateralTranslationCommand, // 5 + DiagonalMotionCommand, // 6 + ModuleAlignmentCheckCommand, // 7 + CoordinatedSteeringCommand // 8 +>; + +struct MotionPrimitiveRequest { + RequestHeader header; + std::string test_case_id; + uint32_t task_purpose{0}; // TaskPurpose enum value + ChassisMotionPrimitiveType selected_primitive{ChassisMotionPrimitiveType::UNSPECIFIED}; + ChassisMotionPrimitive primitive; // 对应 selected_primitive 的具体命令 + bool brake_when_finished{false}; + double timeout_sec{0.0}; + std::string source_iteration_id; +}; + +// 从 JSON 解析 MotionPrimitiveRequest（对应 gateway_codec.hpp encode_motion_primitive_request） +inline void from_json(const nlohmann::json& j, MotionPrimitiveRequest& r) +{ + if (j.contains("header")) r.header = j["header"].get(); + r.test_case_id = j.value("test_case_id", std::string{}); + r.task_purpose = j.value("task_purpose", uint32_t{0}); + r.selected_primitive = static_cast(j.value("selected_primitive", uint32_t{0})); + r.brake_when_finished = j.value("brake_when_finished", false); + r.timeout_sec = j.value("timeout_sec", 0.0); + r.source_iteration_id = j.value("source_iteration_id", std::string{}); + + switch (r.selected_primitive) { + case ChassisMotionPrimitiveType::STRAIGHT_LINE: + if (j.contains("straight_line")) + r.primitive = j["straight_line"].get(); + break; + case ChassisMotionPrimitiveType::ARC: + if (j.contains("arc")) + r.primitive = j["arc"].get(); + break; + case ChassisMotionPrimitiveType::IN_PLACE_ROTATION: + if (j.contains("in_place_rotation")) + r.primitive = j["in_place_rotation"].get(); + break; + case ChassisMotionPrimitiveType::STEERING_SWEEP: + if (j.contains("steering_sweep")) + r.primitive = j["steering_sweep"].get(); + break; + case ChassisMotionPrimitiveType::LATERAL_TRANSLATION: + if (j.contains("lateral_translation")) + r.primitive = j["lateral_translation"].get(); + break; + case ChassisMotionPrimitiveType::DIAGONAL_MOTION: + if (j.contains("diagonal_motion")) + r.primitive = j["diagonal_motion"].get(); + break; + case ChassisMotionPrimitiveType::MODULE_ALIGNMENT: + if (j.contains("module_alignment")) + r.primitive = j["module_alignment"].get(); + break; + case ChassisMotionPrimitiveType::COORDINATED_STEERING: + if (j.contains("coordinated_steering")) + r.primitive = j["coordinated_steering"].get(); + break; + default: + r.primitive = std::monostate{}; + break; + } +} + +// =========================================================================== +// 底盘响应类型 +// =========================================================================== + +struct ChassisReadinessResponse { + bool success{false}; + ErrorCode error_code{ErrorCode::OK}; + std::string message; + bool agent_ready{false}; + bool chassis_driver_online{false}; + bool motion_control_ready{false}; + bool estop_released{false}; + bool vehicle_safe_to_move{false}; + bool telemetry_available{false}; + int64_t checked_timestamp_us{0}; +}; + +inline void to_json(nlohmann::json& j, const ChassisReadinessResponse& r) +{ + j = { + {"success", r.success}, + {"error_code", {"code", static_cast(r.error_code)}}, + {"message", r.message}, + {"agent_ready", r.agent_ready}, + {"chassis_driver_online", r.chassis_driver_online}, + {"motion_control_ready", r.motion_control_ready}, + {"estop_released", r.estop_released}, + {"vehicle_safe_to_move", r.vehicle_safe_to_move}, + {"telemetry_available", r.telemetry_available}, + {"checked_timestamp_us", r.checked_timestamp_us}, + }; +} + +struct ChassisValidationSummary { + double straight_line_error_m{0.0}; + double heading_error_deg{0.0}; + double arc_radius_error_m{0.0}; + double rotation_error_deg{0.0}; + double repeatability_error_m{0.0}; + double curvature_error{0.0}; + double module_consistency_error{0.0}; + bool auto_acceptance_passed{false}; +}; + +inline void to_json(nlohmann::json& j, const ChassisValidationSummary& s) +{ + j = { + {"straight_line_error_m", s.straight_line_error_m}, + {"heading_error_deg", s.heading_error_deg}, + {"arc_radius_error_m", s.arc_radius_error_m}, + {"rotation_error_deg", s.rotation_error_deg}, + {"repeatability_error_m", s.repeatability_error_m}, + {"curvature_error", s.curvature_error}, + {"module_consistency_error", s.module_consistency_error}, + {"auto_acceptance_passed", s.auto_acceptance_passed}, + }; +} + +struct ChassisJobResult { + bool success{false}; + ErrorCode error_code{ErrorCode::OK}; + std::string message; + std::string job_id; + bool data_quality_passed{false}; + bool suitable_for_commit{false}; + std::string recommended_parameter_version; + double estimated_straight_line_bias{0.0}; + ChassisValidationSummary validation_summary; + std::vector artifacts; +}; + +inline void to_json(nlohmann::json& j, const ChassisJobResult& r) +{ + j = { + {"success", r.success}, + {"error_code", {"code", static_cast(r.error_code)}}, + {"message", r.message}, + {"job_id", r.job_id}, + {"data_quality_passed", r.data_quality_passed}, + {"suitable_for_commit", r.suitable_for_commit}, + {"recommended_parameter_version", r.recommended_parameter_version}, + {"estimated_straight_line_bias", r.estimated_straight_line_bias}, + {"validation_summary", r.validation_summary}, + {"artifacts", r.artifacts}, + }; +} + +// =========================================================================== +// 运控命令类型 +// =========================================================================== + +struct TrajectoryPoint { + double x_m{0.0}; + double y_m{0.0}; + double yaw_rad{0.0}; + double target_speed_ms{0.0}; +}; +inline void from_json(const nlohmann::json& j, TrajectoryPoint& p) { + p.x_m = j.value("x_m", 0.0); + p.y_m = j.value("y_m", 0.0); + p.yaw_rad = j.value("yaw_rad", 0.0); + p.target_speed_ms = j.value("target_speed_ms", 0.0); +} + +struct TrajectoryTrackingTask { + std::vector path; + bool stop_at_end{false}; + double timeout_sec{0.0}; +}; +inline void from_json(const nlohmann::json& j, TrajectoryTrackingTask& t) { + if (j.contains("path") && j["path"].is_array()) + t.path = j["path"].get>(); + t.stop_at_end = j.value("stop_at_end", false); + t.timeout_sec = j.value("timeout_sec", 0.0); +} + +struct VelocityStepTask { + double target_velocity_ms{0.0}; + double hold_time_sec{0.0}; + double settle_before_step_sec{0.0}; +}; +inline void from_json(const nlohmann::json& j, VelocityStepTask& t) { + t.target_velocity_ms = j.value("target_velocity_ms", 0.0); + t.hold_time_sec = j.value("hold_time_sec", 0.0); + t.settle_before_step_sec = j.value("settle_before_step_sec", 0.0); +} + +struct AccelerationDecelerationTask { + double start_velocity_ms{0.0}; + double target_velocity_ms{0.0}; + double target_accel_ms2{0.0}; + double hold_time_sec{0.0}; +}; +inline void from_json(const nlohmann::json& j, AccelerationDecelerationTask& t) { + t.start_velocity_ms = j.value("start_velocity_ms", 0.0); + t.target_velocity_ms = j.value("target_velocity_ms", 0.0); + t.target_accel_ms2 = j.value("target_accel_ms2", 0.0); + t.hold_time_sec = j.value("hold_time_sec", 0.0); +} + +struct StopAccuracyTask { + double target_stop_x_m{0.0}; + double target_stop_y_m{0.0}; + double target_stop_yaw_rad{0.0}; + double timeout_sec{0.0}; +}; +inline void from_json(const nlohmann::json& j, StopAccuracyTask& t) { + t.target_stop_x_m = j.value("target_stop_x_m", 0.0); + t.target_stop_y_m = j.value("target_stop_y_m", 0.0); + t.target_stop_yaw_rad = j.value("target_stop_yaw_rad", 0.0); + t.timeout_sec = j.value("timeout_sec", 0.0); +} + +using ControlEvaluationTask = std::variant< + std::monostate, // UNSPECIFIED (0) + TrajectoryTrackingTask, // 1 + VelocityStepTask, // 2 + AccelerationDecelerationTask, // 3 + StopAccuracyTask // 4 +>; + +struct ControllerEvaluationRequest { + RequestHeader header; + std::string test_case_id; + uint32_t task_purpose{0}; + ControlEvaluationTaskType selected_task{ControlEvaluationTaskType::UNSPECIFIED}; + ControlEvaluationTask task; + double timeout_sec{0.0}; + std::string source_iteration_id; +}; + +inline void from_json(const nlohmann::json& j, ControllerEvaluationRequest& r) +{ + if (j.contains("header")) r.header = j["header"].get(); + r.test_case_id = j.value("test_case_id", std::string{}); + r.task_purpose = j.value("task_purpose", uint32_t{0}); + r.selected_task = static_cast(j.value("selected_task", uint32_t{0})); + r.timeout_sec = j.value("timeout_sec", 0.0); + r.source_iteration_id = j.value("source_iteration_id", std::string{}); + + switch (r.selected_task) { + case ControlEvaluationTaskType::TRAJECTORY_TRACKING: + if (j.contains("trajectory_tracking")) + r.task = j["trajectory_tracking"].get(); + break; + case ControlEvaluationTaskType::VELOCITY_STEP: + if (j.contains("velocity_step")) + r.task = j["velocity_step"].get(); + break; + case ControlEvaluationTaskType::ACCELERATION_DECELERATION: + if (j.contains("acceleration_deceleration")) + r.task = j["acceleration_deceleration"].get(); + break; + case ControlEvaluationTaskType::STOP_ACCURACY: + if (j.contains("stop_accuracy")) + r.task = j["stop_accuracy"].get(); + break; + default: + r.task = std::monostate{}; + break; + } +} + +// =========================================================================== +// 运控响应类型 +// =========================================================================== + +struct ControlReadinessResponse { + bool success{false}; + ErrorCode error_code{ErrorCode::OK}; + std::string message; + bool agent_ready{false}; + bool trajectory_executor_ready{false}; + bool vehicle_feedback_ready{false}; + bool control_output_ready{false}; + bool estop_released{false}; + bool vehicle_safe_to_move{false}; + int64_t checked_timestamp_us{0}; +}; + +inline void to_json(nlohmann::json& j, const ControlReadinessResponse& r) +{ + j = { + {"success", r.success}, + {"error_code", {"code", static_cast(r.error_code)}}, + {"message", r.message}, + {"agent_ready", r.agent_ready}, + {"trajectory_executor_ready", r.trajectory_executor_ready}, + {"vehicle_feedback_ready", r.vehicle_feedback_ready}, + {"control_output_ready", r.control_output_ready}, + {"estop_released", r.estop_released}, + {"vehicle_safe_to_move", r.vehicle_safe_to_move}, + {"checked_timestamp_us", r.checked_timestamp_us}, + }; +} + +struct ControlValidationSummary { + double rms_lateral_error_m{0.0}; + double rms_heading_error_rad{0.0}; + double rms_speed_error_ms{0.0}; + double overshoot_ratio{0.0}; + double settle_time_sec{0.0}; + double stop_position_error_m{0.0}; + double max_jerk{0.0}; + double saturation_ratio{0.0}; + bool auto_acceptance_passed{false}; +}; + +inline void to_json(nlohmann::json& j, const ControlValidationSummary& s) +{ + j = { + {"rms_lateral_error_m", s.rms_lateral_error_m}, + {"rms_heading_error_rad", s.rms_heading_error_rad}, + {"rms_speed_error_ms", s.rms_speed_error_ms}, + {"overshoot_ratio", s.overshoot_ratio}, + {"settle_time_sec", s.settle_time_sec}, + {"stop_position_error_m", s.stop_position_error_m}, + {"max_jerk", s.max_jerk}, + {"saturation_ratio", s.saturation_ratio}, + {"auto_acceptance_passed", s.auto_acceptance_passed}, + }; +} + +struct ControlJobResult { + bool success{false}; + ErrorCode error_code{ErrorCode::OK}; + std::string message; + std::string job_id; + bool data_quality_passed{false}; + bool suitable_for_commit{false}; + std::string recommended_parameter_version; + ControlValidationSummary validation_summary; + std::vector artifacts; +}; + +inline void to_json(nlohmann::json& j, const ControlJobResult& r) +{ + j = { + {"success", r.success}, + {"error_code", {"code", static_cast(r.error_code)}}, + {"message", r.message}, + {"job_id", r.job_id}, + {"data_quality_passed", r.data_quality_passed}, + {"suitable_for_commit", r.suitable_for_commit}, + {"recommended_parameter_version", r.recommended_parameter_version}, + {"validation_summary", r.validation_summary}, + {"artifacts", r.artifacts}, + }; +} + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/frame_codec.hpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/frame_codec.hpp new file mode 100644 index 0000000..d1fe9ad --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/include/frame_codec.hpp @@ -0,0 +1,123 @@ +#pragma once +// frame_codec.hpp +// 跨平台 TCP 帧读写工具。 +// 帧格式（与 Ubuntu 侧 proto_frame.hpp 完全一致）： +// [4字节 LE uint32: msg_type][4字节 LE uint32: payload_len][payload_len字节: UTF-8 JSON] + +#include +#include +#include + +#ifdef _WIN32 +# include +# include + using sock_t = SOCKET; +# define SOCK_INVALID INVALID_SOCKET +# define SOCK_CLOSE(s) closesocket(s) +#else +# include +# include +# include +# include +# include + using sock_t = int; +# define SOCK_INVALID (-1) +# define SOCK_CLOSE(s) ::close(s) +#endif + +namespace vaw // vehicle_agent_windows +{ + +// --------------------------------------------------------------------------- +// 枚举：与 Python frame_codec.py / proto_frame.hpp MsgType 完全一致 +// --------------------------------------------------------------------------- +enum class MsgType : uint32_t { + // 底盘域 + CHASSIS_GET_READINESS_REQ = 1, + CHASSIS_GET_READINESS_RSP = 2, + CHASSIS_MOTION_PRIMITIVE_REQ = 3, + CHASSIS_MOTION_PRIMITIVE_RSP = 4, + CHASSIS_EMERGENCY_BRAKE_REQ = 5, + CHASSIS_EMERGENCY_BRAKE_RSP = 6, + // 运控域 + CONTROL_GET_READINESS_REQ = 11, + CONTROL_GET_READINESS_RSP = 12, + CONTROL_EVALUATION_REQ = 13, + CONTROL_EVALUATION_RSP = 14, +}; + +// --------------------------------------------------------------------------- +// 内部：精确读取 n 字节，不足则抛出 std::runtime_error +// --------------------------------------------------------------------------- +inline void recv_exactly(sock_t s, char* buf, uint32_t n) +{ + uint32_t received = 0; + while (received < n) { + int r = static_cast(::recv(s, buf + received, static_cast(n - received), 0)); + if (r <= 0) { + throw std::runtime_error("连接已关闭或读取错误"); + } + received += static_cast(r); + } +} + +// --------------------------------------------------------------------------- +// 读一帧：返回 (msg_type, payload)，payload 可为空字符串 +// --------------------------------------------------------------------------- +inline void recv_frame(sock_t s, MsgType& msg_type, std::string& payload) +{ + // 读 8 字节帧头 + char header[8]; + recv_exactly(s, header, 8); + + // 小端 uint32 反序列化 + auto le32 = [](const char* p) -> uint32_t { + return static_cast(static_cast(p[0])) + | (static_cast(static_cast(p[1])) << 8) + | (static_cast(static_cast(p[2])) << 16) + | (static_cast(static_cast(p[3])) << 24); + }; + + uint32_t type_val = le32(header); + uint32_t payload_len = le32(header + 4); + + msg_type = static_cast(type_val); + + if (payload_len == 0) { + payload.clear(); + return; + } + + payload.resize(payload_len); + recv_exactly(s, &payload[0], payload_len); +} + +// --------------------------------------------------------------------------- +// 写一帧：payload 可为空字符串 +// --------------------------------------------------------------------------- +inline void send_frame(sock_t s, MsgType msg_type, const std::string& payload) +{ + uint32_t type_val = static_cast(msg_type); + uint32_t payload_len = static_cast(payload.size()); + + // 小端序列化 + auto put_le32 = [](char* p, uint32_t v) { + p[0] = static_cast(v & 0xFF); + p[1] = static_cast((v >> 8) & 0xFF); + p[2] = static_cast((v >> 16) & 0xFF); + p[3] = static_cast((v >> 24) & 0xFF); + }; + + char header[8]; + put_le32(header, type_val); + put_le32(header + 4, payload_len); + + // 发帧头 + ::send(s, header, 8, 0); + // 发 payload + if (payload_len > 0) { + ::send(s, payload.data(), static_cast(payload_len), 0); + } +} + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/chassis_domain_server.cpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/chassis_domain_server.cpp new file mode 100644 index 0000000..890c900 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/chassis_domain_server.cpp @@ -0,0 +1,128 @@ +// chassis_domain_server.cpp +// 底盘域 TCP 服务，监听端口 9001（默认）。 +// 每个连接处理一帧后关闭（与 Ubuntu 侧 ChassisBridgeNode 行为一致）。 + +#include "chassis_domain_server.hpp" + +#include +#include +#include +#include + +#include "frame_codec.hpp" +#include "data_types.hpp" + +namespace vaw +{ + +ChassisDomainServer::ChassisDomainServer(uint16_t port, ChassisHandler* handler) + : port_(port), handler_(handler), running_(false), listen_sock_(SOCK_INVALID) +{} + +ChassisDomainServer::~ChassisDomainServer() +{ + stop(); +} + +void ChassisDomainServer::run() +{ +#ifdef _WIN32 + WSADATA wsa; + WSAStartup(MAKEWORD(2, 2), &wsa); +#endif + + listen_sock_ = ::socket(AF_INET, SOCK_STREAM, 0); + if (listen_sock_ == SOCK_INVALID) + throw std::runtime_error("底盘：创建 socket 失败"); + + int opt = 1; + ::setsockopt(listen_sock_, SOL_SOCKET, SO_REUSEADDR, + reinterpret_cast(&opt), sizeof(opt)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + addr.sin_addr.s_addr = INADDR_ANY; + + if (::bind(listen_sock_, reinterpret_cast(&addr), sizeof(addr)) < 0) + throw std::runtime_error("底盘：bind 失败"); + + ::listen(listen_sock_, 8); + running_ = true; + std::printf("[chassis] 监听端口 %d\n", port_); + + while (running_) { + sockaddr_in client_addr{}; +#ifdef _WIN32 + int addr_len = sizeof(client_addr); +#else + socklen_t addr_len = sizeof(client_addr); +#endif + sock_t conn = ::accept(listen_sock_, + reinterpret_cast(&client_addr), &addr_len); + if (conn == SOCK_INVALID) break; + + // 每个连接起一个线程处理，detach 后自动回收 + std::thread([this, conn]() { handle_connection(conn); }).detach(); + } +} + +void ChassisDomainServer::stop() +{ + running_ = false; + if (listen_sock_ != SOCK_INVALID) { + SOCK_CLOSE(listen_sock_); + listen_sock_ = SOCK_INVALID; + } +} + +void ChassisDomainServer::handle_connection(sock_t conn) +{ + std::printf("[chassis] 新连接\n"); + try { + MsgType msg_type; + std::string payload; + recv_frame(conn, msg_type, payload); + std::printf("[chassis] 收到帧 type=%u\n", + static_cast(msg_type)); + + nlohmann::json j_payload; + if (!payload.empty()) + j_payload = nlohmann::json::parse(payload); + + switch (msg_type) { + + case MsgType::CHASSIS_GET_READINESS_REQ: { + auto rsp = handler_->get_readiness(j_payload); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CHASSIS_GET_READINESS_RSP, j_rsp.dump()); + break; + } + + case MsgType::CHASSIS_MOTION_PRIMITIVE_REQ: { + MotionPrimitiveRequest req; + from_json(j_payload, req); + auto rsp = handler_->execute_motion_primitive(req); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CHASSIS_MOTION_PRIMITIVE_RSP, j_rsp.dump()); + break; + } + + case MsgType::CHASSIS_EMERGENCY_BRAKE_REQ: { + auto j_rsp = handler_->emergency_brake(j_payload); + send_frame(conn, MsgType::CHASSIS_EMERGENCY_BRAKE_RSP, j_rsp.dump()); + break; + } + + default: + std::printf("[chassis] 未知帧类型 %u，忽略\n", + static_cast(msg_type)); + break; + } + } catch (const std::exception& e) { + std::printf("[chassis] 处理帧异常: %s\n", e.what()); + } + SOCK_CLOSE(conn); +} + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/control_domain_server.cpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/control_domain_server.cpp new file mode 100644 index 0000000..ec9584b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/control_domain_server.cpp @@ -0,0 +1,119 @@ +// control_domain_server.cpp +// 运控域 TCP 服务，监听端口 9002（默认）。 + +#include "control_domain_server.hpp" + +#include +#include +#include + +#include "frame_codec.hpp" +#include "data_types.hpp" + +namespace vaw +{ + +ControlDomainServer::ControlDomainServer(uint16_t port, ControlHandler* handler) + : port_(port), handler_(handler), running_(false), listen_sock_(SOCK_INVALID) +{} + +ControlDomainServer::~ControlDomainServer() +{ + stop(); +} + +void ControlDomainServer::run() +{ +#ifdef _WIN32 + WSADATA wsa; + WSAStartup(MAKEWORD(2, 2), &wsa); +#endif + + listen_sock_ = ::socket(AF_INET, SOCK_STREAM, 0); + if (listen_sock_ == SOCK_INVALID) + throw std::runtime_error("运控：创建 socket 失败"); + + int opt = 1; + ::setsockopt(listen_sock_, SOL_SOCKET, SO_REUSEADDR, + reinterpret_cast(&opt), sizeof(opt)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + addr.sin_addr.s_addr = INADDR_ANY; + + if (::bind(listen_sock_, reinterpret_cast(&addr), sizeof(addr)) < 0) + throw std::runtime_error("运控：bind 失败"); + + ::listen(listen_sock_, 8); + running_ = true; + std::printf("[control] 监听端口 %d\n", port_); + + while (running_) { + sockaddr_in client_addr{}; +#ifdef _WIN32 + int addr_len = sizeof(client_addr); +#else + socklen_t addr_len = sizeof(client_addr); +#endif + sock_t conn = ::accept(listen_sock_, + reinterpret_cast(&client_addr), &addr_len); + if (conn == SOCK_INVALID) break; + + std::thread([this, conn]() { handle_connection(conn); }).detach(); + } +} + +void ControlDomainServer::stop() +{ + running_ = false; + if (listen_sock_ != SOCK_INVALID) { + SOCK_CLOSE(listen_sock_); + listen_sock_ = SOCK_INVALID; + } +} + +void ControlDomainServer::handle_connection(sock_t conn) +{ + std::printf("[control] 新连接\n"); + try { + MsgType msg_type; + std::string payload; + recv_frame(conn, msg_type, payload); + std::printf("[control] 收到帧 type=%u\n", + static_cast(msg_type)); + + nlohmann::json j_payload; + if (!payload.empty()) + j_payload = nlohmann::json::parse(payload); + + switch (msg_type) { + + case MsgType::CONTROL_GET_READINESS_REQ: { + auto rsp = handler_->get_readiness(j_payload); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CONTROL_GET_READINESS_RSP, j_rsp.dump()); + break; + } + + case MsgType::CONTROL_EVALUATION_REQ: { + ControllerEvaluationRequest req; + from_json(j_payload, req); + auto rsp = handler_->execute_evaluation(req); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CONTROL_EVALUATION_RSP, j_rsp.dump()); + break; + } + + default: + std::printf("[control] 未知帧类型 %u，忽略\n", + static_cast(msg_type)); + break; + } + } catch (const std::exception& e) { + std::printf("[control] 处理帧异常: %s\n", e.what()); + } + SOCK_CLOSE(conn); +} + +} // namespace vaw diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/main.cpp b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/main.cpp new file mode 100644 index 0000000..76e4f86 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/src/main.cpp @@ -0,0 +1,74 @@ +// main.cpp +// vehicle_agent_windows 入口。 +// 启动底盘域（默认 9001）和运控域（默认 9002）两个 TCP 服务线程。 +// +// 用法： +// vehicle_agent_windows [--chassis-port ] [--control-port ] + +#include +#include +#include +#include +#include +#include + +#include "chassis_domain_server.hpp" +#include "control_domain_server.hpp" +#include "chassis_handler.hpp" +#include "control_handler.hpp" + +static std::atomic g_shutdown{false}; + +static void sig_handler(int) { g_shutdown = true; } + +int main(int argc, char* argv[]) +{ + uint16_t chassis_port = 9001; + uint16_t control_port = 9002; + + // 简单命令行解析 + for (int i = 1; i < argc; ++i) { + if (std::strcmp(argv[i], "--chassis-port") == 0 && i + 1 < argc) + chassis_port = static_cast(std::atoi(argv[++i])); + else if (std::strcmp(argv[i], "--control-port") == 0 && i + 1 < argc) + control_port = static_cast(std::atoi(argv[++i])); + } + + std::signal(SIGINT, sig_handler); + std::signal(SIGTERM, sig_handler); + + // 默认 stub handler，子类化后可接入真实硬件 + vaw::ChassisHandler chassis_handler; + vaw::ControlHandler control_handler; + + vaw::ChassisDomainServer chassis_server(chassis_port, &chassis_handler); + vaw::ControlDomainServer control_server(control_port, &control_handler); + + std::thread t_chassis([&]() { + try { chassis_server.run(); } + catch (const std::exception& e) + { std::fprintf(stderr, "[chassis] 启动失败: %s\n", e.what()); } + }); + + std::thread t_control([&]() { + try { control_server.run(); } + catch (const std::exception& e) + { std::fprintf(stderr, "[control] 启动失败: %s\n", e.what()); } + }); + + std::printf("vehicle_agent_windows 已启动，按 Ctrl+C 退出。\n"); + + // 等待退出信号 + while (!g_shutdown) + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + std::printf("\n正在关闭...\n"); + chassis_server.stop(); + control_server.stop(); + + t_chassis.join(); + t_control.join(); + + std::printf("已退出。\n"); + return 0; +} diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/scripts/windows_stub_server.py b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/scripts/windows_stub_server.py index 68797ff..c29db14 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/scripts/windows_stub_server.py +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/scripts/windows_stub_server.py @@ -1,32 +1,130 @@ #!/usr/bin/env python3 # Windows 侧 TCP stub server（本机联调用） -# 用途：模拟 Windows 车端代理，收到任意帧后回复对应的 RSP 帧（payload 为空，表示成功）。 +# 用途：模拟 Windows 车端代理，收到任意帧后回复对应的 RSP 帧（含最小合法 JSON payload）。 # 启动方式：python3 windows_stub_server.py # 默认监听： # 9001 端口 —— 底盘域 # 9002 端口 —— 运控域 +import json import socket import struct import threading +import time import logging logging.basicConfig(level=logging.INFO, format="[%(threadName)s] %(message)s") -# 帧类型枚举（与 proto_frame.hpp 保持一致） -MSG_TYPE = { - # 底盘域 - 1: 2, # CHASSIS_GET_READINESS_REQ -> CHASSIS_GET_READINESS_RSP - 3: 4, # CHASSIS_MOTION_PRIMITIVE_REQ -> CHASSIS_MOTION_PRIMITIVE_RSP - 5: 6, # CHASSIS_EMERGENCY_BRAKE_REQ -> CHASSIS_EMERGENCY_BRAKE_RSP - # 运控域 - 11: 12, # CONTROL_GET_READINESS_REQ -> CONTROL_GET_READINESS_RSP - 13: 14, # CONTROL_EVALUATION_REQ -> CONTROL_EVALUATION_RSP +# 帧类型（与 proto_frame.hpp 保持一致） +CHASSIS_GET_READINESS_REQ = 1 +CHASSIS_GET_READINESS_RSP = 2 +CHASSIS_MOTION_PRIMITIVE_REQ = 3 +CHASSIS_MOTION_PRIMITIVE_RSP = 4 +CHASSIS_EMERGENCY_BRAKE_REQ = 5 +CHASSIS_EMERGENCY_BRAKE_RSP = 6 +CONTROL_GET_READINESS_REQ = 11 +CONTROL_GET_READINESS_RSP = 12 +CONTROL_EVALUATION_REQ = 13 +CONTROL_EVALUATION_RSP = 14 + +# REQ → RSP 类型映射 +REQ_TO_RSP = { + CHASSIS_GET_READINESS_REQ: CHASSIS_GET_READINESS_RSP, + CHASSIS_MOTION_PRIMITIVE_REQ: CHASSIS_MOTION_PRIMITIVE_RSP, + CHASSIS_EMERGENCY_BRAKE_REQ: CHASSIS_EMERGENCY_BRAKE_RSP, + CONTROL_GET_READINESS_REQ: CONTROL_GET_READINESS_RSP, + CONTROL_EVALUATION_REQ: CONTROL_EVALUATION_RSP, } -def read_exactly(conn, n): - """从 conn 读取恰好 n 个字节，不足则抛出 ConnectionError。""" +def _ts_us() -> int: + return int(time.time() * 1_000_000) + + +def _make_payload(req_type: int, req_payload: dict) -> dict: + """根据请求类型生成最小合法的成功响应 payload。""" + if req_type == CHASSIS_GET_READINESS_REQ: + return { + "success": True, + "error_code": {"code": 1}, + "message": "Stub: 底盘就绪。", + "agent_ready": True, + "chassis_driver_online": True, + "motion_control_ready": True, + "estop_released": True, + "vehicle_safe_to_move": True, + "telemetry_available": True, + "checked_timestamp_us": _ts_us(), + } + elif req_type == CHASSIS_MOTION_PRIMITIVE_REQ: + job_id = req_payload.get("header", {}).get("request_id", "stub_job") + return { + "success": True, + "error_code": {"code": 1}, + "message": "Stub: 动作原语执行完成。", + "job_id": job_id, + "data_quality_passed": True, + "suitable_for_commit": True, + "recommended_parameter_version": "stub_v1", + "estimated_straight_line_bias": 0.001, + "validation_summary": { + "max_lateral_error_m": 0.010, + "max_yaw_error_rad": 0.005, + "rms_lateral_error_m": 0.005, + "rms_yaw_error_rad": 0.003, + "repeatability_error_m": 0.001, + "curvature_error": 0.001, + "module_consistency_error": 0.0, + "auto_acceptance_passed": True, + }, + "artifacts": [], + } + elif req_type == CHASSIS_EMERGENCY_BRAKE_REQ: + return { + "success": True, + "error_code": {"code": 1}, + "message": "Stub: 紧急制动已执行。", + } + elif req_type == CONTROL_GET_READINESS_REQ: + return { + "success": True, + "error_code": {"code": 1}, + "message": "Stub: 运控就绪。", + "agent_ready": True, + "trajectory_executor_ready": True, + "vehicle_feedback_ready": True, + "control_output_ready": True, + "estop_released": True, + "vehicle_safe_to_move": True, + "checked_timestamp_us": _ts_us(), + } + elif req_type == CONTROL_EVALUATION_REQ: + job_id = req_payload.get("header", {}).get("request_id", "stub_job") + return { + "success": True, + "error_code": {"code": 1}, + "message": "Stub: 控制评估完成。", + "job_id": job_id, + "data_quality_passed": True, + "suitable_for_commit": True, + "recommended_parameter_version": "stub_control_v1", + "validation_summary": { + "rms_lateral_error_m": 0.015, + "rms_heading_error_rad": 0.010, + "rms_speed_error_ms": 0.05, + "overshoot_ratio": 0.08, + "settle_time_sec": 1.2, + "stop_position_error_m": 0.02, + "max_jerk": 0.3, + "saturation_ratio": 0.05, + "auto_acceptance_passed": True, + }, + "artifacts": [], + } + return {} + + +def _read_exactly(conn, n: int) -> bytes: buf = b"" while len(buf) < n: chunk = conn.recv(n - len(buf)) @@ -36,31 +134,33 @@ def read_exactly(conn, n): return buf -def handle_connection(conn, addr): +def handle_connection(conn: socket.socket, addr): logging.info(f"新连接来自 {addr}") try: - # 读取帧头：8字节（4字节LE类型 + 4字节LE长度） - header = read_exactly(conn, 8) + header = _read_exactly(conn, 8) msg_type, payload_len = struct.unpack_from(" 0 else b"" + raw = _read_exactly(conn, payload_len) if payload_len > 0 else b"" logging.info(f"收到帧 type={msg_type} payload_len={payload_len}") - rsp_type = MSG_TYPE.get(msg_type) + req_payload = json.loads(raw.decode("utf-8")) if raw else {} + + rsp_type = REQ_TO_RSP.get(msg_type) if rsp_type is None: logging.warning(f"未知帧类型 {msg_type}，忽略") return - # 回复 RSP 帧，payload 为空（stub 固定返回成功） - rsp_header = struct.pack("