diff --git a/agv_calib_brain/auto_calib_flowchart.md b/agv_calib_brain/auto_calib_flowchart.md new file mode 100644 index 0000000..12c65a7 --- /dev/null +++ b/agv_calib_brain/auto_calib_flowchart.md @@ -0,0 +1,50 @@ +# 自动化标定流程图（当前实现状态） + +```mermaid +flowchart TD + A[启动 minimal_workshop_demo.launch.py] --> B[vehicle_profile_manager\n最小画像服务] + A --> C[external_localization_service\n最小外部真值校核服务] + A --> D[chassis_calibration_service\n最小底盘 stub] + A --> E[control_calibration_service\n最小运控 stub] + A --> F[sensor_calibration_service\n最小传感器 stub] + A --> G[workshop_orchestrator_v2\n总控编排器] + + B --> H[create_session] + H --> I[PlanBuilder 生成 stage_plan] + I --> J[execute_session] + J --> K[PrecheckRunner 预检] + K --> L{是否通过预检} + L -- 否 --> M[生成失败报告\nfinish_session_precheck_failed] + L -- 是 --> N[按阶段执行] + + N --> O[外部真值校核\nexternal_localization_client] + N --> P[底盘标定\nchassis_gateway_client] + N --> Q[运控参数调优\ncontrol_gateway_client] + N --> R[传感器标定\nsensor_gateway_client] + + O --> O1[external_localization_service\nreadiness + action] + P --> P1[chassis_calibration_service\nreadiness + action] + Q --> Q1[control_calibration_service\nreadiness + action] + R --> R1[sensor_calibration_service\nreadiness + action] + + O1 --> S[收集 StageResultSummary] + P1 --> S + Q1 --> S + R1 --> S + + S --> T[ReportBuilder 生成 WorkshopReport] + T --> U[get_report 查询最终报告] + + classDef done fill:#d1fae5,stroke:#059669,color:#064e3b; + classDef stub fill:#fef3c7,stroke:#d97706,color:#92400e; + classDef core fill:#dbeafe,stroke:#2563eb,color:#1e3a8a; + + class B,C,G,H,I,J,K,L,M,N,O,P,Q,R,S,T,U core; + class D,E,F,O1,P1,Q1,R1 stub; +``` + +## 说明 + +- **核心编排已完成**：session、plan、precheck、stage dispatch、report。 +- **专项执行端目前是最小 stub**：能接 readiness / action，主要用于跑通链路。 +- **当前可验证闭环**：`create_session -> execute_session -> get_report`。 diff --git a/agv_calib_brain/build.sh b/agv_calib_brain/build.sh index 88ed324..2b928f4 100755 --- a/agv_calib_brain/build.sh +++ b/agv_calib_brain/build.sh @@ -18,7 +18,22 @@ BUILD_ARGS=( ) # 如需部分包编译，把 xxx 换成包名后取消下一行注释 -# BUILD_ARGS+=(--packages-select autoware_pose_initializer) +BUILD_ARGS+=(--packages-select calibration_chassis_interfaces \ + calibration_control_interfaces \ + calibration_common_interfaces \ + calibration_external_localization_interfaces \ + calibration_sensor_interfaces \ + calibration_vehicle_profile_interfaces \ + calibration_workshop_orchestration_interfaces \ + workshop_orchestrator_v2 \ + vehicle_profile_manager \ + external_localization_service \ + chassis_calibration_service \ + control_calibration_service \ + sensor_calibration_service \ + vehicle_agent_gateway \ + win_ubuntu_bridge + ) log "Starting colcon build..." if AUTOWARE_COMPILE_WITH_CUDA=1 "${BUILD_ARGS[@]}"; then @@ -32,6 +47,4 @@ if AUTOWARE_COMPILE_WITH_CUDA=1 "${BUILD_ARGS[@]}"; then log "Done." else warn "Build failed, skip sourcing." -fi - - +fi \ No newline at end of file diff --git a/agv_calib_brain/calibration_architecture.md b/agv_calib_brain/calibration_architecture.md new file mode 100644 index 0000000..fe2f1cd --- /dev/null +++ b/agv_calib_brain/calibration_architecture.md @@ -0,0 +1,63 @@ +# 标定系统分层架构图 + +```mermaid +flowchart TB + UI[UI / 上层系统] --> ORCH[workshop_orchestrator_v2\n总控编排器] + + ORCH -->|create_session| PLAN[PlanBuilder\n生成阶段计划] + ORCH -->|execute_session| PRECHECK[PrecheckRunner\n执行前检查] + ORCH -->|stage dispatch| GW[各专项 Gateway Client] + ORCH -->|report| RPT[ReportBuilder\n生成最终报告] + + GW --> EXT[external_localization_service] + GW --> CHA[chassis_calibration_service] + GW --> CON[control_calibration_service] + GW --> SEN[sensor_calibration_service] + GW --> VPM[vehicle_profile_manager] + + subgraph SERVICE[专项包：ROS 接口层] + EXT1[action/service/readiness] + CHA1[action/service/readiness] + CON1[action/service/readiness] + SEN1[action/service/readiness] + VPM1[profile query / update / applicability] + end + + EXT --> EXT1 + CHA --> CHA1 + CON --> CON1 + SEN --> SEN1 + VPM --> VPM1 + + subgraph ALG[专项包：算法执行层] + EXTALG[外部真值校核算法] + CHAALG[底盘标定算法] + CONALG[运控评估算法] + SENALG[传感器标定算法] + end + + EXT1 --> EXTALG + CHA1 --> CHAALG + CON1 --> CONALG + SEN1 --> SENALG + + EXTALG -->|结果翻译| EXT1 + CHAALG -->|结果翻译| CHA1 + CONALG -->|结果翻译| CON1 + SENALG -->|结果翻译| SEN1 + + classDef core fill:#dbeafe,stroke:#2563eb,color:#1e3a8a; + classDef service fill:#fef3c7,stroke:#d97706,color:#92400e; + classDef algo fill:#d1fae5,stroke:#059669,color:#064e3b; + + class ORCH,PLAN,PRECHECK,RPT,GW core; + class EXT,CHA,CON,SEN,VPM,EXT1,CHA1,CON1,SEN1,VPM1 service; + class EXTALG,CHAALG,CONALG,SENALG algo; +``` + +## 说明 + +- **总控编排器** 只管流程，不直接承载专项算法。 +- **专项包** 对外暴露固定 ROS 接口，内部再挂算法执行层。 +- **算法层** 可以独立替换，只要输入/输出协议不变。 +- **topic / service / action** 用来解耦流程和算法实现。 diff --git a/agv_calib_brain/calibration_architecture_compare.md b/agv_calib_brain/calibration_architecture_compare.md new file mode 100644 index 0000000..4bed96f --- /dev/null +++ b/agv_calib_brain/calibration_architecture_compare.md @@ -0,0 +1,49 @@ +# 标定系统架构对照图 + +```mermaid +flowchart LR + subgraph NOW[当前实现] + UI1[UI / 上层系统] --> ORCH1[workshop_orchestrator_v2\n总控编排器] + ORCH1 --> PLAN1[PlanBuilder / PrecheckRunner / ReportBuilder] + ORCH1 --> GW1[Gateway Clients] + GW1 --> EXT1[external_localization_service\n最小 stub] + GW1 --> CHA1[chassis_calibration_service\n最小 stub] + GW1 --> CON1[control_calibration_service\n最小 stub] + GW1 --> SEN1[sensor_calibration_service\n最小 stub] + ORCH1 --> REP1[get_report] + end + + subgraph FUTURE[未来算法接入] + UI2[UI / 上层系统] --> ORCH2[workshop_orchestrator_v2\n总控编排器] + ORCH2 --> PLAN2[PlanBuilder / PrecheckRunner / ReportBuilder] + ORCH2 --> GW2[Gateway Clients] + GW2 --> EXT2[external_localization_service\nROS 接口层] + GW2 --> CHA2[chassis_calibration_service\nROS 接口层] + GW2 --> CON2[control_calibration_service\nROS 接口层] + GW2 --> SEN2[sensor_calibration_service\nROS 接口层] + EXT2 --> EXTALG[外部真值校核算法] + CHA2 --> CHAALG[底盘标定算法] + CON2 --> CONALG[运控评估算法] + SEN2 --> SENALG[传感器标定算法] + EXTALG --> EXT2 + CHAALG --> CHA2 + CONALG --> CON2 + SENALG --> SEN2 + ORCH2 --> REP2[get_report] + end + + classDef core fill:#dbeafe,stroke:#2563eb,color:#1e3a8a; + classDef stub fill:#fef3c7,stroke:#d97706,color:#92400e; + classDef algo fill:#d1fae5,stroke:#059669,color:#064e3b; + + class ORCH1,PLAN1,GW1,REP1,ORCH2,PLAN2,GW2,REP2 core; + class EXT1,CHA1,CON1,SEN1 stub; + class EXT2,CHA2,CON2,SEN2 algo; + class EXTALG,CHAALG,CONALG,SENALG algo; +``` + +## 读图方式 + +- **左边当前实现**：专项包先用最小 stub 跑通流程。 +- **右边未来接入**：专项包保留 ROS 接口，内部替换成真实算法。 +- **总控不变**：orchestrator 始终只负责编排，不直接承载算法。 diff --git a/agv_calib_brain/src/win_ubuntu_bridge/README.md b/agv_calib_brain/src/README.md similarity index 63% rename from agv_calib_brain/src/win_ubuntu_bridge/README.md rename to agv_calib_brain/src/README.md index 874721f..7ed7d72 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/README.md +++ b/agv_calib_brain/src/README.md @@ -35,7 +35,7 @@ sudo apt install -y protobuf-compiler-grpc libgrpc++-dev libprotobuf-dev protobu | 插件名称 | 开发者 | 用途 | |---------|--------|------| -| **C/C++** | Microsoft | 代码补全与 GDB 调试 | +| **C/C++** | Microsoft | 代码补全与 GDB 调试 | | **CMake Tools** | Microsoft | 底部快速构建状态栏 | | **ROS** | Microsoft | 自动识别 `colcon` 工作空间 | | **vscode-proto3** | zxh404 | `.proto` 文件语法高亮 | @@ -76,6 +76,53 @@ sudo apt install -y protobuf-compiler-grpc libgrpc++-dev libprotobuf-dev protobu ## 4. 编译与运行 (CMake 自动化) +### 4.0 最小联调闭环 + +当前仓库已经补齐了一个最小可运行闭环： + +- `vehicle_profile_manager`：提供默认车辆画像 +- `external_localization_service`：提供外部真值校核的最小执行端 +- `workshop_orchestrator_v2`：负责编排会话、计划和报告 + +启动顺序： + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py +``` + +如果只想单独跑编排器： + +```bash +ros2 launch workshop_orchestrator_v2 workshop_orchestrator_v2.launch.py +``` + +可先调用这些接口做联调： + +- `/vehicle_profile_manager/get_vehicle_profile` +- `/vehicle_profile_manager/evaluate_vehicle_calibration_applicability` +- `/external_localization/get_readiness` +- `/external_localization/execute_task` +- `/workshop_v2/create_session` +- `/workshop_v2/execute_session` +- `/workshop_v2/get_report` + +最小会话建议至少包含： + +- `session.config.localization_source_id = demo_vehicle_001` +- `session.config.workcell_zone_id = demo_workcell` +- 一个 `requested_tasks`，其中 `stage_type = EXTERNAL_REFERENCE_READY_CHECK_STAGE` +- 该任务的 `task_params` 至少包含： + - `external.static_sample_count` + - `external.dynamic_sample_count` + - `external.max_position_stddev_m` + - `external.max_yaw_stddev_rad` + - `external.max_tracking_loss_ratio` + - `external.max_time_sync_offset_ms` + - `external.timeout_sec` + + > ✨ **无需手动执行 `protoc`** —— CMakeLists.txt 已配置自动化脚本，编译时自动生成 C++ 网络源码。 ### 4.1 编译 diff --git a/agv_calib_brain/src/UI/README.md b/agv_calib_brain/src/UI/README.md new file mode 100644 index 0000000..e922f36 --- /dev/null +++ b/agv_calib_brain/src/UI/README.md @@ -0,0 +1,35 @@ + +# workshop_ui_pyside6_config_aligned + +这版 PySide6 原型的目标不是单纯展示界面，而是： + +- 让 UI 直接生成接近 `WorkshopSessionConfig` 的数据结构 +- 让每个勾选项直接映射成 `RequestedCalibrationTask` +- 让你一眼看出：当前界面上的输入，最后会如何进入主控消息 + +## 运行方法 + +```bash +pip install -r requirements.txt +python main.py +``` + +## 这版重点 + +### 固定步骤输入 +直接映射到 `WorkshopSessionConfig`： + +- `localization_source_id` +- `workcell_zone_id` +- `reference_target_id` + +### 具体标定项 +直接映射到 `requested_tasks[]`： + +- `stage_type` +- `task_code` +- `target_id` +- `task_params` + +### 导出 JSON +可以把右侧预览直接导出成 JSON 文件，便于和后端 / ROS 接口一起核对。 diff --git a/agv_calib_brain/src/UI/main.py b/agv_calib_brain/src/UI/main.py new file mode 100644 index 0000000..e097fc7 --- /dev/null +++ b/agv_calib_brain/src/UI/main.py @@ -0,0 +1,680 @@ + +# ========================================================= +# 这个原型界面的目标： +# 1) 不再只做“勾选 UI”，而是让界面直接生成接近 +# WorkshopSessionConfig / RequestedCalibrationTask 的数据结构预览； +# 2) 让你能一眼看到：当前界面上的输入，最后会怎么进入主控消息。 +# 说明： +# - 这里不是 ROS2 运行节点，只是一个 PySide6 原型工具； +# - 右侧 JSON 预览，是为了帮助你核对“UI -> 消息结构”是否对齐。 +# ========================================================= + +import json +import signal +from PySide6.QtCore import Qt, QTimer +from PySide6.QtGui import QFont +from PySide6.QtWidgets import ( + QApplication, QMainWindow, QWidget, QLabel, QLineEdit, QPushButton, QVBoxLayout, + QHBoxLayout, QGridLayout, QTextEdit, QGroupBox, QCheckBox, QListWidget, + QListWidgetItem, QProgressBar, QMessageBox, QTreeWidget, QTreeWidgetItem, + QSplitter, QTabWidget, QComboBox, QFormLayout, QFileDialog +) + +# ========================================================= +# 底盘通用参数 +# 这些最终会映射成 RequestedCalibrationTask： +# - stage_type = CHASSIS_CALIBRATION_STAGE +# - task_code = chassis.common.xxx +# ========================================================= +CHASSIS_COMMON = [ + ("chassis.common.effective_wheel_base_m", "有效轴距"), + ("chassis.common.effective_track_width_m", "有效轮距"), + ("chassis.common.longitudinal_scale", "纵向里程比例补偿"), + ("chassis.common.lateral_scale", "横向里程比例补偿"), + ("chassis.common.yaw_scale", "航向比例补偿"), + ("chassis.common.straight_line_bias", "直线跑偏补偿"), +] + +# ========================================================= +# 底盘专属参数 +# 这些会根据底盘类型动态显示。 +# ========================================================= +CHASSIS_SPECIFIC = { + "ACKERMANN_CHASSIS": [ + ("chassis.ackermann.front_left_steer_zero_offset_deg", "左前舵角零偏"), + ("chassis.ackermann.front_right_steer_zero_offset_deg", "右前舵角零偏"), + ("chassis.ackermann.rear_left_wheel_radius_m", "左后轮有效半径"), + ("chassis.ackermann.rear_right_wheel_radius_m", "右后轮有效半径"), + ("chassis.ackermann.steering_ratio", "转向传动比"), + ], + "DIFFERENTIAL_CHASSIS": [ + ("chassis.differential.left_wheel_radius_m", "左轮有效半径"), + ("chassis.differential.right_wheel_radius_m", "右轮有效半径"), + ("chassis.differential.axle_track_width_m", "驱动轮间距"), + ("chassis.differential.left_encoder_scale", "左编码器比例系数"), + ("chassis.differential.right_encoder_scale", "右编码器比例系数"), + ], + "SINGLE_STEER_WHEEL_CHASSIS": [ + ("chassis.single_steer.drive_wheel_radius_m", "驱动轮有效半径"), + ("chassis.single_steer.steer_zero_offset_deg", "舵角零偏"), + ("chassis.single_steer.steering_ratio", "转向比"), + ("chassis.single_steer.drive_encoder_scale", "驱动编码器比例"), + ], + "MULTI_STEER_WHEEL_CHASSIS": [ + ("chassis.multi_steer.module_wheel_radius_m", "模块轮半径"), + ("chassis.multi_steer.module_steer_zero_offset_deg", "模块舵角零偏"), + ("chassis.multi_steer.module_pos_x_m", "模块在 base_link 下的 X"), + ("chassis.multi_steer.module_pos_y_m", "模块在 base_link 下的 Y"), + ], +} + +# ========================================================= +# 运控参数 +# 按 控制轴 × 算法类型 组织。 +# 最终会映射成 RequestedCalibrationTask： +# - stage_type = CONTROL_CALIBRATION_STAGE +# - task_code = control.lateral.pid / control.longitudinal.mpc 等 +# - task_params = need_tuning / is_default +# ========================================================= +CONTROL_OPTIONS = { + "lateral": [ + ("control.lateral.pid", "PID"), + ("control.lateral.mpc", "MPC"), + ("control.lateral.lqr", "LQR"), + ("control.lateral.pure_pursuit", "Pure Pursuit"), + ], + "longitudinal": [ + ("control.longitudinal.pid", "PID"), + ("control.longitudinal.mpc", "MPC"), + ], +} + +# ========================================================= +# 传感器标定树 +# 最终会映射成 RequestedCalibrationTask： +# - stage_type = SENSOR_INTRINSIC_CALIBRATION_STAGE / SENSOR_EXTRINSIC_CALIBRATION_STAGE / HAND_EYE_CALIBRATION_STAGE +# - task_code / target_id 由树节点决定 +# ========================================================= +SENSOR_TREE = { + "camera": { + "front_camera": [ + ("sensor.front_camera.camera_intrinsic", "相机内参"), + ("sensor.front_camera.sensor_to_base_extrinsic", "传感器到 base_link 外参"), + ], + "downward_camera": [ + ("sensor.downward_camera.camera_intrinsic", "相机内参"), + ("sensor.downward_camera.sensor_to_base_extrinsic", "传感器到 base_link 外参"), + ], + "arm_camera": [ + ("sensor.arm_camera.camera_intrinsic", "相机内参"), + ("sensor.arm_camera.sensor_to_base_extrinsic", "传感器到 base_link 外参"), + ("sensor.arm_camera.hand_eye", "手眼标定"), + ], + }, + "lidar": { + "lidar_3d": [ + ("sensor.lidar_3d.sensor_to_base_extrinsic", "传感器到 base_link 外参"), + ], + "lidar_2d": [ + ("sensor.lidar_2d.sensor_to_base_extrinsic", "传感器到 base_link 外参"), + ], + }, + "imu": { + "imu_01": [ + ("sensor.imu.imu_intrinsic", "IMU 内参"), + ("sensor.imu.sensor_to_base_extrinsic", "传感器到 base_link 外参"), + ], + } +} + +# ========================================================= +# 常量：用字符串模拟 WorkflowStageType / StageExecutionPolicy +# 作用： +# - 方便你直接在 JSON 预览里看懂每一项会落成什么； +# - 后面接 ROS2 时，再替换成真实枚举/消息值即可。 +# ========================================================= +STAGE_CHASSIS = "CHASSIS_CALIBRATION_STAGE" +STAGE_CONTROL = "CONTROL_CALIBRATION_STAGE" +STAGE_SENSOR_INTRINSIC = "SENSOR_INTRINSIC_CALIBRATION_STAGE" +STAGE_SENSOR_EXTRINSIC = "SENSOR_EXTRINSIC_CALIBRATION_STAGE" +STAGE_HAND_EYE = "HAND_EYE_CALIBRATION_STAGE" +POLICY_REQUIRED = "REQUIRED" +POLICY_OPTIONAL = "OPTIONAL" + + +class MainWindow(QMainWindow): + def __init__(self): + super().__init__() + self.setWindowTitle("自动化标定车间 · UI -> WorkshopSessionConfig 对齐版") + self.resize(1700, 980) + + # 当前界面右侧显示的“整场任务运行状态”只是原型演示状态，不是 ROS 真状态。 + self.session_state = "DRAFT" + self.current_step_index = -1 + self.steps = [] + self._timers = [] + + central = QWidget() + self.setCentralWidget(central) + root = QVBoxLayout(central) + root.setContentsMargins(14, 14, 14, 14) + root.setSpacing(12) + + title = QLabel("自动化标定车间 · UI 与消息结构对齐原型") + title.setFont(QFont("", 18, QFont.Bold)) + desc = QLabel( + "这版界面不只是勾选任务，而是直接生成接近 WorkshopSessionConfig / RequestedCalibrationTask 的数据预览。" + ) + desc.setWordWrap(True) + root.addWidget(title) + root.addWidget(desc) + + main_splitter = QSplitter(Qt.Horizontal) + root.addWidget(main_splitter, 1) + + left = QWidget() + right = QWidget() + main_splitter.addWidget(left) + main_splitter.addWidget(right) + main_splitter.setSizes([980, 720]) + + left_layout = QVBoxLayout(left) + right_layout = QVBoxLayout(right) + + # ========================================================= + # 任务基础信息 + # 这些字段不会直接进入 WorkshopSessionConfig， + # 但它们通常属于 CreateWorkshopSessionRequest 的上层输入。 + # ========================================================= + base_box = QGroupBox("任务基础信息") + left_layout.addWidget(base_box) + base_layout = QGridLayout(base_box) + + self.task_name = QLineEdit("AGV_01_完整标定任务") + self.vehicle_id = QLineEdit("AGV_01") + self.line_id = QLineEdit("LINE_A") + self.operator = QLineEdit("张三") + self.base_link = QLineEdit("base_link") + + base_layout.addWidget(QLabel("任务名称"), 0, 0) + base_layout.addWidget(self.task_name, 0, 1) + base_layout.addWidget(QLabel("车辆 ID"), 0, 2) + base_layout.addWidget(self.vehicle_id, 0, 3) + base_layout.addWidget(QLabel("产线 / 工位"), 1, 0) + base_layout.addWidget(self.line_id, 1, 1) + base_layout.addWidget(QLabel("操作者"), 1, 2) + base_layout.addWidget(self.operator, 1, 3) + base_layout.addWidget(QLabel("base_link"), 2, 0) + base_layout.addWidget(self.base_link, 2, 1) + + # ========================================================= + # 固定步骤输入 + # 这几项会直接进入 WorkshopSessionConfig： + # - localization_source_id + # - workcell_zone_id + # - reference_target_id + # ========================================================= + fixed_box = QGroupBox("固定步骤输入 · 外部真值校核") + left_layout.addWidget(fixed_box) + fixed_layout = QFormLayout(fixed_box) + + self.localization_source = QLineEdit("truth_source_01") + self.workcell_zone = QLineEdit("zone_A") + self.reference_target = QLineEdit("board_01") + + fixed_layout.addRow("外部定位源 ID", self.localization_source) + fixed_layout.addRow("工位 / 区域 ID", self.workcell_zone) + fixed_layout.addRow("参考目标 ID", self.reference_target) + + # ========================================================= + # 运行策略 + # 这些会直接进入 WorkshopSessionConfig 的布尔开关。 + # ========================================================= + strategy_box = QGroupBox("运行策略（直接对应 WorkshopSessionConfig）") + left_layout.addWidget(strategy_box) + strategy_layout = QGridLayout(strategy_box) + + self.auto_commit = QCheckBox("自动写入参数") + self.require_approval = QCheckBox("写入前要求人工审批") + self.run_validation = QCheckBox("每阶段后自动复测") + self.stop_on_first_failure = QCheckBox("首错即停") + self.allow_optional_skip = QCheckBox("允许可选阶段跳过") + self.enable_auto_rollback = QCheckBox("验证失败自动回滚") + self.allow_rebuild_plan = QCheckBox("允许运行前重建计划") + + # 给几个更常见的默认值 + self.stop_on_first_failure.setChecked(True) + self.allow_optional_skip.setChecked(True) + + checks = [ + self.auto_commit, + self.require_approval, + self.run_validation, + self.stop_on_first_failure, + self.allow_optional_skip, + self.enable_auto_rollback, + self.allow_rebuild_plan, + ] + for idx, cb in enumerate(checks): + strategy_layout.addWidget(cb, idx // 2, idx % 2) + + # ========================================================= + # 任务定义区 + # 这里的所有选择，最终都会变成 requested_tasks[] + # ========================================================= + tabs = QTabWidget() + left_layout.addWidget(tabs, 1) + + # -------------------- 底盘标定 -------------------- + chassis_tab = QWidget() + chassis_layout = QVBoxLayout(chassis_tab) + + type_row = QHBoxLayout() + type_row.addWidget(QLabel("底盘类型")) + self.chassis_type = QComboBox() + self.chassis_type.addItems([ + "ACKERMANN_CHASSIS", + "DIFFERENTIAL_CHASSIS", + "SINGLE_STEER_WHEEL_CHASSIS", + "MULTI_STEER_WHEEL_CHASSIS", + ]) + self.chassis_type.currentTextChanged.connect(self.rebuild_chassis_tree) + type_row.addWidget(self.chassis_type) + type_row.addWidget(QLabel("多舵轮模块 ID")) + self.multi_module_id = QLineEdit("steering_module_1") + type_row.addWidget(self.multi_module_id) + type_row.addStretch(1) + chassis_layout.addLayout(type_row) + + self.chassis_tree = QTreeWidget() + self.chassis_tree.setHeaderLabels(["对象", "说明"]) + self.chassis_tree.setColumnWidth(0, 320) + chassis_layout.addWidget(self.chassis_tree) + tabs.addTab(chassis_tab, "底盘标定") + + # -------------------- 运控参数 -------------------- + control_tab = QWidget() + control_layout = QVBoxLayout(control_tab) + control_tip = QLabel("勾选项最终会变成 RequestedCalibrationTask；need_tuning / is_default 会写入 task_params。") + control_tip.setWordWrap(True) + control_layout.addWidget(control_tip) + + self.control_rows = [] + for axis, items in CONTROL_OPTIONS.items(): + box = QGroupBox(axis) + form = QGridLayout(box) + row = 0 + for item_id, name in items: + enable = QCheckBox(name) + need_tuning = QCheckBox("need_tuning") + is_default = QCheckBox("is_default") + form.addWidget(enable, row, 0) + form.addWidget(need_tuning, row, 1) + form.addWidget(is_default, row, 2) + self.control_rows.append((item_id, axis, name, enable, need_tuning, is_default)) + row += 1 + control_layout.addWidget(box) + tabs.addTab(control_tab, "运控参数标定") + + # -------------------- 传感器标定 -------------------- + sensor_tab = QWidget() + sensor_layout = QVBoxLayout(sensor_tab) + sensor_tip = QLabel("每个勾选项会映射成 RequestedCalibrationTask，device 名称会作为 target_id。") + sensor_tip.setWordWrap(True) + sensor_layout.addWidget(sensor_tip) + + self.sensor_tree = QTreeWidget() + self.sensor_tree.setHeaderLabels(["对象", "说明"]) + self.sensor_tree.setColumnWidth(0, 320) + self.sensor_checks = {} + self._build_sensor_tree() + sensor_layout.addWidget(self.sensor_tree) + tabs.addTab(sensor_tab, "传感器标定") + + # 按钮区 + btn_row = QHBoxLayout() + self.refresh_btn = QPushButton("刷新预览") + self.create_btn = QPushButton("创建任务（演示）") + self.run_btn = QPushButton("运行流程（演示）") + self.export_btn = QPushButton("导出 JSON") + self.refresh_btn.clicked.connect(self.refresh_preview) + self.create_btn.clicked.connect(self.create_session) + self.run_btn.clicked.connect(self.run_demo) + self.export_btn.clicked.connect(self.export_json) + btn_row.addWidget(self.refresh_btn) + btn_row.addWidget(self.create_btn) + btn_row.addWidget(self.run_btn) + btn_row.addWidget(self.export_btn) + btn_row.addStretch(1) + left_layout.addLayout(btn_row) + + # ========================================================= + # 右侧：概览 + JSON 预览 + # ========================================================= + overview_box = QGroupBox("当前任务概览") + right_layout.addWidget(overview_box) + overview_layout = QVBoxLayout(overview_box) + + state_row = QHBoxLayout() + self.state_label = QLabel("DRAFT") + self.selection_count_label = QLabel("当前选择：0 项") + state_row.addWidget(QLabel("当前状态：")) + state_row.addWidget(self.state_label) + state_row.addStretch(1) + state_row.addWidget(self.selection_count_label) + overview_layout.addLayout(state_row) + + self.progress = QProgressBar() + overview_layout.addWidget(self.progress) + + self.step_list = QListWidget() + overview_layout.addWidget(self.step_list, 1) + + bottom_splitter = QSplitter(Qt.Horizontal) + right_layout.addWidget(bottom_splitter, 1) + + event_box = QGroupBox("运行过程通知") + json_box = QGroupBox("WorkshopSessionConfig / RequestedCalibrationTask 预览") + bottom_splitter.addWidget(event_box) + bottom_splitter.addWidget(json_box) + bottom_splitter.setSizes([360, 520]) + + event_layout = QVBoxLayout(event_box) + self.event_log = QTextEdit() + self.event_log.setReadOnly(True) + self.event_log.setPlainText("系统初始化完成，等待创建标定任务。") + event_layout.addWidget(self.event_log) + + json_layout = QVBoxLayout(json_box) + self.json_preview = QTextEdit() + self.json_preview.setReadOnly(True) + json_layout.addWidget(self.json_preview) + + self.rebuild_chassis_tree() + self.refresh_preview() + + # ========================================================= + # 下面开始是“把 UI 选择转换成消息结构”的核心逻辑 + # ========================================================= + + def rebuild_chassis_tree(self): + # 重新构造底盘任务树： + # 1) 通用参数始终存在； + # 2) 专属参数按当前底盘类型显示。 + self.chassis_tree.clear() + self.chassis_checks = {} + + common_item = QTreeWidgetItem(["通用参数", "所有底盘形式都可能会用到"]) + self.chassis_tree.addTopLevelItem(common_item) + for task_id, name in CHASSIS_COMMON: + item = QTreeWidgetItem([name, "会映射成 RequestedCalibrationTask"]) + item.setFlags(item.flags() | Qt.ItemIsUserCheckable) + item.setCheckState(0, Qt.Unchecked) + item.setData(0, Qt.UserRole, task_id) + common_item.addChild(item) + self.chassis_checks[task_id] = item + + chassis_type = self.chassis_type.currentText() + specific_item = QTreeWidgetItem([f"{chassis_type} 专属参数", "按当前底盘类型展开"]) + self.chassis_tree.addTopLevelItem(specific_item) + for task_id, name in CHASSIS_SPECIFIC[chassis_type]: + item = QTreeWidgetItem([name, "会映射成 RequestedCalibrationTask"]) + item.setFlags(item.flags() | Qt.ItemIsUserCheckable) + item.setCheckState(0, Qt.Unchecked) + item.setData(0, Qt.UserRole, task_id) + specific_item.addChild(item) + self.chassis_checks[task_id] = item + + self.chassis_tree.expandAll() + self.chassis_tree.itemChanged.connect(lambda *_: self.refresh_preview()) + self.refresh_preview() + + def _build_sensor_tree(self): + # 构造传感器任务树： + # 分类 -> 设备 -> 具体标定任务 + self.sensor_tree.clear() + for category, devices in SENSOR_TREE.items(): + cat_item = QTreeWidgetItem([category, "传感器类型"]) + self.sensor_tree.addTopLevelItem(cat_item) + for target_id, tasks in devices.items(): + device_item = QTreeWidgetItem([target_id, "具体设备 / target_id"]) + cat_item.addChild(device_item) + for task_code, display_name, stage_type in tasks: + task_item = QTreeWidgetItem([display_name, "会映射成 RequestedCalibrationTask"]) + task_item.setFlags(task_item.flags() | Qt.ItemIsUserCheckable) + task_item.setCheckState(0, Qt.Unchecked) + task_item.setData(0, Qt.UserRole, (task_code, target_id, stage_type)) + device_item.addChild(task_item) + self.sensor_checks[(task_code, target_id)] = task_item + self.sensor_tree.expandAll() + self.sensor_tree.itemChanged.connect(lambda *_: self.refresh_preview()) + + def make_task(self, stage_type, task_code, target_id="", task_params=None, enabled=True, require_manual_approval=False, + execution_policy=POLICY_REQUIRED, reason=""): + # 这个函数的作用： + # 把当前 UI 上的一个具体选择，转换成一个 RequestedCalibrationTask 对象（这里用 dict 预览）。 + return { + "stage_type": stage_type, + "enabled": enabled, + "require_manual_approval": require_manual_approval, + "execution_policy": execution_policy, + "reason": reason, + "task_code": task_code, + "target_id": target_id, + "task_params": task_params or [], + } + + def selected_chassis_tasks(self): + tasks = [] + chassis_type = self.chassis_type.currentText() + for task_code, item in self.chassis_checks.items(): + if item.checkState(0) == Qt.Checked: + # 多舵轮场景下，target_id 直接复用模块 ID 输入； + # 其它底盘形式先用 chassis_main。 + target_id = self.multi_module_id.text().strip() if chassis_type == "MULTI_STEER_WHEEL_CHASSIS" else "chassis_main" + params = [{"key": "chassis_type", "value": chassis_type}] + tasks.append(self.make_task( + stage_type=STAGE_CHASSIS, + task_code=task_code, + target_id=target_id, + task_params=params, + )) + return tasks + + def selected_control_tasks(self): + tasks = [] + for task_code, axis, alg_name, enable, need_tuning, is_default in self.control_rows: + if enable.isChecked(): + params = [ + {"key": "control_axis", "value": axis}, + {"key": "algorithm_name", "value": alg_name}, + {"key": "need_tuning", "value": "true" if need_tuning.isChecked() else "false"}, + {"key": "is_default", "value": "true" if is_default.isChecked() else "false"}, + ] + tasks.append(self.make_task( + stage_type=STAGE_CONTROL, + task_code=task_code, + target_id=f"{axis}_controller", + task_params=params, + )) + return tasks + + def selected_sensor_tasks(self): + tasks = [] + for (task_code, target_id), item in self.sensor_checks.items(): + if item.checkState(0) == Qt.Checked: + _, _, stage_type = item.data(0, Qt.UserRole) + tasks.append(self.make_task( + stage_type=stage_type, + task_code=task_code, + target_id=target_id, + )) + return tasks + + def build_config_payload(self): + # 这里生成的对象，就是当前这版 UI 对齐出来的 WorkshopSessionConfig 预览。 + requested_tasks = [] + requested_tasks.extend(self.selected_chassis_tasks()) + requested_tasks.extend(self.selected_control_tasks()) + requested_tasks.extend(self.selected_sensor_tasks()) + + config = { + "requested_tasks": requested_tasks, + "auto_commit_parameters": self.auto_commit.isChecked(), + "require_manual_approval_before_commit": self.require_approval.isChecked(), + "run_validation_after_each_stage": self.run_validation.isChecked(), + "stop_on_first_failure": self.stop_on_first_failure.isChecked(), + "allow_optional_stage_skip": self.allow_optional_skip.isChecked(), + "enable_auto_rollback_on_validation_failure": self.enable_auto_rollback.isChecked(), + "allow_rebuild_execution_plan": self.allow_rebuild_plan.isChecked(), + "localization_source_id": self.localization_source.text().strip(), + "workcell_zone_id": self.workcell_zone.text().strip(), + "reference_target_id": self.reference_target.text().strip(), + } + return config + + def build_steps(self): + # 这部分只是为了右侧“执行顺序预览”。 + # 它不是 ROS 消息，而是把 config 里的 requested_tasks 转成用户更容易看懂的步骤列表。 + config = self.build_config_payload() + steps = [("external_reference", "外部真值校核（固定必做）")] + for task in config["requested_tasks"]: + steps.append((task["task_code"], f'{task["stage_type"]} · {task["task_code"]} · {task["target_id"]}')) + steps.append(("report", "生成最终报告")) + return steps + + def refresh_preview(self): + # 每次界面勾选发生变化，都刷新： + # 1) 当前步骤预览 + # 2) JSON 预览 + self.steps = self.build_steps() + self.step_list.clear() + for idx, (_, name) in enumerate(self.steps, start=1): + self.step_list.addItem(QListWidgetItem(f"{idx}. {name}")) + + selected_count = max(len(self.steps) - 2, 0) + self.selection_count_label.setText(f"当前选择：{selected_count} 项") + + payload = { + "create_request_preview": { + "task_name": self.task_name.text().strip(), + "vehicle_id": self.vehicle_id.text().strip(), + "workshop_line_id": self.line_id.text().strip(), + "operator": self.operator.text().strip(), + "base_link": self.base_link.text().strip(), + "config": self.build_config_payload(), + } + } + self.json_preview.setPlainText(json.dumps(payload, ensure_ascii=False, indent=2)) + + def log(self, text): + self.event_log.setPlainText(text + "\n" + self.event_log.toPlainText()) + + def create_session(self): + # 这里只是演示“任务创建成功后的界面反应”，不是真的发 ROS service。 + self.clear_timers() + self.refresh_preview() + config = self.build_config_payload() + if len(config["requested_tasks"]) == 0: + QMessageBox.warning(self, "提示", "请至少选择一个具体标定项。") + return + + self.session_state = "READY" + self.state_label.setText(self.session_state) + self.progress.setValue(0) + self.log(f"已创建标定任务：{self.task_name.text().strip()}") + self.log(f"固定步骤输入：定位源={config['localization_source_id']}，工位={config['workcell_zone_id']}，参考目标={config['reference_target_id']}") + self.log(f"本次共生成 {len(config['requested_tasks'])} 个 RequestedCalibrationTask。") + + def run_demo(self): + # 这里只做主流程演示，不接 ROS2。 + self.clear_timers() + self.refresh_preview() + config = self.build_config_payload() + if len(config["requested_tasks"]) == 0: + QMessageBox.warning(self, "提示", "请至少选择一个具体标定项。") + return + + self.session_state = "PRECHECK_RUNNING" + self.state_label.setText(self.session_state) + self.progress.setValue(0) + self.log("开始预检：检查固定步骤输入是否完整、以及 requested_tasks 是否非空。") + + t = QTimer(self) + t.setSingleShot(True) + t.timeout.connect(self._after_precheck) + t.start(700) + self._timers.append(t) + + def _after_precheck(self): + self.session_state = "RUNNING" + self.state_label.setText(self.session_state) + self.log("预检通过，开始正式执行。") + + for index, (_, name) in enumerate(self.steps): + t = QTimer(self) + t.setSingleShot(True) + t.timeout.connect(lambda idx=index, nm=name: self._run_step(idx, nm)) + t.start(850 * (index + 1)) + self._timers.append(t) + + end_t = QTimer(self) + end_t.setSingleShot(True) + end_t.timeout.connect(self._finish_run) + end_t.start(850 * (len(self.steps) + 1)) + self._timers.append(end_t) + + def _run_step(self, index, name): + # 运行过程演示：让右侧步骤列表看起来像在逐步推进。 + self.current_step_index = index + self.log(f"开始执行：{name}") + for i in range(self.step_list.count()): + original = self.steps[i][1] + prefix = "▶ " if i == index else ("✓ " if i < index else "○ ") + self.step_list.item(i).setText(f"{prefix}{original}") + self.progress.setValue(int(index / max(len(self.steps), 1) * 100)) + + def _finish_run(self): + # 演示整场任务完成后的界面变化。 + self.current_step_index = -1 + self.session_state = "SUCCEEDED" + self.state_label.setText(self.session_state) + self.progress.setValue(100) + self.log("整场标定执行完成。") + for i in range(self.step_list.count()): + self.step_list.item(i).setText(f"✓ {self.steps[i][1]}") + + def export_json(self): + # 导出当前 JSON 预览，方便你拿去和后端 / ROS 接口一起对字段。 + payload_text = self.json_preview.toPlainText().strip() + if not payload_text: + QMessageBox.warning(self, "提示", "当前没有可导出的内容。") + return + path, _ = QFileDialog.getSaveFileName(self, "导出 JSON", "workshop_session_config_preview.json", "JSON Files (*.json)") + if not path: + return + with open(path, "w", encoding="utf-8") as f: + f.write(payload_text) + QMessageBox.information(self, "完成", f"已导出到：{path}") + + def clear_timers(self): + for t in self._timers: + t.stop() + t.deleteLater() + self._timers.clear() + + +if __name__ == "__main__": + app = QApplication([]) + + # 让终端里的 Ctrl+C 可以退出 Qt 程序。 + signal.signal(signal.SIGINT, lambda *args: app.quit()) + + # 让 Python 有机会处理 SIGINT。 + sigint_timer = QTimer() + sigint_timer.start(200) + sigint_timer.timeout.connect(lambda: None) + + w = MainWindow() + w.show() + app.exec() diff --git a/agv_calib_brain/src/UI/requirements.txt b/agv_calib_brain/src/UI/requirements.txt new file mode 100644 index 0000000..5b97c7c --- /dev/null +++ b/agv_calib_brain/src/UI/requirements.txt @@ -0,0 +1 @@ +PySide6>=6.6.0 diff --git a/agv_calib_brain/src/agv_calib_control_pb2.py b/agv_calib_brain/src/agv_calib_control_pb2.py deleted file mode 100644 index edaf732..0000000 --- a/agv_calib_brain/src/agv_calib_control_pb2.py +++ /dev/null @@ -1,56 +0,0 @@ -# -*- coding: utf-8 -*- -# Generated by the protocol buffer compiler. DO NOT EDIT! -# NO CHECKED-IN PROTOBUF GENCODE -# source: agv_calib_control.proto -# Protobuf Python Version: 6.31.1 -"""Generated protocol buffer code.""" -from google.protobuf import descriptor as _descriptor -from google.protobuf import descriptor_pool as _descriptor_pool -from google.protobuf import runtime_version as _runtime_version -from google.protobuf import symbol_database as _symbol_database -from google.protobuf.internal import builder as _builder -_runtime_version.ValidateProtobufRuntimeVersion( - _runtime_version.Domain.PUBLIC, - 6, - 31, - 1, - '', - 'agv_calib_control.proto' -) -# @@protoc_insertion_point(imports) - -_sym_db = _symbol_database.Default() - - - - -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x17\x61gv_calib_control.proto\x12\x17\x61gv.calibration.control\"\x07\n\x05\x45mpty\"4\n\x10StandardResponse\x12\x0f\n\x07success\x18\x01 \x01(\x08\x12\x0f\n\x07message\x18\x02 \x01(\t\"\x8b\x01\n\x0bModeRequest\x12>\n\x0btarget_mode\x18\x01 \x01(\x0e\x32).agv.calibration.control.ModeRequest.Mode\"<\n\x04Mode\x12\x0f\n\x0bNORMAL_MODE\x10\x00\x12\x12\n\x0eOPEN_LOOP_MODE\x10\x01\x12\x0f\n\x0bTUNING_MODE\x10\x02\"p\n\x0fOpenLoopRequest\x12\x16\n\x0eleft_motor_cmd\x18\x01 \x01(\x01\x12\x17\n\x0fright_motor_cmd\x18\x02 \x01(\x01\x12\x16\n\x0esteering_angle\x18\x03 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x04 \x01(\x01\"h\n\x0fTrajectoryPoint\x12\x0b\n\x03x_m\x18\x01 \x01(\x01\x12\x0b\n\x03y_m\x18\x02 \x01(\x01\x12\x0f\n\x07yaw_rad\x18\x03 \x01(\x01\x12\x17\n\x0ftarget_speed_ms\x18\x04 \x01(\x01\x12\x11\n\tcurvature\x18\x05 \x01(\x01\"a\n\x11TrajectoryRequest\x12\x14\n\x0ctest_case_id\x18\x01 \x01(\t\x12\x36\n\x04path\x18\x02 \x03(\x0b\x32(.agv.calibration.control.TrajectoryPoint\"G\n\x13StepResponseRequest\x12\x1a\n\x12target_velocity_ms\x18\x01 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x02 \x01(\x01\"\xf9\x05\n\rControlParams\x12$\n\x17wheel_radius_left_ratio\x18\x01 \x01(\x01H\x00\x88\x01\x01\x12%\n\x18wheel_radius_right_ratio\x18\x02 \x01(\x01H\x01\x88\x01\x01\x12$\n\x17\x65\x66\x66\x65\x63tive_track_width_m\x18\x03 \x01(\x01H\x02\x88\x01\x01\x12%\n\x18steering_zero_offset_deg\x18\x04 \x01(\x01H\x03\x88\x01\x01\x12\x1b\n\x0epid_kp_lateral\x18\x05 \x01(\x01H\x04\x88\x01\x01\x12\x1b\n\x0epid_ki_lateral\x18\x06 \x01(\x01H\x05\x88\x01\x01\x12\x1b\n\x0epid_kd_lateral\x18\x07 \x01(\x01H\x06\x88\x01\x01\x12\x1b\n\x0epid_kp_heading\x18\x08 \x01(\x01H\x07\x88\x01\x01\x12\x1b\n\x0epid_ki_heading\x18\t \x01(\x01H\x08\x88\x01\x01\x12\x1b\n\x0epid_kd_heading\x18\n \x01(\x01H\t\x88\x01\x01\x12%\n\x18pure_pursuit_lookahead_m\x18\x0b \x01(\x01H\n\x88\x01\x01\x12!\n\x14mpc_weight_q_lateral\x18\x0c \x01(\x01H\x0b\x88\x01\x01\x12\"\n\x15mpc_weight_r_steering\x18\r \x01(\x01H\x0c\x88\x01\x01\x42\x1a\n\x18_wheel_radius_left_ratioB\x1b\n\x19_wheel_radius_right_ratioB\x1a\n\x18_effective_track_width_mB\x1b\n\x19_steering_zero_offset_degB\x11\n\x0f_pid_kp_lateralB\x11\n\x0f_pid_ki_lateralB\x11\n\x0f_pid_kd_lateralB\x11\n\x0f_pid_kp_headingB\x11\n\x0f_pid_ki_headingB\x11\n\x0f_pid_kd_headingB\x1b\n\x19_pure_pursuit_lookahead_mB\x17\n\x15_mpc_weight_q_lateralB\x18\n\x16_mpc_weight_r_steering\"\xad\x02\n\rTelemetryData\x12\x1d\n\x15hardware_timestamp_us\x18\x01 \x01(\x03\x12\x10\n\x08odom_x_m\x18\x02 \x01(\x01\x12\x10\n\x08odom_y_m\x18\x03 \x01(\x01\x12\x14\n\x0codom_yaw_rad\x18\x04 \x01(\x01\x12\x1e\n\x16\x66\x65\x65\x64\x62\x61\x63k_linear_vel_ms\x18\x05 \x01(\x01\x12!\n\x19\x66\x65\x65\x64\x62\x61\x63k_angular_vel_rads\x18\x06 \x01(\x01\x12\x1e\n\x16left_motor_current_amp\x18\x07 \x01(\x01\x12\x1f\n\x17right_motor_current_amp\x18\x08 \x01(\x01\x12\"\n\x1asteering_motor_current_amp\x18\t \x01(\x01\x12\x1b\n\x13\x63md_steering_output\x18\n \x01(\x01\x32\xd1\x06\n\x16\x41gvCalibControlService\x12\x61\n\x0eSetControlMode\x12$.agv.calibration.control.ModeRequest\x1a).agv.calibration.control.StandardResponse\x12Z\n\rEmergencyStop\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12i\n\x12\x45xecuteOpenLoopCmd\x12(.agv.calibration.control.OpenLoopRequest\x1a).agv.calibration.control.StandardResponse\x12m\n\x14\x46ollowTestTrajectory\x12*.agv.calibration.control.TrajectoryRequest\x1a).agv.calibration.control.StandardResponse\x12n\n\x13\x45xecuteStepResponse\x12,.agv.calibration.control.StepResponseRequest\x1a).agv.calibration.control.StandardResponse\x12k\n\x16InjectTuningParameters\x12&.agv.calibration.control.ControlParams\x1a).agv.calibration.control.StandardResponse\x12\x64\n\x17\x43ommitControlParameters\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12[\n\x0fStreamTelemetry\x12\x1e.agv.calibration.control.Empty\x1a&.agv.calibration.control.TelemetryData0\x01\x62\x06proto3') - -_globals = globals() -_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals) -_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'agv_calib_control_pb2', _globals) -if not _descriptor._USE_C_DESCRIPTORS: - DESCRIPTOR._loaded_options = None - _globals['_EMPTY']._serialized_start=52 - _globals['_EMPTY']._serialized_end=59 - _globals['_STANDARDRESPONSE']._serialized_start=61 - _globals['_STANDARDRESPONSE']._serialized_end=113 - _globals['_MODEREQUEST']._serialized_start=116 - _globals['_MODEREQUEST']._serialized_end=255 - _globals['_MODEREQUEST_MODE']._serialized_start=195 - _globals['_MODEREQUEST_MODE']._serialized_end=255 - _globals['_OPENLOOPREQUEST']._serialized_start=257 - _globals['_OPENLOOPREQUEST']._serialized_end=369 - _globals['_TRAJECTORYPOINT']._serialized_start=371 - _globals['_TRAJECTORYPOINT']._serialized_end=475 - _globals['_TRAJECTORYREQUEST']._serialized_start=477 - _globals['_TRAJECTORYREQUEST']._serialized_end=574 - _globals['_STEPRESPONSEREQUEST']._serialized_start=576 - _globals['_STEPRESPONSEREQUEST']._serialized_end=647 - _globals['_CONTROLPARAMS']._serialized_start=650 - _globals['_CONTROLPARAMS']._serialized_end=1411 - _globals['_TELEMETRYDATA']._serialized_start=1414 - _globals['_TELEMETRYDATA']._serialized_end=1715 - _globals['_AGVCALIBCONTROLSERVICE']._serialized_start=1718 - _globals['_AGVCALIBCONTROLSERVICE']._serialized_end=2567 -# @@protoc_insertion_point(module_scope) diff --git a/agv_calib_brain/src/agv_calib_control_pb2_grpc.py b/agv_calib_brain/src/agv_calib_control_pb2_grpc.py deleted file mode 100644 index 5dd44cb..0000000 --- a/agv_calib_brain/src/agv_calib_control_pb2_grpc.py +++ /dev/null @@ -1,444 +0,0 @@ -# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT! -"""Client and server classes corresponding to protobuf-defined services.""" -import grpc -import warnings - -import agv_calib_control_pb2 as agv__calib__control__pb2 - -GRPC_GENERATED_VERSION = '1.78.0' -GRPC_VERSION = grpc.__version__ -_version_not_supported = False - -try: - from grpc._utilities import first_version_is_lower - _version_not_supported = first_version_is_lower(GRPC_VERSION, GRPC_GENERATED_VERSION) -except ImportError: - _version_not_supported = True - -if _version_not_supported: - raise RuntimeError( - f'The grpc package installed is at version {GRPC_VERSION},' - + ' but the generated code in agv_calib_control_pb2_grpc.py depends on' - + f' grpcio>={GRPC_GENERATED_VERSION}.' - + f' Please upgrade your grpc module to grpcio>={GRPC_GENERATED_VERSION}' - + f' or downgrade your generated code using grpcio-tools<={GRPC_VERSION}.' - ) - - -class AgvCalibControlServiceStub(object): - """========================================================= - 核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理 - [部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报) - [调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数) - ========================================================= - """ - - def __init__(self, channel): - """Constructor. - - Args: - channel: A grpc.Channel. - """ - self.SetControlMode = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/SetControlMode', - request_serializer=agv__calib__control__pb2.ModeRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.EmergencyStop = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/EmergencyStop', - request_serializer=agv__calib__control__pb2.Empty.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.ExecuteOpenLoopCmd = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd', - request_serializer=agv__calib__control__pb2.OpenLoopRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.FollowTestTrajectory = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory', - request_serializer=agv__calib__control__pb2.TrajectoryRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.ExecuteStepResponse = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse', - request_serializer=agv__calib__control__pb2.StepResponseRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.InjectTuningParameters = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/InjectTuningParameters', - request_serializer=agv__calib__control__pb2.ControlParams.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.CommitControlParameters = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/CommitControlParameters', - request_serializer=agv__calib__control__pb2.Empty.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.StreamTelemetry = channel.unary_stream( - '/agv.calibration.control.AgvCalibControlService/StreamTelemetry', - request_serializer=agv__calib__control__pb2.Empty.SerializeToString, - response_deserializer=agv__calib__control__pb2.TelemetryData.FromString, - _registered_method=True) - - -class AgvCalibControlServiceServicer(object): - """========================================================= - 核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理 - [部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报) - [调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数) - ========================================================= - """ - - def SetControlMode(self, request, context): - """--------------------------------------------------------- - 第一步：权限接管与生命周期安全管控 - --------------------------------------------------------- - 💻 [Linux 发送 -> Windows]：要求切断避障，但保留底层 PID/MPC 算法就绪 - 🚙 [Windows 返回 -> Linux]：回复模式切换成功，准备好接考题 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def EmergencyStop(self, request, context): - """💻 [Linux 发送 -> Windows]：断网或飞车时的最高级别急停，无视一切直接刹车 - 🚙 [Windows 返回 -> Linux]：返回底层抱死结果 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def ExecuteOpenLoopCmd(self, request, context): - """--------------------------------------------------------- - 第二步：运动考题下发 (开环排雷 / 闭环寻优 / 波峰对齐) - --------------------------------------------------------- - 【场景A: 纯物理开环备用】 - 💻 [Linux 发送 -> Windows]：要求切断算法盲跑，多用于摸底或辅助验证 - 🚙 [Windows 返回 -> Linux]：确认已按指定 RPM/PWM 运转 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def FollowTestTrajectory(self, request, context): - """【场景B: 算法闭环调优】 - 💻 [Linux 发送 -> Windows]：下发一条由几百个点组成的测试轨迹(如 S型贝塞尔曲线) - 🚙 [Windows 返回 -> Linux]：收到轨迹后，车端立刻使用它自带的 PID/MPC 算法努力贴合轨迹跑圈 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def ExecuteStepResponse(self, request, context): - """【场景C: 波峰时序对齐】 - 💻 [Linux 发送 -> Windows]：下发极短促的阶跃加速指令，人为制造绝对速度波峰 - 🚙 [Windows 返回 -> Linux]：确认加速。(Linux 借此波峰算出网络的绝对 Time Offset) - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def InjectTuningParameters(self, request, context): - """--------------------------------------------------------- - 第三步：运控参数 AI 寻优：动态热注入与最终固化 - --------------------------------------------------------- - 💻 [Linux 发送 -> Windows]：Linux 发现上一圈跑得差，AI算出了新的 PID/前瞻距离，要求立即热注入 - 🚙 [Windows 返回 -> Linux]：车端将新参数瞬间覆写进运行内存(不重启系统)，随时准备用新参数重跑 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def CommitControlParameters(self, request, context): - """💻 [Linux 发送 -> Windows]：Linux 判定误差极小，调优结束，命令固化目前内存里的最高分参数 - 🚙 [Windows 返回 -> Linux]：车端将这组完美参数永久覆写进硬盘的 config.yaml 或系统注册表 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def StreamTelemetry(self, request, context): - """--------------------------------------------------------- - 第四步：高频数字孪生体感上报 (50Hz) - --------------------------------------------------------- - 💻 [Linux 发送 -> Windows]：空包触发，命令车端开始疯狂推流 - 🚙 [Windows 持续流式返回 -> Linux]：以 50Hz 频率，持续上报自己的里程计坐标、速度和单调时间戳 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - -def add_AgvCalibControlServiceServicer_to_server(servicer, server): - rpc_method_handlers = { - 'SetControlMode': grpc.unary_unary_rpc_method_handler( - servicer.SetControlMode, - request_deserializer=agv__calib__control__pb2.ModeRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'EmergencyStop': grpc.unary_unary_rpc_method_handler( - servicer.EmergencyStop, - request_deserializer=agv__calib__control__pb2.Empty.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'ExecuteOpenLoopCmd': grpc.unary_unary_rpc_method_handler( - servicer.ExecuteOpenLoopCmd, - request_deserializer=agv__calib__control__pb2.OpenLoopRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'FollowTestTrajectory': grpc.unary_unary_rpc_method_handler( - servicer.FollowTestTrajectory, - request_deserializer=agv__calib__control__pb2.TrajectoryRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'ExecuteStepResponse': grpc.unary_unary_rpc_method_handler( - servicer.ExecuteStepResponse, - request_deserializer=agv__calib__control__pb2.StepResponseRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'InjectTuningParameters': grpc.unary_unary_rpc_method_handler( - servicer.InjectTuningParameters, - request_deserializer=agv__calib__control__pb2.ControlParams.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'CommitControlParameters': grpc.unary_unary_rpc_method_handler( - servicer.CommitControlParameters, - request_deserializer=agv__calib__control__pb2.Empty.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'StreamTelemetry': grpc.unary_stream_rpc_method_handler( - servicer.StreamTelemetry, - request_deserializer=agv__calib__control__pb2.Empty.FromString, - response_serializer=agv__calib__control__pb2.TelemetryData.SerializeToString, - ), - } - generic_handler = grpc.method_handlers_generic_handler( - 'agv.calibration.control.AgvCalibControlService', rpc_method_handlers) - server.add_generic_rpc_handlers((generic_handler,)) - server.add_registered_method_handlers('agv.calibration.control.AgvCalibControlService', rpc_method_handlers) - - - # This class is part of an EXPERIMENTAL API. -class AgvCalibControlService(object): - """========================================================= - 核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理 - [部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报) - [调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数) - ========================================================= - """ - - @staticmethod - def SetControlMode(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/SetControlMode', - agv__calib__control__pb2.ModeRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def EmergencyStop(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/EmergencyStop', - agv__calib__control__pb2.Empty.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def ExecuteOpenLoopCmd(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd', - agv__calib__control__pb2.OpenLoopRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def FollowTestTrajectory(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory', - agv__calib__control__pb2.TrajectoryRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def ExecuteStepResponse(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse', - agv__calib__control__pb2.StepResponseRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def InjectTuningParameters(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/InjectTuningParameters', - agv__calib__control__pb2.ControlParams.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def CommitControlParameters(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/CommitControlParameters', - agv__calib__control__pb2.Empty.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def StreamTelemetry(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_stream( - request, - target, - '/agv.calibration.control.AgvCalibControlService/StreamTelemetry', - agv__calib__control__pb2.Empty.SerializeToString, - agv__calib__control__pb2.TelemetryData.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) diff --git a/agv_calib_brain/src/agv_calib_core/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/CMakeLists.txt deleted file mode 100644 index 48d21a3..0000000 --- a/agv_calib_brain/src/agv_calib_core/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(agv_calib_core) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_components REQUIRED) # 🚨 核心依赖：寻找组件库 -find_package(behaviortree_cpp_v3 REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(win_ubuntu_bridge REQUIRED) - -# 1. 编译大脑为动态链接库 (SHARED) 组件 -add_library(brain_node SHARED src/brain_node.cpp) - -# 2. 将 include 暴露给编译器 -target_include_directories(brain_node PUBLIC - "$" -) - -ament_target_dependencies(brain_node - rclcpp - rclcpp_action - rclcpp_components - behaviortree_cpp_v3 - ament_index_cpp - win_ubuntu_bridge -) - -# 3. 注册插件 -rclcpp_components_register_node(brain_node - PLUGIN "agv_calib_core::BrainNode" - EXECUTABLE brain_node_exe -) - -# 4. 安装工程中所有的核心文件夹 (不可遗漏！) -install(TARGETS brain_node brain_node_exe - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ DESTINATION include) -install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) -install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) -install(DIRECTORY behavior_trees/ DESTINATION share/${PROJECT_NAME}/behavior_trees) - -ament_package() \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/behavior_trees/main_pipeline.xml b/agv_calib_brain/src/agv_calib_core/behavior_trees/main_pipeline.xml deleted file mode 100644 index fce81fc..0000000 --- a/agv_calib_brain/src/agv_calib_core/behavior_trees/main_pipeline.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/agv_calib_brain/src/agv_calib_core/behavior_trees/main_tree.xml b/agv_calib_brain/src/agv_calib_core/behavior_trees/main_tree.xml deleted file mode 100644 index 3a5c5ff..0000000 --- a/agv_calib_brain/src/agv_calib_core/behavior_trees/main_tree.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/CMakeLists.txt new file mode 100644 index 0000000..9535b9c --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(chassis_calibration_service) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_chassis_interfaces REQUIRED) +find_package(calibration_control_interfaces REQUIRED) +find_package(calibration_external_localization_interfaces REQUIRED) +find_package(calibration_sensor_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) +find_package(calibration_workshop_orchestration_interfaces REQUIRED) + +include_directories(include) + +add_library(chassis_calibration_service_component SHARED + src/chassis_calibration_service_node.cpp + src/chassis_calibration_algorithm_template.cpp + src/ackermann_chassis_algorithm.cpp + src/differential_chassis_algorithm.cpp + src/single_steer_wheel_chassis_algorithm.cpp + src/multi_steer_wheel_chassis_algorithm.cpp +) + +ament_target_dependencies(chassis_calibration_service_component + rclcpp + rclcpp_action + rclcpp_components + calibration_common_interfaces + calibration_chassis_interfaces + calibration_control_interfaces + calibration_external_localization_interfaces + calibration_sensor_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces +) + +rclcpp_components_register_nodes(chassis_calibration_service_component + "chassis_calibration_service::ChassisCalibrationServiceNode" +) + +add_executable(chassis_calibration_service_node src/main.cpp) +ament_target_dependencies(chassis_calibration_service_node + rclcpp + rclcpp_components +) +target_link_libraries(chassis_calibration_service_node + chassis_calibration_service_component +) + +install(TARGETS + chassis_calibration_service_component + chassis_calibration_service_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +ament_package() diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithm_template.hpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithm_template.hpp new file mode 100644 index 0000000..1cb90fa --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithm_template.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include "chassis_calibration_service/chassis_calibration_algorithms.hpp" + +namespace chassis_calibration_service +{ + +// 底盘标定算法门面： +// 负责输入校验、按底盘类型分发到专属实现类、再统一回填 ROS result。 +// 具体算法细节放在各自的底盘专属类里，node 层不需要知道这些差异。 +class ChassisCalibrationAlgorithmTemplate +{ +public: + using ExecuteTask = chassis_calibration_service::ExecuteTask; + using ChassisType = chassis_calibration_service::ChassisType; + using Input = ChassisCalibrationInput; + using Output = ChassisCalibrationOutput; + + bool run( + const Input & input, + Output & output, + std::string & failure_reason) const; + +private: + bool validate_input(const Input & input, std::string & failure_reason) const; + + const ChassisCalibrationAlgorithm * resolve_algorithm(const ChassisType & chassis_type) const; + + AckermannChassisAlgorithm ackermann_algorithm_; + DifferentialChassisAlgorithm differential_algorithm_; + SingleSteerWheelChassisAlgorithm single_steer_wheel_algorithm_; + MultiSteerWheelChassisAlgorithm multi_steer_wheel_algorithm_; +}; + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp new file mode 100644 index 0000000..d21f127 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp @@ -0,0 +1,160 @@ +#pragma once + +#include +#include + +#include "calibration_chassis_interfaces/action/execute_motion_primitive.hpp" +#include "calibration_chassis_interfaces/msg/applied_chassis_calibration_parameters_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_capability_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" +#include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" +#include "calibration_control_interfaces/msg/control_readiness_response.hpp" +#include "calibration_control_interfaces/msg/control_telemetry.hpp" +#include "calibration_control_interfaces/msg/control_work_mode.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_readiness_response.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_telemetry.hpp" +#include "calibration_sensor_interfaces/msg/applied_sensor_calibration_parameters_response.hpp" +#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_telemetry.hpp" +#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp" +#include "calibration_vehicle_profile_interfaces/msg/chassis_type.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace chassis_calibration_service +{ + +using ExecuteTask = calibration_chassis_interfaces::action::ExecuteMotionPrimitive; +using ChassisType = calibration_vehicle_profile_interfaces::msg::ChassisType; + +struct ChassisCalibrationInput +{ + // ===== 会话与静态画像 ===== + // 当前车间会话：可用于读取本轮阶段计划、人工审批状态、会话级参数等。 + calibration_workshop_orchestration_interfaces::msg::WorkshopSession workshop_session; + + // 车辆画像快照：包含底盘、传感器、控制器、URDF、启用阶段、能力清单等静态信息。 + calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile; + + // 标定任务请求：包含 request_id、任务目的、原语类型、原语参数、超时、是否执行完毕后刹停等。 + ExecuteTask::Goal request; + + // 当前底盘类型：用于把任务路由到对应的算法模板。 + ChassisType chassis_type; + + // ===== 底盘域反馈 ===== + // 底盘能力：先判断当前车到底支持哪些动作原语，再决定算法是否可以进入执行。 + calibration_chassis_interfaces::msg::ChassisCapabilityResponse chassis_capability; + + // 底盘就绪状态：驱动是否在线、急停是否释放、是否允许移动、是否可回传遥测。 + calibration_chassis_interfaces::msg::ChassisReadinessResponse chassis_readiness; + + // 当前工作模式：标定准备、直接执行、验证等，算法可据此决定工作流。 + calibration_chassis_interfaces::msg::ChassisWorkMode chassis_work_mode; + + // 当前已生效的底盘参数：算法通常要读取现有参数，和本轮结果做对比或增量更新。 + calibration_chassis_interfaces::msg::AppliedChassisCalibrationParametersResponse applied_chassis_parameters; + + // 最新一帧底盘遥测：轮速、里程计、舵角、IMU、驱动错误码等。 + calibration_chassis_interfaces::msg::ChassisTelemetry latest_chassis_telemetry; + + // 历史底盘遥测窗口：用于做滤波、拟合、稳态判定、重复性分析和异常剔除。 + std::vector chassis_telemetry_history; + + // ===== 控制域反馈 ===== + // 控制就绪状态：轨迹执行器、车辆反馈链路、控制输出链路、急停等。 + calibration_control_interfaces::msg::ControlReadinessResponse control_readiness; + + // 当前控制工作模式：调参准备、评估、验证等。 + calibration_control_interfaces::msg::ControlWorkMode control_work_mode; + + // 当前已生效控制参数：横向 / 纵向控制器参数集。 + calibration_control_interfaces::msg::ActiveControllerParametersResponse active_controller_parameters; + + // 最新一帧控制遥测：误差、控制输出、饱和情况、当前算法版本等。 + calibration_control_interfaces::msg::ControlTelemetry latest_control_telemetry; + + // 历史控制遥测窗口：用于控制参数拟合、误差统计、响应曲线分析。 + std::vector control_telemetry_history; + + // ===== 传感器域反馈 ===== + // 传感器就绪状态：采集链路、存储链路、遥测链路、机械臂状态等。 + calibration_sensor_interfaces::msg::SensorReadinessResponse sensor_readiness; + + // 当前已生效传感器参数：相机内参、IMU 内参、外参、手眼参数等。 + calibration_sensor_interfaces::msg::AppliedSensorCalibrationParametersResponse applied_sensor_parameters; + + // 最新一帧传感器标定遥测：采集样本数、目标检测状态、质量评分等。 + calibration_sensor_interfaces::msg::SensorCalibrationTelemetry latest_sensor_telemetry; + + // 历史传感器遥测窗口：用于观察稳定性、目标可见性、质量趋势。 + std::vector sensor_telemetry_history; + + // ===== 外部定位 / 真值源反馈 ===== + // 外部定位就绪状态：是否适合作为参考源、时间同步是否达标、质量摘要等。 + calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness; + + // 最新一帧外部定位遥测：真值位姿、标准差、时间同步偏差、质量分数等。 + calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry; + + // 历史外部定位遥测窗口：用于和车端里程计、控制误差、标定结果做对齐分析。 + std::vector external_localization_telemetry_history; + + // 说明：如果后续算法还需要更多原始反馈，可以继续在这里扩展。 + // 常见补充项包括：GNSS、SLAM 里程计、地面真值、控制器内部状态、制动状态、故障码、机械臂反馈等。 +}; + +struct ChassisCalibrationOutput +{ + // 标定算法的输出：成功/失败、错误码、验证摘要、建议参数、产物引用等都写到这里。 + ExecuteTask::Result response; +}; + +class ChassisCalibrationAlgorithm +{ +public: + virtual ~ChassisCalibrationAlgorithm() = default; + virtual bool run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const = 0; +}; + +class AckermannChassisAlgorithm final : public ChassisCalibrationAlgorithm +{ +public: + bool run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class DifferentialChassisAlgorithm final : public ChassisCalibrationAlgorithm +{ +public: + bool run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class SingleSteerWheelChassisAlgorithm final : public ChassisCalibrationAlgorithm +{ +public: + bool run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class MultiSteerWheelChassisAlgorithm final : public ChassisCalibrationAlgorithm +{ +public: + bool run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_common.hpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_common.hpp new file mode 100644 index 0000000..22536b2 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_common.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "chassis_calibration_service/chassis_calibration_algorithms.hpp" + +namespace chassis_calibration_service +{ + +inline void fill_common_result( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output) +{ + output.response.result.success = true; + output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK; + output.response.result.job_id = input.request.goal.header.request_id; + output.response.result.data_quality_passed = true; + output.response.result.suitable_for_commit = true; + output.response.result.estimated_straight_line_bias = 0.0; + output.response.result.validation_summary.max_lateral_error_m = 0.0; + output.response.result.validation_summary.max_yaw_error_rad = 0.0; + output.response.result.validation_summary.rms_lateral_error_m = 0.0; + output.response.result.validation_summary.rms_yaw_error_rad = 0.0; + output.response.result.validation_summary.repeatability_error_m = 0.0; + output.response.result.validation_summary.curvature_error = 0.0; + output.response.result.validation_summary.module_consistency_error = 0.0; + output.response.result.validation_summary.auto_acceptance_passed = true; + output.response.result.estimated_params.chassis_type = input.chassis_type; +} + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_service_node.hpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_service_node.hpp new file mode 100644 index 0000000..bd3ff4a --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_service_node.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_chassis_interfaces/action/execute_motion_primitive.hpp" +#include "calibration_chassis_interfaces/srv/get_chassis_readiness.hpp" +#include "calibration_vehicle_profile_interfaces/srv/get_vehicle_profile.hpp" + +#include "chassis_calibration_service/chassis_calibration_algorithm_template.hpp" + +namespace chassis_calibration_service +{ + +// 底盘标定服务节点： +// 1) 对外提供 readiness 接口； +// 2) 对外提供动作原语 action； +// 3) 后续把真实底盘算法接进来时，算法入口也放在这个节点内部。 +class ChassisCalibrationServiceNode : public rclcpp::Node +{ +public: + explicit ChassisCalibrationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using ReadinessSrv = calibration_chassis_interfaces::srv::GetChassisReadiness; + using GetVehicleProfileSrv = calibration_vehicle_profile_interfaces::srv::GetVehicleProfile; + using ExecuteTask = calibration_chassis_interfaces::action::ExecuteMotionPrimitive; + using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle; + + // readiness 服务：让编排器确认当前底盘是否具备执行条件。 + rclcpp::Service::SharedPtr readiness_service_; + // 车辆画像客户端：从 vehicle_profile_manager 取当前车辆的底盘类型。 + rclcpp::Client::SharedPtr get_vehicle_profile_client_; + // action server：接收底盘动作原语任务，并在后台线程里执行。 + rclcpp_action::Server::SharedPtr execute_task_action_server_; + + // readiness 回调：把底盘运行状态翻译成统一的 readiness 响应。 + void handle_readiness( + const std::shared_ptr request, + std::shared_ptr response); + // goal 回调：在真正执行之前先做一次最小合法性检查。 + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + // cancel 回调：当前最小版本直接接受取消，真正的中止逻辑后面再接。 + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle); + // goal 接收后回调：把长任务放到后台线程，避免卡住 ROS 回调线程。 + void handle_accepted(const std::shared_ptr goal_handle); + // 从车辆画像服务里按 vehicle_id 查询底盘类型。 + bool fetch_chassis_type( + const std::string & vehicle_id, + ChassisCalibrationAlgorithmTemplate::ChassisType & chassis_type, + std::string & failure_reason); + // 后台执行入口：这里不直接写算法细节，而是调用底盘算法模板。 + // 以后真实标定算法只需要替换模板内部实现，节点层仍然保持稳定。 + void execute_goal(const std::shared_ptr goal_handle); + + // 底盘标定算法模板：负责承接输入校验、底盘类型分发与结果填充。 + ChassisCalibrationAlgorithmTemplate algorithm_; +}; + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/launch/chassis_calibration_service_component.launch.py b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/launch/chassis_calibration_service_component.launch.py new file mode 100644 index 0000000..fa02f29 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/launch/chassis_calibration_service_component.launch.py @@ -0,0 +1,33 @@ +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + container = ComposableNodeContainer( + name='chassis_calibration_service_container', + namespace='/', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[], + output='screen', + ) + + load_node = LoadComposableNodes( + target_container='/chassis_calibration_service_container', + composable_node_descriptions=[ + ComposableNode( + package='chassis_calibration_service', + plugin='chassis_calibration_service::ChassisCalibrationServiceNode', + name='chassis_calibration_service', + namespace='/', + ) + ], + ) + + return LaunchDescription([ + container, + load_node, + ]) diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/package.xml b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/package.xml new file mode 100644 index 0000000..d8c5823 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/package.xml @@ -0,0 +1,26 @@ + + + chassis_calibration_service + 0.0.1 + Minimal chassis calibration service stub. + you + Apache-2.0 + + ament_cmake + rclcpp + rclcpp_action + rclcpp_components + calibration_common_interfaces + calibration_chassis_interfaces + calibration_control_interfaces + calibration_external_localization_interfaces + calibration_sensor_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces + launch + launch_ros + + + ament_cmake + + diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp new file mode 100644 index 0000000..f1707d7 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp @@ -0,0 +1,56 @@ +#include "chassis_calibration_service/chassis_calibration_common.hpp" + +namespace chassis_calibration_service +{ + +bool AckermannChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 阿克曼底盘标定模板： + // 【本文件负责什么】 + // - 负责阿克曼底盘相关的标定算法实现。 + // - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。 + // + // 【建议优先读取的输入】 + // 1. input.request + // - request_id、task_purpose、selected_primitive、straight_line / steering_sweep 等任务参数。 + // 2. input.vehicle_profile / input.chassis_type + // - 车辆静态画像、底盘类型、基础结构参数。 + // 3. input.applied_chassis_parameters + // - 当前已生效底盘参数，作为本轮优化的初始值或对照基线。 + // 4. input.latest_chassis_telemetry / input.chassis_telemetry_history + // - 轮速、舵角、角速度、里程计、电机状态、电流、温度、驱动诊断等。 + // 5. input.latest_external_localization_telemetry / input.external_localization_telemetry_history + // - 如果需要真值轨迹、真值航向、真值速度，可从这里读取。 + // 6. input.latest_control_telemetry / input.control_telemetry_history + // - 如果需要分析控制输出与底盘响应差异，可结合控制域反馈。 + // + // 【必写输出】 + // 1. output.response.result.validation_summary + // - 写横向误差、航向误差、重复性、曲率误差、模块一致性等摘要指标。 + // 2. output.response.result.estimated_params + // - 写本轮求解出的阿克曼底盘参数。 + // 3. output.response.result.suitable_for_commit + // - 明确是否建议进入提交环节。 + // + // 【可选输出】 + // - output.response.result.artifacts + // 可挂轨迹图、拟合日志、误差统计 csv、调试报告等。 + // + // 【常见失败原因】 + // - 真值源不可用、车辆未进入可移动状态、有效轨迹长度不足、舵角反馈异常、轮速反馈缺失。 + // + // 【在这里添加真实算法】 + // - 请在 fill_common_result(...) 之前或之后补充真实求解逻辑。 + // - 当前文件仅提供交付模板，不包含真实标定算法。 + fill_common_result(input, output); + output.response.result.message = "ackermann chassis template executed."; + output.response.result.recommended_parameter_version = "ackermann_template_v1"; + return true; +} + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp new file mode 100644 index 0000000..869fa92 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp @@ -0,0 +1,81 @@ +#include "chassis_calibration_service/chassis_calibration_algorithm_template.hpp" + +namespace chassis_calibration_service +{ + +bool ChassisCalibrationAlgorithmTemplate::validate_input( + const Input & input, + std::string & failure_reason) const +{ + if (input.request.goal.header.request_id.empty()) { + failure_reason = "chassis 任务 request_id 不能为空。"; + return false; + } + + if (input.chassis_type.value == ChassisType::CHASSIS_TYPE_UNSPECIFIED) { + failure_reason = "底盘类型不能为空，必须明确是阿克曼、差速、单舵轮或多舵轮。"; + return false; + } + + if (input.request.goal.selected_primitive.value != + calibration_chassis_interfaces::msg::ChassisMotionPrimitiveType::STRAIGHT_LINE) { + failure_reason = "当前模板只演示 straight_line 动作原语；如果要支持转弯、原地旋转、横移等原语，需要在这里扩展分发规则。"; + return false; + } + + if (input.request.goal.straight_line.target_distance_m <= 0.0) { + failure_reason = "straight_line.target_distance_m 必须大于 0。"; + return false; + } + if (input.request.goal.straight_line.target_speed_ms <= 0.0) { + failure_reason = "straight_line.target_speed_ms 必须大于 0。"; + return false; + } + + return true; +} + +const ChassisCalibrationAlgorithm * ChassisCalibrationAlgorithmTemplate::resolve_algorithm( + const ChassisType & chassis_type) const +{ + switch (chassis_type.value) { + case ChassisType::ACKERMANN: + return &ackermann_algorithm_; + case ChassisType::DIFFERENTIAL: + return &differential_algorithm_; + case ChassisType::SINGLE_STEER_WHEEL: + return &single_steer_wheel_algorithm_; + case ChassisType::MULTI_STEER_WHEEL: + return &multi_steer_wheel_algorithm_; + default: + return nullptr; + } +} + +bool ChassisCalibrationAlgorithmTemplate::run( + const Input & input, + Output & output, + std::string & failure_reason) const +{ + if (!validate_input(input, failure_reason)) { + return false; + } + + const auto * algorithm = resolve_algorithm(input.chassis_type); + if (algorithm == nullptr) { + failure_reason = "不支持的底盘类型。"; + return false; + } + + // 这里是标定算法的统一入口：先按底盘类型分发，再由对应模板文件实现具体算法。 + // 算法工程师通常会先检查： + // - input.workshop_session / input.vehicle_profile：会话级上下文和车辆静态画像 + // - input.chassis_*：底盘能力、就绪状态、工作模式、参数、遥测 + // - input.control_*：控制链路、控制参数、控制误差和输出 + // - input.sensor_*：传感器就绪状态、已生效参数、采集质量 + // - input.external_localization_*：真值源就绪状态和外部定位观测 + // 再进入具体拟合与参数求解。 + return algorithm->run(input, output, failure_reason); +} + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithms.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithms.cpp new file mode 100644 index 0000000..0d78d92 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithms.cpp @@ -0,0 +1,80 @@ +#include "chassis_calibration_service/chassis_calibration_algorithms.hpp" + +#include "calibration_common_interfaces/msg/error_code.hpp" + +namespace chassis_calibration_service +{ + +namespace +{ +void fill_common_result( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output) +{ + output.response.result.success = true; + output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK; + output.response.result.job_id = input.request.goal.header.request_id; + output.response.result.data_quality_passed = true; + output.response.result.suitable_for_commit = true; + output.response.result.estimated_straight_line_bias = 0.0; + output.response.result.validation_summary.max_lateral_error_m = 0.0; + output.response.result.validation_summary.max_yaw_error_rad = 0.0; + output.response.result.validation_summary.rms_lateral_error_m = 0.0; + output.response.result.validation_summary.rms_yaw_error_rad = 0.0; + output.response.result.validation_summary.repeatability_error_m = 0.0; + output.response.result.validation_summary.curvature_error = 0.0; + output.response.result.validation_summary.module_consistency_error = 0.0; + output.response.result.validation_summary.auto_acceptance_passed = true; + output.response.result.estimated_params.chassis_type = input.chassis_type; +} +} + +bool AckermannChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + fill_common_result(input, output); + output.response.result.message = "ackermann chassis template executed."; + output.response.result.recommended_parameter_version = "ackermann_template_v1"; + return true; +} + +bool DifferentialChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + fill_common_result(input, output); + output.response.result.message = "differential chassis template executed."; + output.response.result.recommended_parameter_version = "differential_template_v1"; + return true; +} + +bool SingleSteerWheelChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + fill_common_result(input, output); + output.response.result.message = "single steer wheel chassis template executed."; + output.response.result.recommended_parameter_version = "single_steer_template_v1"; + return true; +} + +bool MultiSteerWheelChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + fill_common_result(input, output); + output.response.result.message = "multi steer wheel chassis template executed."; + output.response.result.recommended_parameter_version = "multi_steer_template_v1"; + return true; +} + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_service_node.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_service_node.cpp new file mode 100644 index 0000000..7af0c27 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/chassis_calibration_service_node.cpp @@ -0,0 +1,185 @@ +#include "chassis_calibration_service/chassis_calibration_service_node.hpp" + +#include +#include + +#include "rclcpp_components/register_node_macro.hpp" + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_vehicle_profile_interfaces/msg/chassis_type.hpp" + +namespace chassis_calibration_service +{ + +namespace +{ +// 统一使用微秒时间戳，方便和仓库里其他 msg 的 *_timestamp_us 字段对齐。 +int64_t now_us() +{ + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} +} // namespace + +using calibration_common_interfaces::msg::ErrorCode; +using calibration_common_interfaces::msg::JobState; + +ChassisCalibrationServiceNode::ChassisCalibrationServiceNode(const rclcpp::NodeOptions & options) +: Node("chassis_calibration_service", options) +{ + // readiness：供 orchestrator 在开跑前检查底盘是否具备执行条件。 + readiness_service_ = create_service( + "/chassis/get_readiness", + std::bind(&ChassisCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2)); + + // 车辆画像客户端：底盘类型、车辆静态配置等都从这里查询。 + get_vehicle_profile_client_ = create_client("/vehicle_profile_manager/get_vehicle_profile"); + + // action：底盘动作原语执行入口，后续真实算法也从这里接入。 + execute_task_action_server_ = rclcpp_action::create_server( + this, + "/chassis/execute_motion_primitive", + std::bind(&ChassisCalibrationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ChassisCalibrationServiceNode::handle_cancel, this, std::placeholders::_1), + std::bind(&ChassisCalibrationServiceNode::handle_accepted, this, std::placeholders::_1)); +} + +void ChassisCalibrationServiceNode::handle_readiness( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + // 当前先返回“全部就绪”的最小闭环状态，等真实底盘驱动/安全链路接入后再替换成真实检查。 + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "chassis_calibration_service is ready."; + response->response.agent_ready = true; + response->response.chassis_driver_online = true; + response->response.motion_control_ready = true; + response->response.estop_released = true; + response->response.vehicle_safe_to_move = true; + response->response.telemetry_available = true; + response->response.checked_timestamp_us = now_us(); +} + +rclcpp_action::GoalResponse ChassisCalibrationServiceNode::handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + // 最小合法性检查：只有 request_id 为空时拒绝。 + // 后续接真实算法时，可在这里继续加更严格的参数校验。 + if (goal->goal.header.request_id.empty()) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ChassisCalibrationServiceNode::handle_cancel( + const std::shared_ptr /*goal_handle*/) +{ + // 当前最小版本接受取消请求，但不在这里实现复杂中止逻辑。 + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ChassisCalibrationServiceNode::handle_accepted( + const std::shared_ptr goal_handle) +{ + // 长任务放后台线程，避免阻塞 ROS action server 的处理线程。 + std::thread(std::bind(&ChassisCalibrationServiceNode::execute_goal, this, goal_handle)).detach(); +} + +bool ChassisCalibrationServiceNode::fetch_chassis_type( + const std::string & vehicle_id, + ChassisCalibrationAlgorithmTemplate::ChassisType & chassis_type, + std::string & failure_reason) +{ + if (vehicle_id.empty()) { + failure_reason = "vehicle_id 不能为空，无法查询车辆画像。"; + return false; + } + + if (!get_vehicle_profile_client_->wait_for_service(std::chrono::seconds(2))) { + failure_reason = "车辆画像服务暂不可用。"; + return false; + } + + auto request = std::make_shared(); + request->request.vehicle_id = vehicle_id; + auto future = get_vehicle_profile_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future, std::chrono::seconds(2)) != + rclcpp::FutureReturnCode::SUCCESS) { + failure_reason = "查询车辆画像超时。"; + return false; + } + + const auto response = future.get(); + if (!response->response.success) { + failure_reason = response->response.message.empty() ? "查询车辆画像失败。" : response->response.message; + return false; + } + + chassis_type = response->response.profile.chassis_type; + if (chassis_type.value == ChassisCalibrationAlgorithmTemplate::ChassisType::CHASSIS_TYPE_UNSPECIFIED) { + failure_reason = "车辆画像里没有配置底盘类型。"; + return false; + } + + return true; +} + +void ChassisCalibrationServiceNode::execute_goal( + const std::shared_ptr goal_handle) +{ + // 先发一个进度反馈，让前端和 orchestrator 知道底盘任务已经开始运行。 + auto feedback = std::make_shared(); + feedback->feedback.state.state = JobState::RUNNING; + goal_handle->publish_feedback(feedback); + + // 底盘算法本体通过模板类执行。 + // 这里先把 ROS goal 转成模板输入，再让模板内部按 chassis_type 派发到不同底盘分支。 + // chassis_type 不再靠 request_id 猜，而是从车辆画像服务中按 vehicle_id 查询。 + ChassisCalibrationAlgorithmTemplate::Input input; + input.request = *goal_handle->get_goal(); + std::string failure_reason; + if (!fetch_chassis_type( + input.request.goal.header.vehicle_id, + input.chassis_type, + failure_reason)) { + auto result = std::make_shared(); + result->result.success = false; + result->result.error_code.code = ErrorCode::INVALID_ARGUMENT; + result->result.message = failure_reason; + result->result.job_id = input.request.goal.header.request_id; + result->result.data_quality_passed = false; + result->result.suitable_for_commit = false; + result->result.recommended_parameter_version.clear(); + goal_handle->abort(result); + return; + } + + ChassisCalibrationAlgorithmTemplate::Output output; + if (!algorithm_.run(input, output, failure_reason)) { + output.response.result.success = false; + output.response.result.error_code.code = ErrorCode::INVALID_ARGUMENT; + output.response.result.message = failure_reason; + output.response.result.job_id = input.request.goal.header.request_id; + output.response.result.data_quality_passed = false; + output.response.result.suitable_for_commit = false; + output.response.result.recommended_parameter_version.clear(); + auto result = std::make_shared(); + result->result = output.response.result; + goal_handle->abort(result); + return; + } + + auto result = std::make_shared(); + result->result = output.response.result; + goal_handle->succeed(result); +} + +} // namespace chassis_calibration_service + +RCLCPP_COMPONENTS_REGISTER_NODE(chassis_calibration_service::ChassisCalibrationServiceNode) diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp new file mode 100644 index 0000000..2ff4629 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp @@ -0,0 +1,24 @@ +#include "chassis_calibration_service/chassis_calibration_common.hpp" + +namespace chassis_calibration_service +{ + +bool DifferentialChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // TODO: 在这里填写差速底盘标定算法。 + // 算法工程师应从这里读取并计算： + // - 任务请求：input.request（request_id、任务目的、selected_primitive、straight_line 参数、timeout 等） + // - 底盘反馈：由上层扩展到 ChassisCalibrationInput 中的实时数据（轮速、左右电机反馈、IMU、里程计、定位等） + // - 输出：output.response.result（success、error_code、validation_summary、estimated_params、artifacts 等） + fill_common_result(input, output); + output.response.result.message = "differential chassis template executed."; + output.response.result.recommended_parameter_version = "differential_template_v1"; + return true; +} + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/main.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/main.cpp new file mode 100644 index 0000000..df769ba --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/main.cpp @@ -0,0 +1,11 @@ +#include "rclcpp/rclcpp.hpp" +#include "chassis_calibration_service/chassis_calibration_service_node.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp new file mode 100644 index 0000000..bfef9e8 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp @@ -0,0 +1,24 @@ +#include "chassis_calibration_service/chassis_calibration_common.hpp" + +namespace chassis_calibration_service +{ + +bool MultiSteerWheelChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // TODO: 在这里填写多舵轮底盘标定算法。 + // 算法工程师应从这里读取并计算： + // - 任务请求：input.request（request_id、任务目的、selected_primitive、straight_line 参数、timeout 等） + // - 底盘反馈：由上层扩展到 ChassisCalibrationInput 中的实时数据（各转向模块角度、轮速、电机反馈、IMU、定位等） + // - 输出：output.response.result（success、error_code、validation_summary、estimated_params、artifacts 等） + fill_common_result(input, output); + output.response.result.message = "multi steer wheel chassis template executed."; + output.response.result.recommended_parameter_version = "multi_steer_template_v1"; + return true; +} + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp new file mode 100644 index 0000000..1f3318b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp @@ -0,0 +1,24 @@ +#include "chassis_calibration_service/chassis_calibration_common.hpp" + +namespace chassis_calibration_service +{ + +bool SingleSteerWheelChassisAlgorithm::run( + const ChassisCalibrationInput & input, + ChassisCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // TODO: 在这里填写单舵轮底盘标定算法。 + // 算法工程师应从这里读取并计算： + // - 任务请求：input.request（request_id、任务目的、selected_primitive、straight_line 参数、timeout 等） + // - 底盘反馈：由上层扩展到 ChassisCalibrationInput 中的实时数据（舵角反馈、电机反馈、轮速、IMU、定位等） + // - 输出：output.response.result（success、error_code、validation_summary、estimated_params、artifacts 等） + fill_common_result(input, output); + output.response.result.message = "single steer wheel chassis template executed."; + output.response.result.recommended_parameter_version = "single_steer_template_v1"; + return true; +} + +} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/config/brain_params.yaml b/agv_calib_brain/src/agv_calib_core/config/brain_params.yaml deleted file mode 100644 index 438ff0e..0000000 --- a/agv_calib_brain/src/agv_calib_core/config/brain_params.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - # 动态指定要加载的 XML 剧本文件名 - tree_xml_filename: "main_tree.xml" - # 行为树的 Tick 循环频率 (毫秒) - tick_rate_ms: 50 \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/control_calibration_service/CMakeLists.txt new file mode 100644 index 0000000..011fd76 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(control_calibration_service) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(calibration_chassis_interfaces REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_control_interfaces REQUIRED) +find_package(calibration_external_localization_interfaces REQUIRED) +find_package(calibration_sensor_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) +find_package(calibration_workshop_orchestration_interfaces REQUIRED) + +include_directories(include) + +add_library(control_calibration_service_component SHARED + src/control_calibration_service_node.cpp + src/control_calibration_algorithm_template.cpp + src/pid_control_calibration_algorithm.cpp + src/mpc_control_calibration_algorithm.cpp + src/lqr_control_calibration_algorithm.cpp + src/pure_pursuit_control_calibration_algorithm.cpp +) + +ament_target_dependencies(control_calibration_service_component + rclcpp + rclcpp_action + rclcpp_components + calibration_chassis_interfaces + calibration_common_interfaces + calibration_control_interfaces + calibration_external_localization_interfaces + calibration_sensor_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces +) + +rclcpp_components_register_nodes(control_calibration_service_component + "control_calibration_service::ControlCalibrationServiceNode" +) + +add_executable(control_calibration_service_node src/main.cpp) +ament_target_dependencies(control_calibration_service_node + rclcpp + rclcpp_components +) +target_link_libraries(control_calibration_service_node + control_calibration_service_component +) + +install(TARGETS + control_calibration_service_component + control_calibration_service_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +ament_package() diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithm_template.hpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithm_template.hpp new file mode 100644 index 0000000..b4c2f7b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithm_template.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include + +#include "control_calibration_service/control_calibration_algorithms.hpp" + +namespace control_calibration_service +{ + +// 控制标定算法门面： +// 1. 负责对 node 组装好的 ControlCalibrationInput 做统一入口校验。 +// 2. 负责按控制算法类型分发到 PID / MPC / LQR / Pure Pursuit 的专属实现。 +// 3. 负责保持 node 层与具体算法实现解耦，方便后续整体替换某个算法 cpp 文件。 +// 4. 这是算法工程师和 ROS 接口层之间的边界： +// - node 只负责通信、上下文收集、结果回填； +// - template 只负责路由和基本校验； +// - 具体算法文件只负责“怎么算”。 +class ControlCalibrationAlgorithmTemplate +{ +public: + using Input = ControlCalibrationInput; + using Output = ControlCalibrationOutput; + using ControllerAlgorithmType = control_calibration_service::ControllerAlgorithmType; + + bool run( + const Input & input, + Output & output, + std::string & failure_reason) const; + +private: + bool validate_input(const Input & input, std::string & failure_reason) const; + + const ControlCalibrationAlgorithm * resolve_algorithm( + const ControllerAlgorithmType & controller_algorithm) const; + + PidControlCalibrationAlgorithm pid_algorithm_; + MpcControlCalibrationAlgorithm mpc_algorithm_; + LqrControlCalibrationAlgorithm lqr_algorithm_; + PurePursuitControlCalibrationAlgorithm pure_pursuit_algorithm_; +}; + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp new file mode 100644 index 0000000..66dafb9 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp @@ -0,0 +1,224 @@ +#pragma once + +#include +#include + +#include "calibration_chassis_interfaces/msg/applied_chassis_calibration_parameters_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" +#include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_control_interfaces/action/execute_controller_evaluation.hpp" +#include "calibration_control_interfaces/msg/acceleration_deceleration_task.hpp" +#include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" +#include "calibration_control_interfaces/msg/control_readiness_response.hpp" +#include "calibration_control_interfaces/msg/control_telemetry.hpp" +#include "calibration_control_interfaces/msg/control_work_mode.hpp" +#include "calibration_control_interfaces/msg/controller_evaluation_task_type.hpp" +#include "calibration_control_interfaces/msg/controller_parameter_set.hpp" +#include "calibration_control_interfaces/msg/stop_accuracy_task.hpp" +#include "calibration_control_interfaces/msg/trajectory_point.hpp" +#include "calibration_control_interfaces/msg/trajectory_tracking_task.hpp" +#include "calibration_control_interfaces/msg/velocity_step_task.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_readiness_response.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_telemetry.hpp" +#include "calibration_sensor_interfaces/msg/applied_sensor_calibration_parameters_response.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_telemetry.hpp" +#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp" +#include "calibration_vehicle_profile_interfaces/msg/controller_algorithm_type.hpp" +#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace control_calibration_service +{ + +using ExecuteTask = calibration_control_interfaces::action::ExecuteControllerEvaluation; +using ControllerAlgorithmType = calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType; + +struct ControlCalibrationInput +{ + // ===== 会话与静态画像 ===== + // 当前车间会话：算法可读取本轮 session 的阶段上下文、人工确认状态、操作者信息等。 + calibration_workshop_orchestration_interfaces::msg::WorkshopSession workshop_session; + + // 车辆画像：包含控制器画像、控制轴支持算法、默认算法、车辆基础信息等。 + calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile; + + // 控制评估任务请求：包含 request_id、任务目的、任务类型、各任务 payload、来源迭代号。 + ExecuteTask::Goal request; + + // 本轮需要进入的控制算法类型：用于把任务分发到 PID / MPC / LQR / Pure Pursuit 等专属模板。 + ControllerAlgorithmType controller_algorithm; + + // 当前任务类型快照：和 request.goal.selected_task 一致，但单独展开后更方便算法工程师直接阅读。 + calibration_control_interfaces::msg::ControllerEvaluationTaskType task_type; + + // ===== 任务拆解视图 ===== + // 轨迹跟踪任务的原始 payload。适用于 MPC、LQR、Pure Pursuit 等需要整条参考轨迹的算法。 + calibration_control_interfaces::msg::TrajectoryTrackingTask trajectory_tracking_task; + + // 速度阶跃任务的原始 payload。适用于纵向 PID / MPC 的动态响应评估。 + calibration_control_interfaces::msg::VelocityStepTask velocity_step_task; + + // 加减速任务的原始 payload。适用于纵向平顺性、加速度响应和 jerk 约束分析。 + calibration_control_interfaces::msg::AccelerationDecelerationTask acceleration_deceleration_task; + + // 停车精度任务的原始 payload。适用于停车误差与末端姿态控制算法评估。 + calibration_control_interfaces::msg::StopAccuracyTask stop_accuracy_task; + + // 将轨迹任务里的 path 单独拆出来，方便算法直接访问参考轨迹点序列。 + std::vector reference_trajectory; + + // 将常用参考量单独展开，避免算法工程师总是回到原始 payload 中取值。 + double reference_target_velocity_ms{0.0}; + double reference_start_velocity_ms{0.0}; + double reference_target_accel_ms2{0.0}; + double reference_hold_time_sec{0.0}; + double reference_timeout_sec{0.0}; + double reference_stop_x_m{0.0}; + double reference_stop_y_m{0.0}; + double reference_stop_yaw_rad{0.0}; + bool reference_stop_at_end{false}; + + // ===== 控制域反馈 ===== + // 控制服务就绪状态：轨迹执行器、反馈链路、输出链路、急停、安全移动等是否满足执行条件。 + calibration_control_interfaces::msg::ControlReadinessResponse control_readiness; + + // 当前控制工作模式：调参准备、参数注入、评估、验证等。 + calibration_control_interfaces::msg::ControlWorkMode control_work_mode; + + // 当前已生效的控制参数：横向/纵向参数集合，是本轮调参的基线输入。 + calibration_control_interfaces::msg::ActiveControllerParametersResponse active_controller_parameters; + + // 当前算法准备写回的候选参数集。 + // 推荐做法是：算法先在这个结构里组装新参数，再统一回填到 output.response.result.estimated_parameter_set。 + calibration_control_interfaces::msg::ControllerParameterSet candidate_parameter_set; + + // 最新一帧控制遥测：横向误差、航向误差、速度误差、控制输出、饱和状态、当前算法版本等。 + calibration_control_interfaces::msg::ControlTelemetry latest_control_telemetry; + + // 历史控制遥测窗口：用于误差统计、响应曲线拟合、超调/收敛时间分析、稳定性分析。 + std::vector control_telemetry_history; + + // 控制内部诊断扩展：为后续接入真实控制器内部状态预留。 + // 例如：积分项、观测器状态、预测窗口代价、约束激活信息、状态机状态等。 + std::vector control_internal_diagnostics; + + // 执行器饱和原因扩展：用于区分是转向打满、油门受限、制动限幅还是安全逻辑介入。 + std::vector actuator_saturation_reasons; + + // ===== 底盘域反馈 ===== + // 底盘就绪状态：控制算法通常需要确认底盘驱动链路是否在线、急停是否释放、车辆是否可移动。 + calibration_chassis_interfaces::msg::ChassisReadinessResponse chassis_readiness; + + // 当前底盘工作模式：可辅助判断整车是否处于允许控制评估的阶段。 + calibration_chassis_interfaces::msg::ChassisWorkMode chassis_work_mode; + + // 当前已生效的底盘参数：控制算法通常需要结合已生效底盘参数分析误差来源。 + calibration_chassis_interfaces::msg::AppliedChassisCalibrationParametersResponse applied_chassis_parameters; + + // 最新一帧底盘遥测：车速、角速度、舵角、轮速、里程计、驱动状态等。 + calibration_chassis_interfaces::msg::ChassisTelemetry latest_chassis_telemetry; + + // 历史底盘遥测窗口：用于和控制误差、输出、目标轨迹做时序对齐。 + std::vector chassis_telemetry_history; + + // ===== 传感器域反馈 ===== + // 传感器就绪状态：用于判断定位、姿态、速度等输入是否可信。 + calibration_sensor_interfaces::msg::SensorReadinessResponse sensor_readiness; + + // 当前已生效的传感器参数：外参、内参、IMU 标定结果等，可能影响控制观测质量。 + calibration_sensor_interfaces::msg::AppliedSensorCalibrationParametersResponse applied_sensor_parameters; + + // 最新一帧传感器遥测：采样质量、检测状态、时间同步质量等。 + calibration_sensor_interfaces::msg::SensorCalibrationTelemetry latest_sensor_telemetry; + + // 历史传感器遥测窗口：用于观察采样稳定性、质量趋势、掉帧或时延问题。 + std::vector sensor_telemetry_history; + + // 传感器质量诊断扩展：例如丢帧、视觉目标丢失、IMU 饱和、时间戳抖动等。 + std::vector sensor_quality_diagnostics; + + // ===== 外部定位 / 真值源反馈 ===== + // 外部定位就绪状态：用于判断真值源是否可以参与控制评估与自动验收。 + calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness; + + // 最新一帧外部定位遥测：真值位姿、同步误差、质量评分、观测协方差等。 + calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry; + + // 历史外部定位遥测窗口：用于和控制误差、轨迹点、底盘反馈做时序对齐。 + std::vector external_localization_telemetry_history; + + // 真值源质量诊断扩展：例如时间同步超标、观测缺失、协方差异常、切源事件等。 + std::vector truth_source_diagnostics; + + // ===== 时间同步 / 数据对齐扩展 ===== + // 控制、底盘、传感器、真值源之间的估计时间偏移。 + // 后续如果需要做跨域误差对齐，可直接由上层在进入算法前填进来。 + double estimated_control_to_chassis_time_offset_sec{0.0}; + double estimated_control_to_sensor_time_offset_sec{0.0}; + double estimated_control_to_truth_time_offset_sec{0.0}; + + // 数据窗口有效性标记：上层可在进入算法前先完成预检，再把不满足项写到这里。 + bool control_history_available{false}; + bool chassis_history_available{false}; + bool sensor_history_available{false}; + bool truth_history_available{false}; + + // 说明：如果后续控制算法还需要更细反馈，可继续在这里补充。 + // 常见补充项包括：规划轨迹、参考速度曲线、控制器内部状态、执行器饱和原因、故障码、时间同步诊断信息等。 +} +; +struct ControlCalibrationOutput +{ + // 控制评估算法的统一输出：成功/失败、错误码、验证摘要、建议参数、产物引用都写到这里。 + ExecuteTask::Result response; +}; + +class ControlCalibrationAlgorithm +{ +public: + virtual ~ControlCalibrationAlgorithm() = default; + + virtual bool run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const = 0; +}; + +class PidControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm +{ +public: + bool run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class MpcControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm +{ +public: + bool run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class LqrControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm +{ +public: + bool run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class PurePursuitControlCalibrationAlgorithm final : public ControlCalibrationAlgorithm +{ +public: + bool run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_common.hpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_common.hpp new file mode 100644 index 0000000..99e7dc1 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_common.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "control_calibration_service/control_calibration_algorithms.hpp" + +#include "calibration_common_interfaces/msg/error_code.hpp" + +namespace control_calibration_service +{ + +// 统一填充一份最小可运行的默认结果。 +// 作用： +// 1. 让模板文件在尚未接入真实算法时也能稳定返回一份结构完整的结果； +// 2. 让算法工程师明确最终结果要写入哪些字段； +// 3. 后续真实算法接入时，可以保留这层公共默认值，再按算法结果覆盖具体字段。 +inline void fill_common_result( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output) +{ + output.response.result.success = true; + output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK; + output.response.result.message = "control calibration template executed."; + output.response.result.job_id = input.request.goal.header.request_id; + output.response.result.data_quality_passed = true; + output.response.result.suitable_for_commit = true; + output.response.result.recommended_parameter_version = "demo_control_template_v1"; + output.response.result.validation_summary.rms_lateral_error_m = 0.0; + output.response.result.validation_summary.rms_heading_error_rad = 0.0; + output.response.result.validation_summary.rms_speed_error_ms = 0.0; + output.response.result.validation_summary.overshoot_ratio = 0.0; + output.response.result.validation_summary.settle_time_sec = 0.0; + output.response.result.validation_summary.stop_position_error_m = 0.0; + output.response.result.validation_summary.max_jerk = 0.0; + output.response.result.validation_summary.saturation_ratio = 0.0; + output.response.result.validation_summary.auto_acceptance_passed = true; +} + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_service_node.hpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_service_node.hpp new file mode 100644 index 0000000..e182bc3 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_service_node.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "control_calibration_service/control_calibration_algorithm_template.hpp" + +#include "calibration_control_interfaces/action/execute_controller_evaluation.hpp" +#include "calibration_control_interfaces/srv/get_control_readiness.hpp" + +namespace control_calibration_service +{ + +// 控制标定服务节点： +// 1. 负责暴露 readiness service 和 execute action，作为控制专项对外的 ROS 接口入口。 +// 2. 负责把 ExecuteControllerEvaluation::Goal 组装成 ControlCalibrationInput。 +// 3. 负责在进入算法前，先做最小任务合法性检查，并推导默认控制算法类型。 +// 4. 负责调用控制算法门面，再把输出统一回填到 Action Result。 +// 5. 这里不直接写具体控制算法，避免算法工程师在修改 PID/MPC/LQR/PP 时碰 ROS 通信代码。 +class ControlCalibrationServiceNode : public rclcpp::Node +{ +public: + explicit ControlCalibrationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using ReadinessSrv = calibration_control_interfaces::srv::GetControlReadiness; + using ExecuteTask = calibration_control_interfaces::action::ExecuteControllerEvaluation; + using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle; + using ControllerAlgorithmType = calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType; + + rclcpp::Service::SharedPtr readiness_service_; + rclcpp_action::Server::SharedPtr execute_task_action_server_; + ControlCalibrationAlgorithmTemplate algorithm_; + + void handle_readiness( + const std::shared_ptr request, + std::shared_ptr response); + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute_goal(const std::shared_ptr goal_handle); + + ControllerAlgorithmType resolve_controller_algorithm(const ExecuteTask::Goal & goal) const; +}; + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/launch/control_calibration_service_component.launch.py b/agv_calib_brain/src/agv_calib_core/control_calibration_service/launch/control_calibration_service_component.launch.py new file mode 100644 index 0000000..4f77867 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/launch/control_calibration_service_component.launch.py @@ -0,0 +1,31 @@ +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + container = ComposableNodeContainer( + name='control_calibration_service_container', + namespace='/', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[], + output='screen', + ) + + load_node = LoadComposableNodes( + target_container='/control_calibration_service_container', + composable_node_descriptions=[ + ComposableNode( + package='control_calibration_service', + plugin='control_calibration_service::ControlCalibrationServiceNode', + name='control_calibration_service', + namespace='/', + ) + ], + ) + + return LaunchDescription([ + container, + load_node, + ]) diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/package.xml b/agv_calib_brain/src/agv_calib_core/control_calibration_service/package.xml new file mode 100644 index 0000000..abb30da --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/package.xml @@ -0,0 +1,26 @@ + + + control_calibration_service + 0.0.1 + Minimal control calibration service stub. + you + Apache-2.0 + + ament_cmake + rclcpp + rclcpp_action + rclcpp_components + calibration_chassis_interfaces + calibration_common_interfaces + calibration_control_interfaces + calibration_external_localization_interfaces + calibration_sensor_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces + launch + launch_ros + + + ament_cmake + + diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/control_calibration_algorithm_template.cpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/control_calibration_algorithm_template.cpp new file mode 100644 index 0000000..3926c40 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/control_calibration_algorithm_template.cpp @@ -0,0 +1,83 @@ +#include "control_calibration_service/control_calibration_algorithm_template.hpp" + +#include "calibration_control_interfaces/msg/controller_evaluation_task_type.hpp" + +namespace control_calibration_service +{ + +// 统一入口校验。 +// 这里主要检查： +// 1. 这次控制评估任务是不是一个合法请求； +// 2. node 是否已经推导出明确的控制算法类型； +// 3. 当前任务类型是否已经明确。 +// 更细的业务校验（例如轨迹点数量、速度范围、停车目标是否有效） +// 推荐在具体算法文件中再按任务类型继续做。 +bool ControlCalibrationAlgorithmTemplate::validate_input( + const Input & input, + std::string & failure_reason) const +{ + if (input.request.goal.header.request_id.empty()) { + failure_reason = "control 任务 request_id 不能为空。"; + return false; + } + + if (input.controller_algorithm.value == ControllerAlgorithmType::CONTROLLER_ALGORITHM_UNSPECIFIED) { + failure_reason = "控制算法类型不能为空，必须明确是 PID、MPC、LQR 或 PURE_PURSUIT。"; + return false; + } + + if (input.request.goal.selected_task.value == + calibration_control_interfaces::msg::ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED) { + failure_reason = "selected_task 不能为空，必须明确控制评估任务类型。"; + return false; + } + + return true; +} + +const ControlCalibrationAlgorithm * ControlCalibrationAlgorithmTemplate::resolve_algorithm( + const ControllerAlgorithmType & controller_algorithm) const +{ + switch (controller_algorithm.value) { + case ControllerAlgorithmType::PID: + return &pid_algorithm_; + case ControllerAlgorithmType::MPC: + return &mpc_algorithm_; + case ControllerAlgorithmType::LQR: + return &lqr_algorithm_; + case ControllerAlgorithmType::PURE_PURSUIT: + return &pure_pursuit_algorithm_; + default: + return nullptr; + } +} + +bool ControlCalibrationAlgorithmTemplate::run( + const Input & input, + Output & output, + std::string & failure_reason) const +{ + if (!validate_input(input, failure_reason)) { + return false; + } + + const auto * algorithm = resolve_algorithm(input.controller_algorithm); + if (algorithm == nullptr) { + failure_reason = "不支持的控制算法类型。"; + return false; + } + + // 这里是控制标定/评估算法的统一入口： + // 1. 先按 controller_algorithm 分发到对应算法模板； + // 2. 各算法模板再结合 task_type、轨迹/速度/停车任务 payload、控制遥测、底盘反馈、真值源反馈进入求解； + // 3. 最终统一把结果写回 output.response.result。 + // 算法工程师通常会重点读取： + // - input.vehicle_profile：控制轴支持算法、默认算法、车辆静态画像 + // - input.control_*：当前控制参数、控制误差、控制输出、饱和状态、工作模式 + // - input.chassis_*：底盘运动状态和执行器反馈 + // - input.sensor_*：姿态/速度/定位等观测质量 + // - input.external_localization_*：真值位姿、同步偏差、质量评分 + return algorithm->run(input, output, failure_reason); +} + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/control_calibration_service_node.cpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/control_calibration_service_node.cpp new file mode 100644 index 0000000..5d8b4d8 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/control_calibration_service_node.cpp @@ -0,0 +1,173 @@ +#include "control_calibration_service/control_calibration_service_node.hpp" + +#include +#include + +#include "rclcpp_components/register_node_macro.hpp" + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_control_interfaces/msg/control_job_result.hpp" +#include "calibration_control_interfaces/msg/control_readiness_response.hpp" +#include "calibration_control_interfaces/msg/controller_evaluation_task_type.hpp" + +namespace control_calibration_service +{ + +namespace +{ +int64_t now_us() +{ + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} +} // namespace + +using calibration_common_interfaces::msg::ErrorCode; +using calibration_common_interfaces::msg::JobState; +using calibration_control_interfaces::msg::ControllerEvaluationTaskType; + +ControlCalibrationServiceNode::ControlCalibrationServiceNode(const rclcpp::NodeOptions & options) +: Node("control_calibration_service", options) +{ + readiness_service_ = create_service( + "/control/get_readiness", + std::bind(&ControlCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2)); + + execute_task_action_server_ = rclcpp_action::create_server( + this, + "/control/execute_controller_evaluation", + std::bind(&ControlCalibrationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ControlCalibrationServiceNode::handle_cancel, this, std::placeholders::_1), + std::bind(&ControlCalibrationServiceNode::handle_accepted, this, std::placeholders::_1)); +} + +void ControlCalibrationServiceNode::handle_readiness( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "control_calibration_service is ready."; + response->response.agent_ready = true; + response->response.trajectory_executor_ready = true; + response->response.vehicle_feedback_ready = true; + response->response.control_output_ready = true; + response->response.estop_released = true; + response->response.vehicle_safe_to_move = true; + response->response.checked_timestamp_us = now_us(); +} + +rclcpp_action::GoalResponse ControlCalibrationServiceNode::handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + if (goal->goal.header.request_id.empty()) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ControlCalibrationServiceNode::handle_cancel( + const std::shared_ptr /*goal_handle*/) +{ + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ControlCalibrationServiceNode::handle_accepted( + const std::shared_ptr goal_handle) +{ + std::thread(std::bind(&ControlCalibrationServiceNode::execute_goal, this, goal_handle)).detach(); +} + +void ControlCalibrationServiceNode::execute_goal( + const std::shared_ptr goal_handle) +{ + // 先回一帧 RUNNING feedback，告诉 orchestrator 当前任务已经进入执行阶段。 + auto feedback = std::make_shared(); + feedback->feedback.state.state = JobState::RUNNING; + goal_handle->publish_feedback(feedback); + + // ===== 组装算法输入上下文 ===== + // 当前模板阶段先把“算法最常用的任务侧输入”显式展开。 + // 后续如果要接真实车辆画像、控制参数查询、历史遥测缓存、真值源缓存， + // 也应继续在这里补齐并写入 ControlCalibrationInput。 + ControlCalibrationAlgorithmTemplate::Input input; + input.request = *goal_handle->get_goal(); + input.controller_algorithm = resolve_controller_algorithm(*goal_handle->get_goal()); + input.task_type = goal_handle->get_goal()->goal.selected_task; + input.trajectory_tracking_task = goal_handle->get_goal()->goal.trajectory_tracking; + input.velocity_step_task = goal_handle->get_goal()->goal.velocity_step; + input.acceleration_deceleration_task = goal_handle->get_goal()->goal.accel_decel; + input.stop_accuracy_task = goal_handle->get_goal()->goal.stop_accuracy; + input.reference_trajectory = goal_handle->get_goal()->goal.trajectory_tracking.path; + input.reference_target_velocity_ms = goal_handle->get_goal()->goal.velocity_step.target_velocity_ms; + input.reference_start_velocity_ms = goal_handle->get_goal()->goal.accel_decel.start_velocity_ms; + input.reference_target_accel_ms2 = goal_handle->get_goal()->goal.accel_decel.target_accel_ms2; + input.reference_hold_time_sec = goal_handle->get_goal()->goal.velocity_step.hold_time_sec; + input.reference_timeout_sec = goal_handle->get_goal()->goal.trajectory_tracking.timeout_sec; + input.reference_stop_x_m = goal_handle->get_goal()->goal.stop_accuracy.target_stop_x_m; + input.reference_stop_y_m = goal_handle->get_goal()->goal.stop_accuracy.target_stop_y_m; + input.reference_stop_yaw_rad = goal_handle->get_goal()->goal.stop_accuracy.target_stop_yaw_rad; + input.reference_stop_at_end = goal_handle->get_goal()->goal.trajectory_tracking.stop_at_end; + input.control_history_available = false; + input.chassis_history_available = false; + input.sensor_history_available = false; + input.truth_history_available = false; + + ControlCalibrationAlgorithmTemplate::Output output; + std::string failure_reason; + if (!algorithm_.run(input, output, failure_reason)) { + auto result = std::make_shared(); + result->result.success = false; + result->result.error_code.code = ErrorCode::INVALID_STATE; + result->result.message = failure_reason; + result->result.job_id = goal_handle->get_goal()->goal.header.request_id; + result->result.data_quality_passed = false; + result->result.suitable_for_commit = false; + goal_handle->abort(result); + return; + } + + auto result = std::make_shared(output.response); + goal_handle->succeed(result); +} + +ControlCalibrationServiceNode::ControllerAlgorithmType +ControlCalibrationServiceNode::resolve_controller_algorithm(const ExecuteTask::Goal & goal) const +{ + // 这里给出一版“任务类型 -> 默认控制算法类型”的模板映射。 + // 目的不是固定真实系统必须这么做，而是先告诉算法工程师： + // node 层会在进入算法前给出一个默认算法分支，后续如果项目里已有更精确的控制器选择策略， + // 可以把这里替换成： + // 1. 来自 vehicle_profile.controller_selections 的静态配置； + // 2. 来自 session 的阶段策略； + // 3. 来自上层显式指定的本轮控制器算法。 + ControllerAlgorithmType algorithm; + + switch (goal.goal.selected_task.value) { + case ControllerEvaluationTaskType::TRAJECTORY_TRACKING: + algorithm.value = ControllerAlgorithmType::MPC; + break; + case ControllerEvaluationTaskType::VELOCITY_STEP: + algorithm.value = ControllerAlgorithmType::PID; + break; + case ControllerEvaluationTaskType::ACCELERATION_DECELERATION: + algorithm.value = ControllerAlgorithmType::LQR; + break; + case ControllerEvaluationTaskType::STOP_ACCURACY: + algorithm.value = ControllerAlgorithmType::PURE_PURSUIT; + break; + default: + algorithm.value = ControllerAlgorithmType::CONTROLLER_ALGORITHM_UNSPECIFIED; + break; + } + + return algorithm; +} + +} // namespace control_calibration_service + +RCLCPP_COMPONENTS_REGISTER_NODE(control_calibration_service::ControlCalibrationServiceNode) diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/lqr_control_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/lqr_control_calibration_algorithm.cpp new file mode 100644 index 0000000..19617c7 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/lqr_control_calibration_algorithm.cpp @@ -0,0 +1,33 @@ +#include "control_calibration_service/control_calibration_common.hpp" + +namespace control_calibration_service +{ + +bool LqrControlCalibrationAlgorithm::run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // LQR 控制标定模板： + // 适合基于状态反馈和权重矩阵整定的控制评估。 + // 推荐算法工程师在这里补充： + // 1. 状态定义： + // - 哪些状态来自 input.control_telemetry_history + // - 哪些状态来自 input.chassis_telemetry_history / 真值源反馈 + // 2. 误差与代价： + // - 横向误差、航向误差、速度误差如何进入状态向量 + // - Q / R 权重如何生成或搜索 + // 3. 结果判定： + // - output.response.result.validation_summary 中如何计算自动验收指标 + // - output.response.result.suitable_for_commit 如何判定 + // 4. 参数输出： + // - output.response.result.estimated_parameter_set 中回填新的 LQR 参数 + fill_common_result(input, output); + output.response.result.message = "lqr control calibration template executed."; + output.response.result.recommended_parameter_version = "lqr_template_v1"; + return true; +} + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/main.cpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/main.cpp new file mode 100644 index 0000000..fd0ea52 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/main.cpp @@ -0,0 +1,15 @@ +#include "rclcpp/rclcpp.hpp" +#include "control_calibration_service/control_calibration_service_node.hpp" + +// 薄 main 入口： +// 1. 方便在不走 component container 时，直接通过 ros2 run 启动节点； +// 2. 真实功能都在 ControlCalibrationServiceNode 中，这里只负责初始化、spin 和退出； +// 3. 保留这个文件可以兼容“独立节点启动”和“组件化加载”两种方式。 +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/mpc_control_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/mpc_control_calibration_algorithm.cpp new file mode 100644 index 0000000..2902510 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/mpc_control_calibration_algorithm.cpp @@ -0,0 +1,37 @@ +#include "control_calibration_service/control_calibration_common.hpp" + +namespace control_calibration_service +{ + +bool MpcControlCalibrationAlgorithm::run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // MPC 控制标定模板： + // 适合轨迹跟踪、联合横纵向预测控制参数整定。 + // 推荐算法工程师重点看： + // 1. 参考输入： + // - input.reference_trajectory：参考轨迹点序列 + // - input.trajectory_tracking_task：轨迹跟踪任务原始配置 + // - input.reference_timeout_sec / input.reference_stop_at_end：执行约束 + // 2. 当前控制状态： + // - input.active_controller_parameters：当前已生效 MPC 参数 + // - input.control_internal_diagnostics：后续可接预测窗口代价、约束激活情况等 + // 3. 反馈数据： + // - input.control_telemetry_history：横向误差、航向误差、速度误差、控制输出、饱和比 + // - input.chassis_telemetry_history：执行器与整车运动学反馈 + // - input.external_localization_telemetry_history：真值轨迹与同步质量 + // 4. 输出位置： + // - output.response.result.validation_summary：写跟踪误差、超调、收敛时间、饱和占比 + // - output.response.result.estimated_parameter_set：写新的 MPC 参数 + // - output.response.result.recommended_parameter_version：写本轮推荐参数版本号 + fill_common_result(input, output); + output.response.result.message = "mpc control calibration template executed."; + output.response.result.recommended_parameter_version = "mpc_template_v1"; + return true; +} + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp new file mode 100644 index 0000000..f3cb5a4 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp @@ -0,0 +1,56 @@ +#include "control_calibration_service/control_calibration_common.hpp" + +namespace control_calibration_service +{ + +bool PidControlCalibrationAlgorithm::run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // PID 控制标定模板： + // 【本文件负责什么】 + // - 负责 PID 控制器相关的标定/评估算法实现。 + // - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。 + // + // 【建议优先读取的输入】 + // 1. input.task_type / input.velocity_step_task / input.acceleration_deceleration_task + // - 当前任务到底是速度阶跃、加减速、轨迹跟踪还是停车精度评估。 + // 2. input.reference_target_velocity_ms / input.reference_hold_time_sec / input.reference_timeout_sec + // - 常用参考量已被上层展开，便于直接读取。 + // 3. input.active_controller_parameters / input.candidate_parameter_set + // - 当前已生效 PID 参数，以及本轮候选参数填充区。 + // 4. input.latest_control_telemetry / input.control_telemetry_history + // - 速度误差、控制输出、积分状态、饱和状态、收敛过程等。 + // 5. input.latest_chassis_telemetry / input.chassis_telemetry_history + // - 车速、角速度、执行器状态、底盘响应延迟等。 + // 6. input.latest_external_localization_telemetry + // - 如需真值速度或高精定位，可从这里读取。 + // + // 【必写输出】 + // 1. output.response.result.validation_summary + // - 写 RMS 误差、超调、稳态误差、收敛时间、jerk 等摘要指标。 + // 2. output.response.result.estimated_parameter_set + // - 写回新的 PID 参数集合。 + // 3. output.response.result.suitable_for_commit + // - 明确是否建议提交当前参数。 + // + // 【可选输出】 + // - output.response.result.artifacts + // 可挂日志、波形图、调参报告、误差统计 csv 等。 + // + // 【常见失败原因】 + // - 参考轨迹缺失、有效控制窗口不足、速度反馈异常、底盘执行器受限、真值源不可用。 + // + // 【在这里添加真实算法】 + // - 请在 fill_common_result(...) 之前或之后补充真实 PID 求解与评估逻辑。 + // - 当前文件仅提供交付模板，不包含真实控制标定算法。 + fill_common_result(input, output); + output.response.result.message = "pid control calibration template executed."; + output.response.result.recommended_parameter_version = "pid_template_v1"; + return true; +} + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/pure_pursuit_control_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/pure_pursuit_control_calibration_algorithm.cpp new file mode 100644 index 0000000..a9b20dc --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/control_calibration_service/src/pure_pursuit_control_calibration_algorithm.cpp @@ -0,0 +1,33 @@ +#include "control_calibration_service/control_calibration_common.hpp" + +namespace control_calibration_service +{ + +bool PurePursuitControlCalibrationAlgorithm::run( + const ControlCalibrationInput & input, + ControlCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // Pure Pursuit 控制标定模板： + // 适合基于参考轨迹和前视点策略的横向控制评估。 + // 算法工程师通常会在这里处理： + // 1. 参考轨迹读取： + // - input.reference_trajectory + // - input.reference_stop_at_end / input.reference_timeout_sec + // 2. 观测输入： + // - input.control_telemetry_history 中的横向误差、航向误差、转向输出 + // - input.chassis_telemetry_history 中的速度、姿态和底盘运动状态 + // - input.truth_source_diagnostics / 真值历史，用于判断轨迹对齐是否可靠 + // 3. 参数输出： + // - 可将前视距离、速度相关增益等写入 output.response.result.estimated_parameter_set + // 4. 验收输出： + // - 将最大误差、均方误差、振荡情况、自动验收结论写入 validation_summary + fill_common_result(input, output); + output.response.result.message = "pure pursuit control calibration template executed."; + output.response.result.recommended_parameter_version = "pure_pursuit_template_v1"; + return true; +} + +} // namespace control_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/external_localization_service/CMakeLists.txt new file mode 100644 index 0000000..9deee75 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(external_localization_service) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(calibration_chassis_interfaces REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_control_interfaces REQUIRED) +find_package(calibration_external_localization_interfaces REQUIRED) +find_package(calibration_sensor_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) +find_package(calibration_workshop_orchestration_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) + +include_directories(include) + +add_library(external_reference_executor + src/external_localization_algorithm_template.cpp + src/external_reference_executor.cpp + src/reference_pose_collection_algorithm.cpp + src/marker_alignment_algorithm.cpp + src/truth_source_validation_algorithm.cpp +) + +ament_target_dependencies(external_reference_executor + calibration_chassis_interfaces + calibration_common_interfaces + calibration_control_interfaces + calibration_external_localization_interfaces + calibration_sensor_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces + rclcpp +) + +add_executable(external_localization_service_node + src/main.cpp + src/external_localization_service_node.cpp +) + +ament_target_dependencies(external_localization_service_node + calibration_common_interfaces + calibration_external_localization_interfaces + calibration_workshop_orchestration_interfaces + rclcpp + rclcpp_action +) + +target_link_libraries(external_localization_service_node + external_reference_executor +) + +install(TARGETS + external_reference_executor + external_localization_service_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +ament_package() diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithm_template.hpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithm_template.hpp new file mode 100644 index 0000000..4c2e1d2 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithm_template.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include "external_localization_service/external_localization_algorithms.hpp" + +namespace external_localization_service +{ + +// 这个类是 external_localization_service 的算法门面层。 +// 职责是： +// 1. 对统一输入做最小校验； +// 2. 按外部定位任务类型分发到不同的算法模板文件； +// 3. 让 node 层保持干净，只负责 ROS 通信和输入组装； +// 4. 让算法工程师主要聚焦在各自的 *_algorithm.cpp 中填充真实算法。 +class ExternalLocalizationAlgorithmTemplate +{ +public: + using Input = ExternalLocalizationInput; + using Output = ExternalLocalizationOutput; + + bool run( + const Input & input, + Output & output, + std::string & failure_reason) const; + +private: + bool validate_input(const Input & input, std::string & failure_reason) const; + const ExternalLocalizationAlgorithm * resolve_algorithm(const ExternalTaskType & task_type) const; + + ReferencePoseCollectionAlgorithm reference_pose_collection_algorithm_; + MarkerAlignmentAlgorithm marker_alignment_algorithm_; + TruthSourceValidationAlgorithm truth_source_validation_algorithm_; +}; + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp new file mode 100644 index 0000000..14f2c32 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp @@ -0,0 +1,192 @@ +#pragma once + +#include +#include + +#include "calibration_chassis_interfaces/msg/applied_chassis_calibration_parameters_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" +#include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" +#include "calibration_control_interfaces/msg/control_readiness_response.hpp" +#include "calibration_control_interfaces/msg/control_telemetry.hpp" +#include "calibration_control_interfaces/msg/control_work_mode.hpp" +#include "calibration_external_localization_interfaces/action/execute_external_localization_task.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_calibration_result.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_capability_response.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_readiness_response.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_task_payload_type.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_telemetry.hpp" +#include "calibration_external_localization_interfaces/msg/marker_alignment_task.hpp" +#include "calibration_external_localization_interfaces/msg/reference_pose_collection_task.hpp" +#include "calibration_external_localization_interfaces/msg/truth_source_validation_task.hpp" +#include "calibration_sensor_interfaces/msg/applied_sensor_calibration_parameters_response.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_telemetry.hpp" +#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_work_mode.hpp" +#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace external_localization_service +{ + +using ExecuteTask = calibration_external_localization_interfaces::action::ExecuteExternalLocalizationTask; +using ExternalTaskType = calibration_external_localization_interfaces::msg::ExternalLocalizationTaskPayloadType; + +struct ExternalLocalizationInput +{ + // ===== 会话与静态画像 ===== + // 当前车间会话：算法可读取本轮 session 的阶段上下文、人工确认状态、操作者信息等。 + calibration_workshop_orchestration_interfaces::msg::WorkshopSession workshop_session; + + // 车辆画像：可用于读取车辆基础信息、传感器安装信息、底盘/控制/传感器使能状态等静态配置。 + calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile; + + // 外部定位任务请求：包含 request_id、任务目的、任务类型、各任务 payload、来源迭代号、外部定位源 ID、工位 ID。 + ExecuteTask::Goal request; + + // 当前任务类型快照：与 request.goal.selected_task 一致，但单独展开后更方便算法工程师直接阅读。 + ExternalTaskType task_type; + + // ===== 任务拆解视图 ===== + // 参考位姿采集任务：适用于静止重复性分析、参考位姿基线建立、初始化基准采样等。 + calibration_external_localization_interfaces::msg::ReferencePoseCollectionTask reference_pose_collection_task; + + // 标靶对齐任务：适用于建立 workshop frame 与 localization frame 之间的坐标关系。 + calibration_external_localization_interfaces::msg::MarkerAlignmentTask marker_alignment_task; + + // 真值源验证任务：适用于验证时间同步、稳定性、覆盖范围、丢失率、可作为真值源的可行性。 + calibration_external_localization_interfaces::msg::TruthSourceValidationTask truth_source_validation_task; + + // 常用参考量展开：便于算法工程师直接访问，不需要每次都回原始 payload 中解析。 + uint32_t required_reference_pose_sample_count{0}; + uint32_t required_marker_observation_count{0}; + uint32_t required_static_sample_count{0}; + uint32_t required_dynamic_sample_count{0}; + bool require_vehicle_static{false}; + bool require_short_motion_segment{false}; + double reference_timeout_sec{0.0}; + double min_sample_interval_sec{0.0}; + double accepted_max_position_stddev_m{0.0}; + double accepted_max_yaw_stddev_rad{0.0}; + double accepted_max_tracking_loss_ratio{0.0}; + double accepted_max_time_sync_offset_ms{0.0}; + std::string reference_board_id; + std::string localization_source_id; + std::string workcell_zone_id; + + // ===== 外部定位域反馈 ===== + // 外部定位能力：算法可据此知道当前源支持哪些任务、是否支持持久化、是否支持真值验证等。 + calibration_external_localization_interfaces::msg::ExternalLocalizationCapabilityResponse external_localization_capability; + + // 外部定位就绪状态：服务本身是否就绪、是否允许进入真值验证、当前验证摘要如何。 + calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness; + + // 当前算法准备写回的候选结果。 + // 推荐做法是：算法先在这个结构里组装结果，再统一回填到 output.response.result.result。 + calibration_external_localization_interfaces::msg::ExternalLocalizationCalibrationResult candidate_calibration_result; + + // 最新一帧外部定位遥测：当前观测位姿、标准差、跟踪丢失率、时间同步偏差、质量分数等。 + calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry; + + // 历史外部定位遥测窗口：用于重复性统计、时间同步评估、稳定性分析、轨迹覆盖分析和异常剔除。 + std::vector + external_localization_telemetry_history; + + // 真值源质量诊断扩展：例如时间同步超标、观测缺失、切源事件、协方差异常、目标丢失等。 + std::vector truth_source_diagnostics; + + // 标靶检测与对齐诊断扩展：例如标靶检测失败、有效角点不足、位姿解算不稳定等。 + std::vector marker_alignment_diagnostics; + + // 数据采集与落盘诊断扩展：例如原始位姿采集窗口不足、采样频率不满足、落盘失败等。 + std::vector acquisition_diagnostics; + + // ===== 底盘域反馈 ===== + // 外部定位验证往往需要结合车辆真实运动状态，因此保留底盘域反馈接口。 + calibration_chassis_interfaces::msg::ChassisReadinessResponse chassis_readiness; + calibration_chassis_interfaces::msg::ChassisWorkMode chassis_work_mode; + calibration_chassis_interfaces::msg::AppliedChassisCalibrationParametersResponse applied_chassis_parameters; + calibration_chassis_interfaces::msg::ChassisTelemetry latest_chassis_telemetry; + std::vector chassis_telemetry_history; + + // ===== 控制域反馈 ===== + // 如果真值源验证涉及短距离运动段、闭环跟踪误差验证，需要读取控制域反馈做时序对齐。 + calibration_control_interfaces::msg::ControlReadinessResponse control_readiness; + calibration_control_interfaces::msg::ControlWorkMode control_work_mode; + calibration_control_interfaces::msg::ActiveControllerParametersResponse active_controller_parameters; + calibration_control_interfaces::msg::ControlTelemetry latest_control_telemetry; + std::vector control_telemetry_history; + + // ===== 传感器域反馈 ===== + // 外部定位作为真值源时，也常需要结合相机 / IMU / LiDAR 的采样质量判断观测是否可信。 + calibration_sensor_interfaces::msg::SensorReadinessResponse sensor_readiness; + calibration_sensor_interfaces::msg::SensorCalibrationWorkMode sensor_work_mode; + calibration_sensor_interfaces::msg::AppliedSensorCalibrationParametersResponse applied_sensor_parameters; + calibration_sensor_interfaces::msg::SensorCalibrationTelemetry latest_sensor_telemetry; + std::vector sensor_telemetry_history; + + // 传感器质量诊断扩展：例如相机丢帧、IMU 饱和、LiDAR 数据稀疏、时间戳抖动等。 + std::vector sensor_quality_diagnostics; + + // ===== 时间同步 / 数据窗口有效性 ===== + // 各域之间的估计时间偏移：后续如需跨域误差对齐，可由上层在进入算法前完成填充。 + double estimated_external_to_chassis_time_offset_sec{0.0}; + double estimated_external_to_control_time_offset_sec{0.0}; + double estimated_external_to_sensor_time_offset_sec{0.0}; + + // 历史数据窗口是否可用：便于算法先做前置判定，再决定是否执行拟合或验证。 + bool external_history_available{false}; + bool chassis_history_available{false}; + bool control_history_available{false}; + bool sensor_history_available{false}; + + // 说明：如果后续外部定位算法还需要更细的数据入口，可继续在这里补充。 + // 常见补充项包括：原始观测包、标靶检测结果序列、时间同步统计、点云/图像辅助特征、参考轨迹片段等。 +}; + +struct ExternalLocalizationOutput +{ + // 外部定位算法统一输出：成功/失败、错误码、验证摘要、结果内容、产物引用都写到这里。 + ExecuteTask::Result response; +}; + +class ExternalLocalizationAlgorithm +{ +public: + virtual ~ExternalLocalizationAlgorithm() = default; + + virtual bool run( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + std::string & failure_reason) const = 0; +}; + +class ReferencePoseCollectionAlgorithm final : public ExternalLocalizationAlgorithm +{ +public: + bool run( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + std::string & failure_reason) const override; +}; + +class MarkerAlignmentAlgorithm final : public ExternalLocalizationAlgorithm +{ +public: + bool run( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + std::string & failure_reason) const override; +}; + +class TruthSourceValidationAlgorithm final : public ExternalLocalizationAlgorithm +{ +public: + bool run( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + std::string & failure_reason) const override; +}; + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_common.hpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_common.hpp new file mode 100644 index 0000000..0bd0ce6 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_common.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "external_localization_service/external_localization_algorithms.hpp" + +namespace external_localization_service +{ + +// 这里放外部定位算法模板共用的默认结果填充逻辑。 +// 这样每个具体算法 cpp 不需要从零开始拼装 ExecuteExternalLocalizationTask::Result， +// 只需要在此基础上补充本算法真正求解出的关键结果即可。 +inline void fill_external_localization_common_success( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + const std::string & success_message, + const std::string & parameter_version) +{ + output.response.result.success = true; + output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK; + output.response.result.message = success_message; + output.response.result.job_id = input.request.goal.header.request_id; + output.response.result.task_purpose = input.request.goal.task_purpose; + output.response.result.data_quality_passed = true; + output.response.result.suitable_for_commit = true; + output.response.result.recommended_parameter_version = parameter_version; + + output.response.result.validation_summary.time_sync_ok = true; + output.response.result.validation_summary.coverage_ok = true; + output.response.result.validation_summary.quality_ok = true; + output.response.result.validation_summary.tracking_stable = true; + output.response.result.validation_summary.recommended_as_truth_source = true; + output.response.result.validation_summary.position_stddev_m = 0.0; + output.response.result.validation_summary.yaw_stddev_rad = 0.0; + output.response.result.validation_summary.tracking_loss_ratio = 0.0; + output.response.result.validation_summary.time_sync_offset_ms = 0.0; + output.response.result.validation_summary.issues.clear(); + + output.response.result.result = input.candidate_calibration_result; + output.response.result.result.localization_source_id = input.localization_source_id; + output.response.result.result.workcell_zone_id = input.workcell_zone_id; +} + +inline void fill_external_localization_common_failure( + ExternalLocalizationOutput & output, + const std::string & failure_reason) +{ + output.response.result.success = false; + output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::INVALID_ARGUMENT; + output.response.result.message = failure_reason; + output.response.result.data_quality_passed = false; + output.response.result.suitable_for_commit = false; + output.response.result.recommended_parameter_version.clear(); +} + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_service_node.hpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_service_node.hpp new file mode 100644 index 0000000..3747d2b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_service_node.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_external_localization_interfaces/action/execute_external_localization_task.hpp" +#include "calibration_external_localization_interfaces/srv/get_external_localization_readiness.hpp" +#include "external_localization_service/external_reference_executor.hpp" + +namespace external_localization_service +{ + +// 这个类是 external_localization_service 的 ROS 接口层。 +// 它的职责不是实现外部定位算法本体，而是： +// 1. 暴露 readiness service 与 execute action； +// 2. 接收 ROS 请求并做最小入口校验； +// 3. 调用 ExternalReferenceExecutor 组装算法输入并执行； +// 4. 将算法输出转换为 ROS action result / feedback。 +class ExternalLocalizationServiceNode : public rclcpp::Node +{ +public: + explicit ExternalLocalizationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using ReadinessSrv = calibration_external_localization_interfaces::srv::GetExternalLocalizationReadiness; + using ExecuteTask = calibration_external_localization_interfaces::action::ExecuteExternalLocalizationTask; + using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle; + + rclcpp::Service::SharedPtr readiness_service_; + rclcpp_action::Server::SharedPtr execute_task_action_server_; + ExternalReferenceExecutor executor_; + + void handle_readiness( + const std::shared_ptr request, + std::shared_ptr response); + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute_goal(const std::shared_ptr goal_handle); +}; + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp new file mode 100644 index 0000000..aa68e06 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include + +#include "calibration_workshop_orchestration_interfaces/msg/stage_result_summary.hpp" +#include "external_localization_service/external_localization_algorithm_template.hpp" + +namespace external_localization_service +{ + +// 这个类属于 external_localization_service 侧。 +// 它是 external_localization_service 的专项执行器门面，职责包括： +// 1. 对外部定位 action goal 做最小请求校验； +// 2. 组装统一的算法输入结构 ExternalLocalizationInput； +// 3. 调用 ExternalLocalizationAlgorithmTemplate，按任务类型分发到具体算法模板文件； +// 4. 把外部定位任务结果回填到 StageResultSummary，方便编排器统一消费。 +// +// 这样分层后： +// - node 层只负责 ROS 通信； +// - executor 层负责输入组装与结果转换； +// - algorithm template 层负责按任务类型分发； +// - 各 *_algorithm.cpp 负责真实算法实现。 +class ExternalReferenceExecutor +{ +public: + using ExecuteTask = + calibration_external_localization_interfaces::action::ExecuteExternalLocalizationTask; + + bool validate_goal(const ExecuteTask::Goal & goal, std::string & reject_reason) const; + bool build_result( + const ExecuteTask::Goal & goal, + ExecuteTask::Result & external_result, + std::string & failure_reason) const; + + void fill_stage_result_from_external_result( + const ExecuteTask::Result & external_result, + calibration_workshop_orchestration_interfaces::msg::StageResultSummary & stage_result, + std::string & failure_reason) const; + +private: + ExternalLocalizationAlgorithmTemplate algorithm_; +}; + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/package.xml b/agv_calib_brain/src/agv_calib_core/external_localization_service/package.xml new file mode 100644 index 0000000..6bad1fa --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/package.xml @@ -0,0 +1,24 @@ + + + external_localization_service + 0.0.1 + External localization service skeleton for reference checking and truth-source validation. + you + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + calibration_common_interfaces + calibration_chassis_interfaces + calibration_control_interfaces + calibration_sensor_interfaces + calibration_external_localization_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces + + + ament_cmake + + diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_localization_algorithm_template.cpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_localization_algorithm_template.cpp new file mode 100644 index 0000000..1ae3f6e --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_localization_algorithm_template.cpp @@ -0,0 +1,66 @@ +#include "external_localization_service/external_localization_algorithm_template.hpp" + +namespace external_localization_service +{ + +bool ExternalLocalizationAlgorithmTemplate::validate_input( + const Input & input, + std::string & failure_reason) const +{ + if (input.request.goal.header.request_id.empty()) { + failure_reason = "request_id 不能为空。"; + return false; + } + if (input.localization_source_id.empty()) { + failure_reason = "localization_source_id 不能为空。"; + return false; + } + if (input.workcell_zone_id.empty()) { + failure_reason = "workcell_zone_id 不能为空。"; + return false; + } + return true; +} + +const ExternalLocalizationAlgorithm * ExternalLocalizationAlgorithmTemplate::resolve_algorithm( + const ExternalTaskType & task_type) const +{ + switch (task_type.value) { + case ExternalTaskType::REFERENCE_POSE_COLLECTION: + return &reference_pose_collection_algorithm_; + case ExternalTaskType::MARKER_ALIGNMENT: + return &marker_alignment_algorithm_; + case ExternalTaskType::TRUTH_SOURCE_VALIDATION: + return &truth_source_validation_algorithm_; + default: + return nullptr; + } +} + +bool ExternalLocalizationAlgorithmTemplate::run( + const Input & input, + Output & output, + std::string & failure_reason) const +{ + if (!validate_input(input, failure_reason)) { + return false; + } + + const auto * algorithm = resolve_algorithm(input.task_type); + if (algorithm == nullptr) { + failure_reason = "不支持的外部定位任务类型。"; + return false; + } + + // 这里是外部定位算法的统一入口：先按任务类型分发，再由对应模板文件实现具体算法。 + // 算法工程师通常会先检查： + // - input.workshop_session / input.vehicle_profile：会话级上下文和车辆静态画像 + // - input.external_localization_*：能力、就绪状态、遥测、历史窗口、真值源诊断 + // - input.chassis_*：车辆运动状态，便于做短运动段验证和时序对齐 + // - input.control_*：控制闭环输出，便于和真值源做对比 + // - input.sensor_*：图像 / IMU / LiDAR 等采样质量，辅助判断真值数据是否可信 + // 再进入具体采集、对齐、稳定性分析和结果求解。 + return algorithm->run(input, output, failure_reason); +} + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp new file mode 100644 index 0000000..02f81b6 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp @@ -0,0 +1,120 @@ +#include "external_localization_service/external_localization_service_node.hpp" + +#include +#include + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" + +namespace external_localization_service +{ + +namespace +{ +int64_t now_us() +{ + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} +} // namespace + +using calibration_common_interfaces::msg::ErrorCode; +using calibration_common_interfaces::msg::JobState; + +ExternalLocalizationServiceNode::ExternalLocalizationServiceNode(const rclcpp::NodeOptions & options) +: Node("external_localization_service", options) +{ + readiness_service_ = create_service( + "/external_localization/get_readiness", + std::bind(&ExternalLocalizationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2)); + + execute_task_action_server_ = rclcpp_action::create_server( + this, + "/external_localization/execute_task", + std::bind(&ExternalLocalizationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ExternalLocalizationServiceNode::handle_cancel, this, std::placeholders::_1), + std::bind(&ExternalLocalizationServiceNode::handle_accepted, this, std::placeholders::_1)); +} + +void ExternalLocalizationServiceNode::handle_readiness( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + // 这里先保留最小 readiness 逻辑。 + // 后续若接入真实外部定位设备/真值源桥接程序，可在这里增加： + // - 真值源在线检查 + // - 同步状态检查 + // - 覆盖范围检查 + // - 观测质量检查 + // - 切源稳定性检查 + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "external_localization_service is ready."; + response->response.agent_ready = true; + response->response.ready_for_reference_validation = true; + response->response.checked_timestamp_us = now_us(); + response->response.validation_summary.time_sync_ok = true; + response->response.validation_summary.coverage_ok = true; + response->response.validation_summary.quality_ok = true; + response->response.validation_summary.tracking_stable = true; + response->response.validation_summary.recommended_as_truth_source = true; + response->response.validation_summary.position_stddev_m = 0.0; + response->response.validation_summary.yaw_stddev_rad = 0.0; + response->response.validation_summary.tracking_loss_ratio = 0.0; + response->response.validation_summary.time_sync_offset_ms = 0.0; +} + +rclcpp_action::GoalResponse ExternalLocalizationServiceNode::handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + std::string reject_reason; + if (!executor_.validate_goal(*goal, reject_reason)) { + RCLCPP_WARN(get_logger(), "Reject external_localization goal: %s", reject_reason.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ExternalLocalizationServiceNode::handle_cancel( + const std::shared_ptr /*goal_handle*/) +{ + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ExternalLocalizationServiceNode::handle_accepted( + const std::shared_ptr goal_handle) +{ + std::thread(std::bind(&ExternalLocalizationServiceNode::execute_goal, this, goal_handle)).detach(); +} + +void ExternalLocalizationServiceNode::execute_goal( + const std::shared_ptr goal_handle) +{ + auto feedback = std::make_shared(); + feedback->feedback.job_id = goal_handle->get_goal()->goal.header.request_id; + feedback->feedback.state.state = JobState::RUNNING; + feedback->feedback.progress = 0.5; + feedback->feedback.error_code.code = ErrorCode::OK; + feedback->feedback.message = "external localization task is running."; + feedback->feedback.server_timestamp_us = now_us(); + feedback->feedback.safe_to_retry = false; + goal_handle->publish_feedback(feedback); + + auto result = std::make_shared(); + std::string failure_reason; + if (!executor_.build_result(*goal_handle->get_goal(), *result, failure_reason)) { + result->result.success = false; + result->result.error_code.code = ErrorCode::INVALID_ARGUMENT; + result->result.message = failure_reason; + goal_handle->abort(result); + return; + } + + goal_handle->succeed(result); +} + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_reference_executor.cpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_reference_executor.cpp new file mode 100644 index 0000000..ea685d6 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/external_reference_executor.cpp @@ -0,0 +1,137 @@ +#include "external_localization_service/external_reference_executor.hpp" + +#include + +#include "calibration_common_interfaces/msg/key_value_pair.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "external_localization_service/external_localization_common.hpp" + +namespace external_localization_service +{ + +bool ExternalReferenceExecutor::validate_goal( + const ExecuteTask::Goal & goal, + std::string & reject_reason) const +{ + if (goal.goal.localization_source_id.empty()) { + reject_reason = "localization_source_id 不能为空。"; + return false; + } + if (goal.goal.workcell_zone_id.empty()) { + reject_reason = "workcell_zone_id 不能为空。"; + return false; + } + if (goal.goal.header.request_id.empty()) { + reject_reason = "request_id 不能为空。"; + return false; + } + return true; +} + +bool ExternalReferenceExecutor::build_result( + const ExecuteTask::Goal & goal, + ExecuteTask::Result & external_result, + std::string & failure_reason) const +{ + if (!validate_goal(goal, failure_reason)) { + return false; + } + + ExternalLocalizationInput input; + input.request = goal; + input.task_type = goal.goal.selected_task; + input.reference_pose_collection_task = goal.goal.reference_pose_collection; + input.marker_alignment_task = goal.goal.marker_alignment; + input.truth_source_validation_task = goal.goal.truth_source_validation; + + input.required_reference_pose_sample_count = goal.goal.reference_pose_collection.sample_count; + input.required_marker_observation_count = goal.goal.marker_alignment.min_valid_observation_count; + input.required_static_sample_count = goal.goal.truth_source_validation.static_sample_count; + input.required_dynamic_sample_count = goal.goal.truth_source_validation.dynamic_sample_count; + input.require_vehicle_static = goal.goal.reference_pose_collection.require_vehicle_static; + input.require_short_motion_segment = goal.goal.truth_source_validation.require_short_motion_segment; + input.reference_timeout_sec = 0.0; + if (goal.goal.selected_task.value == input.task_type.REFERENCE_POSE_COLLECTION) { + input.reference_timeout_sec = goal.goal.reference_pose_collection.timeout_sec; + } else if (goal.goal.selected_task.value == input.task_type.MARKER_ALIGNMENT) { + input.reference_timeout_sec = goal.goal.marker_alignment.timeout_sec; + } else if (goal.goal.selected_task.value == input.task_type.TRUTH_SOURCE_VALIDATION) { + input.reference_timeout_sec = goal.goal.truth_source_validation.timeout_sec; + } + input.min_sample_interval_sec = goal.goal.reference_pose_collection.min_sample_interval_sec; + input.accepted_max_position_stddev_m = goal.goal.truth_source_validation.max_position_stddev_m; + input.accepted_max_yaw_stddev_rad = goal.goal.truth_source_validation.max_yaw_stddev_rad; + input.accepted_max_tracking_loss_ratio = goal.goal.truth_source_validation.max_tracking_loss_ratio; + input.accepted_max_time_sync_offset_ms = goal.goal.truth_source_validation.max_time_sync_offset_ms; + input.reference_board_id = goal.goal.marker_alignment.target_board_id; + input.localization_source_id = goal.goal.localization_source_id; + input.workcell_zone_id = goal.goal.workcell_zone_id; + + input.candidate_calibration_result.workshop_frame_id = "workshop"; + input.candidate_calibration_result.localization_frame_id = "localization"; + input.candidate_calibration_result.localization_source_id = goal.goal.localization_source_id; + input.candidate_calibration_result.workcell_zone_id = goal.goal.workcell_zone_id; + + input.external_history_available = !input.external_localization_telemetry_history.empty(); + input.chassis_history_available = !input.chassis_telemetry_history.empty(); + input.control_history_available = !input.control_telemetry_history.empty(); + input.sensor_history_available = !input.sensor_telemetry_history.empty(); + + ExternalLocalizationOutput output; + if (!algorithm_.run(input, output, failure_reason)) { + fill_external_localization_common_failure(output, failure_reason); + external_result = output.response; + return false; + } + + external_result = output.response; + return true; +} + +void ExternalReferenceExecutor::fill_stage_result_from_external_result( + const ExecuteTask::Result & external_result, + calibration_workshop_orchestration_interfaces::msg::StageResultSummary & stage_result, + std::string & failure_reason) const +{ + stage_result.success = external_result.result.success; + stage_result.auto_acceptance_passed = + external_result.result.data_quality_passed && + external_result.result.validation_summary.recommended_as_truth_source; + stage_result.final_job_state.state = + external_result.result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED + : calibration_common_interfaces::msg::JobState::FAILED; + stage_result.parameter_version = external_result.result.recommended_parameter_version; + stage_result.artifacts = external_result.result.artifacts; + stage_result.summary = external_result.result.message.empty() ? + "外部真值校核完成。" : external_result.result.message; + stage_result.failure_root_cause = external_result.result.success ? "" : stage_result.summary; + stage_result.metadata.clear(); + + calibration_common_interfaces::msg::KeyValuePair kv; + kv.key = "task_purpose"; + kv.value = std::to_string(external_result.result.task_purpose.value); + stage_result.metadata.push_back(kv); + kv.key = "task_type"; + kv.value = std::to_string(external_result.result.task_purpose.value); + stage_result.metadata.push_back(kv); + kv.key = "data_quality_passed"; + kv.value = external_result.result.data_quality_passed ? "true" : "false"; + stage_result.metadata.push_back(kv); + kv.key = "suitable_for_commit"; + kv.value = external_result.result.suitable_for_commit ? "true" : "false"; + stage_result.metadata.push_back(kv); + kv.key = "recommended_as_truth_source"; + kv.value = external_result.result.validation_summary.recommended_as_truth_source ? "true" : "false"; + stage_result.metadata.push_back(kv); + + if (!external_result.result.success) { + failure_reason = stage_result.summary; + return; + } + if (!stage_result.auto_acceptance_passed) { + failure_reason = "外部真值校核未通过自动验收。"; + stage_result.failure_root_cause = failure_reason; + } +} + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/src/main.cpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/main.cpp new file mode 100644 index 0000000..c4bb6e2 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/main.cpp @@ -0,0 +1,15 @@ +#include "rclcpp/rclcpp.hpp" + +#include "external_localization_service/external_localization_service_node.hpp" + +// 这里保留一个很薄的 main 入口。 +// 作用只是为了让 external_localization_service 还能通过 ros2 run 独立启动， +// 真正的专项逻辑仍然都在 node / executor / algorithm template / *_algorithm.cpp 中。 +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/src/marker_alignment_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/marker_alignment_algorithm.cpp new file mode 100644 index 0000000..3936365 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/marker_alignment_algorithm.cpp @@ -0,0 +1,66 @@ +#include "external_localization_service/external_localization_common.hpp" + +namespace external_localization_service +{ + +bool MarkerAlignmentAlgorithm::run( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 标靶对齐模板： + // 【本文件负责什么】 + // - 负责标靶检测结果读取、坐标系对齐求解和残差统计相关算法实现。 + // - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。 + // + // 【建议优先读取的输入】 + // 1. input.marker_alignment_task + // - target_board_id、min_valid_observation_count、timeout_sec 等关键约束。 + // 2. input.latest_external_localization_telemetry / input.external_localization_telemetry_history + // - 读取观测位姿、标准差、丢失率、时间同步偏差和质量评分。 + // 3. input.marker_alignment_diagnostics + // - 读取标靶检测失败、角点不足、姿态求解不稳定等问题描述。 + // 4. input.sensor_quality_diagnostics + // - 如果标靶检测依赖相机 / LiDAR 质量，可在这里读取辅助质量信息。 + // + // 【必写输出】 + // 1. output.response.result.result.workshop_to_localization + // - 这是标靶对齐最核心的输出结果。 + // 2. output.response.result.result.residual_error_m / residual_error_rad + // - 写回对齐残差，供 orchestrator 判断是否自动验收。 + // 3. output.response.result.validation_summary + // - 写位置标准差、航向标准差、时间同步偏差等摘要。 + // + // 【可选输出】 + // - output.response.result.artifacts + // 可挂标靶检测日志、可视化结果、拟合报告、残差统计文件等。 + // + // 【常见失败原因】 + // - 有效观测数不足、标靶检测失败、姿态求解不稳定、时间同步异常、质量评分过低。 + // + // 【在这里添加真实算法】 + // - 请在 fill_external_localization_common_success(...) 之前或之后补充真实对齐求解逻辑。 + // - 当前文件仅提供交付模板，不包含真实外部定位算法。 + + fill_external_localization_common_success( + input, + output, + "标靶对齐完成。", + "demo_external_marker_alignment_v1"); + + output.response.result.result.workshop_frame_id = "workshop"; + output.response.result.result.localization_frame_id = "localization"; + output.response.result.result.workshop_to_localization.z_m = 0.0; + output.response.result.result.position_repeatability_m = 0.0; + output.response.result.result.yaw_repeatability_rad = 0.0; + output.response.result.result.residual_error_m = 0.0; + output.response.result.result.residual_error_rad = 0.0; + output.response.result.result.tracking_loss_ratio = 0.0; + output.response.result.result.time_sync_offset_ms = 0.0; + output.response.result.result.validated_as_truth_source = true; + return true; +} + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/src/reference_pose_collection_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/reference_pose_collection_algorithm.cpp new file mode 100644 index 0000000..eb45138 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/reference_pose_collection_algorithm.cpp @@ -0,0 +1,65 @@ +#include "external_localization_service/external_localization_common.hpp" + +namespace external_localization_service +{ + +bool ReferencePoseCollectionAlgorithm::run( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 参考位姿采集模板： + // 【本文件负责什么】 + // - 负责参考位姿采集与静态重复性分析相关算法实现。 + // - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。 + // + // 【建议优先读取的输入】 + // 1. input.reference_pose_collection_task + // - sample_count、require_vehicle_static、timeout_sec、min_sample_interval_sec 等采样约束。 + // 2. input.latest_external_localization_telemetry / input.external_localization_telemetry_history + // - 每帧外部定位位姿、位置标准差、航向标准差、时间同步偏差、质量评分。 + // 3. input.latest_chassis_telemetry / input.chassis_telemetry_history + // - 用于判断车辆是否真实静止，避免采入无效参考位姿。 + // 4. input.truth_source_diagnostics / input.acquisition_diagnostics + // - 记录时间同步异常、观测缺失、采样不足、落盘失败等问题。 + // + // 【必写输出】 + // 1. output.response.result.validation_summary + // - 写位置标准差、航向标准差、时间同步偏差、是否推荐为真值源等摘要。 + // 2. output.response.result.result + // - 写 workshop_frame_id / localization_frame_id / repeatability / residual 等结果。 + // 3. output.response.result.suitable_for_commit + // - 明确当前采集结果是否建议进入下一阶段。 + // + // 【可选输出】 + // - output.response.result.artifacts + // 可挂采样日志、原始位姿文件、统计报告等文件引用。 + // + // 【常见失败原因】 + // - 车辆未静止、有效样本不足、时间同步超标、外部定位观测丢失、采样频率不足。 + // + // 【在这里添加真实算法】 + // - 请在 fill_external_localization_common_success(...) 之前或之后补充真实采样与统计逻辑。 + // - 当前文件仅提供交付模板，不包含真实外部定位算法。 + + fill_external_localization_common_success( + input, + output, + "参考位姿采集完成。", + "demo_external_reference_pose_collection_v1"); + + output.response.result.result.workshop_frame_id = "workshop"; + output.response.result.result.localization_frame_id = "localization"; + output.response.result.result.position_repeatability_m = 0.0; + output.response.result.result.yaw_repeatability_rad = 0.0; + output.response.result.result.residual_error_m = 0.0; + output.response.result.result.residual_error_rad = 0.0; + output.response.result.result.tracking_loss_ratio = 0.0; + output.response.result.result.time_sync_offset_ms = 0.0; + output.response.result.result.validated_as_truth_source = true; + return true; +} + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/external_localization_service/src/truth_source_validation_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/truth_source_validation_algorithm.cpp new file mode 100644 index 0000000..6987fc9 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/external_localization_service/src/truth_source_validation_algorithm.cpp @@ -0,0 +1,77 @@ +#include "external_localization_service/external_localization_common.hpp" + +namespace external_localization_service +{ + +bool TruthSourceValidationAlgorithm::run( + const ExternalLocalizationInput & input, + ExternalLocalizationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 真值源验证模板： + // 【本文件负责什么】 + // - 负责静态重复性、动态稳定性、时间同步与丢失率等真值源验证算法实现。 + // - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。 + // + // 【建议优先读取的输入】 + // 1. input.truth_source_validation_task + // - static_sample_count、dynamic_sample_count、require_short_motion_segment 等任务要求。 + // 2. input.external_localization_telemetry_history + // - 外部定位历史观测窗口，是稳定性、同步性、重复性分析的核心输入。 + // 3. input.chassis_telemetry_history / input.control_telemetry_history + // - 如果要求短运动段验证，需要结合车辆实际运动状态和控制输出做时序对齐。 + // 4. input.sensor_telemetry_history + // - 用于判断辅助传感器质量是否影响外部定位观测可信度。 + // 5. input.truth_source_diagnostics + // - 记录时间同步超标、观测丢失、切源异常等问题。 + // + // 【必写输出】 + // 1. output.response.result.validation_summary + // - 这是 orchestrator 自动验收最关键的摘要区域。 + // 2. output.response.result.result + // - 写 repeatability、tracking_loss_ratio、time_sync_offset_ms 等核心结果。 + // 3. output.response.result.data_quality_passed / suitable_for_commit + // - 明确当前真值源是否可进入后续标定闭环。 + // + // 【可选输出】 + // - output.response.result.artifacts + // 可挂稳定性分析报告、同步统计图、丢失率分析文件等。 + // + // 【常见失败原因】 + // - 动态窗口不足、时间同步偏差超阈值、观测丢失率过高、重复性不满足要求。 + // + // 【在这里添加真实算法】 + // - 请在 fill_external_localization_common_success(...) 之前或之后补充真实验证逻辑。 + // - 当前文件仅提供交付模板，不包含真实外部定位算法。 + + fill_external_localization_common_success( + input, + output, + "真值源验证完成。", + "demo_external_truth_source_validation_v1"); + + output.response.result.validation_summary.time_sync_ok = true; + output.response.result.validation_summary.coverage_ok = true; + output.response.result.validation_summary.quality_ok = true; + output.response.result.validation_summary.tracking_stable = true; + output.response.result.validation_summary.recommended_as_truth_source = true; + output.response.result.validation_summary.position_stddev_m = 0.0; + output.response.result.validation_summary.yaw_stddev_rad = 0.0; + output.response.result.validation_summary.tracking_loss_ratio = 0.0; + output.response.result.validation_summary.time_sync_offset_ms = 0.0; + + output.response.result.result.workshop_frame_id = "workshop"; + output.response.result.result.localization_frame_id = "localization"; + output.response.result.result.position_repeatability_m = 0.0; + output.response.result.result.yaw_repeatability_rad = 0.0; + output.response.result.result.residual_error_m = 0.0; + output.response.result.result.residual_error_rad = 0.0; + output.response.result.result.tracking_loss_ratio = 0.0; + output.response.result.result.time_sync_offset_ms = 0.0; + output.response.result.result.validated_as_truth_source = true; + return true; +} + +} // namespace external_localization_service diff --git a/agv_calib_brain/src/agv_calib_core/include/agv_calib_core/brain_node.hpp b/agv_calib_brain/src/agv_calib_core/include/agv_calib_core/brain_node.hpp deleted file mode 100644 index a8a91c3..0000000 --- a/agv_calib_brain/src/agv_calib_core/include/agv_calib_core/brain_node.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace agv_calib_core { - -// 继承 Node，化身为标准的 ROS 2 Component -class BrainNode : public rclcpp::Node { -public: - explicit BrainNode(const rclcpp::NodeOptions & options); - ~BrainNode() override; - -private: - // 行为树专属的后台执行线程 (极其重要！绝不能阻塞 ROS 2 容器主线程) - void execute_behavior_tree(); - - std::thread bt_thread_; - std::atomic is_running_; -}; - -} // namespace agv_calib_core \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/include/agv_calib_core/bt_ros2_nodes.hpp b/agv_calib_brain/src/agv_calib_core/include/agv_calib_core/bt_ros2_nodes.hpp deleted file mode 100644 index dc814d2..0000000 --- a/agv_calib_brain/src/agv_calib_core/include/agv_calib_core/bt_ros2_nodes.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -#include -#include -#include - -// 引入底层 win_ubuntu_bridge 接口 -#include "win_ubuntu_bridge/srv/set_diagnostic_mode.hpp" -#include "win_ubuntu_bridge/srv/trigger_sync_capture.hpp" -#include "win_ubuntu_bridge/action/download_sensor_data.hpp" - -namespace agv_calib_core { - -class SetChassisModeNode : public BT::SyncActionNode { -public: - SetChassisModeNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node) - : BT::SyncActionNode(name, config), node_(node) { - client_ = node_->create_client("/chassis_gateway/set_diagnostic_mode"); - } - - static BT::PortsList providedPorts() { return { BT::InputPort("target_mode") }; } - - BT::NodeStatus tick() override { - int mode; if (!getInput("target_mode", mode)) return BT::NodeStatus::FAILURE; - RCLCPP_INFO(node_->get_logger(), "🌲 [BT] 下发底盘夺权指令，模式: %d", mode); - if (!client_->wait_for_service(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE; - - auto req = std::make_shared(); req->target_mode = mode; - auto future = client_->async_send_request(req); - - // 🚨 这里阻塞等待完全没问题！因为外层 BT 跑在独立线程，根本不影响 ROS 2 Executor 的回调！ - if (future.wait_for(std::chrono::seconds(3)) == std::future_status::ready) { - auto res = future.get(); - if (res->success) return BT::NodeStatus::SUCCESS; - } - return BT::NodeStatus::FAILURE; - } -private: - rclcpp::Node* node_; rclcpp::Client::SharedPtr client_; -}; - -class TriggerCaptureNode : public BT::SyncActionNode { -public: - TriggerCaptureNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node) - : BT::SyncActionNode(name, config), node_(node) { - client_ = node_->create_client("/sensor_gateway/trigger_sync_capture"); - } - - static BT::PortsList providedPorts() { - return { BT::InputPort("sensor_id"), BT::OutputPort("capture_code_out") }; - } - - BT::NodeStatus tick() override { - std::string sensor_id; getInput("sensor_id", sensor_id); - RCLCPP_INFO(node_->get_logger(), "📷 [BT] 冻结 %s 数据...", sensor_id.c_str()); - if (!client_->wait_for_service(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE; - - auto req = std::make_shared(); req->sensor_ids.push_back(sensor_id); - auto future = client_->async_send_request(req); - if (future.wait_for(std::chrono::seconds(3)) == std::future_status::ready) { - auto res = future.get(); - if (res->success) { setOutput("capture_code_out", res->capture_timestamp_us); return BT::NodeStatus::SUCCESS; } - } - return BT::NodeStatus::FAILURE; - } -private: - rclcpp::Node* node_; rclcpp::Client::SharedPtr client_; -}; - -class DownloadDataNode : public BT::StatefulActionNode { -public: - DownloadDataNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node) - : BT::StatefulActionNode(name, config), node_(node) { - action_client_ = rclcpp_action::create_client(node_, "/sensor_gateway/download_sensor_data"); - } - - static BT::PortsList providedPorts() { - return { BT::InputPort("sensor_id"), BT::InputPort("capture_code_in"), - BT::InputPort("save_dir"), BT::OutputPort("saved_path_out") }; - } - - BT::NodeStatus onStart() override { - int64_t code; std::string sensor; std::string save_dir; - if (!getInput("capture_code_in", code) || !getInput("sensor_id", sensor) || !getInput("save_dir", save_dir)) return BT::NodeStatus::FAILURE; - - RCLCPP_INFO(node_->get_logger(), "📥 [BT] 挂起下载任务，取件码: %ld", code); - if (!action_client_->wait_for_action_server(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE; - - auto goal_msg = win_ubuntu_bridge::action::DownloadSensorData::Goal(); - goal_msg.capture_timestamp_us = code; goal_msg.sensor_id = sensor; - goal_msg.data_type = win_ubuntu_bridge::action::DownloadSensorData::Goal::DATA_TYPE_IMAGE; goal_msg.save_directory = save_dir; - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->success) { - RCLCPP_INFO(node_->get_logger(), "✅ [BT] 落盘成功！路径: %s", result.result->saved_file_path.c_str()); - setOutput("saved_path_out", result.result->saved_file_path); done_ = true; success_ = true; - } else { done_ = true; success_ = false; } - }; - action_client_->async_send_goal(goal_msg, send_goal_options); done_ = false; return BT::NodeStatus::RUNNING; - } - BT::NodeStatus onRunning() override { if (done_) return success_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; return BT::NodeStatus::RUNNING; } - void onHalted() override { } -private: - rclcpp::Node* node_; rclcpp_action::Client::SharedPtr action_client_; - bool done_ = false; bool success_ = false; -}; - -} // namespace agv_calib_core \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/launch/brain_bringup.launch.py b/agv_calib_brain/src/agv_calib_core/launch/brain_bringup.launch.py deleted file mode 100644 index c482a4f..0000000 --- a/agv_calib_brain/src/agv_calib_core/launch/brain_bringup.launch.py +++ /dev/null @@ -1,31 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -def generate_launch_description(): - # 获取 yaml 文件的绝对路径 - config_file = os.path.join( - get_package_share_directory('agv_calib_core'), - 'config', - 'brain_params.yaml' - ) - - # 建立多线程容器加载大脑组件 (MT 代表 Multi-Threaded Executor) - container = ComposableNodeContainer( - name='brain_container', - namespace='', - package='rclcpp_components', - executable='component_container_mt', - composable_node_descriptions=[ - ComposableNode( - package='agv_calib_core', - plugin='agv_calib_core::BrainNode', - name='brain_node', - parameters=[config_file] # 🚨 动态挂载 YAML 参数表！ - ) - ], - output='screen', - ) - return LaunchDescription([container]) \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/package.xml b/agv_calib_brain/src/agv_calib_core/package.xml deleted file mode 100644 index 5d997ee..0000000 --- a/agv_calib_brain/src/agv_calib_core/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - agv_calib_core - 1.0.0 - 行为树总控大脑 - nvidia - Apache-2.0 - - ament_cmake - - rclcpp - rclcpp_action - rclcpp_components - behaviortree_cpp_v3 - ament_index_cpp - win_ubuntu_bridge - - - ament_cmake - - \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/CMakeLists.txt new file mode 100644 index 0000000..880bad7 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(sensor_calibration_service) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(calibration_chassis_interfaces REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_control_interfaces REQUIRED) +find_package(calibration_external_localization_interfaces REQUIRED) +find_package(calibration_sensor_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) +find_package(calibration_workshop_orchestration_interfaces REQUIRED) + +include_directories(include) + +add_executable(sensor_calibration_service_node + src/main.cpp + src/sensor_calibration_service_node.cpp + src/sensor_calibration_algorithm_template.cpp + src/camera_intrinsic_calibration_algorithm.cpp + src/imu_intrinsic_calibration_algorithm.cpp + src/sensor_to_base_extrinsic_calibration_algorithm.cpp + src/hand_eye_calibration_algorithm.cpp +) + +ament_target_dependencies(sensor_calibration_service_node + rclcpp + rclcpp_action + calibration_chassis_interfaces + calibration_common_interfaces + calibration_control_interfaces + calibration_external_localization_interfaces + calibration_sensor_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces +) + +install(TARGETS sensor_calibration_service_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +ament_package() diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithm_template.hpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithm_template.hpp new file mode 100644 index 0000000..3eae8f9 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithm_template.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include "sensor_calibration_service/sensor_calibration_algorithms.hpp" + +namespace sensor_calibration_service +{ + +// 传感器标定算法门面： +// 1. 负责对 node 组装好的 SensorCalibrationInput 做统一入口校验。 +// 2. 负责按任务类型分发到相机内参、IMU 内参、外参、手眼的专属实现。 +// 3. 负责保持 node 层与具体算法实现解耦，方便后续整体替换某个算法 cpp 文件。 +class SensorCalibrationAlgorithmTemplate +{ +public: + using Input = SensorCalibrationInput; + using Output = SensorCalibrationOutput; + using TaskType = sensor_calibration_service::SensorCalibrationTaskType; + using TaskSubtype = sensor_calibration_service::SensorCalibrationTaskSubtype; + + bool run( + const Input & input, + Output & output, + std::string & failure_reason) const; + +private: + bool validate_input(const Input & input, std::string & failure_reason) const; + const SensorCalibrationAlgorithm * resolve_algorithm( + const TaskType & task_type, + const TaskSubtype & task_subtype) const; + + FrontCameraIntrinsicCalibrationAlgorithm front_camera_intrinsic_algorithm_; + DownwardCameraIntrinsicCalibrationAlgorithm downward_camera_intrinsic_algorithm_; + ImuIntrinsicCalibrationAlgorithm imu_intrinsic_algorithm_; + FrontCameraExtrinsicCalibrationAlgorithm front_camera_extrinsic_algorithm_; + DownwardCameraExtrinsicCalibrationAlgorithm downward_camera_extrinsic_algorithm_; + ImuExtrinsicCalibrationAlgorithm imu_extrinsic_algorithm_; + Lidar2DExtrinsicCalibrationAlgorithm lidar_2d_extrinsic_algorithm_; + Lidar3DExtrinsicCalibrationAlgorithm lidar_3d_extrinsic_algorithm_; + EyeInHandCalibrationAlgorithm eye_in_hand_algorithm_; + EyeToHandCalibrationAlgorithm eye_to_hand_algorithm_; +}; + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp new file mode 100644 index 0000000..94fe881 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp @@ -0,0 +1,246 @@ +#pragma once + +#include +#include + +#include "calibration_chassis_interfaces/msg/applied_chassis_calibration_parameters_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" +#include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" +#include "calibration_control_interfaces/msg/control_readiness_response.hpp" +#include "calibration_control_interfaces/msg/control_telemetry.hpp" +#include "calibration_control_interfaces/msg/control_work_mode.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_readiness_response.hpp" +#include "calibration_external_localization_interfaces/msg/external_localization_telemetry.hpp" +#include "calibration_sensor_interfaces/action/execute_sensor_calibration_task.hpp" +#include "calibration_sensor_interfaces/msg/applied_sensor_calibration_parameters_response.hpp" +#include "calibration_sensor_interfaces/msg/camera_intrinsic_calibration_task.hpp" +#include "calibration_sensor_interfaces/msg/hand_eye_calibration_task.hpp" +#include "calibration_sensor_interfaces/msg/imu_intrinsic_calibration_task.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_parameter_set.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_task_subtype.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_task_type.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_telemetry.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_work_mode.hpp" +#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp" +#include "calibration_sensor_interfaces/msg/sensor_to_base_extrinsic_calibration_task.hpp" +#include "calibration_vehicle_profile_interfaces/msg/sensor_type.hpp" +#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace sensor_calibration_service +{ + +using ExecuteTask = calibration_sensor_interfaces::action::ExecuteSensorCalibrationTask; +using SensorCalibrationTaskSubtype = calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype; +using SensorCalibrationTaskType = calibration_sensor_interfaces::msg::SensorCalibrationTaskType; +using SensorType = calibration_vehicle_profile_interfaces::msg::SensorType; + +struct SensorCalibrationInput +{ + // ===== 会话与静态画像 ===== + // 当前车间会话：算法可读取本轮 session 的阶段上下文、人工确认状态、操作者信息等。 + calibration_workshop_orchestration_interfaces::msg::WorkshopSession workshop_session; + + // 车辆画像：包含传感器清单、安装方式、base_link、控制器和启用阶段等静态信息。 + calibration_vehicle_profile_interfaces::msg::VehicleProfile vehicle_profile; + + // 传感器标定任务请求：包含 request_id、任务目的、任务类型、各 payload、来源迭代号。 + ExecuteTask::Goal request; + + // 当前任务类型快照：与 request.goal.selected_task 一致，但单独展开更方便算法工程师阅读。 + SensorCalibrationTaskType task_type; + + // 当前任务子类型：用于区分前视/下视相机、IMU 外参、2D/3D 激光雷达、眼在手内/外等。 + SensorCalibrationTaskSubtype task_subtype; + + // 当前目标传感器 ID：从具体 payload 中提取并展开。 + std::string target_sensor_id; + + // 当前目标传感器类型：上层若已知，可在进入算法前写入；未知时可保持未指定。 + SensorType target_sensor_type; + + // ===== 任务拆解视图 ===== + calibration_sensor_interfaces::msg::CameraIntrinsicCalibrationTask camera_intrinsic_task; + calibration_sensor_interfaces::msg::IMUIntrinsicCalibrationTask imu_intrinsic_task; + calibration_sensor_interfaces::msg::SensorToBaseExtrinsicCalibrationTask sensor_to_base_extrinsic_task; + calibration_sensor_interfaces::msg::HandEyeCalibrationTask hand_eye_task; + + // 常用展开参考量：便于算法直接取值，而不用总回到原始 payload 中解析。 + uint32_t required_image_count{0}; + uint32_t required_static_segment_count{0}; + uint32_t required_motion_segment_count{0}; + uint32_t required_sample_count{0}; + uint32_t required_pose_count{0}; + double reference_timeout_sec{0.0}; + std::string reference_board_id; + std::string reference_base_frame_id; + std::string reference_arm_id; + + // ===== 传感器域反馈 ===== + // 传感器服务就绪状态：采集链路、存储链路、遥测链路、车辆安全、机械臂就绪等。 + calibration_sensor_interfaces::msg::SensorReadinessResponse sensor_readiness; + + // 当前传感器工作模式：调参准备、采集、求解、验证等。 + calibration_sensor_interfaces::msg::SensorCalibrationWorkMode sensor_work_mode; + + // 当前已生效的传感器参数：相机内参、IMU 内参、外参、手眼等。 + calibration_sensor_interfaces::msg::AppliedSensorCalibrationParametersResponse applied_sensor_parameters; + + // 当前算法准备写回的候选参数集。 + calibration_sensor_interfaces::msg::SensorCalibrationParameterSet candidate_parameter_set; + + // 最新一帧传感器遥测：当前采样数、目标检测状态、质量评分、当前任务 ID 等。 + calibration_sensor_interfaces::msg::SensorCalibrationTelemetry latest_sensor_telemetry; + + // 历史传感器遥测窗口：用于观测采集进度、质量趋势、稳定性和异常剔除。 + std::vector sensor_telemetry_history; + + // 采集质量诊断扩展：例如棋盘格检测失败、LiDAR 反光不足、IMU 饱和、图像模糊等。 + std::vector capture_quality_diagnostics; + + // 存储与产物诊断扩展：例如落盘失败、目录权限、磁盘空间不足、文件损坏等。 + std::vector storage_diagnostics; + + // ===== 底盘域反馈 ===== + calibration_chassis_interfaces::msg::ChassisReadinessResponse chassis_readiness; + calibration_chassis_interfaces::msg::ChassisWorkMode chassis_work_mode; + calibration_chassis_interfaces::msg::AppliedChassisCalibrationParametersResponse applied_chassis_parameters; + calibration_chassis_interfaces::msg::ChassisTelemetry latest_chassis_telemetry; + std::vector chassis_telemetry_history; + + // ===== 控制域反馈 ===== + calibration_control_interfaces::msg::ControlReadinessResponse control_readiness; + calibration_control_interfaces::msg::ControlWorkMode control_work_mode; + calibration_control_interfaces::msg::ActiveControllerParametersResponse active_controller_parameters; + calibration_control_interfaces::msg::ControlTelemetry latest_control_telemetry; + std::vector control_telemetry_history; + + // ===== 外部定位 / 真值源反馈 ===== + calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness; + calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry; + std::vector external_localization_telemetry_history; + std::vector truth_source_diagnostics; + + // ===== 时间同步 / 数据窗口有效性 ===== + double estimated_sensor_to_chassis_time_offset_sec{0.0}; + double estimated_sensor_to_control_time_offset_sec{0.0}; + double estimated_sensor_to_truth_time_offset_sec{0.0}; + bool sensor_history_available{false}; + bool chassis_history_available{false}; + bool control_history_available{false}; + bool truth_history_available{false}; + + // 说明：如果后续某类传感器算法还需要更细原始数据入口，可继续在这里补充。 + // 常见补充项包括：图像帧缓存、IMU 原始包、点云帧列表、机械臂位姿序列、标定板检测结果等。 +}; + +struct SensorCalibrationOutput +{ + // 传感器标定算法统一输出：成功/失败、错误码、验证摘要、估计参数、产物引用都写到这里。 + ExecuteTask::Result response; +}; + +class SensorCalibrationAlgorithm +{ +public: + virtual ~SensorCalibrationAlgorithm() = default; + + virtual bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const = 0; +}; + +class FrontCameraIntrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class DownwardCameraIntrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class ImuIntrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class FrontCameraExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class DownwardCameraExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class ImuExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class Lidar2DExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class Lidar3DExtrinsicCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class EyeInHandCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +class EyeToHandCalibrationAlgorithm final : public SensorCalibrationAlgorithm +{ +public: + bool run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const override; +}; + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_common.hpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_common.hpp new file mode 100644 index 0000000..20e49c7 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_common.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include "sensor_calibration_service/sensor_calibration_algorithms.hpp" + +#include "calibration_common_interfaces/msg/error_code.hpp" + +namespace sensor_calibration_service +{ + +// 统一填充一份最小可运行的默认结果。 +// 作用： +// 1. 让模板文件在尚未接入真实算法时也能稳定返回一份结构完整的结果； +// 2. 让算法工程师明确最终结果要写入哪些字段； +// 3. 后续真实算法接入时，可以保留这层公共默认值，再按算法结果覆盖具体字段。 +inline void fill_common_result( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output) +{ + output.response.result.success = true; + output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK; + output.response.result.message = "sensor calibration template executed."; + output.response.result.job_id = input.request.goal.header.request_id; + output.response.result.data_quality_passed = true; + output.response.result.suitable_for_commit = true; + output.response.result.recommended_parameter_version = "demo_sensor_template_v1"; + output.response.result.validation_summary.reprojection_error_px = 0.0; + output.response.result.validation_summary.translation_residual_m = 0.0; + output.response.result.validation_summary.rotation_residual_rad = 0.0; + output.response.result.validation_summary.plane_residual_m = 0.0; + output.response.result.validation_summary.repeatability_error_m = 0.0; + output.response.result.validation_summary.auto_acceptance_passed = true; +} + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_service_node.hpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_service_node.hpp new file mode 100644 index 0000000..d4cabd4 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_service_node.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "sensor_calibration_service/sensor_calibration_algorithm_template.hpp" + +#include "calibration_sensor_interfaces/action/execute_sensor_calibration_task.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_task_type.hpp" +#include "calibration_sensor_interfaces/srv/get_sensor_readiness.hpp" + +namespace sensor_calibration_service +{ + +// 传感器标定服务节点： +// 1. 负责暴露 readiness service 和 execute action，作为传感器专项对外的 ROS 接口入口。 +// 2. 负责把 ExecuteSensorCalibrationTask::Goal 组装成 SensorCalibrationInput。 +// 3. 负责在进入算法前，先做最小任务合法性检查，并拆解常用任务字段。 +// 4. 负责调用传感器算法门面，再把输出统一回填到 Action Result。 +// 5. 这里不直接写具体标定算法，避免算法工程师在修改相机/IMU/外参/手眼逻辑时碰 ROS 通信代码。 +class SensorCalibrationServiceNode : public rclcpp::Node +{ +public: + explicit SensorCalibrationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using ReadinessSrv = calibration_sensor_interfaces::srv::GetSensorReadiness; + using ExecuteTask = calibration_sensor_interfaces::action::ExecuteSensorCalibrationTask; + using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle; + using TaskType = calibration_sensor_interfaces::msg::SensorCalibrationTaskType; + + rclcpp::Service::SharedPtr readiness_service_; + rclcpp_action::Server::SharedPtr execute_task_action_server_; + SensorCalibrationAlgorithmTemplate algorithm_; + + void handle_readiness( + const std::shared_ptr request, + std::shared_ptr response); + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute_goal(const std::shared_ptr goal_handle); + + std::string resolve_target_sensor_id(const ExecuteTask::Goal & goal) const; +}; + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/package.xml b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/package.xml new file mode 100644 index 0000000..cfbab71 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/package.xml @@ -0,0 +1,23 @@ + + + sensor_calibration_service + 0.0.1 + Sensor calibration service template for algorithm integration. + you + Apache-2.0 + + ament_cmake + rclcpp + rclcpp_action + calibration_chassis_interfaces + calibration_common_interfaces + calibration_control_interfaces + calibration_external_localization_interfaces + calibration_sensor_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces + + + ament_cmake + + diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/camera_intrinsic_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/camera_intrinsic_calibration_algorithm.cpp new file mode 100644 index 0000000..e9cb6c6 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/camera_intrinsic_calibration_algorithm.cpp @@ -0,0 +1,50 @@ +#include "sensor_calibration_service/sensor_calibration_common.hpp" + +namespace sensor_calibration_service +{ + +bool FrontCameraIntrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 前视相机内参标定模板： + // 【本文件负责什么】 + // - 负责前视相机内参标定算法实现。 + // - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。 + // + // 【建议优先读取的输入】 + // 1. input.camera_intrinsic_task / input.required_image_count / input.reference_board_id + // - 标定板类型、目标图像数、采集约束和超时信息。 + // 2. input.target_sensor_id / input.target_sensor_type + // - 当前需要求解内参的目标传感器。 + // 3. input.latest_sensor_telemetry / input.sensor_telemetry_history + // - 采样进度、检测状态、质量评分、时间同步质量等。 + // 4. input.capture_quality_diagnostics / input.storage_diagnostics + // - 图像模糊、角点不足、落盘失败、目录权限等诊断信息。 + // 5. input.external_localization_* / input.chassis_* + // - 如果采集过程需要车辆静止判断或真值辅助，可读取这些跨域反馈。 + fill_common_result(input, output); + output.response.result.message = "front camera intrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "front_camera_intrinsic_template_v1"; + return true; +} + +bool DownwardCameraIntrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 下视相机内参标定模板： + // 典型场景是地面标靶、货叉区域或工作面观测，相比前视更关注近距离畸变与俯视覆盖。 + fill_common_result(input, output); + output.response.result.message = "downward camera intrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "downward_camera_intrinsic_template_v1"; + return true; +} + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/hand_eye_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/hand_eye_calibration_algorithm.cpp new file mode 100644 index 0000000..27fd3d2 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/hand_eye_calibration_algorithm.cpp @@ -0,0 +1,34 @@ +#include "sensor_calibration_service/sensor_calibration_common.hpp" + +namespace sensor_calibration_service +{ + +bool EyeInHandCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 眼在手内标定模板：相机/传感器安装在机械臂末端执行器上。 + fill_common_result(input, output); + output.response.result.message = "eye-in-hand calibration template executed."; + output.response.result.recommended_parameter_version = "eye_in_hand_template_v1"; + return true; +} + +bool EyeToHandCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 眼在手外标定模板：相机/传感器固定在工作站环境中观测机械臂。 + fill_common_result(input, output); + output.response.result.message = "eye-to-hand calibration template executed."; + output.response.result.recommended_parameter_version = "eye_to_hand_template_v1"; + return true; +} + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/imu_intrinsic_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/imu_intrinsic_calibration_algorithm.cpp new file mode 100644 index 0000000..108a4a8 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/imu_intrinsic_calibration_algorithm.cpp @@ -0,0 +1,24 @@ +#include "sensor_calibration_service/sensor_calibration_common.hpp" + +namespace sensor_calibration_service +{ + +bool ImuIntrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // IMU 内参标定模板： + // 1. 读取任务输入：input.imu_intrinsic_task、input.required_static_segment_count、input.required_motion_segment_count。 + // 2. 读取采样质量：input.latest_sensor_telemetry / input.sensor_telemetry_history。 + // 3. 如需真值或车体运动参考，可结合底盘/控制/外部定位反馈。 + // 4. 将估计出的 bias、scale、noise 等写回 output.response.result.estimated_params。 + fill_common_result(input, output); + output.response.result.message = "imu intrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "imu_intrinsic_template_v1"; + return true; +} + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/main.cpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/main.cpp new file mode 100644 index 0000000..7ec9d57 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/main.cpp @@ -0,0 +1,16 @@ +#include "rclcpp/rclcpp.hpp" + +#include "sensor_calibration_service/sensor_calibration_service_node.hpp" + +// 薄 main 入口： +// 1. 方便直接通过 ros2 run 启动传感器标定服务节点； +// 2. 真实功能都在 SensorCalibrationServiceNode 中，这里只负责初始化、spin 和退出； +// 3. 保留这个文件可以让调试和集成更简单。 +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_calibration_algorithm_template.cpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_calibration_algorithm_template.cpp new file mode 100644 index 0000000..720b72e --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_calibration_algorithm_template.cpp @@ -0,0 +1,112 @@ +#include "sensor_calibration_service/sensor_calibration_algorithm_template.hpp" + +namespace sensor_calibration_service +{ + +// 统一入口校验。 +// 这里主要检查： +// 1. 这次传感器标定任务是不是一个合法请求； +// 2. 当前任务类型是否已经明确； +// 3. 对于大多数任务，目标传感器 ID 是否已经解析出来。 +// 更细的业务校验（例如图像数、采样数、机械臂位姿数是否合法） +// 推荐在具体算法文件中再按任务类型继续做。 +bool SensorCalibrationAlgorithmTemplate::validate_input( + const Input & input, + std::string & failure_reason) const +{ + if (input.request.goal.header.request_id.empty()) { + failure_reason = "sensor 任务 request_id 不能为空。"; + return false; + } + + if (input.task_type.value == TaskType::SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED) { + failure_reason = "传感器标定任务类型不能为空。"; + return false; + } + + if (input.task_subtype.value == TaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED) { + failure_reason = "传感器标定任务子类型不能为空。"; + return false; + } + + if (input.task_type.value != TaskType::IMU_INTRINSIC && input.target_sensor_id.empty()) { + failure_reason = "目标传感器 ID 不能为空。"; + return false; + } + + return true; +} + +const SensorCalibrationAlgorithm * SensorCalibrationAlgorithmTemplate::resolve_algorithm( + const TaskType & task_type, + const TaskSubtype & task_subtype) const +{ + switch (task_type.value) { + case TaskType::CAMERA_INTRINSIC: + switch (task_subtype.value) { + case TaskSubtype::FRONT_CAMERA_INTRINSIC: + return &front_camera_intrinsic_algorithm_; + case TaskSubtype::DOWNWARD_CAMERA_INTRINSIC: + return &downward_camera_intrinsic_algorithm_; + default: + return nullptr; + } + case TaskType::IMU_INTRINSIC: + switch (task_subtype.value) { + case TaskSubtype::IMU_INTRINSIC: + return &imu_intrinsic_algorithm_; + default: + return nullptr; + } + case TaskType::SENSOR_TO_BASE_EXTRINSIC: + switch (task_subtype.value) { + case TaskSubtype::FRONT_CAMERA_EXTRINSIC: + return &front_camera_extrinsic_algorithm_; + case TaskSubtype::DOWNWARD_CAMERA_EXTRINSIC: + return &downward_camera_extrinsic_algorithm_; + case TaskSubtype::IMU_EXTRINSIC: + return &imu_extrinsic_algorithm_; + case TaskSubtype::LIDAR_2D_EXTRINSIC: + return &lidar_2d_extrinsic_algorithm_; + case TaskSubtype::LIDAR_3D_EXTRINSIC: + return &lidar_3d_extrinsic_algorithm_; + default: + return nullptr; + } + case TaskType::HAND_EYE: + switch (task_subtype.value) { + case TaskSubtype::EYE_IN_HAND: + return &eye_in_hand_algorithm_; + case TaskSubtype::EYE_TO_HAND: + return &eye_to_hand_algorithm_; + default: + return nullptr; + } + default: + return nullptr; + } +} + +bool SensorCalibrationAlgorithmTemplate::run( + const Input & input, + Output & output, + std::string & failure_reason) const +{ + if (!validate_input(input, failure_reason)) { + return false; + } + + const auto * algorithm = resolve_algorithm(input.task_type, input.task_subtype); + if (algorithm == nullptr) { + failure_reason = "当前 task_type 与 task_subtype 组合不受支持。"; + return false; + } + + // 这里是传感器标定/评估算法的统一入口： + // 1. 先按 task_type 分发到相机内参、IMU 内参、外参、手眼的专属模板； + // 2. 各算法模板再结合遥测、底盘反馈、控制反馈、真值源反馈进入求解； + // 3. 最终统一把结果写回 output.response.result。 + return algorithm->run(input, output, failure_reason); +} + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_calibration_service_node.cpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_calibration_service_node.cpp new file mode 100644 index 0000000..624f09b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_calibration_service_node.cpp @@ -0,0 +1,167 @@ +#include "sensor_calibration_service/sensor_calibration_service_node.hpp" + +#include +#include + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_job_result.hpp" +#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp" + +namespace sensor_calibration_service +{ + +namespace +{ +int64_t now_us() +{ + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} +} // namespace + +using calibration_common_interfaces::msg::ErrorCode; +using calibration_common_interfaces::msg::JobState; + +SensorCalibrationServiceNode::SensorCalibrationServiceNode(const rclcpp::NodeOptions & options) +: Node("sensor_calibration_service", options) +{ + readiness_service_ = create_service( + "/sensor_calibration/get_readiness", + std::bind(&SensorCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2)); + + execute_task_action_server_ = rclcpp_action::create_server( + this, + "/sensor_calibration/execute_task", + std::bind(&SensorCalibrationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&SensorCalibrationServiceNode::handle_cancel, this, std::placeholders::_1), + std::bind(&SensorCalibrationServiceNode::handle_accepted, this, std::placeholders::_1)); +} + +void SensorCalibrationServiceNode::handle_readiness( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "sensor_calibration_service is ready."; + response->response.agent_ready = true; + response->response.capture_pipeline_ready = true; + response->response.storage_ready = true; + response->response.telemetry_ready = true; + response->response.vehicle_safe_to_move = true; + response->response.arm_ready = true; + response->response.ready_sensor_ids.push_back("demo_sensor_001"); + response->response.checked_timestamp_us = now_us(); +} + +rclcpp_action::GoalResponse SensorCalibrationServiceNode::handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + if (goal->goal.header.request_id.empty()) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse SensorCalibrationServiceNode::handle_cancel( + const std::shared_ptr /*goal_handle*/) +{ + return rclcpp_action::CancelResponse::ACCEPT; +} + +void SensorCalibrationServiceNode::handle_accepted( + const std::shared_ptr goal_handle) +{ + std::thread(std::bind(&SensorCalibrationServiceNode::execute_goal, this, goal_handle)).detach(); +} + +void SensorCalibrationServiceNode::execute_goal( + const std::shared_ptr goal_handle) +{ + // 先回一帧 RUNNING feedback，告诉 orchestrator 当前任务已经进入执行阶段。 + auto feedback = std::make_shared(); + feedback->feedback.state.state = JobState::RUNNING; + goal_handle->publish_feedback(feedback); + + // ===== 组装算法输入上下文 ===== + // 当前模板阶段先把“算法最常用的任务侧输入”显式展开。 + // 后续如果要接真实车辆画像、已生效参数查询、历史遥测缓存、外部定位缓存， + // 也应继续在这里补齐并写入 SensorCalibrationInput。 + SensorCalibrationAlgorithmTemplate::Input input; + input.request = *goal_handle->get_goal(); + input.task_type = goal_handle->get_goal()->goal.selected_task; + input.task_subtype = goal_handle->get_goal()->goal.task_subtype; + input.target_sensor_id = resolve_target_sensor_id(*goal_handle->get_goal()); + input.camera_intrinsic_task = goal_handle->get_goal()->goal.camera_intrinsic; + input.imu_intrinsic_task = goal_handle->get_goal()->goal.imu_intrinsic; + input.sensor_to_base_extrinsic_task = goal_handle->get_goal()->goal.sensor_to_base_extrinsic; + input.hand_eye_task = goal_handle->get_goal()->goal.hand_eye; + input.required_image_count = goal_handle->get_goal()->goal.camera_intrinsic.required_image_count; + input.required_static_segment_count = goal_handle->get_goal()->goal.imu_intrinsic.required_static_segment_count; + input.required_motion_segment_count = goal_handle->get_goal()->goal.imu_intrinsic.required_motion_segment_count; + input.required_sample_count = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.required_sample_count; + input.required_pose_count = goal_handle->get_goal()->goal.hand_eye.required_pose_count; + switch (goal_handle->get_goal()->goal.selected_task.value) { + case TaskType::CAMERA_INTRINSIC: + input.reference_timeout_sec = goal_handle->get_goal()->goal.camera_intrinsic.timeout_sec; + break; + case TaskType::IMU_INTRINSIC: + input.reference_timeout_sec = goal_handle->get_goal()->goal.imu_intrinsic.timeout_sec; + break; + case TaskType::SENSOR_TO_BASE_EXTRINSIC: + input.reference_timeout_sec = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.timeout_sec; + break; + case TaskType::HAND_EYE: + input.reference_timeout_sec = goal_handle->get_goal()->goal.hand_eye.timeout_sec; + break; + default: + input.reference_timeout_sec = 0.0; + break; + } + input.reference_board_id = goal_handle->get_goal()->goal.camera_intrinsic.target_board_id; + input.reference_base_frame_id = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.base_frame_id; + input.reference_arm_id = goal_handle->get_goal()->goal.hand_eye.arm_id; + input.sensor_history_available = false; + input.chassis_history_available = false; + input.control_history_available = false; + input.truth_history_available = false; + + SensorCalibrationAlgorithmTemplate::Output output; + std::string failure_reason; + if (!algorithm_.run(input, output, failure_reason)) { + auto result = std::make_shared(); + result->result.success = false; + result->result.error_code.code = ErrorCode::INVALID_STATE; + result->result.message = failure_reason; + result->result.job_id = goal_handle->get_goal()->goal.header.request_id; + result->result.data_quality_passed = false; + result->result.suitable_for_commit = false; + goal_handle->abort(result); + return; + } + + auto result = std::make_shared(output.response); + goal_handle->succeed(result); +} + +std::string SensorCalibrationServiceNode::resolve_target_sensor_id(const ExecuteTask::Goal & goal) const +{ + switch (goal.goal.selected_task.value) { + case TaskType::CAMERA_INTRINSIC: + return goal.goal.camera_intrinsic.sensor_id; + case TaskType::IMU_INTRINSIC: + return goal.goal.imu_intrinsic.sensor_id; + case TaskType::SENSOR_TO_BASE_EXTRINSIC: + return goal.goal.sensor_to_base_extrinsic.sensor_id; + case TaskType::HAND_EYE: + return goal.goal.hand_eye.sensor_id; + default: + return ""; + } +} + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_to_base_extrinsic_calibration_algorithm.cpp b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_to_base_extrinsic_calibration_algorithm.cpp new file mode 100644 index 0000000..246fec8 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/sensor_calibration_service/src/sensor_to_base_extrinsic_calibration_algorithm.cpp @@ -0,0 +1,76 @@ +#include "sensor_calibration_service/sensor_calibration_common.hpp" + +namespace sensor_calibration_service +{ + +bool FrontCameraExtrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 前视相机到 base_link 外参标定模板。 + fill_common_result(input, output); + output.response.result.message = "front camera extrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "front_camera_extrinsic_template_v1"; + return true; +} + +bool DownwardCameraExtrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 下视相机到 base_link 外参标定模板。 + fill_common_result(input, output); + output.response.result.message = "downward camera extrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "downward_camera_extrinsic_template_v1"; + return true; +} + +bool ImuExtrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // IMU 到 base_link 外参标定模板。 + fill_common_result(input, output); + output.response.result.message = "imu extrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "imu_extrinsic_template_v1"; + return true; +} + +bool Lidar2DExtrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 2D 激光雷达到 base_link 外参标定模板。 + fill_common_result(input, output); + output.response.result.message = "2d lidar extrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "lidar_2d_extrinsic_template_v1"; + return true; +} + +bool Lidar3DExtrinsicCalibrationAlgorithm::run( + const SensorCalibrationInput & input, + SensorCalibrationOutput & output, + std::string & failure_reason) const +{ + (void)failure_reason; + + // 3D 激光雷达到 base_link 外参标定模板。 + fill_common_result(input, output); + output.response.result.message = "3d lidar extrinsic calibration template executed."; + output.response.result.recommended_parameter_version = "lidar_3d_extrinsic_template_v1"; + return true; +} + +} // namespace sensor_calibration_service diff --git a/agv_calib_brain/src/agv_calib_core/src/brain_node.cpp b/agv_calib_brain/src/agv_calib_core/src/brain_node.cpp deleted file mode 100644 index 5fa85d8..0000000 --- a/agv_calib_brain/src/agv_calib_core/src/brain_node.cpp +++ /dev/null @@ -1,87 +0,0 @@ -#include "agv_calib_core/brain_node.hpp" -#include "agv_calib_core/bt_ros2_nodes.hpp" - -#include -#include -#include -#include - -namespace agv_calib_core { - -BrainNode::BrainNode(const rclcpp::NodeOptions & options) -: Node("brain_node", options), is_running_(false) { - - RCLCPP_INFO(this->get_logger(), "👑 AGV 标定中央大脑 (Component 版) 正在挂载..."); - - // 1. 从 YAML 配置文件读取动态参数！绝不硬编码！ - this->declare_parameter("tree_xml_filename", "main_tree.xml"); - this->declare_parameter("tick_rate_ms", 50); - - // 2. 启动专属的后台守护线程去运行行为树。将主线程交还给容器处理网络回调！ - is_running_ = true; - bt_thread_ = std::thread(&BrainNode::execute_behavior_tree, this); -} - -BrainNode::~BrainNode() { - is_running_ = false; - if (bt_thread_.joinable()) { - bt_thread_.join(); - } -} - -void BrainNode::execute_behavior_tree() { - // 稍微延时 0.5 秒，确保节点完全被容器接管，再发起 Client 寻址 - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - - BT::BehaviorTreeFactory factory; - - // 注册业务积木，传递 this 裸指针给所有积木 - factory.registerBuilder("SetChassisMode", - [this](const std::string& name, const BT::NodeConfiguration& config) { - return std::make_unique(name, config, this); - }); - - factory.registerBuilder("TriggerCapture", - [this](const std::string& name, const BT::NodeConfiguration& config) { - return std::make_unique(name, config, this); - }); - - factory.registerBuilder("DownloadData", - [this](const std::string& name, const BT::NodeConfiguration& config) { - return std::make_unique(name, config, this); - }); - - try { - std::string xml_filename = this->get_parameter("tree_xml_filename").as_string(); - int tick_rate = this->get_parameter("tick_rate_ms").as_int(); - - std::string pkg_path = ament_index_cpp::get_package_share_directory("agv_calib_core"); - std::string xml_file = pkg_path + "/behavior_trees/" + xml_filename; - - auto tree = factory.createTreeFromFile(xml_file); - BT::StdCoutLogger logger_cout(tree); - - RCLCPP_INFO(this->get_logger(), "📜 XML 剧本 [%s] 加载完毕，开始全自动流水线...", xml_filename.c_str()); - - // 按照 YAML 配置的频率持续 Tick - BT::NodeStatus status = BT::NodeStatus::RUNNING; - while (rclcpp::ok() && is_running_ && status == BT::NodeStatus::RUNNING) { - status = tree.tickRoot(); - std::this_thread::sleep_for(std::chrono::milliseconds(tick_rate)); - } - - if (status == BT::NodeStatus::SUCCESS) { - RCLCPP_INFO(this->get_logger(), "🎉 标定流水线全流程完美结束！"); - } else { - RCLCPP_WARN(this->get_logger(), "⚠️ 流水线未成功完成 (可能被中止)。"); - } - - } catch (const std::exception& e) { - RCLCPP_ERROR(this->get_logger(), "❌ 行为树崩溃: %s", e.what()); - } -} - -} // namespace agv_calib_core - -// 🚨 终极一步：将该类注册为 ROS 2 Component (插件)！ -RCLCPP_COMPONENTS_REGISTER_NODE(agv_calib_core::BrainNode) \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2.py b/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2.py deleted file mode 100644 index edaf732..0000000 --- a/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2.py +++ /dev/null @@ -1,56 +0,0 @@ -# -*- coding: utf-8 -*- -# Generated by the protocol buffer compiler. DO NOT EDIT! -# NO CHECKED-IN PROTOBUF GENCODE -# source: agv_calib_control.proto -# Protobuf Python Version: 6.31.1 -"""Generated protocol buffer code.""" -from google.protobuf import descriptor as _descriptor -from google.protobuf import descriptor_pool as _descriptor_pool -from google.protobuf import runtime_version as _runtime_version -from google.protobuf import symbol_database as _symbol_database -from google.protobuf.internal import builder as _builder -_runtime_version.ValidateProtobufRuntimeVersion( - _runtime_version.Domain.PUBLIC, - 6, - 31, - 1, - '', - 'agv_calib_control.proto' -) -# @@protoc_insertion_point(imports) - -_sym_db = _symbol_database.Default() - - - - -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x17\x61gv_calib_control.proto\x12\x17\x61gv.calibration.control\"\x07\n\x05\x45mpty\"4\n\x10StandardResponse\x12\x0f\n\x07success\x18\x01 \x01(\x08\x12\x0f\n\x07message\x18\x02 \x01(\t\"\x8b\x01\n\x0bModeRequest\x12>\n\x0btarget_mode\x18\x01 \x01(\x0e\x32).agv.calibration.control.ModeRequest.Mode\"<\n\x04Mode\x12\x0f\n\x0bNORMAL_MODE\x10\x00\x12\x12\n\x0eOPEN_LOOP_MODE\x10\x01\x12\x0f\n\x0bTUNING_MODE\x10\x02\"p\n\x0fOpenLoopRequest\x12\x16\n\x0eleft_motor_cmd\x18\x01 \x01(\x01\x12\x17\n\x0fright_motor_cmd\x18\x02 \x01(\x01\x12\x16\n\x0esteering_angle\x18\x03 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x04 \x01(\x01\"h\n\x0fTrajectoryPoint\x12\x0b\n\x03x_m\x18\x01 \x01(\x01\x12\x0b\n\x03y_m\x18\x02 \x01(\x01\x12\x0f\n\x07yaw_rad\x18\x03 \x01(\x01\x12\x17\n\x0ftarget_speed_ms\x18\x04 \x01(\x01\x12\x11\n\tcurvature\x18\x05 \x01(\x01\"a\n\x11TrajectoryRequest\x12\x14\n\x0ctest_case_id\x18\x01 \x01(\t\x12\x36\n\x04path\x18\x02 \x03(\x0b\x32(.agv.calibration.control.TrajectoryPoint\"G\n\x13StepResponseRequest\x12\x1a\n\x12target_velocity_ms\x18\x01 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x02 \x01(\x01\"\xf9\x05\n\rControlParams\x12$\n\x17wheel_radius_left_ratio\x18\x01 \x01(\x01H\x00\x88\x01\x01\x12%\n\x18wheel_radius_right_ratio\x18\x02 \x01(\x01H\x01\x88\x01\x01\x12$\n\x17\x65\x66\x66\x65\x63tive_track_width_m\x18\x03 \x01(\x01H\x02\x88\x01\x01\x12%\n\x18steering_zero_offset_deg\x18\x04 \x01(\x01H\x03\x88\x01\x01\x12\x1b\n\x0epid_kp_lateral\x18\x05 \x01(\x01H\x04\x88\x01\x01\x12\x1b\n\x0epid_ki_lateral\x18\x06 \x01(\x01H\x05\x88\x01\x01\x12\x1b\n\x0epid_kd_lateral\x18\x07 \x01(\x01H\x06\x88\x01\x01\x12\x1b\n\x0epid_kp_heading\x18\x08 \x01(\x01H\x07\x88\x01\x01\x12\x1b\n\x0epid_ki_heading\x18\t \x01(\x01H\x08\x88\x01\x01\x12\x1b\n\x0epid_kd_heading\x18\n \x01(\x01H\t\x88\x01\x01\x12%\n\x18pure_pursuit_lookahead_m\x18\x0b \x01(\x01H\n\x88\x01\x01\x12!\n\x14mpc_weight_q_lateral\x18\x0c \x01(\x01H\x0b\x88\x01\x01\x12\"\n\x15mpc_weight_r_steering\x18\r \x01(\x01H\x0c\x88\x01\x01\x42\x1a\n\x18_wheel_radius_left_ratioB\x1b\n\x19_wheel_radius_right_ratioB\x1a\n\x18_effective_track_width_mB\x1b\n\x19_steering_zero_offset_degB\x11\n\x0f_pid_kp_lateralB\x11\n\x0f_pid_ki_lateralB\x11\n\x0f_pid_kd_lateralB\x11\n\x0f_pid_kp_headingB\x11\n\x0f_pid_ki_headingB\x11\n\x0f_pid_kd_headingB\x1b\n\x19_pure_pursuit_lookahead_mB\x17\n\x15_mpc_weight_q_lateralB\x18\n\x16_mpc_weight_r_steering\"\xad\x02\n\rTelemetryData\x12\x1d\n\x15hardware_timestamp_us\x18\x01 \x01(\x03\x12\x10\n\x08odom_x_m\x18\x02 \x01(\x01\x12\x10\n\x08odom_y_m\x18\x03 \x01(\x01\x12\x14\n\x0codom_yaw_rad\x18\x04 \x01(\x01\x12\x1e\n\x16\x66\x65\x65\x64\x62\x61\x63k_linear_vel_ms\x18\x05 \x01(\x01\x12!\n\x19\x66\x65\x65\x64\x62\x61\x63k_angular_vel_rads\x18\x06 \x01(\x01\x12\x1e\n\x16left_motor_current_amp\x18\x07 \x01(\x01\x12\x1f\n\x17right_motor_current_amp\x18\x08 \x01(\x01\x12\"\n\x1asteering_motor_current_amp\x18\t \x01(\x01\x12\x1b\n\x13\x63md_steering_output\x18\n \x01(\x01\x32\xd1\x06\n\x16\x41gvCalibControlService\x12\x61\n\x0eSetControlMode\x12$.agv.calibration.control.ModeRequest\x1a).agv.calibration.control.StandardResponse\x12Z\n\rEmergencyStop\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12i\n\x12\x45xecuteOpenLoopCmd\x12(.agv.calibration.control.OpenLoopRequest\x1a).agv.calibration.control.StandardResponse\x12m\n\x14\x46ollowTestTrajectory\x12*.agv.calibration.control.TrajectoryRequest\x1a).agv.calibration.control.StandardResponse\x12n\n\x13\x45xecuteStepResponse\x12,.agv.calibration.control.StepResponseRequest\x1a).agv.calibration.control.StandardResponse\x12k\n\x16InjectTuningParameters\x12&.agv.calibration.control.ControlParams\x1a).agv.calibration.control.StandardResponse\x12\x64\n\x17\x43ommitControlParameters\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12[\n\x0fStreamTelemetry\x12\x1e.agv.calibration.control.Empty\x1a&.agv.calibration.control.TelemetryData0\x01\x62\x06proto3') - -_globals = globals() -_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals) -_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'agv_calib_control_pb2', _globals) -if not _descriptor._USE_C_DESCRIPTORS: - DESCRIPTOR._loaded_options = None - _globals['_EMPTY']._serialized_start=52 - _globals['_EMPTY']._serialized_end=59 - _globals['_STANDARDRESPONSE']._serialized_start=61 - _globals['_STANDARDRESPONSE']._serialized_end=113 - _globals['_MODEREQUEST']._serialized_start=116 - _globals['_MODEREQUEST']._serialized_end=255 - _globals['_MODEREQUEST_MODE']._serialized_start=195 - _globals['_MODEREQUEST_MODE']._serialized_end=255 - _globals['_OPENLOOPREQUEST']._serialized_start=257 - _globals['_OPENLOOPREQUEST']._serialized_end=369 - _globals['_TRAJECTORYPOINT']._serialized_start=371 - _globals['_TRAJECTORYPOINT']._serialized_end=475 - _globals['_TRAJECTORYREQUEST']._serialized_start=477 - _globals['_TRAJECTORYREQUEST']._serialized_end=574 - _globals['_STEPRESPONSEREQUEST']._serialized_start=576 - _globals['_STEPRESPONSEREQUEST']._serialized_end=647 - _globals['_CONTROLPARAMS']._serialized_start=650 - _globals['_CONTROLPARAMS']._serialized_end=1411 - _globals['_TELEMETRYDATA']._serialized_start=1414 - _globals['_TELEMETRYDATA']._serialized_end=1715 - _globals['_AGVCALIBCONTROLSERVICE']._serialized_start=1718 - _globals['_AGVCALIBCONTROLSERVICE']._serialized_end=2567 -# @@protoc_insertion_point(module_scope) diff --git a/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2_grpc.py b/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2_grpc.py deleted file mode 100644 index 5dd44cb..0000000 --- a/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2_grpc.py +++ /dev/null @@ -1,444 +0,0 @@ -# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT! -"""Client and server classes corresponding to protobuf-defined services.""" -import grpc -import warnings - -import agv_calib_control_pb2 as agv__calib__control__pb2 - -GRPC_GENERATED_VERSION = '1.78.0' -GRPC_VERSION = grpc.__version__ -_version_not_supported = False - -try: - from grpc._utilities import first_version_is_lower - _version_not_supported = first_version_is_lower(GRPC_VERSION, GRPC_GENERATED_VERSION) -except ImportError: - _version_not_supported = True - -if _version_not_supported: - raise RuntimeError( - f'The grpc package installed is at version {GRPC_VERSION},' - + ' but the generated code in agv_calib_control_pb2_grpc.py depends on' - + f' grpcio>={GRPC_GENERATED_VERSION}.' - + f' Please upgrade your grpc module to grpcio>={GRPC_GENERATED_VERSION}' - + f' or downgrade your generated code using grpcio-tools<={GRPC_VERSION}.' - ) - - -class AgvCalibControlServiceStub(object): - """========================================================= - 核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理 - [部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报) - [调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数) - ========================================================= - """ - - def __init__(self, channel): - """Constructor. - - Args: - channel: A grpc.Channel. - """ - self.SetControlMode = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/SetControlMode', - request_serializer=agv__calib__control__pb2.ModeRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.EmergencyStop = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/EmergencyStop', - request_serializer=agv__calib__control__pb2.Empty.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.ExecuteOpenLoopCmd = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd', - request_serializer=agv__calib__control__pb2.OpenLoopRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.FollowTestTrajectory = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory', - request_serializer=agv__calib__control__pb2.TrajectoryRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.ExecuteStepResponse = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse', - request_serializer=agv__calib__control__pb2.StepResponseRequest.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.InjectTuningParameters = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/InjectTuningParameters', - request_serializer=agv__calib__control__pb2.ControlParams.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.CommitControlParameters = channel.unary_unary( - '/agv.calibration.control.AgvCalibControlService/CommitControlParameters', - request_serializer=agv__calib__control__pb2.Empty.SerializeToString, - response_deserializer=agv__calib__control__pb2.StandardResponse.FromString, - _registered_method=True) - self.StreamTelemetry = channel.unary_stream( - '/agv.calibration.control.AgvCalibControlService/StreamTelemetry', - request_serializer=agv__calib__control__pb2.Empty.SerializeToString, - response_deserializer=agv__calib__control__pb2.TelemetryData.FromString, - _registered_method=True) - - -class AgvCalibControlServiceServicer(object): - """========================================================= - 核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理 - [部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报) - [调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数) - ========================================================= - """ - - def SetControlMode(self, request, context): - """--------------------------------------------------------- - 第一步：权限接管与生命周期安全管控 - --------------------------------------------------------- - 💻 [Linux 发送 -> Windows]：要求切断避障，但保留底层 PID/MPC 算法就绪 - 🚙 [Windows 返回 -> Linux]：回复模式切换成功，准备好接考题 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def EmergencyStop(self, request, context): - """💻 [Linux 发送 -> Windows]：断网或飞车时的最高级别急停，无视一切直接刹车 - 🚙 [Windows 返回 -> Linux]：返回底层抱死结果 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def ExecuteOpenLoopCmd(self, request, context): - """--------------------------------------------------------- - 第二步：运动考题下发 (开环排雷 / 闭环寻优 / 波峰对齐) - --------------------------------------------------------- - 【场景A: 纯物理开环备用】 - 💻 [Linux 发送 -> Windows]：要求切断算法盲跑，多用于摸底或辅助验证 - 🚙 [Windows 返回 -> Linux]：确认已按指定 RPM/PWM 运转 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def FollowTestTrajectory(self, request, context): - """【场景B: 算法闭环调优】 - 💻 [Linux 发送 -> Windows]：下发一条由几百个点组成的测试轨迹(如 S型贝塞尔曲线) - 🚙 [Windows 返回 -> Linux]：收到轨迹后，车端立刻使用它自带的 PID/MPC 算法努力贴合轨迹跑圈 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def ExecuteStepResponse(self, request, context): - """【场景C: 波峰时序对齐】 - 💻 [Linux 发送 -> Windows]：下发极短促的阶跃加速指令，人为制造绝对速度波峰 - 🚙 [Windows 返回 -> Linux]：确认加速。(Linux 借此波峰算出网络的绝对 Time Offset) - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def InjectTuningParameters(self, request, context): - """--------------------------------------------------------- - 第三步：运控参数 AI 寻优：动态热注入与最终固化 - --------------------------------------------------------- - 💻 [Linux 发送 -> Windows]：Linux 发现上一圈跑得差，AI算出了新的 PID/前瞻距离，要求立即热注入 - 🚙 [Windows 返回 -> Linux]：车端将新参数瞬间覆写进运行内存(不重启系统)，随时准备用新参数重跑 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def CommitControlParameters(self, request, context): - """💻 [Linux 发送 -> Windows]：Linux 判定误差极小，调优结束，命令固化目前内存里的最高分参数 - 🚙 [Windows 返回 -> Linux]：车端将这组完美参数永久覆写进硬盘的 config.yaml 或系统注册表 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - def StreamTelemetry(self, request, context): - """--------------------------------------------------------- - 第四步：高频数字孪生体感上报 (50Hz) - --------------------------------------------------------- - 💻 [Linux 发送 -> Windows]：空包触发，命令车端开始疯狂推流 - 🚙 [Windows 持续流式返回 -> Linux]：以 50Hz 频率，持续上报自己的里程计坐标、速度和单调时间戳 - """ - context.set_code(grpc.StatusCode.UNIMPLEMENTED) - context.set_details('Method not implemented!') - raise NotImplementedError('Method not implemented!') - - -def add_AgvCalibControlServiceServicer_to_server(servicer, server): - rpc_method_handlers = { - 'SetControlMode': grpc.unary_unary_rpc_method_handler( - servicer.SetControlMode, - request_deserializer=agv__calib__control__pb2.ModeRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'EmergencyStop': grpc.unary_unary_rpc_method_handler( - servicer.EmergencyStop, - request_deserializer=agv__calib__control__pb2.Empty.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'ExecuteOpenLoopCmd': grpc.unary_unary_rpc_method_handler( - servicer.ExecuteOpenLoopCmd, - request_deserializer=agv__calib__control__pb2.OpenLoopRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'FollowTestTrajectory': grpc.unary_unary_rpc_method_handler( - servicer.FollowTestTrajectory, - request_deserializer=agv__calib__control__pb2.TrajectoryRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'ExecuteStepResponse': grpc.unary_unary_rpc_method_handler( - servicer.ExecuteStepResponse, - request_deserializer=agv__calib__control__pb2.StepResponseRequest.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'InjectTuningParameters': grpc.unary_unary_rpc_method_handler( - servicer.InjectTuningParameters, - request_deserializer=agv__calib__control__pb2.ControlParams.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'CommitControlParameters': grpc.unary_unary_rpc_method_handler( - servicer.CommitControlParameters, - request_deserializer=agv__calib__control__pb2.Empty.FromString, - response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString, - ), - 'StreamTelemetry': grpc.unary_stream_rpc_method_handler( - servicer.StreamTelemetry, - request_deserializer=agv__calib__control__pb2.Empty.FromString, - response_serializer=agv__calib__control__pb2.TelemetryData.SerializeToString, - ), - } - generic_handler = grpc.method_handlers_generic_handler( - 'agv.calibration.control.AgvCalibControlService', rpc_method_handlers) - server.add_generic_rpc_handlers((generic_handler,)) - server.add_registered_method_handlers('agv.calibration.control.AgvCalibControlService', rpc_method_handlers) - - - # This class is part of an EXPERIMENTAL API. -class AgvCalibControlService(object): - """========================================================= - 核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理 - [部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报) - [调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数) - ========================================================= - """ - - @staticmethod - def SetControlMode(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/SetControlMode', - agv__calib__control__pb2.ModeRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def EmergencyStop(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/EmergencyStop', - agv__calib__control__pb2.Empty.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def ExecuteOpenLoopCmd(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd', - agv__calib__control__pb2.OpenLoopRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def FollowTestTrajectory(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory', - agv__calib__control__pb2.TrajectoryRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def ExecuteStepResponse(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse', - agv__calib__control__pb2.StepResponseRequest.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def InjectTuningParameters(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/InjectTuningParameters', - agv__calib__control__pb2.ControlParams.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def CommitControlParameters(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_unary( - request, - target, - '/agv.calibration.control.AgvCalibControlService/CommitControlParameters', - agv__calib__control__pb2.Empty.SerializeToString, - agv__calib__control__pb2.StandardResponse.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) - - @staticmethod - def StreamTelemetry(request, - target, - options=(), - channel_credentials=None, - call_credentials=None, - insecure=False, - compression=None, - wait_for_ready=None, - timeout=None, - metadata=None): - return grpc.experimental.unary_stream( - request, - target, - '/agv.calibration.control.AgvCalibControlService/StreamTelemetry', - agv__calib__control__pb2.Empty.SerializeToString, - agv__calib__control__pb2.TelemetryData.FromString, - options, - channel_credentials, - insecure, - call_credentials, - compression, - wait_for_ready, - timeout, - metadata, - _registered_method=True) diff --git a/agv_calib_brain/src/agv_calib_core/test/mock_agv_server.py b/agv_calib_brain/src/agv_calib_core/test/mock_agv_server.py deleted file mode 100644 index b7f04bd..0000000 --- a/agv_calib_brain/src/agv_calib_core/test/mock_agv_server.py +++ /dev/null @@ -1,26 +0,0 @@ -import grpc -from concurrent import futures -import time -import agv_calib_control_pb2 as pb2 -import agv_calib_control_pb2_grpc as pb2_grpc - -# 扮演 Windows 车端的角色 -class FakeAgvServer(pb2_grpc.AgvCalibControlServiceServicer): - def SetControlMode(self, request, context): - print(f"\n[🚙 Windows 假车端] 收到 Linux 夺权指令! 目标模式: {request.target_mode}") - time.sleep(0.5) # 假装底层继电器切换花了点时间 - print("[🚙 Windows 假车端] 避障已切断，乖乖交出控制权！") - - # 返回成功回执给 Linux - return pb2.StandardResponse(success=True, message="Windows: 已交出底盘控制权！") - -def serve(): - server = grpc.server(futures.ThreadPoolExecutor(max_workers=10)) - pb2_grpc.add_AgvCalibControlServiceServicer_to_server(FakeAgvServer(), server) - server.add_insecure_port('[::]:50051') - print("🚀 [Windows 假车端] 已启动，正在监听 50051 端口，等待 Linux 大脑连接...") - server.start() - server.wait_for_termination() - -if __name__ == '__main__': - serve() \ No newline at end of file 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 new file mode 100644 index 0000000..5b647ab --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_agent_windows/docs/protocol.md @@ -0,0 +1,304 @@ +# Vehicle Agent Windows — Ubuntu 通信协议文档 + +## 1. 总体架构 + +``` +┌─────────────────────────────────┐ TCP 长连接（每次请求建立/断开） +│ Ubuntu 车间电脑 │ ──────────────────────────────────────► +│ vehicle_agent_gateway │ +│ ├─ ChassisBridgeNode :9001 │ ◄────────────────────────────────────── +│ └─ ControlBridgeNode :9002 │ +└─────────────────────────────────┘ + +┌─────────────────────────────────┐ +│ Windows 车端电脑 │ +│ vehicle_agent_windows │ +│ ├─ ChassisDomainServer :9001 │ +│ └─ ControlDomainServer :9002 │ +└─────────────────────────────────┘ +``` + +- Ubuntu 侧**主动发起**每次连接，发完请求等响应后关闭 socket。 +- Windows 侧持续监听两个 TCP 端口，每次连接处理一条请求后可关闭或保持，均可。 +- 两个端口完全独立，可以用同一进程的两个线程分别监听。 + +--- + +## 2. 帧格式（Frame Protocol） + +所有消息使用统一的二进制帧封装，**与业务域无关**： + +``` + ┌──────────────┬──────────────┬──────────────────────────┐ + │ msg_type │ payload_len │ payload │ + │ 4 bytes LE │ 4 bytes LE │ payload_len bytes │ + └──────────────┴──────────────┴──────────────────────────┘ +``` + +| 字段 | 类型 | 说明 | +|------|------|------| +| `msg_type` | uint32 小端 | 消息类型枚举值（见第3节）| +| `payload_len` | uint32 小端 | payload 字节数，可为 0 | +| `payload` | UTF-8 JSON | 业务数据，格式见第4、5节 | + +**注意**： +- 若 `payload_len == 0`，则 payload 段缺省，不发送任何额外字节。 +- payload 为 **UTF-8 JSON 字符串**，不含 BOM，不含换行，紧凑格式（不强制，解析时容忍空白）。 + +Python 读帧示例： +```python +import struct, socket + +def read_frame(conn): + header = recv_exactly(conn, 8) + msg_type, payload_len = struct.unpack_from(' 0 else b'' + return msg_type, payload.decode('utf-8') + +def send_frame(conn, msg_type: int, payload: str): + data = payload.encode('utf-8') + conn.sendall(struct.pack(' + $ +) + +ament_target_dependencies(vehicle_profile_manager_component + rclcpp + rclcpp_components + calibration_common_interfaces + calibration_vehicle_profile_interfaces +) + +rclcpp_components_register_nodes(vehicle_profile_manager_component + "vehicle_profile_manager::VehicleProfileManagerNode" +) + +# 可执行：独立进程启动 +add_executable(vehicle_profile_manager_node src/main.cpp) +ament_target_dependencies(vehicle_profile_manager_node + rclcpp + rclcpp_components +) +target_link_libraries(vehicle_profile_manager_node + vehicle_profile_manager_component +) + +install(TARGETS + vehicle_profile_manager_component + vehicle_profile_manager_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +ament_package() diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/include/vehicle_profile_manager/vehicle_profile_manager_node.hpp b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/include/vehicle_profile_manager/vehicle_profile_manager_node.hpp new file mode 100644 index 0000000..dc62997 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/include/vehicle_profile_manager/vehicle_profile_manager_node.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp" +#include "calibration_vehicle_profile_interfaces/srv/evaluate_vehicle_calibration_applicability.hpp" +#include "calibration_vehicle_profile_interfaces/srv/get_vehicle_profile.hpp" +#include "calibration_vehicle_profile_interfaces/srv/heartbeat.hpp" +#include "calibration_vehicle_profile_interfaces/srv/register_or_update_vehicle_profile.hpp" + +namespace vehicle_profile_manager +{ + +// 车辆画像管理节点： +// 1. 内存存储 vehicle_id → VehicleProfile 映射。 +// 2. 提供 get_vehicle_profile / register_or_update_vehicle_profile / evaluate_applicability / heartbeat 四个服务。 +// 3. 启动时预载一份 demo 车辆画像，供本地联调使用。 +class VehicleProfileManagerNode : public rclcpp::Node +{ +public: + explicit VehicleProfileManagerNode( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using GetProfileSrv = calibration_vehicle_profile_interfaces::srv::GetVehicleProfile; + using RegisterSrv = calibration_vehicle_profile_interfaces::srv::RegisterOrUpdateVehicleProfile; + using ApplicabilitySrv = calibration_vehicle_profile_interfaces::srv::EvaluateVehicleCalibrationApplicability; + using HeartbeatSrv = calibration_vehicle_profile_interfaces::srv::Heartbeat; + using VehicleProfile = calibration_vehicle_profile_interfaces::msg::VehicleProfile; + using WorkflowStageType = calibration_vehicle_profile_interfaces::msg::WorkflowStageType; + + rclcpp::Service::SharedPtr get_profile_service_; + rclcpp::Service::SharedPtr register_service_; + rclcpp::Service::SharedPtr applicability_service_; + rclcpp::Service::SharedPtr heartbeat_service_; + + // 内存存储：vehicle_id → 完整车辆画像 + std::unordered_map profiles_; + + void handle_get_profile( + const std::shared_ptr request, + std::shared_ptr response); + + void handle_register( + const std::shared_ptr request, + std::shared_ptr response); + + void handle_applicability( + const std::shared_ptr request, + std::shared_ptr response); + + void handle_heartbeat( + const std::shared_ptr request, + std::shared_ptr response); + + // 根据车辆画像内容推断支持哪些工作流阶段。 + std::vector evaluate_supported_stages( + const VehicleProfile & profile) const; + + // 预载 demo 车辆画像，vehicle_id = "demo_agv_001"。 + void load_demo_profile(); +}; + +} // namespace vehicle_profile_manager diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/package.xml b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/package.xml new file mode 100644 index 0000000..4480f54 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/package.xml @@ -0,0 +1,22 @@ + + + vehicle_profile_manager + 0.0.1 + Vehicle profile manager service for automated calibration workshop. + you + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_components + calibration_common_interfaces + calibration_vehicle_profile_interfaces + + launch + launch_ros + + + ament_cmake + + diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/src/main.cpp b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/src/main.cpp new file mode 100644 index 0000000..82737b1 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/src/main.cpp @@ -0,0 +1,12 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "vehicle_profile_manager/vehicle_profile_manager_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/src/vehicle_profile_manager_node.cpp b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/src/vehicle_profile_manager_node.cpp new file mode 100644 index 0000000..96e9f6a --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/vehicle_profile_manager/src/vehicle_profile_manager_node.cpp @@ -0,0 +1,180 @@ +#include "vehicle_profile_manager/vehicle_profile_manager_node.hpp" + +#include + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_vehicle_profile_interfaces/msg/chassis_type.hpp" +#include "calibration_vehicle_profile_interfaces/msg/workflow_stage_type.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace vehicle_profile_manager +{ + +using calibration_common_interfaces::msg::ErrorCode; +using calibration_vehicle_profile_interfaces::msg::ChassisType; +using calibration_vehicle_profile_interfaces::msg::WorkflowStageType; + +VehicleProfileManagerNode::VehicleProfileManagerNode(const rclcpp::NodeOptions & options) +: Node("vehicle_profile_manager", options) +{ + get_profile_service_ = create_service( + "/vehicle_profile_manager/get_vehicle_profile", + std::bind( + &VehicleProfileManagerNode::handle_get_profile, this, + std::placeholders::_1, std::placeholders::_2)); + + register_service_ = create_service( + "/vehicle_profile_manager/register_or_update_vehicle_profile", + std::bind( + &VehicleProfileManagerNode::handle_register, this, + std::placeholders::_1, std::placeholders::_2)); + + applicability_service_ = create_service( + "/vehicle_profile_manager/evaluate_vehicle_calibration_applicability", + std::bind( + &VehicleProfileManagerNode::handle_applicability, this, + std::placeholders::_1, std::placeholders::_2)); + + heartbeat_service_ = create_service( + "/vehicle_profile_manager/heartbeat", + std::bind( + &VehicleProfileManagerNode::handle_heartbeat, this, + std::placeholders::_1, std::placeholders::_2)); + + load_demo_profile(); + + RCLCPP_INFO(get_logger(), "VehicleProfileManagerNode 启动，已预载 demo 车辆画像。"); +} + +void VehicleProfileManagerNode::handle_get_profile( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto & vehicle_id = request->request.vehicle_id; + auto it = profiles_.find(vehicle_id); + if (it == profiles_.end()) { + response->response.success = false; + response->response.error_code.code = ErrorCode::INVALID_ARGUMENT; + response->response.message = "找不到 vehicle_id=[" + vehicle_id + "] 的车辆画像。"; + return; + } + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "查询成功。"; + response->response.profile = it->second; +} + +void VehicleProfileManagerNode::handle_register( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto & vehicle_id = request->request.profile.base_info.vehicle_id; + if (vehicle_id.empty()) { + response->response.success = false; + response->response.error_code.code = ErrorCode::INVALID_ARGUMENT; + response->response.message = "vehicle_id 不能为空。"; + return; + } + profiles_[vehicle_id] = request->request.profile; + RCLCPP_INFO(get_logger(), "已注册/更新车辆画像 vehicle_id=[%s]", vehicle_id.c_str()); + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "注册/更新成功。"; +} + +void VehicleProfileManagerNode::handle_applicability( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto & profile = request->request.profile_snapshot; + auto stages = evaluate_supported_stages(profile); + + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "适用性评估完成。"; + response->response.overall_supported = !stages.empty(); + response->response.recommended_workflow_stages = stages; +} + +void VehicleProfileManagerNode::handle_heartbeat( + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + const auto now_us = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + response->response.success = true; + response->response.error_code.code = ErrorCode::OK; + response->response.message = "vehicle_profile_manager 在线。"; + response->response.server_timestamp_us = now_us; + response->response.vehicle_ready = true; +} + +std::vector +VehicleProfileManagerNode::evaluate_supported_stages(const VehicleProfile & profile) const +{ + std::vector stages; + + auto make_stage = [](uint8_t v) { + WorkflowStageType s; + s.value = v; + return s; + }; + + // 预检和画像校验始终支持。 + stages.push_back(make_stage(WorkflowStageType::PROFILE_VALIDATION_STAGE)); + stages.push_back(make_stage(WorkflowStageType::WORKSHOP_PRECHECK_STAGE)); + + // 底盘标定：底盘类型已指定时支持。 + if (profile.chassis_type.value != ChassisType::CHASSIS_TYPE_UNSPECIFIED) { + stages.push_back(make_stage(WorkflowStageType::CHASSIS_CALIBRATION_STAGE)); + stages.push_back(make_stage(WorkflowStageType::CONTROL_CALIBRATION_STAGE)); + } + + // 传感器标定：有传感器配置时支持。 + if (!profile.sensors.empty()) { + stages.push_back(make_stage(WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE)); + stages.push_back(make_stage(WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE)); + } + + // 手眼标定：有机械臂且有传感器时支持。 + if (profile.arm_profile.has_mechanical_arm && !profile.sensors.empty()) { + stages.push_back(make_stage(WorkflowStageType::HAND_EYE_CALIBRATION_STAGE)); + } + + // 最终阶段始终加入。 + stages.push_back(make_stage(WorkflowStageType::PARAMETER_COMMIT_STAGE)); + stages.push_back(make_stage(WorkflowStageType::REPORT_ARCHIVE_STAGE)); + + return stages; +} + +void VehicleProfileManagerNode::load_demo_profile() +{ + VehicleProfile demo; + + // 基础信息 + demo.base_info.vehicle_id = "demo_agv_001"; + demo.base_info.vehicle_name = "Demo AGV"; + demo.base_info.model_name = "DemoModel-X1"; + demo.base_info.manufacturer = "Demo Manufacturer"; + + // 底盘类型：差速 + demo.chassis_type.value = ChassisType::DIFFERENTIAL; + + // base_link + demo.base_link_frame = "base_link"; + + // 画像版本 + demo.profile_version = "demo_v1"; + + // 启用的工作流阶段 + demo.enabled_workflow_stages = evaluate_supported_stages(demo); + + profiles_[demo.base_info.vehicle_id] = demo; + RCLCPP_INFO(get_logger(), "已预载 demo 车辆画像 vehicle_id=[%s]", + demo.base_info.vehicle_id.c_str()); +} + +} // namespace vehicle_profile_manager + +RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_profile_manager::VehicleProfileManagerNode) diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/ANNOTATION_NOTES.md b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/ANNOTATION_NOTES.md new file mode 100644 index 0000000..861bf1e --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/ANNOTATION_NOTES.md @@ -0,0 +1,2 @@ +这是一份能直接下载的中文具体作用版。 +重点已重写 types.hpp / workshop_orchestrator_v2_node.hpp / workshop_orchestrator_v2_node.cpp 的注释。 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/CMakeLists.txt new file mode 100644 index 0000000..7cc5eb4 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.8) +project(workshop_orchestrator_v2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) +find_package(calibration_workshop_orchestration_interfaces REQUIRED) +find_package(calibration_external_localization_interfaces REQUIRED) +find_package(calibration_chassis_interfaces REQUIRED) +find_package(calibration_control_interfaces REQUIRED) +find_package(calibration_sensor_interfaces REQUIRED) + +include_directories(include) + +add_executable(workshop_orchestrator_v2_node + src/main.cpp + src/plan_builder.cpp + src/precheck_runner.cpp + src/report_builder.cpp + src/external_localization_client.cpp + src/chassis_gateway_client.cpp + src/control_gateway_client.cpp + src/sensor_gateway_client.cpp + src/workshop_orchestrator_v2_node.cpp +) + +ament_target_dependencies(workshop_orchestrator_v2_node + rclcpp + rclcpp_action + std_msgs + calibration_common_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces + calibration_external_localization_interfaces + calibration_chassis_interfaces + calibration_control_interfaces + calibration_sensor_interfaces +) + +install(TARGETS workshop_orchestrator_v2_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +ament_package() diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/INTEGRATION_NOTES.md b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/INTEGRATION_NOTES.md new file mode 100644 index 0000000..484c60e --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/INTEGRATION_NOTES.md @@ -0,0 +1,11 @@ +把这个包接入你现在的仓库时，建议放到： + +`src/win_ubuntu_bridge/service/workshop_orchestrator_v2` + +如果你已经创建了同名包，可以直接把 `include/`、`src/`、`launch/` 里的对应文件合并进去。 + +这一版的核心变化不是新功能，而是： + +- `execute_session()` 只保留薄流程 +- 预检失败、取消、成功、单步执行都拆成独立函数 +- 下一步就可以把 `run_single_stage()` 改成真实 gateway 调用 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/MIGRATION_NOTES.md b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/MIGRATION_NOTES.md new file mode 100644 index 0000000..d50693e --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/MIGRATION_NOTES.md @@ -0,0 +1,25 @@ +# 这次是直接按“职责归位”拆出来的代码版本 + +## workshop_orchestrator_v2 保留 +- workshop_orchestrator_v2_node.* +- plan_builder.* +- precheck_runner.* +- report_builder.* +- types.hpp +- external_localization_client.* + +## 从 orchestrator 里迁出的含义 +- external_localization_gateway.* 不再保留在 orchestrator +- external 真值校核专项执行骨架，改放 external_localization_service/external_reference_executor.* + +## 你接下来本地替换时建议这样做 +1. 删除 orchestrator 包中的 external_localization_gateway.hpp/.cpp +2. 删除 orchestrator 包中的 task_inputs.hpp 及其引用 +3. 用这次目录里的 workshop_orchestrator_v2/workshop_orchestrator_v2_node.* 覆盖你的当前版本 +4. 新增 workshop_orchestrator_v2/external_localization_client.* +5. 在 external_localization_service 中新增 external_reference_executor.* + +## 这次仍然是“职责拆分版”，不是最终可编译联调版 +- orchestrator 侧已经从“gateway/专项逻辑”收缩成“薄 client” +- external 侧则有了对应目录下的专项任务执行骨架 +- 后续还要把 chassis / control / sensor 按同样原则继续归位 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/README.md b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/README.md new file mode 100644 index 0000000..870dc19 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/README.md @@ -0,0 +1,358 @@ +# workshop_orchestrator + +这是自动化标定车间的总编排说明文档。这个 README 需要跟着代码一起更新，用来保留当前流程设计、任务粒度和模块职责。 + +## 目标 + +总控只负责“编排”和“流程治理”，不直接承载专项标定细节。专项细节分别由独立模块完成： + +- 底盘标定 +- 运控参数标定 +- 传感器内参标定 +- 传感器外参标定 +- 手眼标定 +- 外部定位 / 真值接入 + +## 当前架构 + +### 1. 车间总控 + +`workshop_orchestrator_v2` 负责： + +- 创建会话 +- 生成执行计划 +- 启动整场执行 +- 暂停 / 恢复 / 取消 +- 人工确认 +- 阶段审批 +- 报告生成与查询 + +### 2. 任务选择方式 + +当前设计是“**由 UI 或上层显式选择要执行的阶段**”。 + +如果 `requested_tasks` 为空，总控会按车辆能力自动生成最小计划； +如果 `requested_tasks` 不为空，总控会优先按用户显式选择的阶段生成计划。 + +## 当前执行主线 + +`CreateSession -> BuildPlan -> Precheck -> Stage Execution -> Commit/Validation -> Report` + +其中阶段执行会按会话配置和车辆画像决定是否加入： + +- 外部参考接入 +- 底盘标定 +- 运控参数标定 +- 传感器内参标定 +- 传感器外参标定 +- 手眼标定 + +## 当前流程粒度 + +现在总控层已经支持“按阶段选择”，但细分方法仍由专项协议和专项服务承载。 + +### 最小联调闭环 + +当前仓库里可以先验证下面这条最小链路： + +1. 启动 `vehicle_profile_manager` +2. 启动 `external_localization_service` +3. 启动 `workshop_orchestrator_v2` +4. 创建会话并自动生成计划 +5. 触发 `execute_session` +6. 走完外部真值校核并生成报告 + +这条链路现在是最小可验证闭环，后续再逐步补 chassis / control / sensor 的服务端实现。 + + +## 按模块拆分后的代码位置 + +### 模块接口总表 + +| 模块 | 职责 | 输入 | 输出 | 实现位置 | +|---|---|---|---|---| +| 车间总控 `workshop_orchestrator_v2` | 管会话、生成计划、调度执行、处理人工确认/审批、生成报告 | `VehicleProfile`、`WorkshopSessionConfig`、`RequestedCalibrationTask`、`StagePlan`、`StageResultSummary` | `WorkshopSession`、`WorkshopPrecheckResponse`、`WorkshopReport`、`WorkshopEvent` | `src/agv_calib_core/workshop_orchestrator/src/main.cpp`
`src/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp`
`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp`
`src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` / `include/workshop_orchestrator_v2/plan_builder.hpp`
`src/agv_calib_core/workshop_orchestrator/src/precheck_runner.cpp` / `include/workshop_orchestrator_v2/precheck_runner.hpp`
`src/agv_calib_core/workshop_orchestrator/src/report_builder.cpp` / `include/workshop_orchestrator_v2/report_builder.hpp`
`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp` | +| 外部定位 / 真值接入 `external_localization_service` | goal 校验、外部参考接入、真值校核、结果回填 | `ExecuteExternalLocalizationTask::Goal`、`WorkshopSession`、`StagePlan` | `StageResultSummary`、`ExecuteExternalLocalizationTask::Result` | `src/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp`
`src/agv_calib_core/external_localization_service/src/external_reference_executor.cpp` | +| 底盘标定客户端 | readiness 检查、底盘动作原语任务下发、结果翻译 | `WorkshopSession`、`StagePlan` | `StageResultSummary`、底盘专项 goal / result | `src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/chassis_gateway_client.hpp`
`src/agv_calib_core/workshop_orchestrator/src/chassis_gateway_client.cpp` | +| 运控参数标定客户端 | readiness 检查、控制评估任务下发、轨迹组装、结果翻译 | `WorkshopSession`、`StagePlan`、`stage.metadata` 中的轨迹与参数 | `StageResultSummary`、运控专项 goal / result | `src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/control_gateway_client.hpp`
`src/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp` | +| 传感器标定客户端 | readiness 检查、传感器任务路由、结果翻译 | `WorkshopSession`、`StagePlan`、`stage.metadata` 中的传感器信息 | `StageResultSummary`、传感器专项 goal / result | `src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/sensor_gateway_client.hpp`
`src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp` | +| 外部定位客户端 | readiness 检查、外部参考接入任务下发、结果接入总控 | `WorkshopSession`、`StagePlan` | `StageResultSummary`、外部定位专项 goal / result | `src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/external_localization_client.hpp`
`src/agv_calib_core/workshop_orchestrator/src/external_localization_client.cpp` | + +### 1. 车间总控模块 `workshop_orchestrator_v2` + +**职责** +- 管会话生命周期：创建、查询、执行、暂停、恢复、报告 +- 按车辆画像和会话配置生成阶段计划 +- 调度各专项模块执行 +- 处理人工确认和审批 + +**输入** +- `VehicleProfile` +- `WorkshopSessionConfig` +- `RequestedCalibrationTask` +- `StagePlan` +- `StageResultSummary` + +**输出** +- `WorkshopSession` +- `WorkshopPrecheckResponse` +- `WorkshopReport` +- `WorkshopEvent` + +**实现位置** +- 节点入口：`src/agv_calib_core/workshop_orchestrator/src/main.cpp` +- 总控节点：`src/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp` +- 总控头文件：`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp` +- 阶段规划：`src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` / `include/workshop_orchestrator_v2/plan_builder.hpp` +- 预检逻辑：`src/agv_calib_core/workshop_orchestrator/src/precheck_runner.cpp` / `include/workshop_orchestrator_v2/precheck_runner.hpp` +- 报告生成：`src/agv_calib_core/workshop_orchestrator/src/report_builder.cpp` / `include/workshop_orchestrator_v2/report_builder.hpp` +- 共享类型：`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp` + +### 2. 外部定位 / 真值接入模块 `external_localization_service` + +**职责** +- 执行外部真值接入前的 goal 校验 +- 执行外部参考接入 / 真值校核任务 +- 把专项执行结果映射成总控可用的阶段结果 + +**输入** +- `ExecuteExternalLocalizationTask::Goal` +- `WorkshopSession` +- `StagePlan` + +**输出** +- `StageResultSummary` +- `ExecuteExternalLocalizationTask::Result` + +**实现位置** +- 执行器头文件：`src/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp` +- 执行器实现：`src/agv_calib_core/external_localization_service/src/external_reference_executor.cpp` + +### 3. 底盘标定客户端 + +**职责** +- 向底盘专项服务发起 readiness 检查 +- 发送底盘动作原语任务 +- 将底盘结果翻译成总控阶段结果 + +**输入** +- `WorkshopSession` +- `StagePlan` + +**输出** +- `StageResultSummary` +- 底盘专项 goal / result + +**实现位置** +- 头文件：`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/chassis_gateway_client.hpp` +- 实现：`src/agv_calib_core/workshop_orchestrator/src/chassis_gateway_client.cpp` + +### 4. 运控参数标定客户端 + +**职责** +- 向运控专项服务发起 readiness 检查 +- 发送控制评估任务 +- 从 `stage.metadata` 解析轨迹点并组装 goal +- 将控制结果翻译成总控阶段结果 + +**输入** +- `WorkshopSession` +- `StagePlan` +- `stage.metadata` 中的轨迹与参数信息 + +**输出** +- `StageResultSummary` +- 运控专项 goal / result + +**实现位置** +- 头文件：`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/control_gateway_client.hpp` +- 实现：`src/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp` + +### 5. 传感器标定客户端 + +**职责** +- 向传感器专项服务发起 readiness 检查 +- 处理相机内参、传感器外参、手眼标定等任务入口 +- 根据阶段与元数据选择具体传感器标定任务类型 +- 将传感器结果翻译成总控阶段结果 + +**输入** +- `WorkshopSession` +- `StagePlan` +- `stage.metadata` 中的传感器 ID、样本数、板卡 ID、基准坐标系等信息 + +**输出** +- `StageResultSummary` +- 传感器专项 goal / result + +**实现位置** +- 头文件：`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/sensor_gateway_client.hpp` +- 实现：`src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp` + +### 6. 外部定位客户端 + +**职责** +- 向外部定位专项服务发起 readiness 检查 +- 发送外部参考接入任务 +- 将外部定位结果交给总控执行链路 + +**输入** +- `WorkshopSession` +- `StagePlan` + +**输出** +- `StageResultSummary` +- 外部定位专项 goal / result + +**实现位置** +- 头文件：`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/external_localization_client.hpp` +- 实现：`src/agv_calib_core/workshop_orchestrator/src/external_localization_client.cpp` + +### 7. 外部定位专项执行骨架 + +**职责** +- 作为外部定位服务侧的专项执行器骨架 +- 封装 goal 校验和结果回填 +- 将专项逻辑从总控里剥离出去 + +**输入** +- `ExecuteExternalLocalizationTask::Goal` +- `ExecuteExternalLocalizationTask::Result` + +**输出** +- `StageResultSummary` + +**实现位置** +- 头文件：`src/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp` +- 实现：`src/agv_calib_core/external_localization_service/src/external_reference_executor.cpp` + +## 专项任务清单 + +这部分是给 UI 和后续维护看的，目的是把“总控阶段”下面真正会出现的专项任务说清楚。 + +### 1. 外部定位 / 真值接入 + +**对应代码** +- `src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` +- `src/agv_calib_core/workshop_orchestrator/src/external_localization_client.cpp` +- `src/agv_calib_core/external_localization_service/src/external_reference_executor.cpp` + +**当前任务** +- 外部真值参考接入 +- 外部定位 readiness 检查 +- 真值校核 / 接入确认 + +**典型输入** +- 位置参考源 ID +- 工位区域 ID +- 静态/动态采样数量 +- 位置标准差、yaw 标准差、跟踪丢失阈值、时间同步阈值 + +### 2. 底盘标定 + +**对应代码** +- `src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` +- `src/agv_calib_core/workshop_orchestrator/src/chassis_gateway_client.cpp` + +**当前任务** +- 底盘 readiness 检查 +- 直线动作原语执行 +- 标定后刹停/收尾 + +**说明** +- 目前总控层默认是“底盘标定”这一阶段 +- 具体底盘形态由车辆画像决定，例如阿克曼、差速、单舵轮、多舵轮 +- 当前最小流实现里，底盘阶段的动作原语以 `straight_line` 为主 + +### 3. 运控参数标定 + +**对应代码** +- `src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` +- `src/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp` + +**当前任务** +- 运控 readiness 检查 +- 控制评估任务下发 +- 轨迹点组装与参数注入 +- 结果回收与翻译 + +**说明** +- 总控当前只暴露“运控参数调优”这一阶段 +- 具体执行内容由 `stage.metadata` 中的轨迹和控制参数决定 +- 代码里已支持按控制轴和算法组织参数，例如横向/纵向、PID/MPC/LQR/Pure Pursuit + +### 4. 传感器内参标定 + +**对应代码** +- `src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` +- `src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp` + +**当前任务** +- 相机内参标定 +- IMU 内参标定 + +**典型输入** +- 传感器 ID +- 标定板 ID +- 所需图像数 +- 静态段数量 +- 运动段数量 +- 超时时间 + +### 5. 传感器外参标定 + +**对应代码** +- `src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` +- `src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp` + +**当前任务** +- 相机外参标定 +- 2D 激光雷达外参标定 +- 3D 激光雷达外参标定 +- IMU 外参标定 + +**典型输入** +- 传感器 ID +- base_link / 基准坐标系 +- 样本数 +- 参考目标 +- 超时时间 + +### 6. 手眼标定 + +**对应代码** +- `src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` +- `src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp` + +**当前任务** +- 眼在手上（EYE_IN_HAND） +- 眼在手外（EYE_TO_HAND） + +**典型输入** +- 相机 ID +- 机械臂 ID +- 位姿数量 +- 参考目标 +- 超时时间 + +## 当前接口 + +- `/workshop_v2/create_session` +- `/workshop_v2/get_session` +- `/workshop_v2/get_report` +- `/workshop_v2/execute_session` action +- `/workshop_v2/events` topic +- `/workshop_v2/pause_session` +- `/workshop_v2/resume_session` +- `/workshop_v2/acknowledge_manual_step` +- `/workshop_v2/approve_stage_result` + +## 维护规则 + +以后只要代码里的流程、阶段、任务选择方式或模块职责发生变化，就要同步重写这份 README，让它始终反映当前真实实现，而不是历史版本。 + +## 下一步 + +1. 继续把各专项执行接到真实 gateway / service +2. 让 UI 的任务选择和总控阶段保持一致 +3. 让 README 与实际实现保持同步更新 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/chassis_gateway_client.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/chassis_gateway_client.hpp new file mode 100644 index 0000000..9978b28 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/chassis_gateway_client.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_chassis_interfaces/action/execute_motion_primitive.hpp" +#include "calibration_chassis_interfaces/srv/get_chassis_readiness.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_plan.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_result_summary.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace workshop_orchestrator_v2 +{ + +// 底盘 gateway 薄 client。 +// 职责： +// 1) 调 /chassis/get_readiness 确认底盘可执行； +// 2) 向 /chassis/execute_motion_primitive 发送动作原语 goal； +// 3) 把 ChassisJobResult 翻译成 StageResultSummary 返回给主控。 +// +// 底盘专项实现细节保留在车端代理侧，此处只负责调用和结果转换。 +class ChassisGatewayClient +{ +public: + using ReadinessSrv = + calibration_chassis_interfaces::srv::GetChassisReadiness; + using ExecuteTask = + calibration_chassis_interfaces::action::ExecuteMotionPrimitive; + using GoalHandleExecuteTask = rclcpp_action::ClientGoalHandle; + + explicit ChassisGatewayClient(rclcpp::Node * node); + + // 对主控暴露的唯一入口：执行底盘标定阶段（动作原语序列）。 + bool execute_chassis_stage( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + calibration_workshop_orchestration_interfaces::msg::StageResultSummary & result, + std::string & failure_reason); + +private: + // 检查底盘 agent 是否就绪。 + bool check_ready(std::string & failure_reason); + + // 根据 session/stage 构造底盘动作 goal。 + bool build_goal( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + ExecuteTask::Goal & goal, + std::string & failure_reason) const; + + std::string make_request_id() const; + + rclcpp::Node * node_{nullptr}; + rclcpp::Client::SharedPtr readiness_client_; + rclcpp_action::Client::SharedPtr execute_task_client_; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/control_gateway_client.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/control_gateway_client.hpp new file mode 100644 index 0000000..4539f5f --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/control_gateway_client.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_control_interfaces/action/execute_controller_evaluation.hpp" +#include "calibration_control_interfaces/srv/get_control_readiness.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_plan.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_result_summary.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace workshop_orchestrator_v2 +{ + +using StagePlan = calibration_workshop_orchestration_interfaces::msg::StagePlan; + +// 运控 gateway 薄 client。 +// 职责： +// 1) 调 /control/get_readiness 确认运控服务可执行； +// 2) 向 /control/execute_controller_evaluation 发送评估任务 goal； +// 3) 把 ControlJobResult 翻译成 StageResultSummary 返回给主控。 +class ControlGatewayClient +{ +public: + using ReadinessSrv = + calibration_control_interfaces::srv::GetControlReadiness; + using ExecuteTask = + calibration_control_interfaces::action::ExecuteControllerEvaluation; + using GoalHandleExecuteTask = rclcpp_action::ClientGoalHandle; + + explicit ControlGatewayClient(rclcpp::Node * node); + + // 对主控暴露的唯一入口：执行运控参数评估阶段。 + bool execute_control_stage( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + calibration_workshop_orchestration_interfaces::msg::StageResultSummary & result, + std::string & failure_reason); + +private: + bool check_ready(std::string & failure_reason); + + uint8_t resolve_task_type( + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + std::string & failure_reason) const; + + bool build_goal( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + ExecuteTask::Goal & goal, + std::string & failure_reason) const; + + std::string make_request_id() const; + + // 根据 stage.metadata 解析轨迹点；未提供必需数据时直接失败。 + std::vector + build_trajectory_from_metadata(const StagePlan & stage, std::string & failure_reason) const; + + rclcpp::Node * node_{nullptr}; + rclcpp::Client::SharedPtr readiness_client_; + rclcpp_action::Client::SharedPtr execute_task_client_; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/external_localization_client.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/external_localization_client.hpp new file mode 100644 index 0000000..1a2b5ee --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/external_localization_client.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_external_localization_interfaces/action/execute_external_localization_task.hpp" +#include "calibration_external_localization_interfaces/srv/get_external_localization_readiness.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_plan.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_result_summary.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace workshop_orchestrator_v2 +{ + +// 这个类是 orchestrator 侧的“薄 client”。 +// 作用只有一个： +// 1) 调 external_localization_service 的 readiness； +// 2) 调 external_localization_service 的 execute action； +// 3) 把 external 返回的结果整理成 StageResultSummary。 +// +// 注意： +// 它不是 external 的“专项实现”。 +// 真正的专项逻辑应该留在 external_localization_service 包里。 +class ExternalLocalizationClient +{ +public: + using ReadinessSrv = + calibration_external_localization_interfaces::srv::GetExternalLocalizationReadiness; + using ExecuteTask = + calibration_external_localization_interfaces::action::ExecuteExternalLocalizationTask; + using GoalHandleExecuteTask = rclcpp_action::ClientGoalHandle; + + explicit ExternalLocalizationClient(rclcpp::Node * node); + + // 对 orchestrator 暴露的唯一主入口： + // 执行“外部真值校核”这一步。 + bool execute_reference_check( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + calibration_workshop_orchestration_interfaces::msg::StageResultSummary & result, + std::string & failure_reason); + +private: + bool check_ready(std::string & failure_reason); + bool build_goal( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + ExecuteTask::Goal & goal, + std::string & failure_reason) const; + std::string make_request_id() const; + + rclcpp::Node * node_{nullptr}; + rclcpp::Client::SharedPtr readiness_client_; + rclcpp_action::Client::SharedPtr execute_task_client_; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/plan_builder.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/plan_builder.hpp new file mode 100644 index 0000000..e407663 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/plan_builder.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include + +#include "calibration_vehicle_profile_interfaces/msg/calibration_ability_type.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session_config.hpp" +#include "workshop_orchestrator_v2/types.hpp" + +namespace workshop_orchestrator_v2 +{ + +using WorkshopSessionConfig = calibration_workshop_orchestration_interfaces::msg::WorkshopSessionConfig; +using RequestedCalibrationTask = calibration_workshop_orchestration_interfaces::msg::RequestedCalibrationTask; + +// PlanBuilder 负责根据车辆画像和会话配置，生成这次标定任务应该执行的阶段列表。 +// 默认顺序：外部真值校核 → 底盘标定 → 运控标定 → 传感器内参 → 传感器外参 → 手眼标定。 +// 如果 config.requested_tasks 非空，则只生成用户显式启用的阶段。 +class PlanBuilder +{ +public: + // 根据车辆画像和会话配置生成阶段列表。 + std::vector build_minimal_plan( + const VehicleProfile & profile, + const WorkshopSessionConfig & config) const; + +private: + // 判断 profile.capabilities 中某个 ability_type 是否 supported。 + bool has_capability( + const VehicleProfile & profile, + uint8_t ability_type) const; + + // 在 requested_tasks 中查找某个 stage_type 对应的配置。 + // 找到返回指针，没找到返回 nullptr。 + const RequestedCalibrationTask * find_requested_task( + const WorkshopSessionConfig & config, + uint8_t stage_type) const; + + // 在 requested_tasks 中查找某个 task_code 对应的配置。 + const RequestedCalibrationTask * find_requested_task_by_code( + const WorkshopSessionConfig & config, + const std::string & task_code) const; + + // 判断某个阶段是否应该加入计划。 + // 综合考虑 requested_tasks 过滤和 capabilities 支持。 + bool should_include_stage( + const VehicleProfile & profile, + const WorkshopSessionConfig & config, + uint8_t stage_type, + uint8_t ability_type) const; + + // 各阶段的构造方法。 + StagePlan make_external_reference_stage(int order_index, const RequestedCalibrationTask * task_cfg) const; + StagePlan make_chassis_stage(int order_index, const RequestedCalibrationTask * task_cfg) const; + StagePlan make_control_stage(int order_index, const RequestedCalibrationTask * task_cfg) const; + StagePlan make_sensor_intrinsic_stage(int order_index, const RequestedCalibrationTask * task_cfg) const; + StagePlan make_sensor_extrinsic_stage(int order_index, const RequestedCalibrationTask * task_cfg) const; + StagePlan make_hand_eye_stage(int order_index, const RequestedCalibrationTask * task_cfg) const; + StagePlan make_stage_for_task(int order_index, const RequestedCalibrationTask & task) const; + + // 把 RequestedCalibrationTask 中的策略和配置应用到 StagePlan 上。 + void apply_task_config(StagePlan & stage, const RequestedCalibrationTask * task_cfg) const; + void apply_task_identity(StagePlan & stage, const RequestedCalibrationTask & task) const; + std::string make_task_suffix(const std::string & task_code) const; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/precheck_runner.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/precheck_runner.hpp new file mode 100644 index 0000000..8f6cfa0 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/precheck_runner.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "workshop_orchestrator_v2/types.hpp" + +namespace workshop_orchestrator_v2 +{ + +class PrecheckRunner +{ +public: + WorkshopPrecheckResponse run(const WorkshopSession & session) const; + +private: + bool has_metadata_key(const StagePlan & stage, const std::string & key) const; + bool has_metadata_prefix(const StagePlan & stage, const std::string & prefix) const; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/report_builder.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/report_builder.hpp new file mode 100644 index 0000000..c03ea33 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/report_builder.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +#include "workshop_orchestrator_v2/types.hpp" + +namespace workshop_orchestrator_v2 +{ + +class ReportBuilder +{ +public: + WorkshopReport build( + const WorkshopSession & session, + const std::vector & stage_results, + bool overall_success, + int64_t started_timestamp_us, + int64_t finished_timestamp_us, + const std::string & summary) const; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/sensor_gateway_client.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/sensor_gateway_client.hpp new file mode 100644 index 0000000..7a3ce0b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/sensor_gateway_client.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_sensor_interfaces/action/execute_sensor_calibration_task.hpp" +#include "calibration_sensor_interfaces/srv/get_sensor_readiness.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_plan.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/stage_result_summary.hpp" +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" + +namespace workshop_orchestrator_v2 +{ + +// 传感器 gateway 薄 client。 +// 职责： +// 1) 调 /sensor_calibration/get_readiness 确认传感器服务可执行； +// 2) 向 /sensor_calibration/execute_task 发送标定任务 goal； +// 3) 把 SensorCalibrationJobResult 翻译成 StageResultSummary 返回给主控。 +class SensorGatewayClient +{ +public: + using ReadinessSrv = + calibration_sensor_interfaces::srv::GetSensorReadiness; + using ExecuteTask = + calibration_sensor_interfaces::action::ExecuteSensorCalibrationTask; + using GoalHandleExecuteTask = rclcpp_action::ClientGoalHandle; + + explicit SensorGatewayClient(rclcpp::Node * node); + + // 对主控暴露的唯一入口：执行传感器标定阶段。 + bool execute_sensor_stage( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + calibration_workshop_orchestration_interfaces::msg::StageResultSummary & result, + std::string & failure_reason); + +private: + bool check_ready(std::string & failure_reason); + + uint8_t resolve_task_type( + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + std::string & failure_reason) const; + + bool build_goal( + const calibration_workshop_orchestration_interfaces::msg::WorkshopSession & session, + const calibration_workshop_orchestration_interfaces::msg::StagePlan & stage, + uint8_t selected_task_type, + ExecuteTask::Goal & goal, + std::string & failure_reason) const; + + std::string make_request_id() const; + + rclcpp::Node * node_{nullptr}; + rclcpp::Client::SharedPtr readiness_client_; + rclcpp_action::Client::SharedPtr execute_task_client_; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp new file mode 100644 index 0000000..8e37f9b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp @@ -0,0 +1,145 @@ +#pragma once + +#include // 用来取当前时间，给创建时间、更新时间、报告时间戳赋值。 +#include // 保存字符串字段，比如 session_id、summary。 +#include // 保存步骤列表、步骤执行结果列表。 + +#include "calibration_common_interfaces/msg/error_code.hpp" // 通用错误码，比如 OK、INVALID_ARGUMENT。 +#include "calibration_common_interfaces/msg/job_state.hpp" // 通用任务状态，比如 RUNNING、SUCCEEDED。 +#include "calibration_vehicle_profile_interfaces/msg/calibration_module_type.hpp" // 模块类型，比如 external_localization。 +#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp" // 车辆画像，告诉主控这台车是什么配置。 +#include "calibration_vehicle_profile_interfaces/msg/workflow_stage_type.hpp" // 步骤类型，比如 external_reference。 +#include "calibration_workshop_orchestration_interfaces/msg/stage_plan.hpp" // 单个步骤计划：这一步做什么、找谁执行。 +#include "calibration_workshop_orchestration_interfaces/msg/stage_result_summary.hpp" // 单个步骤结果：这一步是否成功。 +#include "calibration_workshop_orchestration_interfaces/msg/workshop_precheck_response.hpp" // 开跑前检查结果。 +#include "calibration_workshop_orchestration_interfaces/msg/workshop_report.hpp" // 最终报告：整场跑完后返回给外部。 +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session.hpp" // 一次完整标定任务的主对象。 +#include "calibration_workshop_orchestration_interfaces/msg/workshop_session_state.hpp" // 这次标定当前跑到哪一步了。 + +namespace workshop_orchestrator_v2 +{ + +// 起短名字只是为了后面少写长命名空间。 +using ErrorCode = calibration_common_interfaces::msg::ErrorCode; +using JobState = calibration_common_interfaces::msg::JobState; +using CalibrationModuleType = calibration_vehicle_profile_interfaces::msg::CalibrationModuleType; +using VehicleProfile = calibration_vehicle_profile_interfaces::msg::VehicleProfile; +using WorkflowStageType = calibration_vehicle_profile_interfaces::msg::WorkflowStageType; +using ApprovalState = calibration_workshop_orchestration_interfaces::msg::ApprovalState; +using ManualActionType = calibration_workshop_orchestration_interfaces::msg::ManualActionType; +using StagePlan = calibration_workshop_orchestration_interfaces::msg::StagePlan; +using StageResultSummary = calibration_workshop_orchestration_interfaces::msg::StageResultSummary; +using WorkshopPrecheckResponse = calibration_workshop_orchestration_interfaces::msg::WorkshopPrecheckResponse; +using WorkshopReport = calibration_workshop_orchestration_interfaces::msg::WorkshopReport; +using WorkshopSession = calibration_workshop_orchestration_interfaces::msg::WorkshopSession; +using WorkshopSessionState = calibration_workshop_orchestration_interfaces::msg::WorkshopSessionState; + +// 返回当前时间的“微秒”整数。 +// 这里统一用微秒，是为了直接写入项目里现成的 *_timestamp_us 字段。 +inline int64_t now_us() +{ + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} + +// 把一个状态值包成 WorkshopSessionState 消息。 +inline WorkshopSessionState make_session_state(uint8_t value) +{ + WorkshopSessionState state; + state.value = value; + return state; +} + +// 把步骤类型值包成消息。 +inline calibration_vehicle_profile_interfaces::msg::WorkflowStageType make_stage_type(uint8_t value) +{ + calibration_vehicle_profile_interfaces::msg::WorkflowStageType stage_type; + stage_type.value = value; + return stage_type; +} + +// 把模块类型值包成消息。 +inline calibration_vehicle_profile_interfaces::msg::CalibrationModuleType make_module_type(uint8_t value) +{ + calibration_vehicle_profile_interfaces::msg::CalibrationModuleType module_type; + module_type.value = value; + return module_type; +} + +// 把任务状态值包成消息。 +inline JobState make_job_state(uint8_t value) +{ + JobState state; + state.state = value; + return state; +} + +// 把错误码值包成消息。 +inline ErrorCode make_error_code(uint16_t value) +{ + ErrorCode code; + code.code = value; + return code; +} + +namespace metadata_keys +{ +// common template identity +inline constexpr const char * TASK_CODE = "task_code"; +inline constexpr const char * TARGET_ID = "target_id"; + +// chassis +inline constexpr const char * CHASSIS_PRIMITIVE_TYPE = "primitive_type"; +inline constexpr const char * CHASSIS_STRAIGHT_LINE_DISTANCE_M = "straight_line.target_distance_m"; +inline constexpr const char * CHASSIS_STRAIGHT_LINE_SPEED_MS = "straight_line.target_speed_ms"; +inline constexpr const char * CHASSIS_STRAIGHT_LINE_REVERSE = "straight_line.reverse"; +inline constexpr const char * COMMON_BRAKE_WHEN_FINISHED = "brake_when_finished"; +inline constexpr const char * COMMON_TIMEOUT_SEC = "timeout_sec"; + +// control +inline constexpr const char * CONTROL_TASK_TYPE = "control.task_type"; +inline constexpr const char * CONTROL_TRAJECTORY_PREFIX = "traj_pt_"; + +// sensor +inline constexpr const char * SENSOR_ID = "sensor.sensor_id"; +inline constexpr const char * SENSOR_TASK_SUBTYPE = "sensor.task_subtype"; +inline constexpr const char * CAMERA_INTRINSIC_REQUIRED_IMAGE_COUNT = "camera_intrinsic.required_image_count"; +inline constexpr const char * CAMERA_INTRINSIC_TARGET_BOARD_ID = "camera_intrinsic.target_board_id"; +inline constexpr const char * CAMERA_INTRINSIC_TIMEOUT_SEC = "camera_intrinsic.timeout_sec"; +inline constexpr const char * SENSOR_EXTRINSIC_BASE_FRAME_ID = "sensor_extrinsic.base_frame_id"; +inline constexpr const char * SENSOR_EXTRINSIC_REQUIRED_SAMPLE_COUNT = "sensor_extrinsic.required_sample_count"; +inline constexpr const char * SENSOR_EXTRINSIC_TIMEOUT_SEC = "sensor_extrinsic.timeout_sec"; +inline constexpr const char * HAND_EYE_ARM_ID = "hand_eye.arm_id"; +inline constexpr const char * HAND_EYE_REQUIRED_POSE_COUNT = "hand_eye.required_pose_count"; +inline constexpr const char * HAND_EYE_TIMEOUT_SEC = "hand_eye.timeout_sec"; + +// external +inline constexpr const char * EXTERNAL_STATIC_SAMPLE_COUNT = "external.static_sample_count"; +inline constexpr const char * EXTERNAL_DYNAMIC_SAMPLE_COUNT = "external.dynamic_sample_count"; +inline constexpr const char * EXTERNAL_REQUIRE_SHORT_MOTION_SEGMENT = "external.require_short_motion_segment"; +inline constexpr const char * EXTERNAL_MAX_POSITION_STDDEV_M = "external.max_position_stddev_m"; +inline constexpr const char * EXTERNAL_MAX_YAW_STDDEV_RAD = "external.max_yaw_stddev_rad"; +inline constexpr const char * EXTERNAL_MAX_TRACKING_LOSS_RATIO = "external.max_tracking_loss_ratio"; +inline constexpr const char * EXTERNAL_MAX_TIME_SYNC_OFFSET_MS = "external.max_time_sync_offset_ms"; +inline constexpr const char * EXTERNAL_TIMEOUT_SEC = "external.timeout_sec"; +} // namespace metadata_keys + +// SessionRecord 是主控在内存里保存的一条“这台车这次标定任务记录”。 +struct SessionRecord +{ + WorkshopSession session; // 这次标定任务本体：ID、状态、车辆画像、步骤计划都在这里。 + WorkshopPrecheckResponse last_precheck; // 最近一次开跑前检查结果；外部想知道“为什么没开跑”时会看它。 + WorkshopReport report; // 这次标定最终产出的报告；跑完或失败后会写这里。 + std::vector stage_results; // 每一步执行完后的结果列表；最终报告会直接用它拼出来。 + std::string waiting_stage_id; // 当前卡在哪个阶段的人工门控上。 + bool manual_action_pending{false}; // 当前是否在等人工动作完成。 + bool manual_action_confirmed{false}; // 人工动作是否已经确认完成。 + bool approval_pending{false}; // 当前是否在等阶段结果审批。 + bool approval_decided{false}; // 审批是否已经给出结论。 + bool approval_approved{false}; // 审批是否通过。 + bool report_ready{false}; // 报告是否已经生成好；没生成好之前，get_report 不能返回有效报告。 + bool executing{false}; // 是否有后台线程正在执行这条任务；防止同一 session 被并发执行。 +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp new file mode 100644 index 0000000..fa28bfc --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp @@ -0,0 +1,224 @@ +#pragma once + +#include // 暂停/恢复、人工确认、审批等待都需要条件变量。 +#include +#include // 保护 sessions_，避免 service 和 action 线程同时改同一条任务记录。 +#include // 保存 topic 名、ID、说明文本。 +#include // 按 session_id 快速找到某次标定任务。 + +#include "rclcpp/rclcpp.hpp" // ROS 2 普通节点基础能力。 +#include "rclcpp_action/rclcpp_action.hpp" // ROS 2 action 能力，适合承载"整场任务执行"这种长操作。 + +#include "calibration_workshop_orchestration_interfaces/action/execute_workshop_session.hpp" // 外部要求主控开始跑某次标定任务的 action。 +#include "calibration_workshop_orchestration_interfaces/msg/workshop_event.hpp" // 发给界面/日志/上位机的进度通知消息。 +#include "calibration_workshop_orchestration_interfaces/msg/workshop_event_type.hpp" // 进度通知里的事件类型枚举。 +#include "calibration_workshop_orchestration_interfaces/srv/acknowledge_manual_step.hpp" // 人工确认。 +#include "calibration_workshop_orchestration_interfaces/srv/approve_stage_result.hpp" // 审批阶段结果。 +#include "calibration_workshop_orchestration_interfaces/srv/create_workshop_session.hpp" // 创建一条新的标定任务。 +#include "calibration_workshop_orchestration_interfaces/srv/get_workshop_report.hpp" // 查询最终报告。 +#include "calibration_workshop_orchestration_interfaces/srv/get_workshop_session.hpp" // 查询当前任务状态。 +#include "calibration_workshop_orchestration_interfaces/srv/pause_workshop_session.hpp" // 暂停。 +#include "calibration_workshop_orchestration_interfaces/srv/resume_workshop_session.hpp" // 恢复。 + +#include "workshop_orchestrator_v2/plan_builder.hpp" // 生成步骤列表。 +#include "workshop_orchestrator_v2/precheck_runner.hpp" // 开跑前检查。 +#include "workshop_orchestrator_v2/report_builder.hpp" // 生成最终报告。 +#include "workshop_orchestrator_v2/types.hpp" // 公共类型和 SessionRecord。 +#include "workshop_orchestrator_v2/external_localization_client.hpp" // 外部定位模块薄 client。 +#include "workshop_orchestrator_v2/chassis_gateway_client.hpp" // 底盘模块薄 client。 +#include "workshop_orchestrator_v2/control_gateway_client.hpp" // 运控模块薄 client。 +#include "workshop_orchestrator_v2/sensor_gateway_client.hpp" // 传感器模块薄 client。 + +namespace workshop_orchestrator_v2 +{ + +using StageExecutionPolicy = calibration_workshop_orchestration_interfaces::msg::StageExecutionPolicy; + +class WorkshopOrchestratorV2Node : public rclcpp::Node +{ +public: + using ExecuteWorkshopSession = + calibration_workshop_orchestration_interfaces::action::ExecuteWorkshopSession; + using GoalHandleExecuteWorkshopSession = + rclcpp_action::ServerGoalHandle; + + explicit WorkshopOrchestratorV2Node(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + // 执行上下文：主控内部在"正式开跑这条任务"时临时保存的信息。 + struct SessionExecutionContext + { + std::string session_id; + int64_t started_timestamp_us{0}; + bool has_required_failure{false}; // 是否有 REQUIRED 阶段失败。 + }; + + // ─── 原有 service handler ─── + // ─── 原有 service handler ─── + void handle_create_session( + const std::shared_ptr request, + std::shared_ptr response); + void handle_get_session( + const std::shared_ptr request, + std::shared_ptr response); + void handle_get_report( + const std::shared_ptr request, + std::shared_ptr response); + + // ─── 新增 service handler ─── + void handle_pause_session( + const std::shared_ptr request, + std::shared_ptr response); + void handle_resume_session( + const std::shared_ptr request, + std::shared_ptr response); + void handle_acknowledge_manual_step( + const std::shared_ptr request, + std::shared_ptr response); + void handle_approve_stage_result( + const std::shared_ptr request, + std::shared_ptr response); + + // ─── Action handler ─── + rclcpp_action::GoalResponse handle_execute_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_execute_cancel( + const std::shared_ptr goal_handle); + void handle_execute_accepted(const std::shared_ptr goal_handle); + + // ─── 执行主流程 ─── + void execute_session(const std::shared_ptr goal_handle); + SessionExecutionContext prepare_session_for_run(const std::string & session_id); + WorkshopPrecheckResponse run_precheck_step(const std::string & session_id); + void mark_session_running(const std::string & session_id); + bool handle_cancel_if_requested( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context); + bool run_all_stages( + const std::shared_ptr goal_handle, + SessionExecutionContext & context, + std::string & failure_reason); + bool run_single_stage( + const std::shared_ptr goal_handle, + const std::string & session_id, + const StagePlan & stage, + std::string & failure_reason); + bool wait_for_manual_confirmation( + const std::shared_ptr goal_handle, + const std::string & session_id, + const StagePlan & stage, + std::string & failure_reason); + bool wait_for_stage_approval( + const std::shared_ptr goal_handle, + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason); + bool dispatch_stage_to_module( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason); + bool run_external_localization_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason); + bool run_chassis_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason); + bool run_control_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason); + bool run_sensor_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason); + + // ─── 暂停等待 ─── + void wait_if_paused(const std::string & session_id); + + // ─── 阶段结果记录 ─── + void record_stage_result(const std::string & session_id, const StageResultSummary & result); + + // ─── 状态标记 ─── + void mark_stage_failed( + const std::string & session_id, + const StagePlan & stage, + const std::string & failure_reason); + void mark_stage_started(const std::string & session_id, const StagePlan & stage); + void mark_stage_completed( + const std::string & session_id, + const StagePlan & stage, + const StageResultSummary & result); + + // ─── 收尾 ─── + void finish_session_precheck_failed( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context, + const WorkshopPrecheckResponse & precheck); + void finish_session_failed( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context, + const std::string & failure_reason); + void finish_session_succeeded( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context); + void finish_session_canceled( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context); + + // ─── 事件发布（同时发 topic + action feedback） ─── + void publish_event( + const SessionRecord & record, + uint8_t event_type, + const std::string & message, + const std::string & stage_id = std::string(), + uint8_t stage_job_state = JobState::JOB_STATE_UNSPECIFIED, + uint8_t stage_type = WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED, + uint8_t module_type = CalibrationModuleType::CALIBRATION_MODULE_UNSPECIFIED, + bool requires_manual_ack = false, + uint8_t approval_state = ApprovalState::APPROVAL_STATE_UNSPECIFIED); + + std::string make_session_id() const; + + // ─── 数据成员 ─── + std::mutex mutex_; + std::unordered_map sessions_; + + // 暂停/恢复、人工动作、审批等待。 + std::condition_variable pause_cv_; + std::condition_variable manual_action_cv_; + std::condition_variable approval_cv_; + bool pause_requested_{false}; + + // 当前正在执行的 action goal handle，用于发 feedback。 + std::shared_ptr active_goal_handle_; + + PlanBuilder plan_builder_; + PrecheckRunner precheck_runner_; + ReportBuilder report_builder_; + std::unique_ptr external_localization_client_; + std::unique_ptr chassis_gateway_client_; + std::unique_ptr control_gateway_client_; + std::unique_ptr sensor_gateway_client_; + + // ─── ROS 接口 ─── + rclcpp::Service::SharedPtr create_session_service_; + rclcpp::Service::SharedPtr get_session_service_; + rclcpp::Service::SharedPtr get_report_service_; + rclcpp::Service::SharedPtr pause_session_service_; + rclcpp::Service::SharedPtr resume_session_service_; + rclcpp::Service::SharedPtr acknowledge_manual_step_service_; + rclcpp::Service::SharedPtr approve_stage_result_service_; + rclcpp_action::Server::SharedPtr execute_session_action_server_; + rclcpp::Publisher::SharedPtr event_publisher_; +}; + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/launch/workshop_orchestrator_v2.launch.py b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/launch/workshop_orchestrator_v2.launch.py new file mode 100644 index 0000000..35a8946 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/launch/workshop_orchestrator_v2.launch.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="workshop_orchestrator_v2", + executable="workshop_orchestrator_v2_node", + name="workshop_orchestrator_v2", + output="screen", + ) + ]) diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/package.xml b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/package.xml new file mode 100644 index 0000000..8ad6930 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/package.xml @@ -0,0 +1,28 @@ + + + workshop_orchestrator_v2 + 0.0.3 + Workshop orchestrator v2 with external localization thin client. + you + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + std_msgs + calibration_common_interfaces + calibration_vehicle_profile_interfaces + calibration_workshop_orchestration_interfaces + calibration_external_localization_interfaces + calibration_chassis_interfaces + calibration_control_interfaces + calibration_sensor_interfaces + + launch + launch_ros + + + ament_cmake + + diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/chassis_gateway_client.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/chassis_gateway_client.cpp new file mode 100644 index 0000000..b4c0e99 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/chassis_gateway_client.cpp @@ -0,0 +1,278 @@ +#include "workshop_orchestrator_v2/chassis_gateway_client.hpp" + +#include +#include +#include +#include + +#include "workshop_orchestrator_v2/types.hpp" + +namespace workshop_orchestrator_v2 +{ + +using namespace std::chrono_literals; +using calibration_chassis_interfaces::msg::ChassisMotionPrimitiveType; +using calibration_chassis_interfaces::msg::MotionPrimitiveTaskPurpose; +using calibration_workshop_orchestration_interfaces::msg::StagePlan; +using calibration_workshop_orchestration_interfaces::msg::StageResultSummary; +using calibration_workshop_orchestration_interfaces::msg::WorkshopSession; + +ChassisGatewayClient::ChassisGatewayClient(rclcpp::Node * node) +: node_(node) +{ + readiness_client_ = node_->create_client("/chassis/get_readiness"); + execute_task_client_ = rclcpp_action::create_client( + node_, "/chassis/execute_motion_primitive"); +} + +std::string ChassisGatewayClient::make_request_id() const +{ + std::ostringstream oss; + oss << "chassis_req_" << now_us(); + return oss.str(); +} + +bool ChassisGatewayClient::check_ready(std::string & failure_reason) +{ + if (!readiness_client_->wait_for_service(2s)) { + failure_reason = "底盘 readiness 服务不可用。"; + return false; + } + + auto request = std::make_shared(); + request->request.agent_name = "chassis"; + request->request.include_details = true; + + auto future = readiness_client_->async_send_request(request); + if (future.wait_for(3s) != std::future_status::ready) { + failure_reason = "等待底盘 readiness 响应超时。"; + return false; + } + + const auto response = future.get(); + if (!response->response.success) { + failure_reason = response->response.message.empty() ? + "底盘 readiness 检查失败。" : response->response.message; + return false; + } + + if (!response->response.agent_ready) { + failure_reason = "底盘 agent 当前未就绪。"; + return false; + } + + // 急停未释放或车辆不安全时，禁止启动标定动作。 + if (!response->response.estop_released) { + failure_reason = "底盘急停未释放，禁止执行标定动作。"; + return false; + } + + if (!response->response.vehicle_safe_to_move) { + failure_reason = "当前车辆不允许移动，无法执行底盘标定。"; + return false; + } + + if (!response->response.motion_control_ready) { + failure_reason = "运控链路未就绪，无法执行底盘动作原语。"; + return false; + } + + return true; +} + +bool ChassisGatewayClient::build_goal( + const WorkshopSession & session, + const StagePlan & stage, + ExecuteTask::Goal & goal, + std::string & failure_reason) const +{ + + // 填充请求头，标识来源与任务归属。 + goal.goal.header.session_id = session.session_id; + goal.goal.header.task_id = stage.stage_id; + goal.goal.header.vehicle_id = session.vehicle_profile_snapshot.base_info.vehicle_id; + goal.goal.header.request_id = make_request_id(); + goal.goal.header.client_send_timestamp_us = now_us(); + goal.goal.header.operator_id = session.operator_info.operator_id; + goal.goal.header.workshop_host = "workshop_orchestrator_v2"; + + // 当前阶段目的：数据采集（具体底盘动作和参数由流程配置或底盘专项包决定）。 + goal.goal.task_purpose.value = MotionPrimitiveTaskPurpose::DATA_COLLECTION; + + if (stage.metadata.empty()) { + failure_reason = "底盘阶段缺少动作原语配置，无法启动底盘任务。"; + return false; + } + + std::unordered_map metadata_map; + for (const auto & kv : stage.metadata) { + metadata_map[kv.key] = kv.value; + } + + const auto primitive_it = metadata_map.find(metadata_keys::CHASSIS_PRIMITIVE_TYPE); + if (primitive_it == metadata_map.end()) { + failure_reason = "底盘阶段缺少 primitive_type，无法确定要执行的动作原语。"; + return false; + } + + if (primitive_it->second != "straight_line") { + failure_reason = "当前最小流仅支持 straight_line 底盘动作原语。"; + return false; + } + + const auto distance_it = metadata_map.find(metadata_keys::CHASSIS_STRAIGHT_LINE_DISTANCE_M); + const auto speed_it = metadata_map.find(metadata_keys::CHASSIS_STRAIGHT_LINE_SPEED_MS); + if (distance_it == metadata_map.end() || speed_it == metadata_map.end()) { + failure_reason = "straight_line 缺少 target_distance_m 或 target_speed_ms。"; + return false; + } + + goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::STRAIGHT_LINE; + try { + goal.goal.straight_line.target_distance_m = std::stod(distance_it->second); + goal.goal.straight_line.target_speed_ms = std::stod(speed_it->second); + } catch (...) { + failure_reason = "straight_line 参数格式错误，无法解析为数字。"; + return false; + } + + const auto reverse_it = metadata_map.find(metadata_keys::CHASSIS_STRAIGHT_LINE_REVERSE); + goal.goal.straight_line.reverse = + reverse_it != metadata_map.end() && (reverse_it->second == "true" || reverse_it->second == "1"); + + goal.goal.brake_when_finished = true; + const auto brake_it = metadata_map.find(metadata_keys::COMMON_BRAKE_WHEN_FINISHED); + if (brake_it != metadata_map.end()) { + goal.goal.brake_when_finished = brake_it->second == "true" || brake_it->second == "1"; + } + + goal.goal.timeout_sec = 60.0; + const auto timeout_it = metadata_map.find(metadata_keys::COMMON_TIMEOUT_SEC); + if (timeout_it != metadata_map.end()) { + try { + goal.goal.timeout_sec = std::stod(timeout_it->second); + } catch (...) { + failure_reason = "timeout_sec 参数格式错误，无法解析为数字。"; + return false; + } + } + + goal.goal.source_iteration_id = session.session_id; + goal.goal.test_case_id = stage.stage_id; + return true; +} + +bool ChassisGatewayClient::execute_chassis_stage( + const WorkshopSession & session, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + // Step 1：就绪检查。 + if (!check_ready(failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + ExecuteTask::Goal goal; + if (!build_goal(session, stage, goal, failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 2：等待 action server 上线。 + if (!execute_task_client_->wait_for_action_server(2s)) { + failure_reason = "底盘 action server 不可用。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 3：发送 goal。 + auto goal_handle_future = execute_task_client_->async_send_goal(goal); + if (goal_handle_future.wait_for(5s) != std::future_status::ready) { + failure_reason = "发送底盘动作原语请求超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) { + failure_reason = "底盘拒绝了本次动作原语请求。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 4：等待执行结果，超时 90 秒（底盘动作比外部定位耗时更长）。 + auto result_future = execute_task_client_->async_get_result(goal_handle); + if (result_future.wait_for(90s) != std::future_status::ready) { + failure_reason = "等待底盘动作原语结果超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto wrapped_result = result_future.get(); + if (!wrapped_result.result) { + failure_reason = "底盘返回了空结果。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 5：翻译结果到 StageResultSummary。 + const auto & job_result = wrapped_result.result->result; + result.success = job_result.success; + // 自动验收：数据质量达标 且 估计参数适合写入。 + result.auto_acceptance_passed = job_result.data_quality_passed && job_result.suitable_for_commit; + result.final_job_state = make_job_state( + job_result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED + : calibration_common_interfaces::msg::JobState::FAILED); + result.parameter_version = job_result.recommended_parameter_version; + + // 底盘接口中 artifacts 类型为 FileReference，此处直接赋值。 + // （StageResultSummary.artifacts 为 FileReference[]，类型一致。） + result.artifacts = job_result.artifacts; + + result.summary = job_result.message.empty() ? "底盘标定数据采集完成。" : job_result.message; + result.failure_root_cause = job_result.success ? "" : result.summary; + + if (!job_result.success) { + failure_reason = result.summary; + return false; + } + if (!result.auto_acceptance_passed) { + failure_reason = "底盘标定数据未通过自动验收（质量或参数适合性不满足）。"; + result.failure_root_cause = failure_reason; + return false; + } + + return true; +} + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp new file mode 100644 index 0000000..06b22ae --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp @@ -0,0 +1,335 @@ +#include "workshop_orchestrator_v2/control_gateway_client.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "calibration_common_interfaces/msg/key_value_pair.hpp" + +#include "calibration_control_interfaces/msg/trajectory_point.hpp" +#include "workshop_orchestrator_v2/types.hpp" + +namespace workshop_orchestrator_v2 +{ + +using namespace std::chrono_literals; +using calibration_control_interfaces::msg::ControllerEvaluationTaskPurpose; +using calibration_control_interfaces::msg::ControllerEvaluationTaskType; +using calibration_workshop_orchestration_interfaces::msg::StagePlan; +using calibration_workshop_orchestration_interfaces::msg::StageResultSummary; +using calibration_workshop_orchestration_interfaces::msg::WorkshopSession; + +ControlGatewayClient::ControlGatewayClient(rclcpp::Node * node) +: node_(node) +{ + readiness_client_ = node_->create_client("/control/get_readiness"); + execute_task_client_ = rclcpp_action::create_client( + node_, "/control/execute_controller_evaluation"); +} + +std::string ControlGatewayClient::make_request_id() const +{ + std::ostringstream oss; + oss << "control_req_" << now_us(); + return oss.str(); +} + +bool ControlGatewayClient::check_ready(std::string & failure_reason) +{ + if (!readiness_client_->wait_for_service(2s)) { + failure_reason = "运控 readiness 服务不可用。"; + return false; + } + + auto request = std::make_shared(); + request->request.agent_name = "control"; + request->request.include_details = true; + + auto future = readiness_client_->async_send_request(request); + if (future.wait_for(3s) != std::future_status::ready) { + failure_reason = "等待运控 readiness 响应超时。"; + return false; + } + + const auto response = future.get(); + if (!response->response.success) { + failure_reason = response->response.message.empty() ? + "运控 readiness 检查失败。" : response->response.message; + return false; + } + + if (!response->response.agent_ready) { + failure_reason = "运控 agent 当前未就绪。"; + return false; + } + + if (!response->response.estop_released) { + failure_reason = "运控急停未释放，禁止执行控制评估任务。"; + return false; + } + + if (!response->response.vehicle_safe_to_move) { + failure_reason = "当前车辆不允许移动，无法执行运控参数评估。"; + return false; + } + + if (!response->response.trajectory_executor_ready) { + failure_reason = "轨迹执行器未就绪，无法执行运控评估任务。"; + return false; + } + + if (!response->response.control_output_ready) { + failure_reason = "控制输出链路未就绪，无法执行运控评估任务。"; + return false; + } + + return true; +} + +uint8_t ControlGatewayClient::resolve_task_type( + const StagePlan & stage, + std::string & failure_reason) const +{ + std::unordered_map metadata_map; + for (const auto & kv : stage.metadata) { + metadata_map[kv.key] = kv.value; + } + + const auto task_type_it = metadata_map.find(metadata_keys::CONTROL_TASK_TYPE); + if (task_type_it == metadata_map.end()) { + failure_reason = "运控阶段缺少 control.task_type。"; + return ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED; + } + + if (task_type_it->second == "trajectory_tracking") { + return ControllerEvaluationTaskType::TRAJECTORY_TRACKING; + } + + failure_reason = "当前最小流仅支持 trajectory_tracking 运控任务。"; + return ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED; +} + +bool ControlGatewayClient::build_goal( + const WorkshopSession & session, + const StagePlan & stage, + ExecuteTask::Goal & goal, + std::string & failure_reason) const +{ + + // 填充请求头。 + goal.goal.header.session_id = session.session_id; + goal.goal.header.task_id = stage.stage_id; + goal.goal.header.vehicle_id = session.vehicle_profile_snapshot.base_info.vehicle_id; + goal.goal.header.request_id = make_request_id(); + goal.goal.header.client_send_timestamp_us = now_us(); + goal.goal.header.operator_id = session.operator_info.operator_id; + goal.goal.header.workshop_host = "workshop_orchestrator_v2"; + + goal.goal.test_case_id = stage.stage_id; + + // 当前目的：调参评估（不是最终验收）。 + goal.goal.task_purpose.value = ControllerEvaluationTaskPurpose::TUNING_EVALUATION; + + const auto selected_task_type = resolve_task_type(stage, failure_reason); + if (selected_task_type == ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED) { + return false; + } + goal.goal.selected_task.value = selected_task_type; + + goal.goal.trajectory_tracking.stop_at_end = true; // 执行完后停车，保证安全。 + goal.goal.trajectory_tracking.timeout_sec = 60.0; + + // 轨迹必须由流程配置显式提供；编排器不再隐式生成默认轨迹。 + goal.goal.trajectory_tracking.path = build_trajectory_from_metadata(stage, failure_reason); + if (goal.goal.trajectory_tracking.path.empty()) { + if (failure_reason.empty()) { + failure_reason = "运控阶段缺少轨迹输入，无法启动评估任务。"; + } + return false; + } + + goal.goal.source_iteration_id = session.session_id; + + return true; +} + +std::vector +ControlGatewayClient::build_trajectory_from_metadata( + const StagePlan & stage, + std::string & failure_reason) const +{ + using TrajectoryPoint = calibration_control_interfaces::msg::TrajectoryPoint; + + // 从 metadata 中按约定的 key 解析轨迹点。 + // key 约定："traj_pt_{index}_{field}"，field 取 x_m / y_m / yaw_rad / speed_ms。 + // 例如：traj_pt_0_x_m = "0.0", traj_pt_0_y_m = "0.0", ... + std::map pt_map; + for (const auto & kv : stage.metadata) { + // 解析 key 格式 + const std::string & key = kv.key; + if (key.rfind(metadata_keys::CONTROL_TRAJECTORY_PREFIX, 0) != 0) { + continue; // 不是轨迹点 key，跳过。 + } + // key 形如 "traj_pt_0_x_m"，提取 index 和 field。 + const auto after_prefix = key.substr(8); // 去掉 metadata_keys::CONTROL_TRAJECTORY_PREFIX + const auto sep = after_prefix.find('_'); + if (sep == std::string::npos) { + continue; + } + int idx = 0; + try { + idx = std::stoi(after_prefix.substr(0, sep)); + } catch (...) { + continue; + } + const std::string field = after_prefix.substr(sep + 1); + double val = 0.0; + try { + val = std::stod(kv.value); + } catch (...) { + continue; + } + + if (field == "x_m") { + pt_map[idx].x_m = val; + } else if (field == "y_m") { + pt_map[idx].y_m = val; + } else if (field == "yaw_rad") { + pt_map[idx].yaw_rad = val; + } else if (field == "speed_ms") { + pt_map[idx].target_speed_ms = val; + } + } + + if (!pt_map.empty()) { + // 按 index 升序收集轨迹点。 + std::vector path; + path.reserve(pt_map.size()); + for (const auto & [idx, pt] : pt_map) { + (void)idx; + path.push_back(pt); + } + return path; + } + + failure_reason = "trajectory_tracking 缺少轨迹点数据（traj_pt_{i}_{x_m|y_m|yaw_rad|speed_ms}）。"; + return {}; +} + +bool ControlGatewayClient::execute_control_stage( + const WorkshopSession & session, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + // Step 1：就绪检查。 + if (!check_ready(failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + ExecuteTask::Goal goal; + if (!build_goal(session, stage, goal, failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 2：等待 action server 上线。 + if (!execute_task_client_->wait_for_action_server(2s)) { + failure_reason = "运控评估 action server 不可用。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 3：发送 goal。 + auto goal_handle_future = execute_task_client_->async_send_goal(goal); + if (goal_handle_future.wait_for(5s) != std::future_status::ready) { + failure_reason = "发送运控评估请求超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) { + failure_reason = "运控服务拒绝了本次评估请求。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 4：等待结果，超时 120 秒（轨迹跟踪评估需要较长时间）。 + auto result_future = execute_task_client_->async_get_result(goal_handle); + if (result_future.wait_for(120s) != std::future_status::ready) { + failure_reason = "等待运控评估结果超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto wrapped_result = result_future.get(); + if (!wrapped_result.result) { + failure_reason = "运控服务返回了空结果。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 5：翻译 ControlJobResult → StageResultSummary。 + const auto & job_result = wrapped_result.result->result; + result.success = job_result.success; + // 自动验收：数据质量达标 且 参数适合写入。 + result.auto_acceptance_passed = job_result.data_quality_passed && job_result.suitable_for_commit; + result.final_job_state = make_job_state( + job_result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED + : calibration_common_interfaces::msg::JobState::FAILED); + result.parameter_version = job_result.recommended_parameter_version; + result.artifacts.clear(); + for (const auto & artifact : job_result.artifacts) { + result.artifacts.push_back(artifact); + } + result.summary = job_result.message.empty() ? "运控参数评估完成。" : job_result.message; + result.failure_root_cause = job_result.success ? "" : result.summary; + + if (!job_result.success) { + failure_reason = result.summary; + return false; + } + if (!result.auto_acceptance_passed) { + failure_reason = "运控评估未通过自动验收（数据质量或参数适合性不满足）。"; + result.failure_root_cause = failure_reason; + return false; + } + + return true; +} + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/external_localization_client.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/external_localization_client.cpp new file mode 100644 index 0000000..c46df2d --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/external_localization_client.cpp @@ -0,0 +1,252 @@ +#include "workshop_orchestrator_v2/external_localization_client.hpp" + +#include +#include +#include +#include + +#include "workshop_orchestrator_v2/types.hpp" + +namespace workshop_orchestrator_v2 +{ + +using namespace std::chrono_literals; +using calibration_workshop_orchestration_interfaces::msg::StagePlan; +using calibration_workshop_orchestration_interfaces::msg::StageResultSummary; +using calibration_workshop_orchestration_interfaces::msg::WorkshopSession; + +ExternalLocalizationClient::ExternalLocalizationClient(rclcpp::Node * node) +: node_(node) +{ + // orchestrator 这里只保留对 external service 的调用入口。 + readiness_client_ = node_->create_client("/external_localization/get_readiness"); + execute_task_client_ = rclcpp_action::create_client( + node_, "/external_localization/execute_task"); +} + +std::string ExternalLocalizationClient::make_request_id() const +{ + std::ostringstream oss; + oss << "extloc_req_" << now_us(); + return oss.str(); +} + +bool ExternalLocalizationClient::check_ready(std::string & failure_reason) +{ + if (!readiness_client_->wait_for_service(2s)) { + failure_reason = "external_localization readiness 服务不可用。"; + return false; + } + + auto request = std::make_shared(); + request->request.agent_name = "external_localization"; + request->request.include_details = true; + + auto future = readiness_client_->async_send_request(request); + if (future.wait_for(3s) != std::future_status::ready) { + failure_reason = "等待 external_localization readiness 响应超时。"; + return false; + } + + const auto response = future.get(); + if (!response->response.success) { + failure_reason = response->response.message.empty() ? + "external_localization readiness 检查失败。" : response->response.message; + return false; + } + + if (!response->response.agent_ready) { + failure_reason = "external_localization agent 当前未就绪。"; + return false; + } + + if (!response->response.ready_for_reference_validation) { + failure_reason = response->response.message.empty() ? + "external_localization 当前不允许做参考源校核。" : response->response.message; + return false; + } + + return true; +} + +bool ExternalLocalizationClient::build_goal( + const WorkshopSession & session, + const StagePlan & stage, + ExecuteTask::Goal & goal, + std::string & failure_reason) const +{ + + // 这一组字段表达的是“哪条任务、哪个步骤、谁发起、在哪个工位上做校核”。 + goal.goal.header.session_id = session.session_id; + goal.goal.header.task_id = stage.stage_id; + goal.goal.header.vehicle_id = session.vehicle_profile_snapshot.base_info.vehicle_id; + goal.goal.header.request_id = make_request_id(); + goal.goal.header.client_send_timestamp_us = now_us(); + goal.goal.header.operator_id = session.operator_info.operator_id; + goal.goal.header.workshop_host = "workshop_orchestrator_v2"; + + goal.goal.task_purpose.value = + calibration_external_localization_interfaces::msg::ExternalLocalizationTaskPurpose::REFERENCE_READY_CHECK; + goal.goal.selected_task.value = + calibration_external_localization_interfaces::msg::ExternalLocalizationTaskPayloadType::TRUTH_SOURCE_VALIDATION; + + std::unordered_map metadata_map; + for (const auto & kv : stage.metadata) { + metadata_map[kv.key] = kv.value; + } + + if (session.config.localization_source_id.empty()) { + failure_reason = "external 阶段缺少 localization_source_id。"; + return false; + } + if (session.config.workcell_zone_id.empty()) { + failure_reason = "external 阶段缺少 workcell_zone_id。"; + return false; + } + + goal.goal.localization_source_id = session.config.localization_source_id; + goal.goal.workcell_zone_id = session.config.workcell_zone_id; + + const auto static_count_it = metadata_map.find(metadata_keys::EXTERNAL_STATIC_SAMPLE_COUNT); + const auto dynamic_count_it = metadata_map.find(metadata_keys::EXTERNAL_DYNAMIC_SAMPLE_COUNT); + const auto pos_stddev_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_POSITION_STDDEV_M); + const auto yaw_stddev_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_YAW_STDDEV_RAD); + const auto loss_ratio_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_TRACKING_LOSS_RATIO); + const auto sync_offset_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_TIME_SYNC_OFFSET_MS); + const auto timeout_it = metadata_map.find(metadata_keys::EXTERNAL_TIMEOUT_SEC); + if (static_count_it == metadata_map.end() || dynamic_count_it == metadata_map.end() || + pos_stddev_it == metadata_map.end() || yaw_stddev_it == metadata_map.end() || + loss_ratio_it == metadata_map.end() || sync_offset_it == metadata_map.end() || + timeout_it == metadata_map.end()) { + failure_reason = "external 阶段缺少真值校核阈值配置。"; + return false; + } + + try { + goal.goal.truth_source_validation.static_sample_count = static_cast(std::stoul(static_count_it->second)); + goal.goal.truth_source_validation.dynamic_sample_count = static_cast(std::stoul(dynamic_count_it->second)); + goal.goal.truth_source_validation.max_position_stddev_m = std::stod(pos_stddev_it->second); + goal.goal.truth_source_validation.max_yaw_stddev_rad = std::stod(yaw_stddev_it->second); + goal.goal.truth_source_validation.max_tracking_loss_ratio = std::stod(loss_ratio_it->second); + goal.goal.truth_source_validation.max_time_sync_offset_ms = std::stod(sync_offset_it->second); + goal.goal.truth_source_validation.timeout_sec = std::stod(timeout_it->second); + } catch (...) { + failure_reason = "external 阶段阈值配置格式错误。"; + return false; + } + + const auto short_motion_it = metadata_map.find(metadata_keys::EXTERNAL_REQUIRE_SHORT_MOTION_SEGMENT); + goal.goal.truth_source_validation.require_short_motion_segment = + short_motion_it == metadata_map.end() || short_motion_it->second == "true" || short_motion_it->second == "1"; + + goal.goal.test_case_id = session.config.reference_target_id.empty() ? stage.stage_id : session.config.reference_target_id; + goal.goal.source_iteration_id = session.session_id; + + return true; +} + +bool ExternalLocalizationClient::execute_reference_check( + const WorkshopSession & session, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + if (!check_ready(failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + ExecuteTask::Goal goal; + if (!build_goal(session, stage, goal, failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + if (!execute_task_client_->wait_for_action_server(2s)) { + failure_reason = "external_localization action server 不可用。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto goal_handle_future = execute_task_client_->async_send_goal(goal); + if (goal_handle_future.wait_for(5s) != std::future_status::ready) { + failure_reason = "发送 external_localization 任务请求超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) { + failure_reason = "external_localization 拒绝了这次外部真值校核请求。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto result_future = execute_task_client_->async_get_result(goal_handle); + if (result_future.wait_for(35s) != std::future_status::ready) { + failure_reason = "等待 external_localization 结果超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto wrapped_result = result_future.get(); + if (!wrapped_result.result) { + failure_reason = "external_localization 返回了空结果。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + const auto & job_result = wrapped_result.result->result; + result.success = job_result.success; + result.auto_acceptance_passed = + job_result.data_quality_passed && job_result.validation_summary.recommended_as_truth_source; + result.final_job_state = make_job_state( + job_result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED + : calibration_common_interfaces::msg::JobState::FAILED); + result.parameter_version = job_result.recommended_parameter_version; + result.artifacts = job_result.artifacts; + result.summary = job_result.message.empty() ? "外部真值校核完成。" : job_result.message; + result.failure_root_cause = job_result.success ? "" : result.summary; + + if (!job_result.success) { + failure_reason = result.summary; + return false; + } + if (!result.auto_acceptance_passed) { + failure_reason = "外部真值校核未通过自动验收。"; + result.failure_root_cause = failure_reason; + return false; + } + + return true; +} + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/main.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/main.cpp new file mode 100644 index 0000000..21b0491 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/main.cpp @@ -0,0 +1,12 @@ +#include "rclcpp/rclcpp.hpp" + +#include "workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp new file mode 100644 index 0000000..f2c4c6e --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp @@ -0,0 +1,377 @@ +#include "workshop_orchestrator_v2/plan_builder.hpp" + +namespace workshop_orchestrator_v2 +{ + +using calibration_vehicle_profile_interfaces::msg::CalibrationAbilityType; +using calibration_workshop_orchestration_interfaces::msg::StageExecutionPolicy; + +bool PlanBuilder::has_capability(const VehicleProfile & profile, uint8_t ability_type) const +{ + for (const auto & cap : profile.capabilities) { + if (cap.ability_type.value == ability_type && cap.supported) { + return true; + } + } + return false; +} + +const RequestedCalibrationTask * PlanBuilder::find_requested_task( + const WorkshopSessionConfig & config, + uint8_t stage_type) const +{ + for (const auto & task : config.requested_tasks) { + if (task.stage_type.value == stage_type) { + return &task; + } + } + return nullptr; +} + +const RequestedCalibrationTask * PlanBuilder::find_requested_task_by_code( + const WorkshopSessionConfig & config, + const std::string & task_code) const +{ + if (task_code.empty()) { + return nullptr; + } + for (const auto & task : config.requested_tasks) { + if (task.task_code == task_code) { + return &task; + } + } + return nullptr; +} + +bool PlanBuilder::should_include_stage( + const VehicleProfile & profile, + const WorkshopSessionConfig & config, + uint8_t stage_type, + uint8_t ability_type) const +{ + // 如果 requested_tasks 非空，以用户显式配置为准。 + if (!config.requested_tasks.empty()) { + const auto * task = find_requested_task(config, stage_type); + // 用户没有列出这个阶段 → 不加入。 + if (!task) { + return false; + } + // 用户显式禁用 → 不加入。 + if (!task->enabled) { + return false; + } + // 用户启用了，但车辆不支持 → 看执行策略。 + if (ability_type != 0 && !has_capability(profile, ability_type)) { + return task->execution_policy.value == StageExecutionPolicy::SKIP_IF_UNSUPPORTED; + } + return true; + } + + // requested_tasks 为空：按 capabilities 全量生成。 + if (ability_type == 0) { + return true; // 无需 capability 检查的阶段（如外部真值校核）始终加入。 + } + return has_capability(profile, ability_type); +} + +void PlanBuilder::apply_task_config(StagePlan & stage, const RequestedCalibrationTask * task_cfg) const +{ + if (!task_cfg) { + // 没有用户配置时，默认 REQUIRED。 + stage.execution_policy.value = StageExecutionPolicy::REQUIRED; + return; + } + stage.execution_policy = task_cfg->execution_policy; + stage.requires_approval_before_commit = task_cfg->require_manual_approval; + // 把 task_params 合并到 stage.metadata。 + for (const auto & kv : task_cfg->task_params) { + stage.metadata.push_back(kv); + } +} + +void PlanBuilder::apply_task_identity(StagePlan & stage, const RequestedCalibrationTask & task) const +{ + if (!task.task_code.empty()) { + calibration_common_interfaces::msg::KeyValuePair kv; + kv.key = metadata_keys::TASK_CODE; + kv.value = task.task_code; + stage.metadata.push_back(kv); + } + if (!task.target_id.empty()) { + calibration_common_interfaces::msg::KeyValuePair kv; + kv.key = metadata_keys::TARGET_ID; + kv.value = task.target_id; + stage.metadata.push_back(kv); + stage.target_resource_ids.push_back(task.target_id); + } +} + +std::string PlanBuilder::make_task_suffix(const std::string & task_code) const +{ + if (task_code.empty()) { + return std::string(); + } + std::string suffix = "_"; + suffix.reserve(task_code.size() + 1); + for (char ch : task_code) { + if ((ch >= 'a' && ch <= 'z') || (ch >= 'A' && ch <= 'Z') || (ch >= '0' && ch <= '9')) { + suffix.push_back(ch); + } else { + suffix.push_back('_'); + } + } + return suffix; +} + +StagePlan PlanBuilder::make_stage_for_task(int order_index, const RequestedCalibrationTask & task) const +{ + if (task.stage_type.value == WorkflowStageType::CHASSIS_CALIBRATION_STAGE) { + return make_chassis_stage(order_index, &task); + } + if (task.stage_type.value == WorkflowStageType::CONTROL_CALIBRATION_STAGE) { + return make_control_stage(order_index, &task); + } + if (task.stage_type.value == WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE) { + return make_sensor_intrinsic_stage(order_index, &task); + } + if (task.stage_type.value == WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE) { + return make_sensor_extrinsic_stage(order_index, &task); + } + if (task.stage_type.value == WorkflowStageType::HAND_EYE_CALIBRATION_STAGE) { + return make_hand_eye_stage(order_index, &task); + } + if (task.stage_type.value == WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE) { + return make_external_reference_stage(order_index, &task); + } + + StagePlan stage; + stage.stage_id = "stage_unknown" + make_task_suffix(task.task_code); + stage.stage_type = make_stage_type(WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED); + stage.module_type = make_module_type(CalibrationModuleType::CALIBRATION_MODULE_UNSPECIFIED); + stage.display_name = task.task_code.empty() ? "未知任务" : task.task_code; + stage.order_index = order_index; + stage.executor_endpoint_name = std::string(); + stage.description = "未知的任务模板。"; + stage.auto_generated = true; + apply_task_config(stage, &task); + apply_task_identity(stage, task); + return stage; +} + +std::vector PlanBuilder::build_minimal_plan( + const VehicleProfile & profile, + const WorkshopSessionConfig & config) const +{ + std::vector stages; + int order = 0; + + if (!config.requested_tasks.empty()) { + for (const auto & task : config.requested_tasks) { + if (!task.enabled) { + continue; + } + if (task.stage_type.value != WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED) { + if (task.stage_type.value == WorkflowStageType::CHASSIS_CALIBRATION_STAGE && + !has_capability(profile, CalibrationAbilityType::CHASSIS_CALIBRATION)) { + if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) { + continue; + } + } + if (task.stage_type.value == WorkflowStageType::CONTROL_CALIBRATION_STAGE && + !has_capability(profile, CalibrationAbilityType::CONTROL_CALIBRATION)) { + if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) { + continue; + } + } + if ((task.stage_type.value == WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE || + task.stage_type.value == WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE) && + !has_capability(profile, CalibrationAbilityType::SENSOR_CALIBRATION)) { + if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) { + continue; + } + } + if (task.stage_type.value == WorkflowStageType::HAND_EYE_CALIBRATION_STAGE && + !has_capability(profile, CalibrationAbilityType::HAND_EYE_CALIBRATION)) { + if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) { + continue; + } + } + stages.push_back(make_stage_for_task(order++, task)); + } + } + return stages; + } + + // 外部真值校核：始终加入（除非 requested_tasks 中显式 enabled=false）。 + if (should_include_stage(profile, config, + WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE, 0)) { + const auto * task_cfg = find_requested_task(config, + WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE); + stages.push_back(make_external_reference_stage(order++, task_cfg)); + } + + // 底盘标定。 + if (should_include_stage(profile, config, + WorkflowStageType::CHASSIS_CALIBRATION_STAGE, + CalibrationAbilityType::CHASSIS_CALIBRATION)) { + const auto * task_cfg = find_requested_task(config, + WorkflowStageType::CHASSIS_CALIBRATION_STAGE); + stages.push_back(make_chassis_stage(order++, task_cfg)); + } + + // 运控参数标定。 + if (should_include_stage(profile, config, + WorkflowStageType::CONTROL_CALIBRATION_STAGE, + CalibrationAbilityType::CONTROL_CALIBRATION)) { + const auto * task_cfg = find_requested_task(config, + WorkflowStageType::CONTROL_CALIBRATION_STAGE); + stages.push_back(make_control_stage(order++, task_cfg)); + } + + // 传感器内参标定。 + if (should_include_stage(profile, config, + WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE, + CalibrationAbilityType::SENSOR_CALIBRATION)) { + const auto * task_cfg = find_requested_task(config, + WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE); + stages.push_back(make_sensor_intrinsic_stage(order++, task_cfg)); + } + + // 传感器外参标定。 + if (should_include_stage(profile, config, + WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE, + CalibrationAbilityType::SENSOR_CALIBRATION)) { + const auto * task_cfg = find_requested_task(config, + WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE); + stages.push_back(make_sensor_extrinsic_stage(order++, task_cfg)); + } + + // 手眼标定。 + if (should_include_stage(profile, config, + WorkflowStageType::HAND_EYE_CALIBRATION_STAGE, + CalibrationAbilityType::HAND_EYE_CALIBRATION)) { + const auto * task_cfg = find_requested_task(config, + WorkflowStageType::HAND_EYE_CALIBRATION_STAGE); + stages.push_back(make_hand_eye_stage(order++, task_cfg)); + } + + return stages; +} + +StagePlan PlanBuilder::make_external_reference_stage(int order_index, const RequestedCalibrationTask * task_cfg) const +{ + StagePlan stage; + stage.stage_id = "stage_external_reference"; + stage.stage_type = make_stage_type(WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE); + stage.module_type = make_module_type(CalibrationModuleType::EXTERNAL_LOCALIZATION_MODULE); + stage.display_name = "外部真值校核"; + stage.order_index = order_index; + stage.executor_endpoint_name = "/external_localization/execute_task"; + stage.description = "验证并接入车间外部定位真值参考源。需要 session.config 提供 localization_source_id/workcell_zone_id；stage.metadata 需要提供 external.static_sample_count、external.dynamic_sample_count、external.max_position_stddev_m、external.max_yaw_stddev_rad、external.max_tracking_loss_ratio、external.max_time_sync_offset_ms、external.timeout_sec。"; + stage.retry_limit = 0; + stage.auto_generated = true; + apply_task_config(stage, task_cfg); + return stage; +} + +StagePlan PlanBuilder::make_chassis_stage(int order_index, const RequestedCalibrationTask * task_cfg) const +{ + StagePlan stage; + stage.stage_id = "stage_chassis_calibration"; + stage.stage_type = make_stage_type(WorkflowStageType::CHASSIS_CALIBRATION_STAGE); + stage.module_type = make_module_type(CalibrationModuleType::CHASSIS_MODULE); + stage.display_name = "底盘标定"; + stage.order_index = order_index; + stage.executor_endpoint_name = "/chassis/execute_motion_primitive"; + stage.description = "调用独立底盘标定包执行动作原语任务。stage.metadata 需要提供 primitive_type；当前最小流支持 straight_line，并要求 straight_line.target_distance_m、straight_line.target_speed_ms；可选 straight_line.reverse、brake_when_finished、timeout_sec。"; + stage.retry_limit = 1; + stage.auto_generated = true; + apply_task_config(stage, task_cfg); + if (task_cfg) { + stage.stage_id += make_task_suffix(task_cfg->task_code); + apply_task_identity(stage, *task_cfg); + } + return stage; +} + +StagePlan PlanBuilder::make_control_stage(int order_index, const RequestedCalibrationTask * task_cfg) const +{ + StagePlan stage; + stage.stage_id = "stage_control_calibration"; + stage.stage_type = make_stage_type(WorkflowStageType::CONTROL_CALIBRATION_STAGE); + stage.module_type = make_module_type(CalibrationModuleType::CONTROL_MODULE); + stage.display_name = "运控参数调优"; + stage.order_index = order_index; + stage.executor_endpoint_name = "/control/execute_controller_evaluation"; + stage.description = "调用独立运控标定包执行控制评估任务。stage.metadata 需要提供 control.task_type；当前最小流支持 trajectory_tracking，并要求至少一组 traj_pt_{i}_{x_m|y_m|yaw_rad|speed_ms} 轨迹点。"; + stage.retry_limit = 0; + stage.auto_generated = true; + apply_task_config(stage, task_cfg); + if (task_cfg) { + stage.stage_id += make_task_suffix(task_cfg->task_code); + apply_task_identity(stage, *task_cfg); + } + return stage; +} + +StagePlan PlanBuilder::make_sensor_intrinsic_stage(int order_index, const RequestedCalibrationTask * task_cfg) const +{ + StagePlan stage; + stage.stage_id = "stage_sensor_intrinsic_calibration"; + stage.stage_type = make_stage_type(WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE); + stage.module_type = make_module_type(CalibrationModuleType::SENSOR_INTRINSIC_MODULE); + stage.display_name = "传感器内参标定"; + stage.order_index = order_index; + stage.executor_endpoint_name = "/sensor_calibration/execute_task"; + stage.description = "启动独立传感器标定包执行内参标定任务。stage.metadata 需要提供 sensor.sensor_id、camera_intrinsic.required_image_count；可选 camera_intrinsic.target_board_id、camera_intrinsic.timeout_sec。"; + stage.retry_limit = 0; + stage.auto_generated = true; + apply_task_config(stage, task_cfg); + if (task_cfg) { + stage.stage_id += make_task_suffix(task_cfg->task_code); + apply_task_identity(stage, *task_cfg); + } + return stage; +} + +StagePlan PlanBuilder::make_sensor_extrinsic_stage(int order_index, const RequestedCalibrationTask * task_cfg) const +{ + StagePlan stage; + stage.stage_id = "stage_sensor_extrinsic_calibration"; + stage.stage_type = make_stage_type(WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE); + stage.module_type = make_module_type(CalibrationModuleType::SENSOR_EXTRINSIC_MODULE); + stage.display_name = "传感器外参标定"; + stage.order_index = order_index; + stage.executor_endpoint_name = "/sensor_calibration/execute_task"; + stage.description = "启动独立传感器标定包执行外参标定任务。stage.metadata 需要提供 sensor.sensor_id、sensor_extrinsic.base_frame_id、sensor_extrinsic.required_sample_count；可选 sensor_extrinsic.timeout_sec。"; + stage.retry_limit = 0; + stage.auto_generated = true; + apply_task_config(stage, task_cfg); + if (task_cfg) { + stage.stage_id += make_task_suffix(task_cfg->task_code); + apply_task_identity(stage, *task_cfg); + } + return stage; +} + +StagePlan PlanBuilder::make_hand_eye_stage(int order_index, const RequestedCalibrationTask * task_cfg) const +{ + StagePlan stage; + stage.stage_id = "stage_hand_eye_calibration"; + stage.stage_type = make_stage_type(WorkflowStageType::HAND_EYE_CALIBRATION_STAGE); + stage.module_type = make_module_type(CalibrationModuleType::HAND_EYE_MODULE); + stage.display_name = "手眼标定"; + stage.order_index = order_index; + stage.executor_endpoint_name = "/sensor_calibration/execute_task"; + stage.description = "启动独立传感器标定包执行手眼标定任务。stage.metadata 需要提供 sensor.sensor_id、hand_eye.arm_id、hand_eye.required_pose_count；可选 hand_eye.timeout_sec。"; + stage.retry_limit = 0; + stage.auto_generated = true; + apply_task_config(stage, task_cfg); + if (task_cfg) { + stage.stage_id += make_task_suffix(task_cfg->task_code); + apply_task_identity(stage, *task_cfg); + } + return stage; +} + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/precheck_runner.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/precheck_runner.cpp new file mode 100644 index 0000000..e568d9b --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/precheck_runner.cpp @@ -0,0 +1,153 @@ +#include "workshop_orchestrator_v2/precheck_runner.hpp" + +#include "calibration_workshop_orchestration_interfaces/msg/precheck_item.hpp" + +namespace workshop_orchestrator_v2 +{ + +bool PrecheckRunner::has_metadata_key(const StagePlan & stage, const std::string & key) const +{ + for (const auto & kv : stage.metadata) { + if (kv.key == key) { + return true; + } + } + return false; +} + +bool PrecheckRunner::has_metadata_prefix(const StagePlan & stage, const std::string & prefix) const +{ + for (const auto & kv : stage.metadata) { + if (kv.key.rfind(prefix, 0) == 0) { + return true; + } + } + return false; +} + +WorkshopPrecheckResponse PrecheckRunner::run(const WorkshopSession & session) const +{ + WorkshopPrecheckResponse response; + response.success = true; + response.error_code = make_error_code(ErrorCode::OK); + response.checked_timestamp_us = now_us(); + + auto append_item = [&](const std::string & code, + const std::string & name, + bool passed, + const std::string & message, + uint8_t stage_type, + uint8_t module_type) { + calibration_workshop_orchestration_interfaces::msg::PrecheckItem item; + item.item_code = code; + item.display_name = name; + item.passed = passed; + item.blocking = true; + item.error_code = make_error_code(passed ? ErrorCode::OK : ErrorCode::INVALID_STATE); + item.message = message; + item.related_stage_type = make_stage_type(stage_type); + item.related_module_type = make_module_type(module_type); + response.items.push_back(item); + if (!passed) { + response.blocking_issue_count += 1; + response.all_passed = false; + } + }; + + response.all_passed = true; + response.blocking_issue_count = 0; + + append_item( + "plan_not_empty", + "Execution plan exists", + !session.stage_plan.empty(), + session.stage_plan.empty() ? "Session has no executable stages." : "Session has executable stages.", + WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED, + CalibrationModuleType::CALIBRATION_MODULE_UNSPECIFIED); + + for (const auto & stage : session.stage_plan) { + if (stage.module_type.value == CalibrationModuleType::CHASSIS_MODULE) { + const bool passed = + has_metadata_key(stage, metadata_keys::CHASSIS_PRIMITIVE_TYPE) && + has_metadata_key(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_DISTANCE_M) && + has_metadata_key(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_SPEED_MS); + append_item( + stage.stage_id + ".metadata", + stage.display_name + " metadata", + passed, + passed ? "底盘阶段输入完整。" : "底盘阶段缺少 primitive_type 或直线动作参数。", + stage.stage_type.value, + stage.module_type.value); + } else if (stage.module_type.value == CalibrationModuleType::CONTROL_MODULE) { + const bool passed = + has_metadata_key(stage, metadata_keys::CONTROL_TASK_TYPE) && + has_metadata_prefix(stage, metadata_keys::CONTROL_TRAJECTORY_PREFIX); + append_item( + stage.stage_id + ".metadata", + stage.display_name + " metadata", + passed, + passed ? "运控阶段输入完整。" : "运控阶段缺少 control.task_type 或轨迹点输入。", + stage.stage_type.value, + stage.module_type.value); + } else if ( + stage.module_type.value == CalibrationModuleType::SENSOR_INTRINSIC_MODULE || + stage.module_type.value == CalibrationModuleType::SENSOR_EXTRINSIC_MODULE || + stage.module_type.value == CalibrationModuleType::HAND_EYE_MODULE) { + bool passed = + has_metadata_key(stage, metadata_keys::SENSOR_ID) && + has_metadata_key(stage, metadata_keys::SENSOR_TASK_SUBTYPE); + std::string message = passed ? "传感器阶段基础输入完整。" : "传感器阶段缺少 sensor.sensor_id 或 sensor.task_subtype。"; + + if (passed && stage.module_type.value == CalibrationModuleType::SENSOR_INTRINSIC_MODULE) { + const bool has_image_count = has_metadata_key(stage, metadata_keys::CAMERA_INTRINSIC_REQUIRED_IMAGE_COUNT); + const bool has_board = has_metadata_key(stage, metadata_keys::CAMERA_INTRINSIC_TARGET_BOARD_ID); + passed = has_image_count || has_board; + message = passed ? "传感器内参阶段输入完整。" : "传感器内参阶段缺少图像数或标定板信息。"; + } + + if (passed && stage.module_type.value == CalibrationModuleType::SENSOR_EXTRINSIC_MODULE) { + passed = + has_metadata_key(stage, metadata_keys::SENSOR_EXTRINSIC_BASE_FRAME_ID) && + has_metadata_key(stage, metadata_keys::SENSOR_EXTRINSIC_REQUIRED_SAMPLE_COUNT); + message = passed ? "传感器外参阶段输入完整。" : "传感器外参阶段缺少 base_frame_id 或 required_sample_count。"; + } + + if (passed && stage.module_type.value == CalibrationModuleType::HAND_EYE_MODULE) { + passed = + has_metadata_key(stage, metadata_keys::HAND_EYE_ARM_ID) && + has_metadata_key(stage, metadata_keys::HAND_EYE_REQUIRED_POSE_COUNT); + message = passed ? "手眼阶段输入完整。" : "手眼阶段缺少 arm_id 或 required_pose_count。"; + } + + append_item( + stage.stage_id + ".metadata", + stage.display_name + " metadata", + passed, + message, + stage.stage_type.value, + stage.module_type.value); + } else if (stage.module_type.value == CalibrationModuleType::EXTERNAL_LOCALIZATION_MODULE) { + const bool passed = + has_metadata_key(stage, metadata_keys::EXTERNAL_STATIC_SAMPLE_COUNT) && + has_metadata_key(stage, metadata_keys::EXTERNAL_DYNAMIC_SAMPLE_COUNT) && + has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_POSITION_STDDEV_M) && + has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_YAW_STDDEV_RAD) && + has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_TRACKING_LOSS_RATIO) && + has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_TIME_SYNC_OFFSET_MS) && + has_metadata_key(stage, metadata_keys::EXTERNAL_TIMEOUT_SEC); + append_item( + stage.stage_id + ".metadata", + stage.display_name + " metadata", + passed, + passed ? "external 阶段输入完整。" : "external 阶段缺少真值校核阈值配置。", + stage.stage_type.value, + stage.module_type.value); + } + } + + response.ready_for_start = response.all_passed; + response.message = response.ready_for_start ? "Precheck passed." : "Precheck failed."; + return response; +} + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/report_builder.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/report_builder.cpp new file mode 100644 index 0000000..021d469 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/report_builder.cpp @@ -0,0 +1,72 @@ +#include "workshop_orchestrator_v2/report_builder.hpp" + +#include + +namespace workshop_orchestrator_v2 +{ + +WorkshopReport ReportBuilder::build( + const WorkshopSession & session, + const std::vector & stage_results, + bool overall_success, + int64_t started_timestamp_us, + int64_t finished_timestamp_us, + const std::string & summary) const +{ + WorkshopReport report; + report.session_id = session.session_id; + report.overall_success = overall_success; + report.started_timestamp_us = started_timestamp_us; + report.finished_timestamp_us = finished_timestamp_us; + report.stage_results = stage_results; + report.summary = summary; + + std::string active_parameter_bundle_version; + uint32_t succeeded_stage_count = 0; + for (const auto & stage_result : stage_results) { + report.report_files.insert( + report.report_files.end(), + stage_result.artifacts.begin(), + stage_result.artifacts.end()); + + if (stage_result.success) { + ++succeeded_stage_count; + } + if (!stage_result.parameter_version.empty()) { + active_parameter_bundle_version = stage_result.parameter_version; + } + } + + if (overall_success && active_parameter_bundle_version.empty()) { + active_parameter_bundle_version = session.session_id; + } + + report.active_parameter_bundle_version = overall_success ? active_parameter_bundle_version : ""; + + calibration_common_interfaces::msg::KeyValuePair kv; + kv.key = "stage_count"; + kv.value = std::to_string(stage_results.size()); + report.metadata.push_back(kv); + + kv.key = "succeeded_stage_count"; + kv.value = std::to_string(succeeded_stage_count); + report.metadata.push_back(kv); + + kv.key = "failed_stage_count"; + kv.value = std::to_string(stage_results.size() - succeeded_stage_count); + report.metadata.push_back(kv); + + kv.key = "overall_success"; + kv.value = overall_success ? "true" : "false"; + report.metadata.push_back(kv); + + if (!report.active_parameter_bundle_version.empty()) { + kv.key = "active_parameter_bundle_version"; + kv.value = report.active_parameter_bundle_version; + report.metadata.push_back(kv); + } + + return report; +} + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp new file mode 100644 index 0000000..284c210 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/sensor_gateway_client.cpp @@ -0,0 +1,460 @@ +#include "workshop_orchestrator_v2/sensor_gateway_client.hpp" + +#include +#include +#include +#include +#include + +#include "calibration_sensor_interfaces/msg/hand_eye_calibration_mode.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_task_purpose.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_task_subtype.hpp" +#include "calibration_sensor_interfaces/msg/sensor_calibration_task_type.hpp" +#include "workshop_orchestrator_v2/types.hpp" + +namespace workshop_orchestrator_v2 +{ + +using namespace std::chrono_literals; +using calibration_sensor_interfaces::msg::HandEyeCalibrationMode; +using calibration_sensor_interfaces::msg::SensorCalibrationTaskPurpose; +using calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype; +using calibration_sensor_interfaces::msg::SensorCalibrationTaskType; +using calibration_vehicle_profile_interfaces::msg::CalibrationModuleType; +using calibration_workshop_orchestration_interfaces::msg::StagePlan; +using calibration_workshop_orchestration_interfaces::msg::StageResultSummary; +using calibration_workshop_orchestration_interfaces::msg::WorkshopSession; + +SensorGatewayClient::SensorGatewayClient(rclcpp::Node * node) +: node_(node) +{ + readiness_client_ = node_->create_client("/sensor_calibration/get_readiness"); + execute_task_client_ = rclcpp_action::create_client( + node_, "/sensor_calibration/execute_task"); +} + +std::string SensorGatewayClient::make_request_id() const +{ + std::ostringstream oss; + oss << "sensor_req_" << now_us(); + return oss.str(); +} + +bool SensorGatewayClient::check_ready(std::string & failure_reason) +{ + if (!readiness_client_->wait_for_service(2s)) { + failure_reason = "传感器标定 readiness 服务不可用。"; + return false; + } + + auto request = std::make_shared(); + request->request.agent_name = "sensor_calibration"; + request->request.include_details = true; + + auto future = readiness_client_->async_send_request(request); + if (future.wait_for(3s) != std::future_status::ready) { + failure_reason = "等待传感器标定 readiness 响应超时。"; + return false; + } + + const auto response = future.get(); + if (!response->response.success) { + failure_reason = response->response.message.empty() ? + "传感器标定 readiness 检查失败。" : response->response.message; + return false; + } + + if (!response->response.agent_ready) { + failure_reason = "传感器标定 agent 当前未就绪。"; + return false; + } + + if (!response->response.capture_pipeline_ready) { + failure_reason = "传感器数据采集链路未就绪。"; + return false; + } + + if (!response->response.storage_ready) { + failure_reason = "传感器标定存储链路未就绪，无法保存标定产物。"; + return false; + } + + return true; +} + +uint8_t SensorGatewayClient::resolve_task_type( + const StagePlan & stage, + std::string & failure_reason) const +{ + switch (stage.module_type.value) { + case CalibrationModuleType::SENSOR_INTRINSIC_MODULE: + return SensorCalibrationTaskType::CAMERA_INTRINSIC; + case CalibrationModuleType::SENSOR_EXTRINSIC_MODULE: + return SensorCalibrationTaskType::SENSOR_TO_BASE_EXTRINSIC; + case CalibrationModuleType::HAND_EYE_MODULE: + return SensorCalibrationTaskType::HAND_EYE; + default: + failure_reason = "当前传感器阶段未映射到具体标定任务类型。"; + return SensorCalibrationTaskType::SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED; + } +} + +uint8_t resolve_task_subtype_from_metadata( + const std::unordered_map & metadata_map, + std::string & failure_reason) +{ + const auto subtype_it = metadata_map.find(metadata_keys::SENSOR_TASK_SUBTYPE); + if (subtype_it == metadata_map.end()) { + failure_reason = "传感器阶段缺少 sensor.task_subtype。"; + return SensorCalibrationTaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED; + } + + const auto & value = subtype_it->second; + if (value == "front_camera_intrinsic") { + return SensorCalibrationTaskSubtype::FRONT_CAMERA_INTRINSIC; + } + if (value == "downward_camera_intrinsic") { + return SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_INTRINSIC; + } + if (value == "imu_intrinsic") { + return SensorCalibrationTaskSubtype::IMU_INTRINSIC; + } + if (value == "front_camera_extrinsic") { + return SensorCalibrationTaskSubtype::FRONT_CAMERA_EXTRINSIC; + } + if (value == "downward_camera_extrinsic") { + return SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_EXTRINSIC; + } + if (value == "imu_extrinsic") { + return SensorCalibrationTaskSubtype::IMU_EXTRINSIC; + } + if (value == "lidar_2d_extrinsic") { + return SensorCalibrationTaskSubtype::LIDAR_2D_EXTRINSIC; + } + if (value == "lidar_3d_extrinsic") { + return SensorCalibrationTaskSubtype::LIDAR_3D_EXTRINSIC; + } + if (value == "eye_in_hand") { + return SensorCalibrationTaskSubtype::EYE_IN_HAND; + } + if (value == "eye_to_hand") { + return SensorCalibrationTaskSubtype::EYE_TO_HAND; + } + + failure_reason = "sensor.task_subtype 不支持：" + value; + return SensorCalibrationTaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED; +} + +bool task_and_subtype_match( + uint8_t task_type, + uint8_t task_subtype) +{ + if (task_type == SensorCalibrationTaskType::CAMERA_INTRINSIC) { + return task_subtype == SensorCalibrationTaskSubtype::FRONT_CAMERA_INTRINSIC || + task_subtype == SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_INTRINSIC || + task_subtype == SensorCalibrationTaskSubtype::IMU_INTRINSIC; + } + if (task_type == SensorCalibrationTaskType::SENSOR_TO_BASE_EXTRINSIC) { + return task_subtype == SensorCalibrationTaskSubtype::FRONT_CAMERA_EXTRINSIC || + task_subtype == SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_EXTRINSIC || + task_subtype == SensorCalibrationTaskSubtype::IMU_EXTRINSIC || + task_subtype == SensorCalibrationTaskSubtype::LIDAR_2D_EXTRINSIC || + task_subtype == SensorCalibrationTaskSubtype::LIDAR_3D_EXTRINSIC; + } + if (task_type == SensorCalibrationTaskType::HAND_EYE) { + return task_subtype == SensorCalibrationTaskSubtype::EYE_IN_HAND || + task_subtype == SensorCalibrationTaskSubtype::EYE_TO_HAND; + } + return false; +} + +bool SensorGatewayClient::build_goal( + const WorkshopSession & session, + const StagePlan & stage, + uint8_t selected_task_type, + ExecuteTask::Goal & goal, + std::string & failure_reason) const +{ + + // 填充请求头。 + goal.goal.header.session_id = session.session_id; + goal.goal.header.task_id = stage.stage_id; + goal.goal.header.vehicle_id = session.vehicle_profile_snapshot.base_info.vehicle_id; + goal.goal.header.request_id = make_request_id(); + goal.goal.header.client_send_timestamp_us = now_us(); + goal.goal.header.operator_id = session.operator_info.operator_id; + goal.goal.header.workshop_host = "workshop_orchestrator_v2"; + + goal.goal.test_case_id = stage.stage_id; + + // 当前目的：数据采集（具体标定算法和流程由下游传感器标定包负责）。 + goal.goal.task_purpose.value = SensorCalibrationTaskPurpose::DATA_COLLECTION; + goal.goal.selected_task.value = selected_task_type; + + std::unordered_map metadata_map; + for (const auto & kv : stage.metadata) { + metadata_map[kv.key] = kv.value; + } + + const auto task_subtype = resolve_task_subtype_from_metadata(metadata_map, failure_reason); + if (task_subtype == SensorCalibrationTaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED) { + return false; + } + if (!task_and_subtype_match(selected_task_type, task_subtype)) { + failure_reason = "sensor.task_subtype 与当前模块映射出的 task_type 不匹配。"; + return false; + } + goal.goal.task_subtype.value = task_subtype; + + const auto sensor_id_it = metadata_map.find(metadata_keys::SENSOR_ID); + if (sensor_id_it == metadata_map.end()) { + failure_reason = "传感器阶段缺少 sensor.sensor_id。"; + return false; + } + + if (selected_task_type == SensorCalibrationTaskType::CAMERA_INTRINSIC) { + goal.goal.camera_intrinsic.sensor_id = sensor_id_it->second; + const auto image_count_it = metadata_map.find(metadata_keys::CAMERA_INTRINSIC_REQUIRED_IMAGE_COUNT); + if (image_count_it == metadata_map.end()) { + failure_reason = "camera_intrinsic 缺少 required_image_count。"; + return false; + } + try { + goal.goal.camera_intrinsic.required_image_count = static_cast(std::stoul(image_count_it->second)); + } catch (...) { + failure_reason = "camera_intrinsic.required_image_count 格式错误。"; + return false; + } + const auto board_it = metadata_map.find(metadata_keys::CAMERA_INTRINSIC_TARGET_BOARD_ID); + if (board_it != metadata_map.end()) { + goal.goal.camera_intrinsic.target_board_id = board_it->second; + } + const auto timeout_it = metadata_map.find(metadata_keys::CAMERA_INTRINSIC_TIMEOUT_SEC); + if (timeout_it != metadata_map.end()) { + try { + goal.goal.camera_intrinsic.timeout_sec = std::stod(timeout_it->second); + } catch (...) { + failure_reason = "camera_intrinsic.timeout_sec 格式错误。"; + return false; + } + } + } else if (selected_task_type == SensorCalibrationTaskType::SENSOR_TO_BASE_EXTRINSIC) { + goal.goal.sensor_to_base_extrinsic.sensor_id = sensor_id_it->second; + const auto base_frame_it = metadata_map.find(metadata_keys::SENSOR_EXTRINSIC_BASE_FRAME_ID); + if (base_frame_it == metadata_map.end()) { + failure_reason = "sensor_extrinsic 缺少 base_frame_id。"; + return false; + } + goal.goal.sensor_to_base_extrinsic.base_frame_id = base_frame_it->second; + const auto sample_count_it = metadata_map.find(metadata_keys::SENSOR_EXTRINSIC_REQUIRED_SAMPLE_COUNT); + if (sample_count_it == metadata_map.end()) { + failure_reason = "sensor_extrinsic 缺少 required_sample_count。"; + return false; + } + try { + goal.goal.sensor_to_base_extrinsic.required_sample_count = static_cast(std::stoul(sample_count_it->second)); + } catch (...) { + failure_reason = "sensor_extrinsic.required_sample_count 格式错误。"; + return false; + } + const auto timeout_it = metadata_map.find(metadata_keys::SENSOR_EXTRINSIC_TIMEOUT_SEC); + if (timeout_it != metadata_map.end()) { + try { + goal.goal.sensor_to_base_extrinsic.timeout_sec = std::stod(timeout_it->second); + } catch (...) { + failure_reason = "sensor_extrinsic.timeout_sec 格式错误。"; + return false; + } + } + + if (task_subtype == SensorCalibrationTaskSubtype::FRONT_CAMERA_EXTRINSIC || + task_subtype == SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_EXTRINSIC) + { + goal.goal.sensor_to_base_extrinsic.capture_intent.value = + calibration_sensor_interfaces::msg::CaptureIntentType::CAMERA_EXTRINSIC_CAPTURE; + } else if (task_subtype == SensorCalibrationTaskSubtype::LIDAR_2D_EXTRINSIC) { + goal.goal.sensor_to_base_extrinsic.capture_intent.value = + calibration_sensor_interfaces::msg::CaptureIntentType::LIDAR_2D_EXTRINSIC_CAPTURE; + } else if (task_subtype == SensorCalibrationTaskSubtype::LIDAR_3D_EXTRINSIC) { + goal.goal.sensor_to_base_extrinsic.capture_intent.value = + calibration_sensor_interfaces::msg::CaptureIntentType::LIDAR_3D_EXTRINSIC_CAPTURE; + } else if (task_subtype == SensorCalibrationTaskSubtype::IMU_EXTRINSIC) { + goal.goal.sensor_to_base_extrinsic.capture_intent.value = + calibration_sensor_interfaces::msg::CaptureIntentType::IMU_DYNAMIC_CAPTURE; + } + } else if (selected_task_type == SensorCalibrationTaskType::HAND_EYE) { + goal.goal.hand_eye.sensor_id = sensor_id_it->second; + const auto arm_id_it = metadata_map.find(metadata_keys::HAND_EYE_ARM_ID); + if (arm_id_it == metadata_map.end()) { + failure_reason = "hand_eye 缺少 arm_id。"; + return false; + } + goal.goal.hand_eye.arm_id = arm_id_it->second; + const auto pose_count_it = metadata_map.find(metadata_keys::HAND_EYE_REQUIRED_POSE_COUNT); + if (pose_count_it == metadata_map.end()) { + failure_reason = "hand_eye 缺少 required_pose_count。"; + return false; + } + try { + goal.goal.hand_eye.required_pose_count = static_cast(std::stoul(pose_count_it->second)); + } catch (...) { + failure_reason = "hand_eye.required_pose_count 格式错误。"; + return false; + } + const auto timeout_it = metadata_map.find(metadata_keys::HAND_EYE_TIMEOUT_SEC); + if (timeout_it != metadata_map.end()) { + try { + goal.goal.hand_eye.timeout_sec = std::stod(timeout_it->second); + } catch (...) { + failure_reason = "hand_eye.timeout_sec 格式错误。"; + return false; + } + } + + if (task_subtype == SensorCalibrationTaskSubtype::EYE_IN_HAND) { + goal.goal.hand_eye.mode.value = HandEyeCalibrationMode::EYE_IN_HAND; + } else if (task_subtype == SensorCalibrationTaskSubtype::EYE_TO_HAND) { + goal.goal.hand_eye.mode.value = HandEyeCalibrationMode::EYE_TO_HAND; + } + } + + goal.goal.source_iteration_id = session.session_id; + + return true; +} + +bool SensorGatewayClient::execute_sensor_stage( + const WorkshopSession & session, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + // Step 1：就绪检查。 + if (!check_ready(failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + std::string task_type_error; + const auto selected_task_type = resolve_task_type(stage, task_type_error); + if (selected_task_type == SensorCalibrationTaskType::SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED) { + failure_reason = task_type_error; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + ExecuteTask::Goal goal; + if (!build_goal(session, stage, selected_task_type, goal, failure_reason)) { + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 2：等待 action server 上线。 + if (!execute_task_client_->wait_for_action_server(2s)) { + failure_reason = "传感器标定 action server 不可用。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 3：发送 goal。 + auto goal_handle_future = execute_task_client_->async_send_goal(goal); + if (goal_handle_future.wait_for(5s) != std::future_status::ready) { + failure_reason = "发送传感器标定任务请求超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) { + failure_reason = "传感器标定服务拒绝了本次任务请求。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 4：等待结果，超时 180 秒（传感器标定耗时最长）。 + auto result_future = execute_task_client_->async_get_result(goal_handle); + if (result_future.wait_for(180s) != std::future_status::ready) { + failure_reason = "等待传感器标定结果超时。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + auto wrapped_result = result_future.get(); + if (!wrapped_result.result) { + failure_reason = "传感器标定服务返回了空结果。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + // Step 5：翻译 SensorCalibrationJobResult → StageResultSummary。 + const auto & job_result = wrapped_result.result->result; + result.success = job_result.success; + // 自动验收：数据质量达标 且 标定参数适合写入。 + result.auto_acceptance_passed = job_result.data_quality_passed && job_result.suitable_for_commit; + result.final_job_state = make_job_state( + job_result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED + : calibration_common_interfaces::msg::JobState::FAILED); + result.parameter_version = job_result.recommended_parameter_version; + + // 传感器接口中 artifacts 类型为 CalibrationArtifact[]， + // StageResultSummary.artifacts 为 FileReference[]，需逐条转换。 + result.artifacts.clear(); + for (const auto & artifact : job_result.artifacts) { + calibration_common_interfaces::msg::FileReference ref; + ref.file_name = artifact.file.file_name; + ref.file_uri = artifact.file.file_uri; + ref.size_bytes = artifact.file.size_bytes; + ref.description = artifact.file.description; + ref.digest = artifact.file.digest; + result.artifacts.push_back(ref); + } + + result.summary = job_result.message.empty() ? "传感器标定完成。" : job_result.message; + result.failure_root_cause = job_result.success ? "" : result.summary; + + if (!job_result.success) { + failure_reason = result.summary; + return false; + } + if (!result.auto_acceptance_passed) { + failure_reason = "传感器标定未通过自动验收（数据质量或参数适合性不满足）。"; + result.failure_root_cause = failure_reason; + return false; + } + + return true; +} + +} // namespace workshop_orchestrator_v2 diff --git a/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp new file mode 100644 index 0000000..47f4b19 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp @@ -0,0 +1,997 @@ +#include "workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp" // 对应头文件实现。 + +#include // sleep_for 需要的时间单位。 +#include // 拼接 session_id 时要用字符串流。 +#include // 把整场执行放到后台线程时要用 std::thread。 + +namespace workshop_orchestrator_v2 +{ + +using calibration_workshop_orchestration_interfaces::msg::WorkshopEvent; // 进度通知消息短名。 +using calibration_workshop_orchestration_interfaces::msg::WorkshopEventType; // 进度通知类型短名。 +using calibration_workshop_orchestration_interfaces::srv::AcknowledgeManualStep; // 人工确认接口短名。 +using calibration_workshop_orchestration_interfaces::srv::ApproveStageResult; // 审批接口短名。 +using calibration_workshop_orchestration_interfaces::srv::CreateWorkshopSession; // 创建任务接口短名。 +using calibration_workshop_orchestration_interfaces::srv::GetWorkshopReport; // 取报告接口短名。 +using calibration_workshop_orchestration_interfaces::srv::GetWorkshopSession; // 查任务接口短名。 +using calibration_workshop_orchestration_interfaces::srv::PauseWorkshopSession; // 暂停接口短名。 +using calibration_workshop_orchestration_interfaces::srv::ResumeWorkshopSession; // 恢复接口短名。 + +WorkshopOrchestratorV2Node::WorkshopOrchestratorV2Node(const rclcpp::NodeOptions & options) +: Node("workshop_orchestrator_v2", options) // 创建一个名为 workshop_orchestrator_v2 的 ROS 节点。 +{ + event_publisher_ = this->create_publisher("/workshop_v2/events", 10); // 建一个 topic 发布器，后面把“任务进度通知”都发到这里。 + + create_session_service_ = this->create_service( + "/workshop_v2/create_session", // 外部以后调这个 service 来创建一条新的标定任务。 + std::bind(&WorkshopOrchestratorV2Node::handle_create_session, this, std::placeholders::_1, std::placeholders::_2)); + + get_session_service_ = this->create_service( + "/workshop_v2/get_session", // 外部以后调这个 service 来看任务现在跑到哪了。 + std::bind(&WorkshopOrchestratorV2Node::handle_get_session, this, std::placeholders::_1, std::placeholders::_2)); + + get_report_service_ = this->create_service( + "/workshop_v2/get_report", // 外部以后调这个 service 来拿最终报告。 + std::bind(&WorkshopOrchestratorV2Node::handle_get_report, this, std::placeholders::_1, std::placeholders::_2)); + + pause_session_service_ = this->create_service( + "/workshop_v2/pause_session", + std::bind(&WorkshopOrchestratorV2Node::handle_pause_session, this, std::placeholders::_1, std::placeholders::_2)); + + resume_session_service_ = this->create_service( + "/workshop_v2/resume_session", + std::bind(&WorkshopOrchestratorV2Node::handle_resume_session, this, std::placeholders::_1, std::placeholders::_2)); + + acknowledge_manual_step_service_ = this->create_service( + "/workshop_v2/acknowledge_manual_step", + std::bind(&WorkshopOrchestratorV2Node::handle_acknowledge_manual_step, this, std::placeholders::_1, std::placeholders::_2)); + + approve_stage_result_service_ = this->create_service( + "/workshop_v2/approve_stage_result", + std::bind(&WorkshopOrchestratorV2Node::handle_approve_stage_result, this, std::placeholders::_1, std::placeholders::_2)); + + execute_session_action_server_ = rclcpp_action::create_server( + this, + "/workshop_v2/execute_session", // 外部以后向这个 action 发起“开始跑这台车这次标定”的长任务请求。 + std::bind(&WorkshopOrchestratorV2Node::handle_execute_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&WorkshopOrchestratorV2Node::handle_execute_cancel, this, std::placeholders::_1), + std::bind(&WorkshopOrchestratorV2Node::handle_execute_accepted, this, std::placeholders::_1)); + + // 各模块薄 client 统一在这里初始化。 + // 每个 client 只负责：发请求、等结果、翻译结果，不持有专项实现细节。 + external_localization_client_ = std::make_unique(this); + chassis_gateway_client_ = std::make_unique(this); + control_gateway_client_ = std::make_unique(this); + sensor_gateway_client_ = std::make_unique(this); +} + +void WorkshopOrchestratorV2Node::handle_create_session( + const std::shared_ptr request, + std::shared_ptr response) +{ + SessionRecord record; // 先准备一条新的任务记录，后面把这次标定的基础信息填进去。 + record.session.session_id = make_session_id(); // 给这次标定任务生成唯一 ID；后续查询、执行、拿报告都靠它。 + record.session.state = make_session_state(WorkshopSessionState::DRAFT); // 新任务刚创建时先记成 DRAFT，意思是“任务档案先建好了，但还没正式开跑”。 + record.session.vehicle_profile_snapshot = request->request.vehicle_profile_snapshot; // 保存创建当下这台车的画像快照，后面生成步骤时就按这份信息来。 + record.session.operator_info = request->request.operator_info; // 记下是谁发起了这次标定，后面查责任和追溯都要用。 + record.session.config = request->request.config; // 保存这次任务自己的配置，比如执行开关、策略参数等。 + record.session.workshop_line_id = request->request.workshop_line_id; // 记下这次任务属于哪条产线/工位，方便现场区分。 + record.session.created_timestamp_us = now_us(); // 记录任务创建时间。 + record.session.updated_timestamp_us = record.session.created_timestamp_us; // 刚创建完成时，最近更新时间就等于创建时间。 + + if (request->request.auto_build_execution_plan) { // 如果调用方要求“创建任务时顺手把步骤也列出来”。 + record.session.stage_plan = plan_builder_.build_minimal_plan( + record.session.vehicle_profile_snapshot, + record.session.config); // 立刻生成这次任务要跑的步骤列表。 + record.session.state = make_session_state(WorkshopSessionState::READY); // 既然步骤已经有了，就把任务状态推进到 READY，表示“可以准备开跑了”。 + } + + { + std::lock_guard lock(mutex_); // 加锁，防止后台执行线程和当前 service 同时改 sessions_。 + sessions_[record.session.session_id] = record; // 把这条新任务写进内存表里，key 就是任务 ID。 + } + + response->response.success = true; // 表示“创建任务”这个 service 调用成功了。 + response->response.error_code = make_error_code(ErrorCode::OK); // 没有错误，返回 OK。 + response->response.message = "Session created."; // 给调用方一句简短说明。 + response->response.session_id = record.session.session_id; // 把新任务 ID 返回给外部，外部后面必须靠它继续操作。 + response->response.state = record.session.state; // 把当前状态返回给外部，外部马上就知道是 DRAFT 还是 READY。 + response->response.session = record.session; // 把整条任务快照一起返回，外部不用马上再查一次。 + + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Session created."); // 发一条“新任务已经创建”通知，界面和日志系统可以立刻显示。 +} + +void WorkshopOrchestratorV2Node::handle_get_session( + const std::shared_ptr request, + std::shared_ptr response) +{ + std::lock_guard lock(mutex_); // 查任务前先加锁，避免读到一半后台线程正在修改。 + const auto it = sessions_.find(request->request.session_id); // 按请求里给的任务 ID 去内存表里查。 + if (it == sessions_.end()) { // 这说明外部给的任务 ID 根本不存在。 + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Session not found."; + return; + } + + response->response.success = true; // 查询成功。 + response->response.error_code = make_error_code(ErrorCode::OK); + response->response.message = "OK"; + response->response.session = it->second.session; // 把当前任务快照原样返回给外部。 +} + +void WorkshopOrchestratorV2Node::handle_get_report( + const std::shared_ptr request, + std::shared_ptr response) +{ + std::lock_guard lock(mutex_); // 查报告前先加锁，避免后台线程正在写报告。 + const auto it = sessions_.find(request->request.session_id); // 先找到对应任务。 + if (it == sessions_.end()) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Session not found."; + return; + } + + if (!it->second.report_ready) { // 任务存在，但报告还没生成好。 + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_STATE); + response->response.message = "Report not ready."; + return; + } + + response->response.success = true; + response->response.error_code = make_error_code(ErrorCode::OK); + response->response.message = "OK"; + response->response.report = it->second.report; // 把最终报告返回出去。 +} + +void WorkshopOrchestratorV2Node::handle_pause_session( + const std::shared_ptr request, + std::shared_ptr response) +{ + std::lock_guard lock(mutex_); + const auto it = sessions_.find(request->request.session_id); + if (it == sessions_.end()) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Session not found."; + return; + } + if (it->second.session.state.value != WorkshopSessionState::RUNNING) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_STATE); + response->response.message = "Only running sessions can be paused."; + return; + } + + pause_requested_ = true; + response->response.success = true; + response->response.error_code = make_error_code(ErrorCode::OK); + response->response.message = "Pause requested."; +} + +void WorkshopOrchestratorV2Node::handle_resume_session( + const std::shared_ptr request, + std::shared_ptr response) +{ + std::lock_guard lock(mutex_); + const auto it = sessions_.find(request->request.session_id); + if (it == sessions_.end()) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Session not found."; + return; + } + if (it->second.session.state.value != WorkshopSessionState::PAUSED) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_STATE); + response->response.message = "Only paused sessions can be resumed."; + return; + } + + pause_requested_ = false; + pause_cv_.notify_all(); + response->response.success = true; + response->response.error_code = make_error_code(ErrorCode::OK); + response->response.message = "Session resumed."; +} + +void WorkshopOrchestratorV2Node::handle_acknowledge_manual_step( + const std::shared_ptr request, + std::shared_ptr response) +{ + std::lock_guard lock(mutex_); + const auto it = sessions_.find(request->request.session_id); + if (it == sessions_.end()) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Session not found."; + return; + } + + auto & record = it->second; + if (!record.manual_action_pending || record.waiting_stage_id != request->request.stage_id) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_STATE); + response->response.message = "Session is not waiting for this manual action."; + return; + } + if (!request->request.confirmed) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Manual action must be confirmed."; + return; + } + + record.manual_action_confirmed = true; + record.manual_action_pending = false; + record.waiting_stage_id.clear(); + record.session.state = make_session_state(WorkshopSessionState::RUNNING); + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Manual action confirmed."); + manual_action_cv_.notify_all(); + response->response.success = true; + response->response.error_code = make_error_code(ErrorCode::OK); + response->response.message = "Manual action confirmed."; +} + +void WorkshopOrchestratorV2Node::handle_approve_stage_result( + const std::shared_ptr request, + std::shared_ptr response) +{ + std::lock_guard lock(mutex_); + const auto it = sessions_.find(request->request.session_id); + if (it == sessions_.end()) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Session not found."; + return; + } + + auto & record = it->second; + if (!record.approval_pending || record.waiting_stage_id != request->request.stage_id) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_STATE); + response->response.message = "Session is not waiting for this approval."; + return; + } + if (request->request.decision.value == ApprovalState::APPROVAL_STATE_UNSPECIFIED) { + response->response.success = false; + response->response.error_code = make_error_code(ErrorCode::INVALID_ARGUMENT); + response->response.message = "Approval decision is required."; + return; + } + + record.approval_decided = true; + record.approval_approved = request->request.decision.value == ApprovalState::APPROVED; + record.approval_pending = false; + record.session.state = record.approval_approved + ? make_session_state(WorkshopSessionState::RUNNING) + : make_session_state(WorkshopSessionState::FAILED); + record.session.updated_timestamp_us = now_us(); + publish_event( + record, + record.approval_approved ? WorkshopEventType::SESSION_STATE_CHANGED : WorkshopEventType::STAGE_FAILED, + record.approval_approved ? "Stage approved." : "Stage approval rejected."); + approval_cv_.notify_all(); + response->response.success = true; + response->response.error_code = make_error_code(ErrorCode::OK); + response->response.message = record.approval_approved ? "Stage approved." : "Stage rejected."; +} + +rclcpp_action::GoalResponse WorkshopOrchestratorV2Node::handle_execute_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + std::lock_guard lock(mutex_); // 判断能不能执行前先加锁，避免别人同时改任务状态。 + const auto it = sessions_.find(goal->goal.session_id); // 从执行请求里拿到任务 ID，再去找对应任务。 + if (it == sessions_.end()) { // 外部要求执行一条并不存在的任务。 + RCLCPP_WARN(get_logger(), "Reject execute_session goal: session not found."); + return rclcpp_action::GoalResponse::REJECT; + } + if (it->second.executing) { + RCLCPP_WARN(get_logger(), "Reject execute_session goal: session already executing."); + return rclcpp_action::GoalResponse::REJECT; + } + if (it->second.session.state.value != WorkshopSessionState::READY && + it->second.session.state.value != WorkshopSessionState::DRAFT) { // 只有 READY 和 DRAFT 两种状态允许开始跑。 + RCLCPP_WARN(get_logger(), "Reject execute_session goal: invalid session state %u.", it->second.session.state.value); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; // 条件满足，同意开始跑这条任务。 +} + +rclcpp_action::CancelResponse WorkshopOrchestratorV2Node::handle_execute_cancel( + const std::shared_ptr /*goal_handle*/) +{ + return rclcpp_action::CancelResponse::ACCEPT; // 先接受取消请求；真正停下来的时机在执行线程里判断。 +} + +void WorkshopOrchestratorV2Node::handle_execute_accepted( + const std::shared_ptr goal_handle) +{ + std::thread(std::bind(&WorkshopOrchestratorV2Node::execute_session, this, goal_handle)).detach(); // 新开后台线程去跑整场任务，避免把 ROS 回调线程卡住。 +} + +void WorkshopOrchestratorV2Node::execute_session( + const std::shared_ptr goal_handle) +{ + const auto goal = goal_handle->get_goal(); // 取出外部发来的“开始跑这台车这次标定”的请求内容。 + auto context = prepare_session_for_run(goal->goal.session_id); // 把这条任务切到“准备开跑”状态，并记录真正开始时间。 + + const auto precheck = run_precheck_step(context.session_id); // 正式开跑前先检查：这条任务现在有没有条件开始跑。 + if (!precheck.ready_for_start) { // 如果检查结论是不允许开跑。 + finish_session_precheck_failed(goal_handle, context, precheck); // 直接按“预检失败”路径收尾并返回。 + return; + } + + mark_session_running(context.session_id); // 预检通过后，把整条任务切到“正式运行中”。 + + std::string failure_reason; // 如果中途某一步失败，就把失败原因写到这里。 + if (handle_cancel_if_requested(goal_handle, context)) { // 先看外部有没有在正式执行前就要求取消。 + return; // 如果已经取消并收尾，这里直接结束。 + } + + if (!run_all_stages(goal_handle, context, failure_reason)) { // 按顺序执行每一个步骤。 + if (failure_reason.empty()) { // 失败原因为空，通常表示走的是“取消”分支。 + return; + } + finish_session_failed(goal_handle, context, failure_reason); // 某一步执行失败时，按失败路径收尾。 + return; + } + + finish_session_succeeded(goal_handle, context); // 全部步骤都成功后，按成功路径收尾。 +} + +WorkshopOrchestratorV2Node::SessionExecutionContext WorkshopOrchestratorV2Node::prepare_session_for_run( + const std::string & session_id) +{ + SessionExecutionContext context; // 这是一份“本次执行上下文”，只给 execute_session 这次调用自己用。 + context.session_id = session_id; // 记下当前正在跑的是哪条任务。 + context.started_timestamp_us = now_us(); // 记下真正开始执行的时间，后面生成报告时要用。 + + std::lock_guard lock(mutex_); + auto & record = sessions_.at(session_id); // 取出这条任务记录。 + if (record.session.stage_plan.empty()) { // 如果这条任务之前还没生成步骤列表。 + record.session.stage_plan = plan_builder_.build_minimal_plan( + record.session.vehicle_profile_snapshot, + record.session.config); // 就在正式开跑前补一份最小步骤列表。 + } + record.executing = true; + record.report_ready = false; + record.stage_results.clear(); + record.session.state = make_session_state(WorkshopSessionState::PRECHECK_RUNNING); // 任务状态先切到“正在做开跑前检查”。 + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::PRECHECK_STARTED, "Precheck started."); // 给外部发通知：预检开始了。 + return context; +} + +WorkshopPrecheckResponse WorkshopOrchestratorV2Node::run_precheck_step(const std::string & session_id) +{ + std::lock_guard lock(mutex_); + auto & record = sessions_.at(session_id); + const auto precheck = precheck_runner_.run(record.session); // 真正执行开跑前检查：当前主要看“有没有步骤可跑”。 + record.last_precheck = precheck; // 把这次预检结果存下来，后面查“为什么没开跑”时可以直接返回。 + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::PRECHECK_COMPLETED, precheck.message); // 给外部发通知：预检做完了，并把结论文本一起带出去。 + return precheck; +} + +void WorkshopOrchestratorV2Node::mark_session_running(const std::string & session_id) +{ + std::lock_guard lock(mutex_); + auto & record = sessions_.at(session_id); + record.session.state = make_session_state(WorkshopSessionState::RUNNING); // 现在正式进入“主控正在按步骤跑任务”的状态。 + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Session running."); // 给外部发通知：整场已经正式开始跑了。 +} + +bool WorkshopOrchestratorV2Node::handle_cancel_if_requested( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context) +{ + if (!goal_handle->is_canceling()) { + return false; + } + { + std::lock_guard lock(mutex_); + auto & record = sessions_.at(context.session_id); + record.manual_action_pending = false; + record.approval_pending = false; + } + finish_session_canceled(goal_handle, context); + return true; +} + +bool WorkshopOrchestratorV2Node::run_all_stages( + const std::shared_ptr goal_handle, + SessionExecutionContext & context, + std::string & failure_reason) +{ + std::vector stage_plan; + { + std::lock_guard lock(mutex_); + const auto & record = sessions_.at(context.session_id); + stage_plan = record.session.stage_plan; + } + + if (stage_plan.empty()) { + failure_reason = "Session has no stages to execute."; + return false; + } + + for (const auto & stage : stage_plan) { + if (handle_cancel_if_requested(goal_handle, context)) { + failure_reason.clear(); + return false; + } + + bool stage_succeeded = false; + std::string stage_failure_reason; + for (uint32_t attempt = 0; attempt <= stage.retry_limit; ++attempt) { + failure_reason.clear(); + if (run_single_stage(goal_handle, context.session_id, stage, failure_reason)) { + stage_succeeded = true; + break; + } + stage_failure_reason = failure_reason; + if (goal_handle->is_canceling()) { + failure_reason.clear(); + return false; + } + if (attempt < stage.retry_limit) { + continue; + } + break; + } + + if (!stage_succeeded) { + if (stage.execution_policy.value == StageExecutionPolicy::OPTIONAL || + stage.execution_policy.value == StageExecutionPolicy::SKIP_IF_UNSUPPORTED) { + failure_reason.clear(); + continue; + } + failure_reason = stage_failure_reason.empty() ? "Stage execution failed." : stage_failure_reason; + return false; + } + } + + return true; +} + +bool WorkshopOrchestratorV2Node::run_single_stage( + const std::shared_ptr goal_handle, + const std::string & session_id, + const StagePlan & stage, + std::string & failure_reason) +{ + wait_if_paused(session_id); + mark_stage_started(session_id, stage); // 先把“这一步开始跑了”的状态写出去，让界面马上能看到当前进度。 + + if (stage.requires_manual_confirmation_before_start || + stage.manual_action_type.value != ManualActionType::MANUAL_ACTION_TYPE_UNSPECIFIED) { + if (!wait_for_manual_confirmation(goal_handle, session_id, stage, failure_reason)) { + return false; + } + } + + StageResultSummary result; // 这一步真正执行完后，要把结果写到这里，再统一交给主控记录。 + result.stage_id = stage.stage_id; // 先把结果和当前步骤绑定起来。 + result.stage_type = stage.stage_type; // 带回这一步属于哪类步骤。 + result.module_type = stage.module_type; // 带回这一步属于哪个模块。 + + if (!dispatch_stage_to_module(session_id, stage, result, failure_reason)) { + if (failure_reason.empty()) { + failure_reason = "Stage dispatch failed."; // 如果下层没有给出明确失败原因，这里补一个兜底原因。 + } + + if (result.summary.empty()) { + result.summary = failure_reason; // 把失败原因同步写进阶段结果，方便后面进报告。 + } + if (result.final_job_state.state == JobState::JOB_STATE_UNSPECIFIED) { + result.final_job_state = make_job_state(JobState::FAILED); // 下层没写状态时，这里明确标成失败。 + } + result.success = false; + result.auto_acceptance_passed = false; + record_stage_result(session_id, result); + mark_stage_failed(session_id, stage, failure_reason); // 发一条“这一步失败了”的通知。 + return false; + } + + if (!result.success || !result.auto_acceptance_passed) { + if (failure_reason.empty()) { + failure_reason = result.summary.empty() ? "Stage failed." : result.summary; // 执行器返回失败但没写原因时，这里补齐失败原因。 + } + record_stage_result(session_id, result); + mark_stage_failed(session_id, stage, failure_reason); + return false; + } + + if (stage.requires_approval_before_commit) { + if (!wait_for_stage_approval(goal_handle, session_id, stage, result, failure_reason)) { + record_stage_result(session_id, result); + mark_stage_failed(session_id, stage, failure_reason); + return false; + } + } else { + result.approval_state.value = calibration_workshop_orchestration_interfaces::msg::ApprovalState::APPROVAL_NOT_REQUIRED; + } + + mark_stage_completed(session_id, stage, result); // 真正成功时，统一走“阶段完成”收尾。 + return true; +} + +bool WorkshopOrchestratorV2Node::wait_for_manual_confirmation( + const std::shared_ptr goal_handle, + const std::string & session_id, + const StagePlan & stage, + std::string & failure_reason) +{ + std::unique_lock lock(mutex_); + auto & record = sessions_.at(session_id); + record.waiting_stage_id = stage.stage_id; + record.manual_action_pending = true; + record.manual_action_confirmed = false; + record.session.state = make_session_state(WorkshopSessionState::WAITING_MANUAL_ACTION); + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::MANUAL_ACTION_REQUIRED, "Manual action required.", stage.stage_id, + JobState::RUNNING, stage.stage_type.value, stage.module_type.value, true); + + manual_action_cv_.wait(lock, [&]() { + return !record.manual_action_pending || + goal_handle->is_canceling(); + }); + + if (goal_handle->is_canceling()) { + failure_reason.clear(); + return false; + } + + if (!record.manual_action_confirmed) { + failure_reason = "Manual action was not confirmed."; + return false; + } + + record.waiting_stage_id.clear(); + record.session.state = make_session_state(WorkshopSessionState::RUNNING); + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Manual action completed."); + return true; +} + +bool WorkshopOrchestratorV2Node::wait_for_stage_approval( + const std::shared_ptr goal_handle, + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + std::unique_lock lock(mutex_); + auto & record = sessions_.at(session_id); + record.waiting_stage_id = stage.stage_id; + record.approval_pending = true; + record.approval_decided = false; + record.approval_approved = false; + record.session.state = make_session_state(WorkshopSessionState::WAITING_APPROVAL); + record.session.updated_timestamp_us = now_us(); + result.approval_state.value = calibration_workshop_orchestration_interfaces::msg::ApprovalState::APPROVAL_PENDING; + publish_event(record, WorkshopEventType::APPROVAL_REQUIRED, "Approval required.", stage.stage_id, + JobState::SUCCEEDED, stage.stage_type.value, stage.module_type.value, false, + calibration_workshop_orchestration_interfaces::msg::ApprovalState::APPROVAL_PENDING); + + approval_cv_.wait(lock, [&]() { + return !record.approval_pending || + goal_handle->is_canceling(); + }); + + if (goal_handle->is_canceling()) { + failure_reason.clear(); + return false; + } + + record.waiting_stage_id.clear(); + if (!record.approval_decided || !record.approval_approved) { + result.approval_state.value = calibration_workshop_orchestration_interfaces::msg::ApprovalState::REJECTED; + result.success = false; + result.auto_acceptance_passed = false; + result.rolled_back = true; + result.summary = "Stage approval rejected."; + result.failure_root_cause = result.summary; + failure_reason = result.summary; + record.session.state = make_session_state(WorkshopSessionState::FAILED); + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::STAGE_FAILED, failure_reason, stage.stage_id, + JobState::FAILED, stage.stage_type.value, stage.module_type.value); + return false; + } + + result.approval_state.value = calibration_workshop_orchestration_interfaces::msg::ApprovalState::APPROVED; + record.session.state = make_session_state(WorkshopSessionState::RUNNING); + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Stage approved.", stage.stage_id, + JobState::SUCCEEDED, stage.stage_type.value, stage.module_type.value, false, + calibration_workshop_orchestration_interfaces::msg::ApprovalState::APPROVED); + return true; +} + +bool WorkshopOrchestratorV2Node::dispatch_stage_to_module( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + // 这一层的作用只有一个：根据步骤所属模块，把执行请求分发到正确的模块入口。 + switch (stage.module_type.value) { + case CalibrationModuleType::EXTERNAL_LOCALIZATION_MODULE: + return run_external_localization_stage(session_id, stage, result, failure_reason); + + case CalibrationModuleType::CHASSIS_MODULE: + return run_chassis_stage(session_id, stage, result, failure_reason); + + case CalibrationModuleType::CONTROL_MODULE: + return run_control_stage(session_id, stage, result, failure_reason); + + case CalibrationModuleType::SENSOR_INTRINSIC_MODULE: + case CalibrationModuleType::SENSOR_EXTRINSIC_MODULE: + case CalibrationModuleType::HAND_EYE_MODULE: + return run_sensor_stage(session_id, stage, result, failure_reason); + + default: + failure_reason = "Unsupported module type in stage dispatch."; // 如果计划里出现主控还不认识的模块类型，直接在这里拦住。 + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(JobState::FAILED); + result.summary = failure_reason; + return false; + } +} + +bool WorkshopOrchestratorV2Node::run_external_localization_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + if (!external_localization_client_) { + failure_reason = "External localization client is not initialized."; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + WorkshopSession session_snapshot; + { + std::lock_guard lock(mutex_); + session_snapshot = sessions_.at(session_id).session; // 先拷一份任务快照，避免拿着主锁去等外部 action。 + } + + // 注意：这里仍然是 orchestrator 在“调 external”。 + // 只是 external 的专项实现细节不再继续长在 orchestrator 里，而是收敛到 external_localization_service。 + return external_localization_client_->execute_reference_check( + session_snapshot, + stage, + result, + failure_reason); +} + +bool WorkshopOrchestratorV2Node::run_chassis_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + if (!chassis_gateway_client_) { + failure_reason = "底盘 gateway client 未初始化。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + WorkshopSession session_snapshot; + { + std::lock_guard lock(mutex_); + session_snapshot = sessions_.at(session_id).session; // 拷贝快照，避免持锁等待 action 结果。 + } + + return chassis_gateway_client_->execute_chassis_stage( + session_snapshot, stage, result, failure_reason); +} + +bool WorkshopOrchestratorV2Node::run_control_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + if (!control_gateway_client_) { + failure_reason = "运控 gateway client 未初始化。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + WorkshopSession session_snapshot; + { + std::lock_guard lock(mutex_); + session_snapshot = sessions_.at(session_id).session; // 拷贝快照，避免持锁等待 action 结果。 + } + + return control_gateway_client_->execute_control_stage( + session_snapshot, stage, result, failure_reason); +} + +bool WorkshopOrchestratorV2Node::run_sensor_stage( + const std::string & session_id, + const StagePlan & stage, + StageResultSummary & result, + std::string & failure_reason) +{ + if (!sensor_gateway_client_) { + failure_reason = "传感器 gateway client 未初始化。"; + result.success = false; + result.auto_acceptance_passed = false; + result.final_job_state = make_job_state(JobState::FAILED); + result.summary = failure_reason; + result.failure_root_cause = failure_reason; + return false; + } + + WorkshopSession session_snapshot; + { + std::lock_guard lock(mutex_); + session_snapshot = sessions_.at(session_id).session; // 拷贝快照，避免持锁等待 action 结果。 + } + + return sensor_gateway_client_->execute_sensor_stage( + session_snapshot, stage, result, failure_reason); +} + +void WorkshopOrchestratorV2Node::mark_stage_failed( + const std::string & session_id, + const StagePlan & stage, + const std::string & failure_reason) +{ + std::lock_guard lock(mutex_); + auto & record = sessions_.at(session_id); + record.session.updated_timestamp_us = now_us(); + publish_event( + record, + WorkshopEventType::STAGE_FAILED, + failure_reason, + stage.stage_id, + JobState::FAILED, + stage.stage_type.value, + stage.module_type.value); +} + +void WorkshopOrchestratorV2Node::mark_stage_started(const std::string & session_id, const StagePlan & stage) +{ + std::lock_guard lock(mutex_); + auto & record = sessions_.at(session_id); + record.session.active_stage_id = stage.stage_id; + record.session.updated_timestamp_us = now_us(); + publish_event( + record, + WorkshopEventType::STAGE_STARTED, + "Stage started.", + stage.stage_id, + JobState::RUNNING, + stage.stage_type.value, + stage.module_type.value); +} + +void WorkshopOrchestratorV2Node::mark_stage_completed( + const std::string & session_id, + const StagePlan & stage, + const StageResultSummary & result) +{ + record_stage_result(session_id, result); + std::lock_guard lock(mutex_); + auto & record = sessions_.at(session_id); + record.session.updated_timestamp_us = now_us(); + publish_event( + record, + WorkshopEventType::STAGE_COMPLETED, + "Stage completed.", + stage.stage_id, + JobState::SUCCEEDED, + stage.stage_type.value, + stage.module_type.value); +} + +void WorkshopOrchestratorV2Node::wait_if_paused(const std::string & session_id) +{ + std::unique_lock lock(mutex_); + auto & record = sessions_.at(session_id); + if (!pause_requested_) { + return; + } + + record.session.state = make_session_state(WorkshopSessionState::PAUSED); + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Session paused."); + + pause_cv_.wait(lock, [this]() { return !pause_requested_; }); + + record.session.state = make_session_state(WorkshopSessionState::RUNNING); + record.session.updated_timestamp_us = now_us(); + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Session resumed."); +} + +void WorkshopOrchestratorV2Node::record_stage_result( + const std::string & session_id, + const StageResultSummary & result) +{ + std::lock_guard lock(mutex_); + auto & record = sessions_.at(session_id); + record.stage_results.push_back(result); +} + +void WorkshopOrchestratorV2Node::finish_session_precheck_failed( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context, + const WorkshopPrecheckResponse & /*precheck*/) +{ + auto result = std::make_shared(); + { + std::lock_guard lock(mutex_); + auto & record = sessions_.at(context.session_id); + record.session.state = make_session_state(WorkshopSessionState::FAILED); + record.session.active_stage_id.clear(); + record.session.active_stage_id.clear(); + record.session.updated_timestamp_us = now_us(); + record.report = report_builder_.build( + record.session, + record.stage_results, + false, + context.started_timestamp_us, + now_us(), + "Precheck failed."); + record.executing = false; + record.report_ready = true; + publish_event(record, WorkshopEventType::REPORT_READY, "Report ready: precheck failed."); + + result->result.success = true; + result->result.error_code = make_error_code(ErrorCode::OK); + result->result.message = "Precheck failed."; + result->result.report = record.report; + } + goal_handle->abort(result); +} + +void WorkshopOrchestratorV2Node::finish_session_failed( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context, + const std::string & failure_reason) +{ + auto result = std::make_shared(); + { + std::lock_guard lock(mutex_); + auto & record = sessions_.at(context.session_id); + record.session.state = make_session_state(WorkshopSessionState::FAILED); + record.session.active_stage_id.clear(); + record.session.updated_timestamp_us = now_us(); + record.report = report_builder_.build( + record.session, + record.stage_results, + false, + context.started_timestamp_us, + now_us(), + failure_reason); + record.executing = false; + record.report_ready = true; + publish_event(record, WorkshopEventType::REPORT_READY, "Report ready: session failed."); + + result->result.success = true; + result->result.error_code = make_error_code(ErrorCode::OK); + result->result.message = failure_reason; + result->result.report = record.report; + } + goal_handle->abort(result); +} + +void WorkshopOrchestratorV2Node::finish_session_succeeded( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context) +{ + auto result = std::make_shared(); + { + std::lock_guard lock(mutex_); + auto & record = sessions_.at(context.session_id); + record.session.active_stage_id.clear(); + record.session.state = make_session_state(WorkshopSessionState::SUCCEEDED); + record.session.updated_timestamp_us = now_us(); + record.report = report_builder_.build( + record.session, + record.stage_results, + true, + context.started_timestamp_us, + now_us(), + "Workshop v2 minimal flow succeeded."); + record.executing = false; + record.report_ready = true; + publish_event(record, WorkshopEventType::REPORT_READY, "Report ready."); + + result->result.success = true; + result->result.error_code = make_error_code(ErrorCode::OK); + result->result.message = "Session succeeded."; + result->result.report = record.report; + } + goal_handle->succeed(result); +} + +void WorkshopOrchestratorV2Node::finish_session_canceled( + const std::shared_ptr goal_handle, + const SessionExecutionContext & context) +{ + auto result = std::make_shared(); + { + std::lock_guard lock(mutex_); + auto & record = sessions_.at(context.session_id); + record.session.active_stage_id.clear(); + record.session.state = make_session_state(WorkshopSessionState::CANCELED); + record.session.updated_timestamp_us = now_us(); + record.report = report_builder_.build( + record.session, + record.stage_results, + false, + context.started_timestamp_us, + now_us(), + "Session canceled."); + record.executing = false; + record.report_ready = true; + publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Session canceled."); + + result->result.success = true; + result->result.error_code = make_error_code(ErrorCode::OK); + result->result.message = "Canceled."; + result->result.report = record.report; + } + goal_handle->canceled(result); +} + +void WorkshopOrchestratorV2Node::publish_event( + const SessionRecord & record, + uint8_t event_type, + const std::string & message, + const std::string & stage_id, + uint8_t stage_job_state, + uint8_t stage_type, + uint8_t module_type, + bool requires_manual_ack, + uint8_t approval_state) +{ + WorkshopEvent event; + event.server_timestamp_us = now_us(); + event.session_id = record.session.session_id; + event.stage_id = stage_id; + event.event_type.value = event_type; + event.session_state = record.session.state; + event.stage_job_state = make_job_state(stage_job_state); + event.stage_type = make_stage_type(stage_type); + event.module_type = make_module_type(module_type); + event.requires_manual_ack = requires_manual_ack; + event.approval_state.value = approval_state; + event.message = message; + event_publisher_->publish(event); +} + +std::string WorkshopOrchestratorV2Node::make_session_id() const +{ + static uint64_t counter = 0; + std::ostringstream oss; + oss << "workshop_v2_" << now_us() << "_" << ++counter; + return oss.str(); +} + +} // namespace workshop_orchestrator_v2 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge.zip b/agv_calib_brain/src/win_ubuntu_bridge.zip new file mode 100644 index 0000000..ec9bd33 Binary files /dev/null and b/agv_calib_brain/src/win_ubuntu_bridge.zip differ diff --git a/agv_calib_brain/src/win_ubuntu_bridge/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/CMakeLists.txt deleted file mode 100644 index 9851b47..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/CMakeLists.txt +++ /dev/null @@ -1,159 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(win_ubuntu_bridge) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# ========================================================== -# 1. 寻找 ROS 2 核心库与消息生成器 -# ========================================================== -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(action_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) - -# ========================================================== -# 2. 编译 ROS 2 自定义接口 (Msg / Srv / Action) -# ========================================================== -# 🚨 已经严格按照你截图中的目录结构为你写好 -set(interface_files - "msg/HardwareState.msg" - "msg/ControlTelemetry.msg" - "msg/TrajectoryPoint.msg" - "msg/CameraIntrinsic.msg" - "msg/SensorExtrinsic.msg" - - # chassis - "srv/CommitKinematics.srv" - "srv/ExecuteRawDrive.srv" - "srv/ExecuteRawSteer.srv" - "srv/SetDiagnosticMode.srv" - - # control - "srv/ExecuteOpenLoopCmd.srv" - "srv/ExecuteStepResponse.srv" - "srv/FollowTestTrajectory.srv" - "srv/InjectTuningParams.srv" - "srv/SetControlMode.srv" - - # sensor - "srv/CommitCalibrationResults.srv" - "srv/MoveToObservationPose.srv" - "srv/TriggerSyncCapture.srv" - - "action/DownloadSensorData.action" -) - -rosidl_generate_interfaces(${PROJECT_NAME} - ${interface_files} - DEPENDENCIES std_msgs std_srvs action_msgs -) - -# ========================================================== -# 3. 寻找系统底层的 gRPC 并编译 .proto 文件 -# ========================================================== -find_package(Protobuf REQUIRED) -find_package(PkgConfig REQUIRED) -pkg_check_modules(GRPC REQUIRED IMPORTED_TARGET grpc++) - -set(PROTO_DIR "${CMAKE_CURRENT_SOURCE_DIR}/proto") -file(GLOB PROTO_FILES "${PROTO_DIR}/*.proto") -set(PROTO_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/grpc_gen") -file(MAKE_DIRECTORY ${PROTO_OUT_DIR}) - -find_program(GRPC_CPP_PLUGIN_EXECUTABLE grpc_cpp_plugin REQUIRED) - -set(PROTO_SRCS "") -set(PROTO_HDRS "") -foreach(PROTO_FILE ${PROTO_FILES}) - get_filename_component(FIL_WE ${PROTO_FILE} NAME_WE) - list(APPEND PROTO_SRCS "${PROTO_OUT_DIR}/${FIL_WE}.pb.cc" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.cc") - list(APPEND PROTO_HDRS "${PROTO_OUT_DIR}/${FIL_WE}.pb.h" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.h") - - add_custom_command( - OUTPUT "${PROTO_OUT_DIR}/${FIL_WE}.pb.cc" "${PROTO_OUT_DIR}/${FIL_WE}.pb.h" - "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.cc" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.h" - COMMAND ${Protobuf_PROTOC_EXECUTABLE} - "--experimental_allow_proto3_optional" - "--proto_path=${PROTO_DIR}" - "--cpp_out=${PROTO_OUT_DIR}" - "--grpc_out=${PROTO_OUT_DIR}" - "--plugin=protoc-gen-grpc=${GRPC_CPP_PLUGIN_EXECUTABLE}" - "${PROTO_FILE}" - DEPENDS "${PROTO_FILE}" - COMMENT "🚀 正在翻译 ${FIL_WE}.proto 到 C++ 源码..." - ) -endforeach() - -# 打包 gRPC 协议库 -add_library(${PROJECT_NAME}_grpc_lib SHARED ${PROTO_SRCS}) -target_include_directories(${PROJECT_NAME}_grpc_lib PUBLIC "${PROTO_OUT_DIR}") -target_link_libraries(${PROJECT_NAME}_grpc_lib PkgConfig::GRPC protobuf::libprotobuf) - -# ========================================================== -# 4. 编译三大 Gateway 组件 (ROS 2 Plugins) -# ========================================================== -# 💡 定义一个宏来简化三个网关的编译代码，让 CMake 极度整洁！ -macro(build_gateway_component target_name source_file plugin_class) - add_library(${target_name} SHARED ${source_file}) - target_include_directories(${target_name} PUBLIC - "$" - "$" - ) - ament_target_dependencies(${target_name} rclcpp rclcpp_components rclcpp_action std_msgs std_srvs action_msgs) - target_link_libraries(${target_name} ${PROJECT_NAME}_grpc_lib) - - # 🚨 极度关键：因为接口在同一个包里，必须让 C++ 代码等待 Msg 编译完成后才能编译！ - rosidl_target_interfaces(${target_name} ${PROJECT_NAME} "rosidl_typesupport_cpp") - - rclcpp_components_register_node(${target_name} - PLUGIN "${plugin_class}" - EXECUTABLE ${target_name}_exe - ) -endmacro() - -# ⚠️ 注意：基于你截图的拼写 gatways。如果你改名了，记得同步改掉这里的路径！ -build_gateway_component(chassis_gateway_node "gateways/chassis_gateway_node.cpp" "win_ubuntu_bridge::ChassisGatewayNode") -build_gateway_component(control_gateway_node "gateways/control_gateway_node.cpp" "win_ubuntu_bridge::ControlGatewayNode") -build_gateway_component(sensor_gateway_node "gateways/sensor_gateway_node.cpp" "win_ubuntu_bridge::SensorGatewayNode") - -# ========================================================== -# 5. 编译其他主节点 (master_node 和 brain_node) -# ========================================================== -add_executable(master_node src/master_node.cpp) -target_include_directories(master_node PUBLIC "$" "${PROTO_OUT_DIR}") -ament_target_dependencies(master_node rclcpp behaviortree_cpp_v3) -target_link_libraries(master_node ${PROJECT_NAME}_grpc_lib) -rosidl_target_interfaces(master_node ${PROJECT_NAME} "rosidl_typesupport_cpp") - -add_executable(brain_node src/brain_node.cpp) -target_include_directories(brain_node PUBLIC "$" "${PROTO_OUT_DIR}") -ament_target_dependencies(brain_node rclcpp) -target_link_libraries(brain_node ${PROJECT_NAME}_grpc_lib) -rosidl_target_interfaces(brain_node ${PROJECT_NAME} "rosidl_typesupport_cpp") - -# ========================================================== -# 6. 安装规则 (Install) -# ========================================================== -install(TARGETS - ${PROJECT_NAME}_grpc_lib - chassis_gateway_node chassis_gateway_node_exe - control_gateway_node control_gateway_node_exe - sensor_gateway_node sensor_gateway_node_exe - master_node brain_node - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib/ - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY proto/ DESTINATION share/${PROJECT_NAME}/proto) -# 若有 launch 或 behavior_trees 文件，随时解开下面这行的注释 -install(DIRECTORY launch/ config/ DESTINATION share/${PROJECT_NAME}/launch) - -ament_package() \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/CMakeLists.txt new file mode 100644 index 0000000..0384eb0 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_chassis_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(action_msgs REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) + +set(msg_files + "msg/ChassisWorkMode.msg" + "msg/ChassisWorkModeRequest.msg" + "msg/ChassisReadinessResponse.msg" + "msg/ChassisCapabilityRequest.msg" + "msg/ChassisCapabilityResponse.msg" + "msg/StraightLineCommand.msg" + "msg/ArcCommand.msg" + "msg/InPlaceRotationCommand.msg" + "msg/SteeringSweepCommand.msg" + "msg/LateralTranslationCommand.msg" + "msg/DiagonalMotionCommand.msg" + "msg/ModuleAlignmentCheckCommand.msg" + "msg/CoordinatedSteeringCommand.msg" + "msg/MotionPrimitiveTaskPurpose.msg" + "msg/ChassisMotionPrimitiveType.msg" + "msg/MotionPrimitiveRequest.msg" + "msg/StreamChassisTelemetryRequest.msg" + "msg/WheelModuleState.msg" + "msg/ChassisTelemetry.msg" + "msg/CommonChassisCalibrationParams.msg" + "msg/AckermannCalibrationParams.msg" + "msg/DifferentialCalibrationParams.msg" + "msg/SingleSteerWheelCalibrationParams.msg" + "msg/SteeringModuleCalibrationParam.msg" + "msg/MultiSteerWheelCalibrationParams.msg" + "msg/ChassisSpecificParamsType.msg" + "msg/ChassisCalibrationParameterSet.msg" + "msg/ChassisValidationSummary.msg" + "msg/CommitChassisCalibrationParametersRequest.msg" + "msg/GetAppliedChassisCalibrationParametersRequest.msg" + "msg/AppliedChassisCalibrationParametersResponse.msg" + "msg/ChassisJobResult.msg" +) + +set(srv_files + "srv/Heartbeat.srv" + "srv/GetChassisReadiness.srv" + "srv/SetChassisWorkMode.srv" + "srv/GetChassisCapability.srv" + "srv/StartMotionPrimitive.srv" + "srv/GetChassisJobStatus.srv" + "srv/GetChassisJobResult.srv" + "srv/CancelChassisJob.srv" + "srv/StreamChassisTelemetry.srv" + "srv/CommitChassisCalibrationParameters.srv" + "srv/GetAppliedChassisCalibrationParameters.srv" + "srv/EmergencyBrake.srv" +) + +set(action_files + "action/ExecuteMotionPrimitive.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES std_msgs action_msgs calibration_common_interfaces calibration_vehicle_profile_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/FILE_TREE.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/FILE_TREE.txt new file mode 100644 index 0000000..06e8ca1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/FILE_TREE.txt @@ -0,0 +1,51 @@ +calibration_chassis_interfaces/ + CMakeLists.txt + package.xml + msg/ + AckermannCalibrationParams.msg + AppliedChassisCalibrationParametersResponse.msg + ArcCommand.msg + ChassisCalibrationParameterSet.msg + ChassisCapabilityRequest.msg + ChassisCapabilityResponse.msg + ChassisJobResult.msg + ChassisMotionPrimitiveType.msg + ChassisReadinessResponse.msg + ChassisSpecificParamsType.msg + ChassisTelemetry.msg + ChassisValidationSummary.msg + ChassisWorkMode.msg + ChassisWorkModeRequest.msg + CommitChassisCalibrationParametersRequest.msg + CommonChassisCalibrationParams.msg + CoordinatedSteeringCommand.msg + DiagonalMotionCommand.msg + DifferentialCalibrationParams.msg + GetAppliedChassisCalibrationParametersRequest.msg + InPlaceRotationCommand.msg + LateralTranslationCommand.msg + ModuleAlignmentCheckCommand.msg + MotionPrimitiveRequest.msg + MotionPrimitiveTaskPurpose.msg + MultiSteerWheelCalibrationParams.msg + SingleSteerWheelCalibrationParams.msg + SteeringModuleCalibrationParam.msg + SteeringSweepCommand.msg + StraightLineCommand.msg + StreamChassisTelemetryRequest.msg + WheelModuleState.msg + srv/ + CancelChassisJob.srv + CommitChassisCalibrationParameters.srv + EmergencyBrake.srv + GetAppliedChassisCalibrationParameters.srv + GetChassisCapability.srv + GetChassisJobResult.srv + GetChassisJobStatus.srv + GetChassisReadiness.srv + Heartbeat.srv + SetChassisWorkMode.srv + StartMotionPrimitive.srv + StreamChassisTelemetry.srv + action/ + ExecuteMotionPrimitive.action diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/action/ExecuteMotionPrimitive.action b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/action/ExecuteMotionPrimitive.action new file mode 100644 index 0000000..f82f9e6 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/action/ExecuteMotionPrimitive.action @@ -0,0 +1,22 @@ +# ========================================================= +# 底盘动作原语执行 Action +# 作用：执行一个完整的底盘动作原语任务 +# 来源： +# 1) StartMotionPrimitive +# 2) GetChassisJobStatus +# 3) GetChassisJobResult +# 4) CancelChassisJob +# 说明： +# 1) goal 对应 MotionPrimitiveRequest +# 2) result 对应 ChassisJobResult +# 3) feedback 统一复用 common 的 JobStatus +# ========================================================= + +# Goal：底盘动作原语请求 +MotionPrimitiveRequest goal +--- +# Result：底盘动作任务最终结果 +ChassisJobResult result +--- +# Feedback：执行过程中的状态反馈 +calibration_common_interfaces/JobStatus feedback diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AckermannCalibrationParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AckermannCalibrationParams.msg new file mode 100644 index 0000000..5b6db9d --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AckermannCalibrationParams.msg @@ -0,0 +1,25 @@ +# ========================================================= +# 阿克曼专属参数 +# 作用：适用于阿克曼车 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +bool has_front_left_steer_zero_offset_deg +# 左前舵角零偏 +float64 front_left_steer_zero_offset_deg + +bool has_front_right_steer_zero_offset_deg +# 右前舵角零偏 +float64 front_right_steer_zero_offset_deg + +bool has_rear_left_wheel_radius_m +# 左后轮有效半径 +float64 rear_left_wheel_radius_m + +bool has_rear_right_wheel_radius_m +# 右后轮有效半径 +float64 rear_right_wheel_radius_m + +bool has_steering_ratio +# 转向传动比 +float64 steering_ratio diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AppliedChassisCalibrationParametersResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AppliedChassisCalibrationParametersResponse.msg new file mode 100644 index 0000000..f96955f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AppliedChassisCalibrationParametersResponse.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 查询当前已生效底盘参数响应 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 当前版本 +string parameter_version +# 当前生效参数 +ChassisCalibrationParameterSet params +# 生效时间 +int64 applied_timestamp_us diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ArcCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ArcCommand.msg new file mode 100644 index 0000000..e73ac48 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ArcCommand.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 圆弧动作命令 +# 作用：用于验证曲线行驶能力和曲率误差 +# ========================================================= + +# 目标线速度（m/s） +float64 target_speed_ms +# 目标半径（m） +float64 radius_m +# 圆弧扫角（度） +float64 sweep_angle_deg +# 是否顺时针 +bool clockwise diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCalibrationParameterSet.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCalibrationParameterSet.msg new file mode 100644 index 0000000..2ab038c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCalibrationParameterSet.msg @@ -0,0 +1,24 @@ +# ========================================================= +# 底盘标定参数总包 +# 作用：统一表达不同底盘类型的标定结果 +# 说明： +# 1) proto 中 specific_params 为 oneof +# 2) ROS2 中通过 selected_specific_params + 全部 payload 字段共同表达 +# ========================================================= + +# 底盘类型 +calibration_vehicle_profile_interfaces/ChassisType chassis_type +# 通用参数 +CommonChassisCalibrationParams common + +# 当前生效的专属参数类型 +ChassisSpecificParamsType selected_specific_params + +# 阿克曼参数 +AckermannCalibrationParams ackermann +# 差速参数 +DifferentialCalibrationParams differential +# 单舵轮参数 +SingleSteerWheelCalibrationParams single_steer +# 多舵轮参数 +MultiSteerWheelCalibrationParams multi_steer diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityRequest.msg new file mode 100644 index 0000000..f2252ba --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityRequest.msg @@ -0,0 +1,7 @@ +# ========================================================= +# 查询底盘能力请求 +# 作用：运行前确认当前车辆支持哪些动作 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityResponse.msg new file mode 100644 index 0000000..ff64e9a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityResponse.msg @@ -0,0 +1,33 @@ +# ========================================================= +# 查询底盘能力响应 +# 作用：返回当前车辆底盘形式和支持的动作原语 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 底盘类型 +calibration_vehicle_profile_interfaces/ChassisType chassis_type + +# 是否支持直线动作 +bool supports_straight_line +# 是否支持圆弧动作 +bool supports_arc +# 是否支持原地旋转 +bool supports_in_place_rotation +# 是否支持舵角扫动 +bool supports_steer_sweep +# 是否支持倒车动作 +bool supports_reverse_motion + +# 是否支持横移动作 +bool supports_lateral_translation +# 是否支持斜移动作 +bool supports_diagonal_motion +# 是否支持模块零位检查 +bool supports_module_alignment_check +# 是否支持模块协同转向 +bool supports_coordinated_steering diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisJobResult.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisJobResult.msg new file mode 100644 index 0000000..f713bf4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisJobResult.msg @@ -0,0 +1,29 @@ +# ========================================================= +# 底盘任务结果 +# 作用：返回底盘动作执行后的分析摘要，供编排器判断是否可进入下一步 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 任务 ID +string job_id + +# 数据质量是否达标 +bool data_quality_passed +# 是否适合写入参数 +bool suitable_for_commit +# 推荐参数版本 +string recommended_parameter_version + +# 估计的直线跑偏 +float64 estimated_straight_line_bias +# 验证摘要 +ChassisValidationSummary validation_summary +# 本轮估计参数 +ChassisCalibrationParameterSet estimated_params +# 关联产物 +calibration_common_interfaces/FileReference[] artifacts diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisMotionPrimitiveType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisMotionPrimitiveType.msg new file mode 100644 index 0000000..a44a330 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisMotionPrimitiveType.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 底盘动作原语类型 +# 作用：补足 proto oneof primitive 的 ROS2 表达 +# ========================================================= + +uint8 CHASSIS_MOTION_PRIMITIVE_UNSPECIFIED=0 +uint8 STRAIGHT_LINE=1 +uint8 ARC=2 +uint8 IN_PLACE_ROTATION=3 +uint8 STEERING_SWEEP=4 +uint8 LATERAL_TRANSLATION=5 +uint8 DIAGONAL_MOTION=6 +uint8 MODULE_ALIGNMENT=7 +uint8 COORDINATED_STEERING=8 + +# 当前动作原语取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisReadinessResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisReadinessResponse.msg new file mode 100644 index 0000000..d94cbb2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisReadinessResponse.msg @@ -0,0 +1,28 @@ +# ========================================================= +# 底盘服务就绪响应 +# 作用：返回当前底盘服务是否满足执行条件 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message + +# 服务本身是否就绪 +bool agent_ready +# 底盘驱动是否在线 +bool chassis_driver_online +# 运控链路是否可执行动作 +bool motion_control_ready +# 急停是否释放 +bool estop_released +# 当前车辆是否允许移动 +bool vehicle_safe_to_move +# 是否可回传遥测 +bool telemetry_available +# 不满足项 +calibration_common_interfaces/ReadinessIssue[] issues +# 检查时间 +int64 checked_timestamp_us diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisSpecificParamsType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisSpecificParamsType.msg new file mode 100644 index 0000000..c50fc14 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisSpecificParamsType.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 底盘专属参数类型 +# 作用：补足 proto oneof specific_params 的 ROS2 表达 +# ========================================================= + +uint8 CHASSIS_SPECIFIC_PARAMS_UNSPECIFIED=0 +uint8 ACKERMANN=1 +uint8 DIFFERENTIAL=2 +uint8 SINGLE_STEER=3 +uint8 MULTI_STEER=4 + +# 当前专属参数类型取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisTelemetry.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisTelemetry.msg new file mode 100644 index 0000000..8421308 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisTelemetry.msg @@ -0,0 +1,37 @@ +# ========================================================= +# 底盘遥测数据 +# 作用：回传原始底盘状态供 Linux 做误差分析 +# 发送方：Windows 车端代理 +# 接收方：Ubuntu 车间电脑 +# ========================================================= + +# 硬件时间戳 +int64 hardware_timestamp_us +# 当前底盘类型 +calibration_vehicle_profile_interfaces/ChassisType chassis_type + +# 车端里程计 X（m） +float64 odom_x_m +# 车端里程计 Y（m） +float64 odom_y_m +# 车端里程计偏航角（rad） +float64 odom_yaw_rad +# 实际线速度（m/s） +float64 linear_velocity_ms +# 实际角速度（rad/s） +float64 angular_velocity_rads + +# 所有轮 / 舵模块状态 +WheelModuleState[] modules + +# 是否急停 +bool estop_engaged +# 驱动器错误码 +uint32 driver_error_code +# 当前任务 ID +string active_job_id + +# 估计横向滑移 +float64 lateral_slip_estimate +# 估计曲率 +float64 curvature_estimate diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisValidationSummary.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisValidationSummary.msg new file mode 100644 index 0000000..ecca595 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisValidationSummary.msg @@ -0,0 +1,21 @@ +# ========================================================= +# 底盘验证摘要 +# 作用：返回底盘专项自动验收的关键指标 +# ========================================================= + +# 最大横向误差 +float64 max_lateral_error_m +# 最大航向误差 +float64 max_yaw_error_rad +# 横向误差均方根 +float64 rms_lateral_error_m +# 航向误差均方根 +float64 rms_yaw_error_rad +# 重复性误差 +float64 repeatability_error_m +# 曲率误差 +float64 curvature_error +# 模块一致性误差 +float64 module_consistency_error +# 自动验收是否通过 +bool auto_acceptance_passed diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkMode.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkMode.msg new file mode 100644 index 0000000..c46d464 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkMode.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 底盘工作模式 +# 来源：ChassisWorkModeRequest.Mode +# 作用：切换底盘到标定准备、直接执行、验证等模式 +# ========================================================= + +uint8 CHASSIS_MODE_UNSPECIFIED=0 +uint8 NORMAL_MODE=1 +uint8 CALIBRATION_READY_MODE=2 +uint8 DIRECT_EXECUTION_MODE=3 +uint8 VALIDATION_MODE=4 + +# 当前模式取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkModeRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkModeRequest.msg new file mode 100644 index 0000000..ecf1bed --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkModeRequest.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 底盘工作模式请求 +# 作用：切换底盘到不同工作模式 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 目标模式 +ChassisWorkMode target_mode +# 切换原因 +string reason diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommitChassisCalibrationParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommitChassisCalibrationParametersRequest.msg new file mode 100644 index 0000000..33f1d21 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommitChassisCalibrationParametersRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 写入底盘标定参数请求 +# 作用：将 Linux 求解结果写入车端 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 参数版本号 +string parameter_version +# 参数总包 +ChassisCalibrationParameterSet params +# 写入原因 +string commit_reason +# 参数摘要 +calibration_common_interfaces/FileDigest digest +# 是否持久化 +bool persistent_write diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommonChassisCalibrationParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommonChassisCalibrationParams.msg new file mode 100644 index 0000000..18d9a64 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommonChassisCalibrationParams.msg @@ -0,0 +1,29 @@ +# ========================================================= +# 通用底盘参数 +# 作用：不同底盘形式都可能共用的几何 / 补偿参数 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +bool has_effective_wheel_base_m +# 有效轴距（m） +float64 effective_wheel_base_m + +bool has_effective_track_width_m +# 有效轮距（m） +float64 effective_track_width_m + +bool has_longitudinal_scale +# 纵向里程比例补偿 +float64 longitudinal_scale + +bool has_lateral_scale +# 横向里程比例补偿 +float64 lateral_scale + +bool has_yaw_scale +# 航向比例补偿 +float64 yaw_scale + +bool has_straight_line_bias +# 直线跑偏补偿 +float64 straight_line_bias diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CoordinatedSteeringCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CoordinatedSteeringCommand.msg new file mode 100644 index 0000000..014e98a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CoordinatedSteeringCommand.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 模块协同转向命令 +# 作用：用于多舵轮模块协同转向与一致性测试 +# ========================================================= + +# 参与测试的模块 ID 列表 +string[] module_ids +# 目标舵角（度） +float64 target_angle_deg +# 保持时长（秒） +float64 hold_time_sec diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DiagonalMotionCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DiagonalMotionCommand.msg new file mode 100644 index 0000000..3b27707 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DiagonalMotionCommand.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 斜移动作命令 +# 作用：用于多舵轮底盘斜向运动能力测试 +# ========================================================= + +# 目标速度（m/s） +float64 target_speed_ms +# 目标距离（m） +float64 target_distance_m +# 目标运动方向角（度） +float64 heading_deg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DifferentialCalibrationParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DifferentialCalibrationParams.msg new file mode 100644 index 0000000..6509fa7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DifferentialCalibrationParams.msg @@ -0,0 +1,25 @@ +# ========================================================= +# 差速专属参数 +# 作用：适用于差速 AGV +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +bool has_left_wheel_radius_m +# 左轮有效半径 +float64 left_wheel_radius_m + +bool has_right_wheel_radius_m +# 右轮有效半径 +float64 right_wheel_radius_m + +bool has_axle_track_width_m +# 驱动轮间距 +float64 axle_track_width_m + +bool has_left_encoder_scale +# 左编码器比例系数 +float64 left_encoder_scale + +bool has_right_encoder_scale +# 右编码器比例系数 +float64 right_encoder_scale diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/GetAppliedChassisCalibrationParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/GetAppliedChassisCalibrationParametersRequest.msg new file mode 100644 index 0000000..bbba706 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/GetAppliedChassisCalibrationParametersRequest.msg @@ -0,0 +1,6 @@ +# ========================================================= +# 查询当前已生效底盘参数请求 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/InPlaceRotationCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/InPlaceRotationCommand.msg new file mode 100644 index 0000000..cccb97c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/InPlaceRotationCommand.msg @@ -0,0 +1,9 @@ +# ========================================================= +# 原地旋转动作命令 +# 作用：用于差速、多舵轮等底盘的旋转能力测试 +# ========================================================= + +# 目标旋转角（度） +float64 target_yaw_deg +# 目标角速度（度/秒） +float64 target_angular_vel_deg_s diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/LateralTranslationCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/LateralTranslationCommand.msg new file mode 100644 index 0000000..9ec8930 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/LateralTranslationCommand.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 横移动作命令 +# 作用：用于多舵轮底盘横向平移能力测试 +# ========================================================= + +# 目标横移速度（m/s） +float64 target_speed_ms +# 目标横移距离（m） +float64 target_distance_m +# 是否向左移动 +bool move_left diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ModuleAlignmentCheckCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ModuleAlignmentCheckCommand.msg new file mode 100644 index 0000000..475e036 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ModuleAlignmentCheckCommand.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 模块零位检查命令 +# 作用：用于多舵轮每个舵轮模块零位与安装状态检查 +# ========================================================= + +# 待检查模块 ID 列表 +string[] module_ids +# 目标零位角（度） +float64 target_zero_deg +# 允许误差（度） +float64 tolerance_deg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveRequest.msg new file mode 100644 index 0000000..8769396 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveRequest.msg @@ -0,0 +1,44 @@ +# ========================================================= +# 底盘动作原语请求 +# 作用：让车端执行一个标准化动作 +# 发送方：Ubuntu 车间电脑 +# 接收方：Windows 车端代理 +# 说明： +# 1) proto 中使用 oneof primitive +# 2) ROS2 中通过 selected_primitive + 全部 payload 字段共同表达 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 测试用例 ID +string test_case_id + +# 任务目的 +MotionPrimitiveTaskPurpose task_purpose + +# 当前生效的动作原语类型 +ChassisMotionPrimitiveType selected_primitive + +# 直线动作 +StraightLineCommand straight_line +# 圆弧动作 +ArcCommand arc +# 原地旋转 +InPlaceRotationCommand in_place_rotation +# 舵角扫动 +SteeringSweepCommand steering_sweep +# 横移 +LateralTranslationCommand lateral_translation +# 斜移 +DiagonalMotionCommand diagonal_motion +# 模块零位检查 +ModuleAlignmentCheckCommand module_alignment +# 模块协同转向 +CoordinatedSteeringCommand coordinated_steering + +# 结束后是否刹停 +bool brake_when_finished +# 任务超时时间 +float64 timeout_sec +# 来源迭代号 +string source_iteration_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveTaskPurpose.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveTaskPurpose.msg new file mode 100644 index 0000000..23c085e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveTaskPurpose.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 底盘动作任务目的 +# 来源：MotionPrimitiveRequest.TaskPurpose +# 作用：区分数据采集、参数验证和直接检查 +# ========================================================= + +uint8 CHASSIS_TASK_PURPOSE_UNSPECIFIED=0 +uint8 DATA_COLLECTION=1 +uint8 VALIDATION=2 +uint8 DIRECT_CHECK=3 + +# 当前任务目的取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MultiSteerWheelCalibrationParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MultiSteerWheelCalibrationParams.msg new file mode 100644 index 0000000..9b003eb --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MultiSteerWheelCalibrationParams.msg @@ -0,0 +1,7 @@ +# ========================================================= +# 多舵轮专属参数 +# 作用：适用于多舵轮 AGV +# ========================================================= + +# 各舵轮模块参数 +SteeringModuleCalibrationParam[] modules diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SingleSteerWheelCalibrationParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SingleSteerWheelCalibrationParams.msg new file mode 100644 index 0000000..71cf939 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SingleSteerWheelCalibrationParams.msg @@ -0,0 +1,21 @@ +# ========================================================= +# 单舵轮专属参数 +# 作用：适用于单舵轮 AGV +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +bool has_drive_wheel_radius_m +# 驱动轮有效半径 +float64 drive_wheel_radius_m + +bool has_steer_zero_offset_deg +# 舵角零偏 +float64 steer_zero_offset_deg + +bool has_steering_ratio +# 转向比 +float64 steering_ratio + +bool has_drive_encoder_scale +# 驱动编码器比例 +float64 drive_encoder_scale diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringModuleCalibrationParam.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringModuleCalibrationParam.msg new file mode 100644 index 0000000..cf3f5ab --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringModuleCalibrationParam.msg @@ -0,0 +1,24 @@ +# ========================================================= +# 多舵轮单模块参数 +# 作用：用于多舵轮车每个模块的独立参数 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +# 模块 ID +string module_id + +bool has_wheel_radius_m +# 模块轮半径 +float64 wheel_radius_m + +bool has_steer_zero_offset_deg +# 模块舵角零偏 +float64 steer_zero_offset_deg + +bool has_module_pos_x_m +# 模块在 base_link 下的 X +float64 module_pos_x_m + +bool has_module_pos_y_m +# 模块在 base_link 下的 Y +float64 module_pos_y_m diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringSweepCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringSweepCommand.msg new file mode 100644 index 0000000..05555ca --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringSweepCommand.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 舵角扫动命令 +# 作用：用于阿克曼、单舵轮、多舵轮的转向零位和转向响应测试 +# ========================================================= + +# 目标舵角（度） +float64 target_angle_deg +# 扫动幅值（度） +float64 sweep_amplitude_deg +# 扫动频率（Hz） +float64 sweep_frequency_hz +# 持续时长（秒） +float64 duration_sec diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StraightLineCommand.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StraightLineCommand.msg new file mode 100644 index 0000000..611f3da --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StraightLineCommand.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 直线动作命令 +# 作用：用于验证直线行驶能力和线速度误差 +# ========================================================= + +# 目标线速度（m/s） +float64 target_speed_ms +# 目标距离（m） +float64 target_distance_m +# 是否倒车 +bool reverse diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StreamChassisTelemetryRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StreamChassisTelemetryRequest.msg new file mode 100644 index 0000000..06fe075 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StreamChassisTelemetryRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 底盘遥测流请求 +# 作用：指定底盘遥测内容和频率 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 期望上报频率 +uint32 expected_hz +# 是否包含里程计 +bool include_odom +# 是否包含轮组状态 +bool include_wheel_state +# 是否包含驱动器状态 +bool include_driver_state +# 是否包含模块级状态 +bool include_module_state diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/WheelModuleState.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/WheelModuleState.msg new file mode 100644 index 0000000..d9273ca --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/msg/WheelModuleState.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 单个轮 / 舵模块状态 +# 作用：兼容差速、单舵轮、多舵轮、阿克曼等形式 +# ========================================================= + +# 模块 ID，例如 fl / fr / drive_center +string module_id +# 编码器累计脉冲 +int64 encoder_ticks +# 当前轮速（RPM） +float64 wheel_speed_rpm +# 当前舵角（度），无舵角则可置 0 +float64 steer_angle_deg +# 电机电流（A） +float64 motor_current_amp diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/package.xml new file mode 100644 index 0000000..efedbff --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/package.xml @@ -0,0 +1,25 @@ + + + calibration_chassis_interfaces + 0.0.1 + ROS 2 interface package generated from chassis_calibration.proto. + + user + Proprietary + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + action_msgs + calibration_common_interfaces + calibration_vehicle_profile_interfaces + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/chassis_calibration.proto b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/proto/chassis_calibration.proto similarity index 61% rename from agv_calib_brain/src/win_ubuntu_bridge/proto/chassis_calibration.proto rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/proto/chassis_calibration.proto index d2b30f9..c7cb854 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/chassis_calibration.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/proto/chassis_calibration.proto @@ -5,13 +5,22 @@ package agv.calibration.chassis; import "calibration_common.proto"; import "vehicle_profile.proto"; +// ========================================================= +// 单位约定： +// 1) 所有“状态量、结果量、误差量、位姿量”中的角度统一使用 rad +// 2) 所有“人工输入更直观的命令量、限幅量、目标角度”允许使用 deg +// 3) 字段名必须显式带单位后缀，例如 _rad / _deg / _m / _ms +// 4) 服务端与客户端都必须严格按字段后缀做单位转换，禁止隐式假定 +// ========================================================= + // ========================================================= // 文件作用：底盘标定执行代理协议 // 运行关系： // Ubuntu 车间电脑(Linux) -> Windows 车端代理 // 说明： -// 1) Linux 负责底盘参数求解、误差分析、验证判断 +// 1) Linux 负责底盘参数求解、误差分析、自动验收、验证判断 // 2) Windows 车端只负责执行动作、回传原始状态、写入参数 +// 3) 该协议同时覆盖：底盘预检 / 数据采集 / 参数验证 / 参数写入 // ========================================================= // ========================================================= @@ -24,6 +33,10 @@ service AgvCalibChassisService { rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) returns (.agv.calibration.common.HeartbeatResponse); + // 查询底盘服务当前是否具备执行条件 + rpc GetChassisReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (ChassisReadinessResponse); + // 设置底盘标定工作模式 rpc SetChassisWorkMode(ChassisWorkModeRequest) returns (.agv.calibration.common.StandardResponse); @@ -67,7 +80,7 @@ service AgvCalibChassisService { // ========================================================= // 底盘工作模式 -// 作用：切换底盘到标定准备、直接执行等模式 +// 作用：切换底盘到标定准备、直接执行、验证等模式 // ========================================================= message ChassisWorkModeRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 @@ -84,6 +97,25 @@ message ChassisWorkModeRequest { string reason = 3; // 切换原因 } +// ========================================================= +// 底盘服务就绪响应 +// 作用：返回当前底盘服务是否满足执行条件 +// ========================================================= +message ChassisReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool chassis_driver_online = 5; // 底盘驱动是否在线 + bool motion_control_ready = 6; // 运控链路是否可执行动作 + bool estop_released = 7; // 急停是否释放 + bool vehicle_safe_to_move = 8; // 当前车辆是否允许移动 + bool telemetry_available = 9; // 是否可回传遥测 + repeated .agv.calibration.common.ReadinessIssue issues = 10; // 不满足项 + int64 checked_timestamp_us = 11; // 检查时间 +} + // ========================================================= // 查询底盘能力请求 // 作用：运行前确认当前车辆支持哪些动作 @@ -101,11 +133,17 @@ message ChassisCapabilityResponse { .agv.calibration.common.ErrorCode error_code = 2; // 错误码 string message = 3; // 说明 .agv.calibration.vehicle.profile.ChassisType chassis_type = 4; // 底盘类型 - bool supports_straight_line = 5; // 是否支持直线动作 - bool supports_arc = 6; // 是否支持圆弧动作 - bool supports_in_place_rotation = 7; // 是否支持原地旋转 - bool supports_steer_sweep = 8; // 是否支持舵角扫动 - bool supports_reverse_motion = 9; // 是否支持倒车动作 + + bool supports_straight_line = 5; // 是否支持直线动作 + bool supports_arc = 6; // 是否支持圆弧动作 + bool supports_in_place_rotation = 7; // 是否支持原地旋转 + bool supports_steer_sweep = 8; // 是否支持舵角扫动 + bool supports_reverse_motion = 9; // 是否支持倒车动作 + + bool supports_lateral_translation = 10; // 是否支持横移动作 + bool supports_diagonal_motion = 11; // 是否支持斜移动作 + bool supports_module_alignment_check = 12; // 是否支持模块零位检查 + bool supports_coordinated_steering = 13; // 是否支持模块协同转向 } // ========================================================= @@ -134,8 +172,8 @@ message ArcCommand { // 作用：用于差速、多舵轮等底盘的旋转能力测试 // ========================================================= message InPlaceRotationCommand { - double target_yaw_deg = 1; // 目标旋转角（度） - double target_angular_vel_deg_s = 2; // 目标角速度（度/秒） + double target_yaw_deg = 1; // 目标旋转角（度） + double target_angular_vel_deg_s = 2; // 目标角速度（度/秒） } // ========================================================= @@ -143,10 +181,50 @@ message InPlaceRotationCommand { // 作用：用于阿克曼、单舵轮、多舵轮的转向零位和转向响应测试 // ========================================================= message SteeringSweepCommand { - double target_angle_deg = 1; // 目标舵角（度） - double sweep_amplitude_deg = 2; // 扫动幅值（度） - double sweep_frequency_hz = 3; // 扫动频率（Hz） - double duration_sec = 4; // 持续时长（秒） + double target_angle_deg = 1; // 目标舵角（度） + double sweep_amplitude_deg = 2; // 扫动幅值（度） + double sweep_frequency_hz = 3; // 扫动频率（Hz） + double duration_sec = 4; // 持续时长（秒） +} + +// ========================================================= +// 横移动作命令 +// 作用：用于多舵轮底盘横向平移能力测试 +// ========================================================= +message LateralTranslationCommand { + double target_speed_ms = 1; // 目标横移速度（m/s） + double target_distance_m = 2; // 目标横移距离（m） + bool move_left = 3; // 是否向左移动 +} + +// ========================================================= +// 斜移动作命令 +// 作用：用于多舵轮底盘斜向运动能力测试 +// ========================================================= +message DiagonalMotionCommand { + double target_speed_ms = 1; // 目标速度（m/s） + double target_distance_m = 2; // 目标距离（m） + double heading_deg = 3; // 目标运动方向角（度） +} + +// ========================================================= +// 模块零位检查命令 +// 作用：用于多舵轮每个舵轮模块零位与安装状态检查 +// ========================================================= +message ModuleAlignmentCheckCommand { + repeated string module_ids = 1; // 待检查模块 ID 列表 + double target_zero_deg = 2; // 目标零位角（度） + double tolerance_deg = 3; // 允许误差（度） +} + +// ========================================================= +// 模块协同转向命令 +// 作用：用于多舵轮模块协同转向与一致性测试 +// ========================================================= +message CoordinatedSteeringCommand { + repeated string module_ids = 1; // 参与测试的模块 ID 列表 + double target_angle_deg = 2; // 目标舵角（度） + double hold_time_sec = 3; // 保持时长（秒） } // ========================================================= @@ -169,15 +247,19 @@ message MotionPrimitiveRequest { TaskPurpose task_purpose = 3; // 任务目的 oneof primitive { - StraightLineCommand straight_line = 4; // 直线动作 - ArcCommand arc = 5; // 圆弧动作 - InPlaceRotationCommand in_place_rotation = 6;// 原地旋转 - SteeringSweepCommand steering_sweep = 7; // 舵角扫动 + StraightLineCommand straight_line = 4; // 直线动作 + ArcCommand arc = 5; // 圆弧动作 + InPlaceRotationCommand in_place_rotation = 6; // 原地旋转 + SteeringSweepCommand steering_sweep = 7; // 舵角扫动 + LateralTranslationCommand lateral_translation = 8; // 横移 + DiagonalMotionCommand diagonal_motion = 9; // 斜移 + ModuleAlignmentCheckCommand module_alignment = 10; // 模块零位检查 + CoordinatedSteeringCommand coordinated_steering = 11; // 模块协同转向 } - bool brake_when_finished = 8; // 结束后是否刹停 - double timeout_sec = 9; // 任务超时时间 - string source_iteration_id = 10; // 来源迭代号 + bool brake_when_finished = 12; // 结束后是否刹停 + double timeout_sec = 13; // 任务超时时间 + string source_iteration_id = 14; // 来源迭代号 } // ========================================================= @@ -190,6 +272,7 @@ message StreamChassisTelemetryRequest { bool include_odom = 3; // 是否包含里程计 bool include_wheel_state = 4; // 是否包含轮组状态 bool include_driver_state = 5; // 是否包含驱动器状态 + bool include_module_state = 6; // 是否包含模块级状态 } // ========================================================= @@ -197,11 +280,11 @@ message StreamChassisTelemetryRequest { // 作用：兼容差速、单舵轮、多舵轮、阿克曼等形式 // ========================================================= message WheelModuleState { - string module_id = 1; // 模块 ID，例如 fl / fr / drive_center - int64 encoder_ticks = 2; // 编码器累计脉冲 - double wheel_speed_rpm = 3; // 当前轮速（RPM） - double steer_angle_deg = 4; // 当前舵角（度），无舵角则可置 0 - double motor_current_amp = 5; // 电机电流（A） + string module_id = 1; // 模块 ID，例如 fl / fr / drive_center + int64 encoder_ticks = 2; // 编码器累计脉冲 + double wheel_speed_rpm = 3; // 当前轮速（RPM） + double steer_angle_deg = 4; // 当前舵角（度），无舵角则可置 0 + double motor_current_amp = 5; // 电机电流（A） } // ========================================================= @@ -211,17 +294,23 @@ message WheelModuleState { // 接收方：Ubuntu 车间电脑 // ========================================================= message ChassisTelemetry { - int64 hardware_timestamp_us = 1; // 硬件时间戳 + int64 hardware_timestamp_us = 1; // 硬件时间戳 .agv.calibration.vehicle.profile.ChassisType chassis_type = 2; // 当前底盘类型 - double odom_x_m = 3; // 车端里程计 X（m） - double odom_y_m = 4; // 车端里程计 Y（m） - double odom_yaw_rad = 5; // 车端里程计偏航角（rad） - double linear_velocity_ms = 6; // 实际线速度（m/s） - double angular_velocity_rads = 7; // 实际角速度（rad/s） - repeated WheelModuleState modules = 8; // 所有轮 / 舵模块状态 - bool estop_engaged = 9; // 是否急停 - uint32 driver_error_code = 10; // 驱动器错误码 - string active_job_id = 11; // 当前任务 ID + + double odom_x_m = 3; // 车端里程计 X（m） + double odom_y_m = 4; // 车端里程计 Y（m） + double odom_yaw_rad = 5; // 车端里程计偏航角（rad） + double linear_velocity_ms = 6; // 实际线速度（m/s） + double angular_velocity_rads = 7; // 实际角速度（rad/s） + + repeated WheelModuleState modules = 8; // 所有轮 / 舵模块状态 + + bool estop_engaged = 9; // 是否急停 + uint32 driver_error_code = 10; // 驱动器错误码 + string active_job_id = 11; // 当前任务 ID + + double lateral_slip_estimate = 12; // 估计横向滑移 + double curvature_estimate = 13; // 估计曲率 } // ========================================================= @@ -254,11 +343,11 @@ message AckermannCalibrationParams { // 作用：适用于差速 AGV // ========================================================= message DifferentialCalibrationParams { - optional double left_wheel_radius_m = 1; // 左轮有效半径 - optional double right_wheel_radius_m = 2; // 右轮有效半径 - optional double axle_track_width_m = 3; // 驱动轮间距 - optional double left_encoder_scale = 4; // 左编码器比例系数 - optional double right_encoder_scale = 5; // 右编码器比例系数 + optional double left_wheel_radius_m = 1; // 左轮有效半径 + optional double right_wheel_radius_m = 2; // 右轮有效半径 + optional double axle_track_width_m = 3; // 驱动轮间距 + optional double left_encoder_scale = 4; // 左编码器比例系数 + optional double right_encoder_scale = 5; // 右编码器比例系数 } // ========================================================= @@ -301,13 +390,28 @@ message ChassisCalibrationParameterSet { CommonChassisCalibrationParams common = 2; // 通用参数 oneof specific_params { - AckermannCalibrationParams ackermann = 3; // 阿克曼参数 - DifferentialCalibrationParams differential = 4;// 差速参数 + AckermannCalibrationParams ackermann = 3; // 阿克曼参数 + DifferentialCalibrationParams differential = 4; // 差速参数 SingleSteerWheelCalibrationParams single_steer = 5; // 单舵轮参数 MultiSteerWheelCalibrationParams multi_steer = 6; // 多舵轮参数 } } +// ========================================================= +// 底盘验证摘要 +// 作用：返回底盘专项自动验收的关键指标 +// ========================================================= +message ChassisValidationSummary { + double max_lateral_error_m = 1; // 最大横向误差 + double max_yaw_error_rad = 2; // 最大航向误差 + double rms_lateral_error_m = 3; // 横向误差均方根 + double rms_yaw_error_rad = 4; // 航向误差均方根 + double repeatability_error_m = 5; // 重复性误差 + double curvature_error = 6; // 曲率误差 + double module_consistency_error = 7; // 模块一致性误差 + bool auto_acceptance_passed = 8; // 自动验收是否通过 +} + // ========================================================= // 写入底盘标定参数请求 // 作用：将 Linux 求解结果写入车端 @@ -349,11 +453,13 @@ message ChassisJobResult { .agv.calibration.common.ErrorCode error_code = 2; // 错误码 string message = 3; // 说明 string job_id = 4; // 任务 ID - bool data_quality_passed = 5; // 数据质量是否达标 - bool suitable_for_commit = 6; // 是否适合写入参数 - string recommended_parameter_version = 7; // 推荐参数版本 - double max_lateral_error_m = 8; // 最大横向误差 - double max_yaw_error_rad = 9; // 最大航向误差 - double estimated_straight_line_bias = 10; // 估计的直线跑偏 + + bool data_quality_passed = 5; // 数据质量是否达标 + bool suitable_for_commit = 6; // 是否适合写入参数 + string recommended_parameter_version = 7; // 推荐参数版本 + + double estimated_straight_line_bias = 8; // 估计的直线跑偏 + ChassisValidationSummary validation_summary = 9; // 验证摘要 + ChassisCalibrationParameterSet estimated_params = 10; // 本轮估计参数 repeated .agv.calibration.common.FileReference artifacts = 11; // 关联产物 } \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CancelChassisJob.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CancelChassisJob.srv new file mode 100644 index 0000000..4316f36 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CancelChassisJob.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 取消底盘任务 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CommitChassisCalibrationParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CommitChassisCalibrationParameters.srv new file mode 100644 index 0000000..d3b7664 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CommitChassisCalibrationParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 写入底盘标定结果 +# ========================================================= + +CommitChassisCalibrationParametersRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/EmergencyBrake.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/EmergencyBrake.srv new file mode 100644 index 0000000..81d017a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/EmergencyBrake.srv @@ -0,0 +1,12 @@ +# ========================================================= +# 紧急刹停 +# 对应 proto： +# rpc EmergencyBrake(.agv.calibration.common.Empty) +# returns (.agv.calibration.common.StandardResponse); +# 说明： +# 1) 不自定义 Empty.msg +# 2) 这里使用空请求 section +# ========================================================= + +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetAppliedChassisCalibrationParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetAppliedChassisCalibrationParameters.srv new file mode 100644 index 0000000..1ccae6f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetAppliedChassisCalibrationParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询当前已生效底盘标定参数 +# ========================================================= + +GetAppliedChassisCalibrationParametersRequest request +--- +AppliedChassisCalibrationParametersResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisCapability.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisCapability.srv new file mode 100644 index 0000000..7b9ff39 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisCapability.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询车端底盘能力 +# ========================================================= + +ChassisCapabilityRequest request +--- +ChassisCapabilityResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisJobResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisJobResult.srv new file mode 100644 index 0000000..a62a29a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisJobResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询底盘任务结果 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +ChassisJobResult response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisJobStatus.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisJobStatus.srv new file mode 100644 index 0000000..1dcb8c6 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisJobStatus.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询底盘任务状态 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/JobStatus response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisReadiness.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisReadiness.srv new file mode 100644 index 0000000..6f92961 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetChassisReadiness.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询底盘服务当前是否具备执行条件 +# ========================================================= + +calibration_common_interfaces/AgentReadinessRequest request +--- +ChassisReadinessResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/Heartbeat.srv new file mode 100644 index 0000000..c7a7adc --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/Heartbeat.srv @@ -0,0 +1,9 @@ +# ========================================================= +# 心跳保活 +# 运行位置：Windows 车端电脑 +# 调用方：Ubuntu 车间电脑 +# ========================================================= + +calibration_common_interfaces/HeartbeatRequest request +--- +calibration_common_interfaces/HeartbeatResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/SetChassisWorkMode.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/SetChassisWorkMode.srv new file mode 100644 index 0000000..71fbe08 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/SetChassisWorkMode.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 设置底盘标定工作模式 +# ========================================================= + +ChassisWorkModeRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/StartMotionPrimitive.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/StartMotionPrimitive.srv new file mode 100644 index 0000000..c71c674 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/StartMotionPrimitive.srv @@ -0,0 +1,10 @@ +# ========================================================= +# 启动一个底盘动作原语任务 +# 说明： +# 1) 这是对 proto 原始 RPC 的 1:1 保真映射 +# 2) 真正的 ROS2 长任务主通路建议优先使用 action/ExecuteMotionPrimitive.action +# ========================================================= + +MotionPrimitiveRequest request +--- +calibration_common_interfaces/JobAccepted response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/StreamChassisTelemetry.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/StreamChassisTelemetry.srv new file mode 100644 index 0000000..30bf7ea --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_chassis_interfaces/srv/StreamChassisTelemetry.srv @@ -0,0 +1,11 @@ +# ========================================================= +# 打开底盘遥测流 +# 说明： +# 1) proto 原始语义是 server streaming +# 2) ROS2 中真正的实时流请使用 ChassisTelemetry topic +# 3) 该 srv 负责配置 / 开启 / 更新遥测推送策略 +# ========================================================= + +StreamChassisTelemetryRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/CMakeLists.txt new file mode 100644 index 0000000..52369cd --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_common_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(action_msgs REQUIRED) + +set(msg_files + "msg/AgentReadinessRequest.msg" + "msg/ErrorCode.msg" + "msg/FileDigest.msg" + "msg/FileReference.msg" + "msg/HeartbeatRequest.msg" + "msg/HeartbeatResponse.msg" + "msg/JobAccepted.msg" + "msg/JobQuery.msg" + "msg/JobState.msg" + "msg/JobStatus.msg" + "msg/KeyValuePair.msg" + "msg/Pose3D.msg" + "msg/ReadinessIssue.msg" + "msg/RequestHeader.msg" + "msg/StandardResponse.msg" + "msg/Vector3D.msg" +) + +set(srv_files + "srv/GetAgentReadiness.srv" + "srv/Heartbeat.srv" + "srv/QueryJobStatus.srv" +) + +set(action_files + "action/MonitorJob.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES action_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/action/MonitorJob.action b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/action/MonitorJob.action new file mode 100644 index 0000000..877b8c8 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/action/MonitorJob.action @@ -0,0 +1,18 @@ +# ========================================================= +# 长任务监控 Action +# 作用：基于 job_id 持续跟踪一个长任务状态，直到任务结束 +# 对应 proto：JobQuery + JobStatus +# 说明： +# 1) 这是对 proto 中 JobQuery + JobStatus 轮询模型的 ROS2 Action 化封装 +# 2) 专项任务本身的执行 Action 仍应在各自领域 proto 中定义 +# 3) common 层只负责提供统一的长任务状态跟踪能力 +# ========================================================= + +# Goal：要监控哪个长任务 +JobQuery query +--- +# Result：任务结束时返回最终状态 +JobStatus final_status +--- +# Feedback：任务执行过程中持续返回当前状态 +JobStatus current_status \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/AgentReadinessRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/AgentReadinessRequest.msg new file mode 100644 index 0000000..120c367 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/AgentReadinessRequest.msg @@ -0,0 +1,18 @@ +# ========================================================= +# Agent 就绪查询请求 +# 作用：用于查询某个服务 / 代理 / 资源集合当前是否具备执行条件 +# 使用场景： +# 1) 外部定位就绪检查 +# 2) 车端控制服务就绪检查 +# 3) 底盘服务就绪检查 +# 4) 传感器服务就绪检查 +# ========================================================= + +# 请求头 +RequestHeader header +# 目标服务 / 代理名称 +string agent_name +# 指定资源 ID 列表，例如 sensor_id +string[] target_resource_ids +# 是否返回详细问题列表 +bool include_details \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/ErrorCode.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/ErrorCode.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/ErrorCode.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/ErrorCode.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/FileDigest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/FileDigest.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/FileDigest.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/FileDigest.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/FileReference.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/FileReference.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/FileReference.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/FileReference.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/HeartbeatRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/HeartbeatRequest.msg new file mode 100644 index 0000000..3ad29ef --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/HeartbeatRequest.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 心跳请求 +# 作用：保持 Linux 与 Windows 车端之间的在线状态 +# 发送方：Ubuntu 车间电脑 +# 接收方：Windows 车端代理 / Linux 子服务 +# ========================================================= + +# 请求头 +RequestHeader header +# 服务名 / 代理名 +string agent_name +# 下次期望心跳间隔（ms） +int32 expect_next_heartbeat_ms \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/HeartbeatResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/HeartbeatResponse.msg new file mode 100644 index 0000000..395e269 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/HeartbeatResponse.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 心跳响应 +# 作用：反馈当前是否在线、是否可接任务 +# 发送方：服务端 +# 接收方：调用方 +# ========================================================= + +# 是否正常 +bool success +# 错误码 +ErrorCode error_code +# 说明文字 +string message +# 响应时间戳 +int64 server_timestamp_us +# 车辆 / 服务是否准备好接任务 +bool vehicle_ready \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobAccepted.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobAccepted.msg new file mode 100644 index 0000000..57d40f8 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobAccepted.msg @@ -0,0 +1,18 @@ +# ========================================================= +# 长任务受理响应 +# 作用：耗时任务先返回 job_id，后续轮询状态 +# 发送方：服务端 +# 接收方：Ubuntu 车间电脑 +# ========================================================= + +# 是否受理成功 +bool accepted + +# 未受理时的错误码 +ErrorCode error_code + +# 说明 +string message + +# 长任务 ID +string job_id \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/JobQuery.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobQuery.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/JobQuery.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobQuery.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/JobState.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobState.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/JobState.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobState.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobStatus.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobStatus.msg new file mode 100644 index 0000000..e8d6eac --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/JobStatus.msg @@ -0,0 +1,27 @@ +# ========================================================= +# 长任务状态响应 +# 作用：返回任务执行状态、进度、错误信息 +# 发送方：服务端 +# 接收方：Ubuntu 车间电脑 +# ========================================================= + +# 任务 ID +string job_id + +# 当前状态 +JobState state + +# 进度，建议范围 0.0 ~ 1.0 +float64 progress + +# 当前错误码 +ErrorCode error_code + +# 状态说明 +string message + +# 服务端时间戳 +int64 server_timestamp_us + +# 是否适合自动重试 +bool safe_to_retry \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/KeyValuePair.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/KeyValuePair.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/KeyValuePair.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/KeyValuePair.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/Pose3D.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/Pose3D.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/Pose3D.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/Pose3D.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/ReadinessIssue.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/ReadinessIssue.msg new file mode 100644 index 0000000..47b8d5b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/ReadinessIssue.msg @@ -0,0 +1,18 @@ +# ========================================================= +# 就绪问题项 +# 作用：表达“当前为什么还不能执行任务” +# 使用场景：各专项服务 readiness 响应中的 issues 列表 +# ========================================================= + +# 问题代码，便于程序识别 +string issue_code +# 问题说明 +string message +# 映射到统一错误码 +ErrorCode mapped_error_code +# 是否为阻塞问题 +bool blocking +# 关联资源 ID，例如某个传感器 ID +string related_resource_id +# 扩展元数据 +KeyValuePair[] metadata \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/RequestHeader.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/RequestHeader.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/RequestHeader.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/RequestHeader.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/StandardResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/StandardResponse.msg new file mode 100644 index 0000000..4cfc144 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/StandardResponse.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 通用短响应 +# 作用：用于同步 RPC 的标准响应 +# 发送方：服务端 +# 接收方：调用方 +# ========================================================= + +# 是否成功 +bool success + +# 错误码 +ErrorCode error_code + +# 说明文字 +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/Vector3D.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/Vector3D.msg similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/msg/Vector3D.msg rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/msg/Vector3D.msg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/package.xml new file mode 100644 index 0000000..b9429d5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/package.xml @@ -0,0 +1,20 @@ + + + calibration_common_interfaces + 0.0.1 + ROS 2 interfaces converted from workshop_orchestration.proto for the automated calibration workshop. + user + Apache-2.0 + + ament_cmake + rosidl_default_generators + + action_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/calibration_common.proto b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/proto/calibration_common.proto similarity index 69% rename from agv_calib_brain/src/win_ubuntu_bridge/proto/calibration_common.proto rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/proto/calibration_common.proto index 488ab34..4d44308 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/calibration_common.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/proto/calibration_common.proto @@ -2,6 +2,30 @@ syntax = "proto3"; package agv.calibration.common; +// ├── msg/ +// │ ├── Vector3d.msg +// │ ├── Pose3dEuler.msg +// │ ├── RequestHeader.msg +// │ ├── ErrorCode.msg +// │ ├── StandardResponse.msg +// │ ├── JobState.msg +// │ ├── JobAccepted.msg +// │ ├── JobQuery.msg +// │ ├── JobStatus.msg +// │ ├── HeartbeatRequest.msg +// │ ├── HeartbeatResponse.msg +// │ ├── AgentReadinessRequest.msg +// │ ├── ReadinessIssue.msg +// │ ├── FileDigest.msg +// │ ├── FileReference.msg +// │ └── KeyValuePair.msg +// ├── srv/ +// │ ├── Heartbeat.srv +// │ ├── QueryJobStatus.srv +// │ └── GetAgentReadiness.srv +// └── action/ +// └── MonitorJob.action + // ========================================================= // 文件作用：自动化标定车间公共协议定义 // 使用范围： @@ -17,7 +41,7 @@ package agv.calibration.common; message Empty {} // ========================================================= -// 三维向量 +// 三维向量（兼容命名 1） // 作用：用于表达三轴偏置、平移量、加速度等三维数据 // ========================================================= message Vector3D { @@ -26,8 +50,9 @@ message Vector3D { double z = 3; // Z 轴分量 } + // ========================================================= -// 六自由度位姿 +// 六自由度位姿（兼容命名 1） // 作用：用于表达 base_link、传感器、外部定位坐标系之间的位姿关系 // 说明： // 1) 平移单位为米 @@ -64,25 +89,25 @@ message RequestHeader { // 作用：所有服务统一返回，便于编排器与状态机处理 // ========================================================= enum ErrorCode { - ERROR_CODE_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; // 需要参数回滚 + ERROR_CODE_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; // 数据质量不足 } @@ -180,6 +205,36 @@ message HeartbeatResponse { bool vehicle_ready = 5; // 车辆 / 服务是否准备好接任务 } +// ========================================================= +// Agent 就绪查询请求 +// 作用：用于查询某个服务 / 代理 / 资源集合当前是否具备执行条件 +// 使用场景： +// 1) 外部定位就绪检查 +// 2) 车端控制服务就绪检查 +// 3) 底盘服务就绪检查 +// 4) 传感器服务就绪检查 +// ========================================================= +message AgentReadinessRequest { + RequestHeader header = 1; // 请求头 + string agent_name = 2; // 目标服务 / 代理名称 + repeated string target_resource_ids = 3; // 指定资源 ID 列表，例如 sensor_id + bool include_details = 4; // 是否返回详细问题列表 +} + +// ========================================================= +// 就绪问题项 +// 作用：表达“当前为什么还不能执行任务” +// 使用场景：各专项服务 readiness 响应中的 issues 列表 +// ========================================================= +message ReadinessIssue { + string issue_code = 1; // 问题代码，便于程序识别 + string message = 2; // 问题说明 + ErrorCode mapped_error_code = 3; // 映射到统一错误码 + bool blocking = 4; // 是否为阻塞问题 + string related_resource_id = 5; // 关联资源 ID，例如某个传感器 ID + repeated KeyValuePair metadata = 6; // 扩展元数据 +} + // ========================================================= // 文件摘要 // 作用：用于参数包、URDF、数据文件校验 diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/GetAgentReadiness.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/GetAgentReadiness.srv new file mode 100644 index 0000000..144ef57 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/GetAgentReadiness.srv @@ -0,0 +1,17 @@ +# ========================================================= +# Agent 就绪查询服务 +# 作用：用于查询某个服务 / 代理 / 资源集合当前是否具备执行条件 +# 对应 proto：AgentReadinessRequest + ReadinessIssue +# 说明： +# 1) proto 中只有请求和问题项，没有统一响应壳 +# 2) ROS2 srv 这里补足 ready 与 response，便于直接调用 +# ========================================================= + +AgentReadinessRequest request +--- +# 本次 readiness 查询本身是否调用成功 +StandardResponse response +# 当前是否具备执行条件 +bool ready +# 详细问题列表 +ReadinessIssue[] issues \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/Heartbeat.srv new file mode 100644 index 0000000..d3a5af2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/Heartbeat.srv @@ -0,0 +1,9 @@ +# ========================================================= +# 心跳服务 +# 作用：保持 Linux 与 Windows 车端之间的在线状态 +# 对应 proto：HeartbeatRequest + HeartbeatResponse +# ========================================================= + +HeartbeatRequest request +--- +HeartbeatResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/QueryJobStatus.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/QueryJobStatus.srv new file mode 100644 index 0000000..fc56a84 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_common_interfaces/srv/QueryJobStatus.srv @@ -0,0 +1,9 @@ +# ========================================================= +# 长任务状态查询服务 +# 作用：通过 job_id 查询执行状态 +# 对应 proto：JobQuery + JobStatus +# ========================================================= + +JobQuery query +--- +JobStatus status \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/CMakeLists.txt new file mode 100644 index 0000000..60811d4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_control_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(action_msgs REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) + +set(msg_files + "msg/AccelerationDecelerationTask.msg" + "msg/ActiveControllerParametersResponse.msg" + "msg/CommitControllerParametersRequest.msg" + "msg/ControlJobResult.msg" + "msg/ControlReadinessResponse.msg" + "msg/ControlTelemetry.msg" + "msg/ControlValidationSummary.msg" + "msg/ControlWorkMode.msg" + "msg/ControlWorkModeRequest.msg" + "msg/ControllerEvaluationRequest.msg" + "msg/ControllerEvaluationTaskPurpose.msg" + "msg/ControllerEvaluationTaskType.msg" + "msg/ControllerParameterKind.msg" + "msg/ControllerParameterPack.msg" + "msg/ControllerParameterSet.msg" + "msg/GetActiveControllerParametersRequest.msg" + "msg/InjectControllerParametersRequest.msg" + "msg/LQRParams.msg" + "msg/LateralMPCParams.msg" + "msg/LongitudinalMPCParams.msg" + "msg/PIDParams.msg" + "msg/PurePursuitParams.msg" + "msg/StopAccuracyTask.msg" + "msg/StreamControlTelemetryRequest.msg" + "msg/TrajectoryPoint.msg" + "msg/TrajectoryTrackingTask.msg" + "msg/VelocityStepTask.msg" +) + +set(srv_files + "srv/CancelControlJob.srv" + "srv/CommitControllerParameters.srv" + "srv/EmergencyStop.srv" + "srv/GetActiveControllerParameters.srv" + "srv/GetControlJobResult.srv" + "srv/GetControlJobStatus.srv" + "srv/GetControlReadiness.srv" + "srv/Heartbeat.srv" + "srv/InjectControllerParameters.srv" + "srv/SetControlWorkMode.srv" + "srv/StartControllerEvaluation.srv" + "srv/StreamControlTelemetry.srv" +) + +set(action_files + "action/ExecuteControllerEvaluation.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES std_msgs action_msgs calibration_common_interfaces calibration_vehicle_profile_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/FILE_TREE.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/FILE_TREE.txt new file mode 100644 index 0000000..97dd4e5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/FILE_TREE.txt @@ -0,0 +1,46 @@ +calibration_control_interfaces/ + CMakeLists.txt + package.xml + action/ + ExecuteControllerEvaluation.action + msg/ + AccelerationDecelerationTask.msg + ActiveControllerParametersResponse.msg + CommitControllerParametersRequest.msg + ControlJobResult.msg + ControlReadinessResponse.msg + ControlTelemetry.msg + ControlValidationSummary.msg + ControlWorkMode.msg + ControlWorkModeRequest.msg + ControllerEvaluationRequest.msg + ControllerEvaluationTaskPurpose.msg + ControllerEvaluationTaskType.msg + ControllerParameterKind.msg + ControllerParameterPack.msg + ControllerParameterSet.msg + GetActiveControllerParametersRequest.msg + InjectControllerParametersRequest.msg + LQRParams.msg + LateralMPCParams.msg + LongitudinalMPCParams.msg + PIDParams.msg + PurePursuitParams.msg + StopAccuracyTask.msg + StreamControlTelemetryRequest.msg + TrajectoryPoint.msg + TrajectoryTrackingTask.msg + VelocityStepTask.msg + srv/ + CancelControlJob.srv + CommitControllerParameters.srv + EmergencyStop.srv + GetActiveControllerParameters.srv + GetControlJobResult.srv + GetControlJobStatus.srv + GetControlReadiness.srv + Heartbeat.srv + InjectControllerParameters.srv + SetControlWorkMode.srv + StartControllerEvaluation.srv + StreamControlTelemetry.srv diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/action/ExecuteControllerEvaluation.action b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/action/ExecuteControllerEvaluation.action new file mode 100644 index 0000000..69fb2be --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/action/ExecuteControllerEvaluation.action @@ -0,0 +1,22 @@ +# ========================================================= +# 控制评估执行 Action +# 作用：执行一个完整的控制评估 / 调参验证任务 +# 来源： +# 1) StartControllerEvaluation +# 2) GetControlJobStatus +# 3) GetControlJobResult +# 4) CancelControlJob +# 说明： +# 1) goal 对应 ControllerEvaluationRequest +# 2) result 对应 ControlJobResult +# 3) feedback 统一复用 common 的 JobStatus +# ========================================================= + +# Goal：控制评估任务请求 +calibration_control_interfaces/ControllerEvaluationRequest goal +--- +# Result：控制评估任务最终结果 +calibration_control_interfaces/ControlJobResult result +--- +# Feedback：执行过程中的状态反馈 +calibration_common_interfaces/JobStatus feedback diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/AccelerationDecelerationTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/AccelerationDecelerationTask.msg new file mode 100644 index 0000000..e5a71ef --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/AccelerationDecelerationTask.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 加减速任务 +# 作用：用于评估纵向平顺性、加减速响应和 jerk 表现 +# ========================================================= + +# 起始速度 +float64 start_velocity_ms +# 目标速度 +float64 target_velocity_ms +# 目标加速度 +float64 target_accel_ms2 +# 保持时间 +float64 hold_time_sec diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ActiveControllerParametersResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ActiveControllerParametersResponse.msg new file mode 100644 index 0000000..956e89a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ActiveControllerParametersResponse.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 查询当前控制参数响应 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 当前参数版本 +string parameter_version +# 当前生效参数 +calibration_control_interfaces/ControllerParameterSet parameter_set +# 生效时间 +int64 applied_timestamp_us diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/CommitControllerParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/CommitControllerParametersRequest.msg new file mode 100644 index 0000000..53d2957 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/CommitControllerParametersRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 固化控制参数请求 +# 作用：将最终收敛的一版参数写入车端 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 参数版本号 +string parameter_version +# 参数集合 +calibration_control_interfaces/ControllerParameterSet parameter_set +# 写入原因 +string commit_reason +# 摘要 +calibration_common_interfaces/FileDigest digest +# 是否持久化 +bool persistent_write diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlJobResult.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlJobResult.msg new file mode 100644 index 0000000..fe35784 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlJobResult.msg @@ -0,0 +1,27 @@ +# ========================================================= +# 控制任务结果 +# 作用：返回一次调参评估 / 验证后的结果摘要 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 任务 ID +string job_id + +# 数据质量是否达标 +bool data_quality_passed +# 是否适合写入 +bool suitable_for_commit +# 推荐参数版本 +string recommended_parameter_version + +# 验证摘要 +calibration_control_interfaces/ControlValidationSummary validation_summary +# 本轮估计参数集 +calibration_control_interfaces/ControllerParameterSet estimated_parameter_set +# 关联产物 +calibration_common_interfaces/FileReference[] artifacts diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlReadinessResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlReadinessResponse.msg new file mode 100644 index 0000000..87dfd3c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlReadinessResponse.msg @@ -0,0 +1,28 @@ +# ========================================================= +# 控制服务就绪响应 +# 作用：返回当前控制服务是否满足执行条件 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message + +# 服务本身是否就绪 +bool agent_ready +# 轨迹执行器是否就绪 +bool trajectory_executor_ready +# 车辆反馈链路是否就绪 +bool vehicle_feedback_ready +# 控制输出链路是否就绪 +bool control_output_ready +# 急停是否释放 +bool estop_released +# 当前车辆是否允许移动 +bool vehicle_safe_to_move +# 不满足项 +calibration_common_interfaces/ReadinessIssue[] issues +# 检查时间 +int64 checked_timestamp_us diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlTelemetry.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlTelemetry.msg new file mode 100644 index 0000000..cbc74ee --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlTelemetry.msg @@ -0,0 +1,45 @@ +# ========================================================= +# 控制遥测 +# 作用：回传调参评估时的关键数据 +# 发送方：Windows 车端代理 +# 接收方：Ubuntu 车间电脑 +# ========================================================= + +# 时间戳 +int64 hardware_timestamp_us + +# 位置 X +float64 odom_x_m +# 位置 Y +float64 odom_y_m +# 偏航角 +float64 odom_yaw_rad +# 实际线速度 +float64 linear_velocity_ms +# 实际角速度 +float64 angular_velocity_rads + +# 横向误差 +float64 lateral_error_m +# 航向误差 +float64 heading_error_rad +# 速度误差 +float64 speed_error_ms + +# 横向控制输出 +float64 steering_output +# 纵向驱动输出 +float64 throttle_output +# 制动输出 +float64 brake_output +# 是否发生饱和 +bool saturation_flag +# 当前任务 ID +string active_job_id + +# 当前生效参数版本 +string parameter_version +# 当前横向算法 +calibration_vehicle_profile_interfaces/ControllerAlgorithmType active_lateral_algorithm +# 当前纵向算法 +calibration_vehicle_profile_interfaces/ControllerAlgorithmType active_longitudinal_algorithm diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlValidationSummary.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlValidationSummary.msg new file mode 100644 index 0000000..674bcb2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlValidationSummary.msg @@ -0,0 +1,23 @@ +# ========================================================= +# 控制验证摘要 +# 作用：返回运控专项自动验收的关键指标 +# ========================================================= + +# 横向误差均方根 +float64 rms_lateral_error_m +# 航向误差均方根 +float64 rms_heading_error_rad +# 速度误差均方根 +float64 rms_speed_error_ms +# 超调比例 +float64 overshoot_ratio +# 收敛时间 +float64 settle_time_sec +# 停车位置误差 +float64 stop_position_error_m +# 最大 jerk +float64 max_jerk +# 饱和占比 +float64 saturation_ratio +# 自动验收是否通过 +bool auto_acceptance_passed diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlWorkMode.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlWorkMode.msg new file mode 100644 index 0000000..8a08a25 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlWorkMode.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 运控工作模式 +# 作用：切换控制系统到调试、评估、验证等状态 +# 来源：ControlWorkModeRequest.Mode +# ========================================================= + +uint8 CONTROL_MODE_UNSPECIFIED=0 # 未指定 +uint8 NORMAL_MODE=1 # 正常模式 +uint8 TUNING_READY_MODE=2 # 调参准备模式 +uint8 EVALUATION_MODE=3 # 参数评估模式 +uint8 VALIDATION_MODE=4 # 参数验证模式 + +# 当前模式取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlWorkModeRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlWorkModeRequest.msg new file mode 100644 index 0000000..3cabe3a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControlWorkModeRequest.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 运控工作模式请求 +# 作用：切换控制系统到调试、评估、验证等状态 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 目标模式 +calibration_control_interfaces/ControlWorkMode target_mode +# 切换原因 +string reason diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationRequest.msg new file mode 100644 index 0000000..7a2ab84 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationRequest.msg @@ -0,0 +1,32 @@ +# ========================================================= +# 控制评估任务请求 +# 作用：启动一次参数评估任务 +# 发送方：Ubuntu 车间电脑 +# 接收方：Windows 车端代理 +# 说明： +# 1) proto 中使用 oneof task +# 2) ROS2 中通过 selected_task + 全部 payload 字段共同表达 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 测试用例 ID +string test_case_id + +# 任务目的 +calibration_control_interfaces/ControllerEvaluationTaskPurpose task_purpose + +# 当前生效的任务类型 +calibration_control_interfaces/ControllerEvaluationTaskType selected_task + +# 轨迹跟踪任务 +calibration_control_interfaces/TrajectoryTrackingTask trajectory_tracking +# 速度阶跃任务 +calibration_control_interfaces/VelocityStepTask velocity_step +# 加减速任务 +calibration_control_interfaces/AccelerationDecelerationTask accel_decel +# 停车精度任务 +calibration_control_interfaces/StopAccuracyTask stop_accuracy + +# 来源迭代号 +string source_iteration_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationTaskPurpose.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationTaskPurpose.msg new file mode 100644 index 0000000..3bded3e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationTaskPurpose.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 控制评估任务目的 +# 来源：ControllerEvaluationRequest.TaskPurpose +# 作用：区分数据采集、调参评估和最终验证 +# ========================================================= + +uint8 CONTROL_TASK_PURPOSE_UNSPECIFIED=0 # 未指定 +uint8 DATA_COLLECTION=1 # 数据采集 +uint8 TUNING_EVALUATION=2 # 调参评估 +uint8 VALIDATION=3 # 最终验证 + +# 当前任务目的取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationTaskType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationTaskType.msg new file mode 100644 index 0000000..a51dde3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerEvaluationTaskType.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 控制评估任务类型 +# 作用：补足 proto oneof task 的 ROS2 表达 +# ========================================================= + +uint8 CONTROL_EVALUATION_TASK_UNSPECIFIED=0 # 未指定 +uint8 TRAJECTORY_TRACKING=1 # 轨迹跟踪任务 +uint8 VELOCITY_STEP=2 # 速度阶跃任务 +uint8 ACCELERATION_DECELERATION=3 # 加减速任务 +uint8 STOP_ACCURACY=4 # 停车精度任务 + +# 当前任务类型取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterKind.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterKind.msg new file mode 100644 index 0000000..6446737 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterKind.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 控制器参数类型 +# 作用：补足 proto oneof params 的 ROS2 表达 +# ========================================================= + +uint8 CONTROLLER_PARAMETER_KIND_UNSPECIFIED=0 # 未指定 +uint8 PID=1 # PID 参数 +uint8 LATERAL_MPC=2 # 横向 MPC 参数 +uint8 LONGITUDINAL_MPC=3 # 纵向 MPC 参数 +uint8 LQR=4 # LQR 参数 +uint8 PURE_PURSUIT=5 # Pure Pursuit 参数 + +# 当前参数类型取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterPack.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterPack.msg new file mode 100644 index 0000000..b320bba --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterPack.msg @@ -0,0 +1,28 @@ +# ========================================================= +# 单个控制器参数包 +# 作用：统一表达“某个控制轴上的某个控制器”的参数 +# 说明： +# 1) control_axis 用来明确这是横向控制器还是纵向控制器 +# 2) algorithm_type 用来明确该控制器采用哪类算法 +# 3) proto 中 params 为 oneof +# 4) ROS2 中通过 selected_params + 全部 payload 字段共同表达 +# ========================================================= + +# 控制轴类型 +calibration_vehicle_profile_interfaces/ControlAxisType control_axis +# 算法类型 +calibration_vehicle_profile_interfaces/ControllerAlgorithmType algorithm_type + +# 当前生效的参数类型 +calibration_control_interfaces/ControllerParameterKind selected_params + +# PID 参数 +calibration_control_interfaces/PIDParams pid +# 横向 MPC 参数 +calibration_control_interfaces/LateralMPCParams lateral_mpc +# 纵向 MPC 参数 +calibration_control_interfaces/LongitudinalMPCParams longitudinal_mpc +# LQR 参数 +calibration_control_interfaces/LQRParams lqr +# Pure Pursuit 参数 +calibration_control_interfaces/PurePursuitParams pure_pursuit diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterSet.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterSet.msg new file mode 100644 index 0000000..4a97701 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/ControllerParameterSet.msg @@ -0,0 +1,10 @@ +# ========================================================= +# 控制参数集合 +# 作用：一版参数里可以同时包含多个控制器参数 +# 说明： +# 1) 例如可同时包含“横向 MPC + 纵向 PID” +# 2) 也可包含“横向 PP + 纵向 PID” +# ========================================================= + +# 参数项列表 +calibration_control_interfaces/ControllerParameterPack[] items diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/GetActiveControllerParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/GetActiveControllerParametersRequest.msg new file mode 100644 index 0000000..4e5df7d --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/GetActiveControllerParametersRequest.msg @@ -0,0 +1,6 @@ +# ========================================================= +# 查询当前控制参数请求 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/InjectControllerParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/InjectControllerParametersRequest.msg new file mode 100644 index 0000000..66249b5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/InjectControllerParametersRequest.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 热注入参数请求 +# 作用：将 Linux 计算出的一版参数加载到车端运行内存 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 参数版本号 +string parameter_version +# 参数集合 +calibration_control_interfaces/ControllerParameterSet parameter_set +# 是否立即生效 +bool apply_immediately +# 来源迭代号 +string source_iteration_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LQRParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LQRParams.msg new file mode 100644 index 0000000..5e056e7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LQRParams.msg @@ -0,0 +1,15 @@ +# ========================================================= +# LQR 参数 +# 作用：LQR 控制器参数 +# 说明：通常用于横向控制器 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +# 状态权重向量 Q +float64[] q_state_weights +# 输入权重向量 R +float64[] r_input_weights + +bool has_preview_time_s +# 前视时间 +float64 preview_time_s diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LateralMPCParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LateralMPCParams.msg new file mode 100644 index 0000000..b9f8b98 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LateralMPCParams.msg @@ -0,0 +1,24 @@ +# ========================================================= +# 横向 MPC 参数 +# 作用：适用于横向控制器的 MPC 参数 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +# 预测步长 +uint32 prediction_horizon +# 控制步长 +uint32 control_horizon +# 离散时间步长 +float64 model_dt_s +# 横向误差权重 +float64 q_lateral +# 航向误差权重 +float64 q_heading +# 转向输出权重 +float64 r_steering +# 转向变化率权重 +float64 r_steering_rate + +bool has_steering_limit_deg +# 转向限幅（deg） +float64 steering_limit_deg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LongitudinalMPCParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LongitudinalMPCParams.msg new file mode 100644 index 0000000..f48562e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/LongitudinalMPCParams.msg @@ -0,0 +1,30 @@ +# ========================================================= +# 纵向 MPC 参数 +# 作用：适用于纵向控制器的 MPC 参数 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +# 预测步长 +uint32 prediction_horizon +# 控制步长 +uint32 control_horizon +# 离散时间步长 +float64 model_dt_s +# 速度误差权重 +float64 q_speed +# 加速度权重 +float64 q_accel +# 油门 / 驱动输出权重 +float64 r_throttle +# 制动输出权重 +float64 r_brake +# jerk 权重 +float64 r_jerk + +bool has_throttle_limit +# 驱动输出限幅 +float64 throttle_limit + +bool has_brake_limit +# 制动输出限幅 +float64 brake_limit diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/PIDParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/PIDParams.msg new file mode 100644 index 0000000..4db92be --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/PIDParams.msg @@ -0,0 +1,23 @@ +# ========================================================= +# PID 参数 +# 作用：PID 控制器参数 +# 说明：适用于横向或纵向，但需由 control_axis 配合解释 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +# 回路名，例如 lateral / heading / speed +string loop_name +# 比例系数 +float64 kp +# 积分系数 +float64 ki +# 微分系数 +float64 kd + +bool has_integral_limit +# 积分限幅 +float64 integral_limit + +bool has_output_limit +# 输出限幅 +float64 output_limit diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/PurePursuitParams.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/PurePursuitParams.msg new file mode 100644 index 0000000..a1b610d --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/PurePursuitParams.msg @@ -0,0 +1,25 @@ +# ========================================================= +# Pure Pursuit 参数 +# 作用：Pure Pursuit 控制器参数 +# 说明：仅适用于横向控制器 +# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真 +# ========================================================= + +# 前瞻距离 +float64 lookahead_m + +bool has_min_lookahead_m +# 最小前瞻距离 +float64 min_lookahead_m + +bool has_max_lookahead_m +# 最大前瞻距离 +float64 max_lookahead_m + +bool has_curvature_gain +# 曲率增益 +float64 curvature_gain + +bool has_steering_limit_deg +# 转向限幅（deg） +float64 steering_limit_deg diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/StopAccuracyTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/StopAccuracyTask.msg new file mode 100644 index 0000000..68e2433 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/StopAccuracyTask.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 停车精度任务 +# 作用：用于评估停车位置误差与速度收敛情况 +# ========================================================= + +# 目标停车点 X +float64 target_stop_x_m +# 目标停车点 Y +float64 target_stop_y_m +# 目标停车姿态 +float64 target_stop_yaw_rad +# 超时时间 +float64 timeout_sec diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/StreamControlTelemetryRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/StreamControlTelemetryRequest.msg new file mode 100644 index 0000000..7e3c3c4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/StreamControlTelemetryRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 控制遥测流请求 +# 作用：配置遥测频率和内容 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 上报频率 +uint32 expected_hz +# 是否包含跟踪误差 +bool include_tracking_error +# 是否包含控制器输出 +bool include_control_output +# 是否包含车辆反馈 +bool include_vehicle_feedback +# 是否包含当前控制器信息 +bool include_controller_identity diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/TrajectoryPoint.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/TrajectoryPoint.msg new file mode 100644 index 0000000..52a6d0e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/TrajectoryPoint.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 轨迹点 +# 作用：控制评估任务中使用的测试轨迹点 +# ========================================================= + +# 目标 X +float64 x_m +# 目标 Y +float64 y_m +# 目标偏航角 +float64 yaw_rad +# 目标速度 +float64 target_speed_ms diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/TrajectoryTrackingTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/TrajectoryTrackingTask.msg new file mode 100644 index 0000000..5de04e6 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/TrajectoryTrackingTask.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 轨迹跟踪评估任务 +# 作用：让车端按当前控制参数跑一条测试轨迹 +# ========================================================= + +# 轨迹点序列 +calibration_control_interfaces/TrajectoryPoint[] path +# 结束后是否停车 +bool stop_at_end +# 超时时间 +float64 timeout_sec diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/VelocityStepTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/VelocityStepTask.msg new file mode 100644 index 0000000..ea4b911 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/msg/VelocityStepTask.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 速度阶跃任务 +# 作用：用于评估纵向动态响应 +# ========================================================= + +# 目标速度 +float64 target_velocity_ms +# 保持时长 +float64 hold_time_sec +# 阶跃前静稳时间 +float64 settle_before_step_sec diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/package.xml new file mode 100644 index 0000000..5eeef22 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/package.xml @@ -0,0 +1,25 @@ + + + calibration_control_interfaces + 0.0.1 + ROS 2 interface package generated from control_calibration.proto. + + user + Proprietary + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + action_msgs + calibration_common_interfaces + calibration_vehicle_profile_interfaces + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/control_calibration.proto b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/proto/control_calibration.proto similarity index 60% rename from agv_calib_brain/src/win_ubuntu_bridge/proto/control_calibration.proto rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/proto/control_calibration.proto index eb3e996..61a04c4 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/control_calibration.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/proto/control_calibration.proto @@ -5,6 +5,14 @@ package agv.calibration.control; import "calibration_common.proto"; import "vehicle_profile.proto"; +// ========================================================= +// 单位约定： +// 1) 所有“状态量、结果量、误差量、位姿量”中的角度统一使用 rad +// 2) 所有“人工输入更直观的命令量、限幅量、目标角度”允许使用 deg +// 3) 字段名必须显式带单位后缀，例如 _rad / _deg / _m / _ms +// 4) 服务端与客户端都必须严格按字段后缀做单位转换，禁止隐式假定 +// ========================================================= + // ========================================================= // 文件作用：运控参数调优执行代理协议 // 运行关系： @@ -13,6 +21,7 @@ import "vehicle_profile.proto"; // 1) Linux 负责控制参数搜索、收敛判断、验证分析 // 2) Windows 负责执行轨迹 / 速度测试并回传遥测 // 3) 控制器明确区分为“横向控制器”和“纵向控制器” +// 4) 本文件已将横向 MPC 与纵向 MPC 参数模型拆开 // ========================================================= // ========================================================= @@ -25,6 +34,10 @@ service AgvCalibControlService { rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) returns (.agv.calibration.common.HeartbeatResponse); + // 查询控制服务当前是否具备执行条件 + rpc GetControlReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (ControlReadinessResponse); + // 设置调参工作模式 rpc SetControlWorkMode(ControlWorkModeRequest) returns (.agv.calibration.common.StandardResponse); @@ -68,7 +81,7 @@ service AgvCalibControlService { // ========================================================= // 运控工作模式 -// 作用：切换控制系统到调试、评估等状态 +// 作用：切换控制系统到调试、评估、验证等状态 // ========================================================= message ControlWorkModeRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 @@ -85,9 +98,29 @@ message ControlWorkModeRequest { string reason = 3; // 切换原因 } +// ========================================================= +// 控制服务就绪响应 +// 作用：返回当前控制服务是否满足执行条件 +// ========================================================= +message ControlReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool trajectory_executor_ready = 5; // 轨迹执行器是否就绪 + bool vehicle_feedback_ready = 6; // 车辆反馈链路是否就绪 + bool control_output_ready = 7; // 控制输出链路是否就绪 + bool estop_released = 8; // 急停是否释放 + bool vehicle_safe_to_move = 9; // 当前车辆是否允许移动 + repeated .agv.calibration.common.ReadinessIssue issues = 10; // 不满足项 + int64 checked_timestamp_us = 11; // 检查时间 +} + // ========================================================= // PID 参数 // 作用：PID 控制器参数 +// 说明：适用于横向或纵向，但需由 control_axis 配合解释 // ========================================================= message PIDParams { string loop_name = 1; // 回路名，例如 lateral / heading / speed @@ -99,24 +132,41 @@ message PIDParams { } // ========================================================= -// MPC 参数 -// 作用：MPC 控制器参数 +// 横向 MPC 参数 +// 作用：适用于横向控制器的 MPC 参数 // ========================================================= -message MPCParams { +message LateralMPCParams { uint32 prediction_horizon = 1; // 预测步长 uint32 control_horizon = 2; // 控制步长 double model_dt_s = 3; // 离散时间步长 double q_lateral = 4; // 横向误差权重 double q_heading = 5; // 航向误差权重 - double q_speed = 6; // 速度误差权重 - double r_control = 7; // 控制输出权重 - double r_control_rate = 8; // 控制变化率权重 - optional double output_limit = 9; // 输出限幅 + double r_steering = 6; // 转向输出权重 + double r_steering_rate = 7; // 转向变化率权重 + optional double steering_limit_deg = 8; // 转向限幅 +} + +// ========================================================= +// 纵向 MPC 参数 +// 作用：适用于纵向控制器的 MPC 参数 +// ========================================================= +message LongitudinalMPCParams { + uint32 prediction_horizon = 1; // 预测步长 + uint32 control_horizon = 2; // 控制步长 + double model_dt_s = 3; // 离散时间步长 + double q_speed = 4; // 速度误差权重 + double q_accel = 5; // 加速度权重 + double r_throttle = 6; // 油门 / 驱动输出权重 + double r_brake = 7; // 制动输出权重 + double r_jerk = 8; // jerk 权重 + optional double throttle_limit = 9; // 驱动输出限幅 + optional double brake_limit = 10; // 制动输出限幅 } // ========================================================= // LQR 参数 // 作用：LQR 控制器参数 +// 说明：通常用于横向控制器 // ========================================================= message LQRParams { repeated double q_state_weights = 1; // 状态权重向量 Q @@ -127,12 +177,13 @@ message LQRParams { // ========================================================= // Pure Pursuit 参数 // 作用：Pure Pursuit 控制器参数 +// 说明：仅适用于横向控制器 // ========================================================= message PurePursuitParams { - double lookahead_m = 1; // 前瞻距离 - optional double min_lookahead_m = 2; // 最小前瞻距离 - optional double max_lookahead_m = 3; // 最大前瞻距离 - optional double curvature_gain = 4; // 曲率增益 + double lookahead_m = 1; // 前瞻距离 + optional double min_lookahead_m = 2; // 最小前瞻距离 + optional double max_lookahead_m = 3; // 最大前瞻距离 + optional double curvature_gain = 4; // 曲率增益 optional double steering_limit_deg = 5; // 转向限幅 } @@ -148,10 +199,11 @@ message ControllerParameterPack { .agv.calibration.vehicle.profile.ControllerAlgorithmType algorithm_type = 2; // 算法类型 oneof params { - PIDParams pid = 3; // PID 参数 - MPCParams mpc = 4; // MPC 参数 - LQRParams lqr = 5; // LQR 参数 - PurePursuitParams pure_pursuit = 6; // Pure Pursuit 参数 + PIDParams pid = 3; // PID 参数 + LateralMPCParams lateral_mpc = 4; // 横向 MPC 参数 + LongitudinalMPCParams longitudinal_mpc = 5; // 纵向 MPC 参数 + LQRParams lqr = 6; // LQR 参数 + PurePursuitParams pure_pursuit = 7; // Pure Pursuit 参数 } } @@ -204,9 +256,31 @@ message TrajectoryTrackingTask { // 作用：用于评估纵向动态响应 // ========================================================= message VelocityStepTask { - double target_velocity_ms = 1; // 目标速度 - double hold_time_sec = 2; // 保持时长 - double settle_before_step_sec = 3; // 阶跃前静稳时间 + double target_velocity_ms = 1; // 目标速度 + double hold_time_sec = 2; // 保持时长 + double settle_before_step_sec = 3; // 阶跃前静稳时间 +} + +// ========================================================= +// 加减速任务 +// 作用：用于评估纵向平顺性、加减速响应和 jerk 表现 +// ========================================================= +message AccelerationDecelerationTask { + double start_velocity_ms = 1; // 起始速度 + double target_velocity_ms = 2; // 目标速度 + double target_accel_ms2 = 3; // 目标加速度 + double hold_time_sec = 4; // 保持时间 +} + +// ========================================================= +// 停车精度任务 +// 作用：用于评估停车位置误差与速度收敛情况 +// ========================================================= +message StopAccuracyTask { + double target_stop_x_m = 1; // 目标停车点 X + double target_stop_y_m = 2; // 目标停车点 Y + double target_stop_yaw_rad = 3; // 目标停车姿态 + double timeout_sec = 4; // 超时时间 } // ========================================================= @@ -229,11 +303,13 @@ message ControllerEvaluationRequest { TaskPurpose task_purpose = 3; // 任务目的 oneof task { - TrajectoryTrackingTask trajectory_tracking = 4; // 轨迹跟踪任务 - VelocityStepTask velocity_step = 5; // 速度阶跃任务 + TrajectoryTrackingTask trajectory_tracking = 4; // 轨迹跟踪任务 + VelocityStepTask velocity_step = 5; // 速度阶跃任务 + AccelerationDecelerationTask accel_decel = 6; // 加减速任务 + StopAccuracyTask stop_accuracy = 7; // 停车精度任务 } - string source_iteration_id = 6; // 来源迭代号 + string source_iteration_id = 8; // 来源迭代号 } // ========================================================= @@ -246,6 +322,7 @@ message StreamControlTelemetryRequest { bool include_tracking_error = 3; // 是否包含跟踪误差 bool include_control_output = 4; // 是否包含控制器输出 bool include_vehicle_feedback = 5; // 是否包含车辆反馈 + bool include_controller_identity = 6; // 是否包含当前控制器信息 } // ========================================================= @@ -255,21 +332,43 @@ message StreamControlTelemetryRequest { // 接收方：Ubuntu 车间电脑 // ========================================================= message ControlTelemetry { - int64 hardware_timestamp_us = 1; // 时间戳 - double odom_x_m = 2; // 位置 X - double odom_y_m = 3; // 位置 Y - double odom_yaw_rad = 4; // 偏航角 - double linear_velocity_ms = 5; // 实际线速度 - double angular_velocity_rads = 6; // 实际角速度 + int64 hardware_timestamp_us = 1; // 时间戳 - double lateral_error_m = 7; // 横向误差 - double heading_error_rad = 8; // 航向误差 - double speed_error_ms = 9; // 速度误差 + double odom_x_m = 2; // 位置 X + double odom_y_m = 3; // 位置 Y + double odom_yaw_rad = 4; // 偏航角 + double linear_velocity_ms = 5; // 实际线速度 + double angular_velocity_rads = 6; // 实际角速度 - double steering_output = 10; // 横向控制输出 - double throttle_output = 11; // 纵向驱动输出 - bool saturation_flag = 12; // 是否发生饱和 - string active_job_id = 13; // 当前任务 ID + double lateral_error_m = 7; // 横向误差 + double heading_error_rad = 8; // 航向误差 + double speed_error_ms = 9; // 速度误差 + + double steering_output = 10; // 横向控制输出 + double throttle_output = 11; // 纵向驱动输出 + double brake_output = 12; // 制动输出 + bool saturation_flag = 13; // 是否发生饱和 + string active_job_id = 14; // 当前任务 ID + + string parameter_version = 15; // 当前生效参数版本 + .agv.calibration.vehicle.profile.ControllerAlgorithmType active_lateral_algorithm = 16; // 当前横向算法 + .agv.calibration.vehicle.profile.ControllerAlgorithmType active_longitudinal_algorithm = 17; // 当前纵向算法 +} + +// ========================================================= +// 控制验证摘要 +// 作用：返回运控专项自动验收的关键指标 +// ========================================================= +message ControlValidationSummary { + double rms_lateral_error_m = 1; // 横向误差均方根 + double rms_heading_error_rad = 2; // 航向误差均方根 + double rms_speed_error_ms = 3; // 速度误差均方根 + double overshoot_ratio = 4; // 超调比例 + double settle_time_sec = 5; // 收敛时间 + double stop_position_error_m = 6; // 停车位置误差 + double max_jerk = 7; // 最大 jerk + double saturation_ratio = 8; // 饱和占比 + bool auto_acceptance_passed = 9; // 自动验收是否通过 } // ========================================================= @@ -313,13 +412,12 @@ message ControlJobResult { .agv.calibration.common.ErrorCode error_code = 2; // 错误码 string message = 3; // 说明 string job_id = 4; // 任务 ID - bool data_quality_passed = 5; // 数据质量是否达标 - bool suitable_for_commit = 6; // 是否适合写入 - string recommended_parameter_version = 7; // 推荐参数版本 - double rms_lateral_error_m = 8; // 横向误差均方根 - double rms_heading_error_rad = 9; // 航向误差均方根 - double rms_speed_error_ms = 10; // 速度误差均方根 - double overshoot_ratio = 11; // 超调比例 - double settle_time_sec = 12; // 收敛时间 - repeated .agv.calibration.common.FileReference artifacts = 13; // 关联产物 + + bool data_quality_passed = 5; // 数据质量是否达标 + bool suitable_for_commit = 6; // 是否适合写入 + string recommended_parameter_version = 7; // 推荐参数版本 + + ControlValidationSummary validation_summary = 8; // 验证摘要 + ControllerParameterSet estimated_parameter_set = 9; // 本轮估计参数集 + repeated .agv.calibration.common.FileReference artifacts = 10; // 关联产物 } \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/CancelControlJob.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/CancelControlJob.srv new file mode 100644 index 0000000..591a8e3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/CancelControlJob.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 取消控制评估任务 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/CommitControllerParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/CommitControllerParameters.srv new file mode 100644 index 0000000..c4480e2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/CommitControllerParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 固化最终控制参数 +# ========================================================= + +calibration_control_interfaces/CommitControllerParametersRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/EmergencyStop.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/EmergencyStop.srv new file mode 100644 index 0000000..aff6e34 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/EmergencyStop.srv @@ -0,0 +1,12 @@ +# ========================================================= +# 紧急停车 +# 对应 proto： +# rpc EmergencyStop(.agv.calibration.common.Empty) +# returns (.agv.calibration.common.StandardResponse); +# 说明： +# 1) 不自定义 Empty.msg +# 2) 这里使用空请求 section +# ========================================================= + +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetActiveControllerParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetActiveControllerParameters.srv new file mode 100644 index 0000000..c1cc57c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetActiveControllerParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询当前已生效控制参数 +# ========================================================= + +calibration_control_interfaces/GetActiveControllerParametersRequest request +--- +calibration_control_interfaces/ActiveControllerParametersResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlJobResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlJobResult.srv new file mode 100644 index 0000000..2f5cc42 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlJobResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询控制评估任务结果 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_control_interfaces/ControlJobResult response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlJobStatus.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlJobStatus.srv new file mode 100644 index 0000000..63dab1f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlJobStatus.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询控制评估任务状态 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/JobStatus response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlReadiness.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlReadiness.srv new file mode 100644 index 0000000..846757e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/GetControlReadiness.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询控制服务当前是否具备执行条件 +# ========================================================= + +calibration_common_interfaces/AgentReadinessRequest request +--- +calibration_control_interfaces/ControlReadinessResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/Heartbeat.srv new file mode 100644 index 0000000..c7a7adc --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/Heartbeat.srv @@ -0,0 +1,9 @@ +# ========================================================= +# 心跳保活 +# 运行位置：Windows 车端电脑 +# 调用方：Ubuntu 车间电脑 +# ========================================================= + +calibration_common_interfaces/HeartbeatRequest request +--- +calibration_common_interfaces/HeartbeatResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/InjectControllerParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/InjectControllerParameters.srv new file mode 100644 index 0000000..7246e91 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/InjectControllerParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 热加载一版控制参数 +# ========================================================= + +calibration_control_interfaces/InjectControllerParametersRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/SetControlWorkMode.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/SetControlWorkMode.srv new file mode 100644 index 0000000..42a3e76 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/SetControlWorkMode.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 设置调参工作模式 +# ========================================================= + +calibration_control_interfaces/ControlWorkModeRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/StartControllerEvaluation.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/StartControllerEvaluation.srv new file mode 100644 index 0000000..c22aedb --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/StartControllerEvaluation.srv @@ -0,0 +1,10 @@ +# ========================================================= +# 启动一个控制评估任务 +# 说明： +# 1) 这是对 proto 原始 RPC 的 1:1 保真映射 +# 2) 真正的 ROS2 长任务主通路建议优先使用 action/ExecuteControllerEvaluation.action +# ========================================================= + +calibration_control_interfaces/ControllerEvaluationRequest request +--- +calibration_common_interfaces/JobAccepted response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/StreamControlTelemetry.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/StreamControlTelemetry.srv new file mode 100644 index 0000000..792ce58 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_control_interfaces/srv/StreamControlTelemetry.srv @@ -0,0 +1,11 @@ +# ========================================================= +# 打开控制遥测流 +# 说明： +# 1) proto 原始语义是 server streaming +# 2) ROS2 中真正的实时流请使用 ControlTelemetry topic +# 3) 该 srv 负责配置 / 开启 / 更新遥测推送策略 +# ========================================================= + +calibration_control_interfaces/StreamControlTelemetryRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/CMakeLists.txt new file mode 100644 index 0000000..b05788a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_external_localization_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(action_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(calibration_common_interfaces REQUIRED) + +set(msg_files + "msg/ExternalLocalizationWorkMode.msg" + "msg/ExternalLocalizationWorkModeRequest.msg" + "msg/ExternalLocalizationCapabilityRequest.msg" + "msg/ExternalLocalizationCapabilityResponse.msg" + "msg/ExternalLocalizationValidationSummary.msg" + "msg/ExternalLocalizationReadinessResponse.msg" + "msg/ReferencePoseCollectionTask.msg" + "msg/MarkerAlignmentTask.msg" + "msg/TruthSourceValidationTask.msg" + "msg/ExternalLocalizationTaskPurpose.msg" + "msg/ExternalLocalizationTaskPayloadType.msg" + "msg/ExternalLocalizationTaskRequest.msg" + "msg/StreamExternalLocalizationTelemetryRequest.msg" + "msg/ExternalLocalizationTelemetry.msg" + "msg/ExternalLocalizationCalibrationResult.msg" + "msg/CommitExternalLocalizationResultRequest.msg" + "msg/GetAppliedExternalLocalizationResultRequest.msg" + "msg/AppliedExternalLocalizationResultResponse.msg" + "msg/ExternalLocalizationJobResult.msg" +) + +set(srv_files + "srv/Heartbeat.srv" + "srv/GetExternalLocalizationReadiness.srv" + "srv/SetExternalLocalizationWorkMode.srv" + "srv/GetExternalLocalizationCapability.srv" + "srv/StartExternalLocalizationTask.srv" + "srv/GetExternalLocalizationJobStatus.srv" + "srv/GetExternalLocalizationJobResult.srv" + "srv/CancelExternalLocalizationJob.srv" + "srv/StreamExternalLocalizationTelemetry.srv" + "srv/CommitExternalLocalizationResult.srv" + "srv/GetAppliedExternalLocalizationResult.srv" + "srv/EmergencyStop.srv" +) + +set(action_files + "action/ExecuteExternalLocalizationTask.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES action_msgs calibration_common_interfaces std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/FILE_TREE.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/FILE_TREE.txt new file mode 100644 index 0000000..6b4be23 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/FILE_TREE.txt @@ -0,0 +1,39 @@ +CMakeLists.txt + package.xml + msg/ + AppliedExternalLocalizationResultResponse.msg + CommitExternalLocalizationResultRequest.msg + ExternalLocalizationCalibrationResult.msg + ExternalLocalizationCapabilityRequest.msg + ExternalLocalizationCapabilityResponse.msg + ExternalLocalizationJobResult.msg + ExternalLocalizationReadinessResponse.msg + ExternalLocalizationTaskPayloadType.msg + ExternalLocalizationTaskPurpose.msg + ExternalLocalizationTaskRequest.msg + ExternalLocalizationTelemetry.msg + ExternalLocalizationValidationSummary.msg + ExternalLocalizationWorkMode.msg + ExternalLocalizationWorkModeRequest.msg + GetAppliedExternalLocalizationResultRequest.msg + MarkerAlignmentTask.msg + ReferencePoseCollectionTask.msg + StreamExternalLocalizationTelemetryRequest.msg + TruthSourceValidationTask.msg + srv/ + CancelExternalLocalizationJob.srv + CommitExternalLocalizationResult.srv + EmergencyStop.srv + GetAppliedExternalLocalizationResult.srv + GetExternalLocalizationCapability.srv + GetExternalLocalizationJobResult.srv + GetExternalLocalizationJobStatus.srv + GetExternalLocalizationReadiness.srv + Heartbeat.srv + SetExternalLocalizationWorkMode.srv + StartExternalLocalizationTask.srv + StreamExternalLocalizationTelemetry.srv + action/ + ExecuteExternalLocalizationTask.action + proto/ + external_localization.proto diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/action/ExecuteExternalLocalizationTask.action b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/action/ExecuteExternalLocalizationTask.action new file mode 100644 index 0000000..952ba11 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/action/ExecuteExternalLocalizationTask.action @@ -0,0 +1,22 @@ +# ========================================================= +# 外部定位任务执行 Action +# 作用：执行一个完整的外部定位长任务 +# 来源： +# 1) StartExternalLocalizationTask +# 2) GetExternalLocalizationJobStatus +# 3) GetExternalLocalizationJobResult +# 4) CancelExternalLocalizationJob +# 说明： +# 1) goal 对应 ExternalLocalizationTaskRequest +# 2) result 对应 ExternalLocalizationJobResult +# 3) feedback 统一复用 common 的 JobStatus +# ========================================================= + +# Goal：外部定位任务请求 +ExternalLocalizationTaskRequest goal +--- +# Result：外部定位任务最终结果 +ExternalLocalizationJobResult result +--- +# Feedback：执行过程中的状态反馈 +calibration_common_interfaces/JobStatus feedback \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/AppliedExternalLocalizationResultResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/AppliedExternalLocalizationResultResponse.msg new file mode 100644 index 0000000..0763ff2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/AppliedExternalLocalizationResultResponse.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 查询当前已生效外部定位结果响应 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 当前版本 +string parameter_version +# 当前生效结果 +ExternalLocalizationCalibrationResult result +# 生效时间 +int64 applied_timestamp_us \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/CommitExternalLocalizationResultRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/CommitExternalLocalizationResultRequest.msg new file mode 100644 index 0000000..e91dfd3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/CommitExternalLocalizationResultRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 外部定位结果写入请求 +# 作用：将 Linux 求解出的外部定位结果写入系统 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 结果版本号 +string parameter_version +# 外部定位结果 +ExternalLocalizationCalibrationResult result +# 写入原因 +string commit_reason +# 摘要 +calibration_common_interfaces/FileDigest digest +# 是否持久化 +bool persistent_write \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCalibrationResult.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCalibrationResult.msg new file mode 100644 index 0000000..0accc0b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCalibrationResult.msg @@ -0,0 +1,32 @@ +# ========================================================= +# 外部定位标定结果 +# 作用：表达车间参考系与外部定位参考系之间的结果关系 +# ========================================================= + +# 车间参考坐标系 ID +string workshop_frame_id +# 外部定位坐标系 ID +string localization_frame_id + +# 车间系到定位系的位姿 +calibration_common_interfaces/Pose3D workshop_to_localization + +# 位置重复性 +float64 position_repeatability_m +# 航向重复性 +float64 yaw_repeatability_rad +# 残差位置误差 +float64 residual_error_m +# 残差姿态误差 +float64 residual_error_rad + +# 跟踪丢失比例 +float64 tracking_loss_ratio +# 时间同步偏差（ms） +float64 time_sync_offset_ms +# 是否已验证可作为真值参考源 +bool validated_as_truth_source +# 该结果对应的外部定位源 ID +string localization_source_id +# 该结果对应的工位 / 区域 ID +string workcell_zone_id \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCapabilityRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCapabilityRequest.msg new file mode 100644 index 0000000..a974569 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCapabilityRequest.msg @@ -0,0 +1,7 @@ +# ========================================================= +# 查询外部定位能力请求 +# 作用：查询当前工位 / 服务支持的外部定位能力 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCapabilityResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCapabilityResponse.msg new file mode 100644 index 0000000..205e4b8 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationCapabilityResponse.msg @@ -0,0 +1,37 @@ +# ========================================================= +# 查询外部定位能力响应 +# 作用：返回当前支持的数据源与功能 +# ========================================================= + +# 是否成功 +bool success + +# 错误码 +calibration_common_interfaces/ErrorCode error_code + +# 说明 +string message + +# 是否支持真值源就绪检查 +bool supports_truth_source_readiness_check + +# 是否支持基于标靶对齐 +bool supports_marker_alignment + +# 是否支持参考位姿采集 +bool supports_reference_pose_collection + +# 是否支持真值参考验证 +bool supports_truth_source_validation + +# 是否支持持久化写入 +bool supports_persistent_commit + +# 外部定位源名称 +string localization_source_name + +# 外部定位源唯一 ID +string localization_source_id + +# 当前工位 / 区域 ID +string workcell_zone_id \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationJobResult.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationJobResult.msg new file mode 100644 index 0000000..d012183 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationJobResult.msg @@ -0,0 +1,30 @@ +# ========================================================= +# 外部定位任务结果 +# 作用：返回求解 / 验证后的结果摘要，供编排器判断是否进入下一阶段 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 任务 ID +string job_id + +# 任务目的 +ExternalLocalizationTaskPurpose task_purpose + +# 数据质量是否达标 +bool data_quality_passed +# 是否适合写入 +bool suitable_for_commit +# 推荐结果版本号 +string recommended_parameter_version + +# 真值参考验证摘要 +ExternalLocalizationValidationSummary validation_summary +# 结果内容 +ExternalLocalizationCalibrationResult result +# 关联产物 +calibration_common_interfaces/FileReference[] artifacts \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationReadinessResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationReadinessResponse.msg new file mode 100644 index 0000000..b5c9ec6 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationReadinessResponse.msg @@ -0,0 +1,27 @@ +# ========================================================= +# 外部定位就绪响应 +# 作用：返回当前是否适合作为真值参考源 +# 发送方：外部定位桥接服务 +# 接收方：Ubuntu 车间电脑 +# ========================================================= + +# 是否成功 +bool success + +# 错误码 +calibration_common_interfaces/ErrorCode error_code + +# 说明 +string message + +# 服务本身是否就绪 +bool agent_ready + +# 是否可进入真值参考验证 +bool ready_for_reference_validation + +# 验证摘要 +ExternalLocalizationValidationSummary validation_summary + +# 检查时间 +int64 checked_timestamp_us \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskPayloadType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskPayloadType.msg new file mode 100644 index 0000000..6f47ff7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskPayloadType.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 外部定位任务载荷类型 +# 作用：在 ROS2 中补足 proto oneof task 的语义 +# 说明： +# 1) ROS2 msg 不支持 oneof +# 2) 通过该字段指定当前哪个任务载荷生效 +# ========================================================= + +uint8 EXTERNAL_LOCALIZATION_TASK_PAYLOAD_UNSPECIFIED=0 # 未指定 +uint8 REFERENCE_POSE_COLLECTION=1 # 参考位姿采集 +uint8 MARKER_ALIGNMENT=2 # 标靶对齐 +uint8 TRUTH_SOURCE_VALIDATION=3 # 真值参考验证 + +# 当前载荷类型 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskPurpose.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskPurpose.msg new file mode 100644 index 0000000..884b40a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskPurpose.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 外部定位任务目的 +# 来源：ExternalLocalizationTaskRequest.TaskPurpose +# 作用：描述当前任务是做准备检查、采集、求解还是验证 +# ========================================================= + +uint8 EXTERNAL_LOCALIZATION_TASK_PURPOSE_UNSPECIFIED=0 # 未指定 +uint8 REFERENCE_READY_CHECK=1 # 真值参考可用性检查 +uint8 DATA_COLLECTION=2 # 数据采集 +uint8 SOLVING=3 # 求解 +uint8 VALIDATION=4 # 验证 + +# 当前任务目的取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskRequest.msg new file mode 100644 index 0000000..f2e7e85 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTaskRequest.msg @@ -0,0 +1,39 @@ +# ========================================================= +# 外部定位任务请求 +# 作用：启动一类外部定位任务 +# 发送方：Ubuntu 车间电脑 +# 接收方：Windows 车端代理 / Linux 桥接服务 +# 说明： +# 1) proto 中使用 oneof task +# 2) ROS2 中通过 selected_task + 三个 payload 字段共同表达 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header + +# 测试用例 ID +string test_case_id + +# 任务目的 +ExternalLocalizationTaskPurpose task_purpose + +# 当前生效的任务载荷类型 +ExternalLocalizationTaskPayloadType selected_task + +# 参考位姿采集 +ReferencePoseCollectionTask reference_pose_collection + +# 标靶对齐 +MarkerAlignmentTask marker_alignment + +# 真值参考验证 +TruthSourceValidationTask truth_source_validation + +# 来源迭代号 +string source_iteration_id + +# 本次任务指定使用的外部定位源 ID +string localization_source_id + +# 本次任务所属工位 / 区域 ID +string workcell_zone_id \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTelemetry.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTelemetry.msg new file mode 100644 index 0000000..24b7610 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationTelemetry.msg @@ -0,0 +1,32 @@ +# ========================================================= +# 外部定位遥测 +# 作用：回传外部定位实时观测结果 +# 发送方：Windows 车端代理 / Linux 桥接服务 +# 接收方：Ubuntu 车间电脑 +# ========================================================= + +# 硬件时间戳 +int64 hardware_timestamp_us +# 位姿是否有效 +bool pose_valid + +# 在车间参考系下的位姿 +calibration_common_interfaces/Pose3D workshop_pose + +# 位置标准差 +float64 position_stddev_m +# 航向标准差 +float64 yaw_stddev_rad +# 跟踪丢失比例 +float64 tracking_loss_ratio +# 时间同步偏差（ms） +float64 time_sync_offset_ms +# 质量分数，数值越高越好 +float64 quality_score + +# 当前观测到的目标数量 +uint32 observed_target_count +# 外部定位源名称 +string reference_source_name +# 当前任务 ID +string active_job_id \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationValidationSummary.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationValidationSummary.msg new file mode 100644 index 0000000..c611de6 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationValidationSummary.msg @@ -0,0 +1,31 @@ +# ========================================================= +# 外部定位验证摘要 +# 作用：表达当前外部定位是否适合作为真值参考输入 +# 使用场景： +# 1) readiness 响应 +# 2) 外部定位任务结果 +# 3) 阶段0自动验收判断 +# ========================================================= + +# 时间同步是否达标 +bool time_sync_ok +# 覆盖范围是否达标 +bool coverage_ok +# 观测质量是否达标 +bool quality_ok +# 跟踪是否稳定 +bool tracking_stable +# 是否推荐作为真值参考源 +bool recommended_as_truth_source + +# 位置标准差 +float64 position_stddev_m +# 航向标准差 +float64 yaw_stddev_rad +# 跟踪丢失比例 +float64 tracking_loss_ratio +# 时间同步偏差（ms） +float64 time_sync_offset_ms + +# 不满足项列表 +calibration_common_interfaces/ReadinessIssue[] issues \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationWorkMode.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationWorkMode.msg new file mode 100644 index 0000000..063c10f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationWorkMode.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 外部定位工作模式 +# 来源：ExternalLocalizationWorkModeRequest.Mode +# 作用：描述外部定位桥接服务当前目标工作模式 +# ========================================================= + +uint8 EXTERNAL_LOCALIZATION_MODE_UNSPECIFIED=0 # 未指定 +uint8 NORMAL_MODE=1 # 正常模式 +uint8 CALIBRATION_READY_MODE=2 # 标定准备模式 +uint8 REFERENCE_COLLECTION_MODE=3 # 参考数据采集模式 +uint8 VALIDATION_MODE=4 # 真值参考验证模式 + +# 当前模式取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationWorkModeRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationWorkModeRequest.msg new file mode 100644 index 0000000..de26fea --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ExternalLocalizationWorkModeRequest.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 外部定位工作模式请求 +# 作用：切换外部定位桥接服务到不同模式 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header + +# 目标模式 +ExternalLocalizationWorkMode target_mode + +# 切换原因 +string reason \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/GetAppliedExternalLocalizationResultRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/GetAppliedExternalLocalizationResultRequest.msg new file mode 100644 index 0000000..a18df18 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/GetAppliedExternalLocalizationResultRequest.msg @@ -0,0 +1,6 @@ +# ========================================================= +# 查询当前已生效外部定位结果请求 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/MarkerAlignmentTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/MarkerAlignmentTask.msg new file mode 100644 index 0000000..91732b9 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/MarkerAlignmentTask.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 标靶对齐任务 +# 作用：通过标靶 / 标定板建立外部参考坐标关系 +# ========================================================= + +# 标靶 / 标定板 ID +string target_board_id + +# 最少有效观测数 +uint32 min_valid_observation_count + +# 超时时间 +float64 timeout_sec \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ReferencePoseCollectionTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ReferencePoseCollectionTask.msg new file mode 100644 index 0000000..0d5c892 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/ReferencePoseCollectionTask.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 参考位姿采集任务 +# 作用：采集一批车辆静止位姿用于重复性分析或基准建立 +# ========================================================= + +# 需要采集的样本数 +uint32 sample_count + +# 是否要求车辆静止 +bool require_vehicle_static + +# 超时时间 +float64 timeout_sec + +# 最小采样间隔 +float64 min_sample_interval_sec \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/StreamExternalLocalizationTelemetryRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/StreamExternalLocalizationTelemetryRequest.msg new file mode 100644 index 0000000..a5a07da --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/StreamExternalLocalizationTelemetryRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 外部定位遥测流请求 +# 作用：指定外部定位数据上报内容 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 期望上报频率 +uint32 expected_hz +# 是否包含位姿 +bool include_pose +# 是否包含质量指标 +bool include_quality_metrics +# 是否包含时间同步指标 +bool include_time_sync_metrics +# 是否包含跟踪状态 +bool include_tracking_state \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/TruthSourceValidationTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/TruthSourceValidationTask.msg new file mode 100644 index 0000000..50c25e7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/msg/TruthSourceValidationTask.msg @@ -0,0 +1,28 @@ +# ========================================================= +# 真值参考验证任务 +# 作用：验证外部定位输出的重复性、稳定性、时间同步与可用性 +# ========================================================= + +# 静态样本数 +uint32 static_sample_count + +# 动态样本数 +uint32 dynamic_sample_count + +# 是否要求短距离运动段验证 +bool require_short_motion_segment + +# 允许的最大位置标准差 +float64 max_position_stddev_m + +# 允许的最大航向标准差 +float64 max_yaw_stddev_rad + +# 允许的最大跟踪丢失比例 +float64 max_tracking_loss_ratio + +# 允许的最大时间同步偏差 +float64 max_time_sync_offset_ms + +# 超时时间 +float64 timeout_sec \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/package.xml new file mode 100644 index 0000000..c5c9332 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/package.xml @@ -0,0 +1,21 @@ + + + calibration_external_localization_interfaces + 0.0.1 + ROS 2 interfaces converted from external_localization.proto. + user + Apache-2.0 + + ament_cmake + rosidl_default_generators + + action_msgs + std_msgs + calibration_common_interfaces + + rosidl_default_runtime + rosidl_interface_packages + + ament_cmake + + diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/external_localization.proto b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/proto/external_localization.proto similarity index 51% rename from agv_calib_brain/src/win_ubuntu_bridge/proto/external_localization.proto rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/proto/external_localization.proto index f12ff0c..35749b7 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/external_localization.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/proto/external_localization.proto @@ -5,12 +5,13 @@ package agv.calibration.localization.external; import "calibration_common.proto"; // ========================================================= -// 文件作用：外部定位 / 基准建立执行代理协议 +// 文件作用：外部定位 / 真值参考接入执行代理协议 // 运行关系： -// Ubuntu 车间电脑(Linux) -> Windows 车端代理 / 外部定位桥接服务 +// Ubuntu 车间电脑(Linux) -> Windows 车端代理 / Linux 外部定位桥接服务 // 说明： -// 1) Linux 负责外部定位基准求解、重复性分析、验证判断 -// 2) 车端 / 桥接服务只负责采集数据、提供位姿流、执行必要动作 +// 1) Linux 负责任务编排、基准求解、重复性分析、真值参考验收判断 +// 2) 车端代理 / 桥接服务只负责采集数据、提供位姿流、执行必要检查 +// 3) 本文件既支持“外部定位结果求解”，也支持“阶段0真值参考接入与确认” // ========================================================= // ========================================================= @@ -23,6 +24,10 @@ service AgvCalibExternalLocalizationService { rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) returns (.agv.calibration.common.HeartbeatResponse); + // 查询外部定位服务当前是否具备作为真值参考的条件 + rpc GetExternalLocalizationReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (ExternalLocalizationReadinessResponse); + // 设置外部定位工作模式 rpc SetExternalLocalizationWorkMode(ExternalLocalizationWorkModeRequest) returns (.agv.calibration.common.StandardResponse); @@ -76,7 +81,7 @@ message ExternalLocalizationWorkModeRequest { NORMAL_MODE = 1; // 正常模式 CALIBRATION_READY_MODE = 2; // 标定准备模式 REFERENCE_COLLECTION_MODE = 3; // 参考数据采集模式 - VALIDATION_MODE = 4; // 验证模式 + VALIDATION_MODE = 4; // 真值参考验证模式 } Mode target_mode = 2; // 目标模式 @@ -96,13 +101,59 @@ message ExternalLocalizationCapabilityRequest { // 作用：返回当前支持的数据源与功能 // ========================================================= message ExternalLocalizationCapabilityResponse { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - bool supports_marker_alignment = 4; // 是否支持基于标靶对齐 - bool supports_reference_pose_collection = 5; // 是否支持参考位姿采集 - bool supports_repeatability_validation = 6; // 是否支持重复性验证 - string localization_source_name = 7; // 外部定位源名称 + string message = 3; // 说明 + + bool supports_truth_source_readiness_check = 4; // 是否支持真值源就绪检查 + bool supports_marker_alignment = 5; // 是否支持基于标靶对齐 + bool supports_reference_pose_collection = 6; // 是否支持参考位姿采集 + bool supports_truth_source_validation = 7; // 是否支持真值参考验证 + bool supports_persistent_commit = 8; // 是否支持持久化写入 + + string localization_source_name = 9; // 外部定位源名称 + string localization_source_id = 10; // 外部定位源唯一 ID + string workcell_zone_id = 11; // 当前工位 / 区域 ID +} + +// ========================================================= +// 外部定位验证摘要 +// 作用：表达当前外部定位是否适合作为真值参考输入 +// 使用场景： +// 1) readiness 响应 +// 2) 外部定位任务结果 +// 3) 阶段0自动验收判断 +// ========================================================= +message ExternalLocalizationValidationSummary { + bool time_sync_ok = 1; // 时间同步是否达标 + bool coverage_ok = 2; // 覆盖范围是否达标 + bool quality_ok = 3; // 观测质量是否达标 + bool tracking_stable = 4; // 跟踪是否稳定 + bool recommended_as_truth_source = 5; // 是否推荐作为真值参考源 + + double position_stddev_m = 6; // 位置标准差 + double yaw_stddev_rad = 7; // 航向标准差 + double tracking_loss_ratio = 8; // 跟踪丢失比例 + double time_sync_offset_ms = 9; // 时间同步偏差（ms） + + repeated .agv.calibration.common.ReadinessIssue issues = 10; // 不满足项列表 +} + +// ========================================================= +// 外部定位就绪响应 +// 作用：返回当前是否适合作为真值参考源 +// 发送方：外部定位桥接服务 +// 接收方：Ubuntu 车间电脑 +// ========================================================= +message ExternalLocalizationReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool ready_for_reference_validation = 5; // 是否可进入真值参考验证 + ExternalLocalizationValidationSummary validation_summary = 6; // 验证摘要 + int64 checked_timestamp_us = 7; // 检查时间 } // ========================================================= @@ -110,9 +161,10 @@ message ExternalLocalizationCapabilityResponse { // 作用：采集一批车辆静止位姿用于重复性分析或基准建立 // ========================================================= message ReferencePoseCollectionTask { - uint32 sample_count = 1; // 需要采集的样本数 + uint32 sample_count = 1; // 需要采集的样本数 bool require_vehicle_static = 2; // 是否要求车辆静止 - double timeout_sec = 3; // 超时时间 + double timeout_sec = 3; // 超时时间 + double min_sample_interval_sec = 4; // 最小采样间隔 } // ========================================================= @@ -120,20 +172,26 @@ message ReferencePoseCollectionTask { // 作用：通过标靶 / 标定板建立外部参考坐标关系 // ========================================================= message MarkerAlignmentTask { - string target_board_id = 1; // 标靶 / 标定板 ID + string target_board_id = 1; // 标靶 / 标定板 ID uint32 min_valid_observation_count = 2; // 最少有效观测数 - double timeout_sec = 3; // 超时时间 + double timeout_sec = 3; // 超时时间 } // ========================================================= -// 一致性验证任务 -// 作用：验证外部定位输出的重复性与稳定性 +// 真值参考验证任务 +// 作用：验证外部定位输出的重复性、稳定性、时间同步与可用性 // ========================================================= -message ConsistencyValidationTask { - uint32 sample_count = 1; // 样本数 - double max_position_stddev_m = 2; // 允许的最大位置标准差 - double max_yaw_stddev_rad = 3; // 允许的最大航向标准差 - double timeout_sec = 4; // 超时时间 +message TruthSourceValidationTask { + uint32 static_sample_count = 1; // 静态样本数 + uint32 dynamic_sample_count = 2; // 动态样本数 + bool require_short_motion_segment = 3; // 是否要求短距离运动段验证 + + double max_position_stddev_m = 4; // 允许的最大位置标准差 + double max_yaw_stddev_rad = 5; // 允许的最大航向标准差 + double max_tracking_loss_ratio = 6; // 允许的最大跟踪丢失比例 + double max_time_sync_offset_ms = 7; // 允许的最大时间同步偏差 + + double timeout_sec = 8; // 超时时间 } // ========================================================= @@ -144,13 +202,14 @@ message ConsistencyValidationTask { // ========================================================= message ExternalLocalizationTaskRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string test_case_id = 2; // 测试用例 ID + string test_case_id = 2; // 测试用例 ID enum TaskPurpose { EXTERNAL_LOCALIZATION_TASK_PURPOSE_UNSPECIFIED = 0; // 未指定 - DATA_COLLECTION = 1; // 数据采集 - SOLVING = 2; // 求解 - VALIDATION = 3; // 验证 + REFERENCE_READY_CHECK = 1; // 真值参考可用性检查 + DATA_COLLECTION = 2; // 数据采集 + SOLVING = 3; // 求解 + VALIDATION = 4; // 验证 } TaskPurpose task_purpose = 3; // 任务目的 @@ -158,10 +217,12 @@ message ExternalLocalizationTaskRequest { oneof task { ReferencePoseCollectionTask reference_pose_collection = 4; // 参考位姿采集 MarkerAlignmentTask marker_alignment = 5; // 标靶对齐 - ConsistencyValidationTask consistency_validation = 6; // 一致性验证 + TruthSourceValidationTask truth_source_validation = 6; // 真值参考验证 } string source_iteration_id = 7; // 来源迭代号 + string localization_source_id = 8; // 本次任务指定使用的外部定位源 ID + string workcell_zone_id = 9; // 本次任务所属工位 / 区域 ID } // ========================================================= @@ -173,6 +234,8 @@ message StreamExternalLocalizationTelemetryRequest { uint32 expected_hz = 2; // 期望上报频率 bool include_pose = 3; // 是否包含位姿 bool include_quality_metrics = 4; // 是否包含质量指标 + bool include_time_sync_metrics = 5; // 是否包含时间同步指标 + bool include_tracking_state = 6; // 是否包含跟踪状态 } // ========================================================= @@ -182,14 +245,20 @@ message StreamExternalLocalizationTelemetryRequest { // 接收方：Ubuntu 车间电脑 // ========================================================= message ExternalLocalizationTelemetry { - int64 hardware_timestamp_us = 1; // 硬件时间戳 - bool pose_valid = 2; // 位姿是否有效 - .agv.calibration.common.Pose3D workshop_pose = 3; // 在车间参考系下的位姿 - double position_stddev_m = 4; // 位置标准差 - double yaw_stddev_rad = 5; // 航向标准差 - uint32 observed_target_count = 6; // 当前观测到的目标数量 - string reference_source_name = 7; // 外部定位源名称 - string active_job_id = 8; // 当前任务 ID + int64 hardware_timestamp_us = 1; // 硬件时间戳 + bool pose_valid = 2; // 位姿是否有效 + + .agv.calibration.common.Pose3dEuler workshop_pose = 3; // 在车间参考系下的位姿 + + double position_stddev_m = 4; // 位置标准差 + double yaw_stddev_rad = 5; // 航向标准差 + double tracking_loss_ratio = 6; // 跟踪丢失比例 + double time_sync_offset_ms = 7; // 时间同步偏差（ms） + double quality_score = 8; // 质量分数，数值越高越好 + + uint32 observed_target_count = 9; // 当前观测到的目标数量 + string reference_source_name = 10; // 外部定位源名称 + string active_job_id = 11; // 当前任务 ID } // ========================================================= @@ -197,13 +266,21 @@ message ExternalLocalizationTelemetry { // 作用：表达车间参考系与外部定位参考系之间的结果关系 // ========================================================= message ExternalLocalizationCalibrationResult { - string workshop_frame_id = 1; // 车间参考坐标系 ID - string localization_frame_id = 2; // 外部定位坐标系 ID - .agv.calibration.common.Pose3D workshop_to_localization = 3; // 车间系到定位系的位姿 - double position_repeatability_m = 4; // 位置重复性 - double yaw_repeatability_rad = 5; // 航向重复性 - double residual_error_m = 6; // 残差位置误差 - double residual_error_rad = 7; // 残差姿态误差 + string workshop_frame_id = 1; // 车间参考坐标系 ID + string localization_frame_id = 2; // 外部定位坐标系 ID + + .agv.calibration.common.Pose3dEuler workshop_to_localization = 3; // 车间系到定位系的位姿 + + double position_repeatability_m = 4; // 位置重复性 + double yaw_repeatability_rad = 5; // 航向重复性 + double residual_error_m = 6; // 残差位置误差 + double residual_error_rad = 7; // 残差姿态误差 + + double tracking_loss_ratio = 8; // 跟踪丢失比例 + double time_sync_offset_ms = 9; // 时间同步偏差（ms） + bool validated_as_truth_source = 10; // 是否已验证可作为真值参考源 + string localization_source_id = 11; // 该结果对应的外部定位源 ID + string workcell_zone_id = 12; // 该结果对应的工位 / 区域 ID } // ========================================================= @@ -215,7 +292,7 @@ message CommitExternalLocalizationResultRequest { string parameter_version = 2; // 结果版本号 ExternalLocalizationCalibrationResult result = 3; // 外部定位结果 string commit_reason = 4; // 写入原因 - .agv.calibration.common.FileDigest digest = 5; // 摘要 + .agv.calibration.common.FileDigest digest = 5; // 摘要 bool persistent_write = 6; // 是否持久化 } @@ -230,12 +307,12 @@ message GetAppliedExternalLocalizationResultRequest { // 查询当前已生效外部定位结果响应 // ========================================================= message AppliedExternalLocalizationResultResponse { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - string parameter_version = 4; // 当前版本 - ExternalLocalizationCalibrationResult result = 5; // 当前生效结果 - int64 applied_timestamp_us = 6; // 生效时间 + string message = 3; // 说明 + string parameter_version = 4; // 当前版本 + ExternalLocalizationCalibrationResult result = 5; // 当前生效结果 + int64 applied_timestamp_us = 6; // 生效时间 } // ========================================================= @@ -243,13 +320,18 @@ message AppliedExternalLocalizationResultResponse { // 作用：返回求解 / 验证后的结果摘要，供编排器判断是否进入下一阶段 // ========================================================= message ExternalLocalizationJobResult { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - string job_id = 4; // 任务 ID - bool data_quality_passed = 5; // 数据质量是否达标 - bool suitable_for_commit = 6; // 是否适合写入 - string recommended_parameter_version = 7; // 推荐结果版本号 - ExternalLocalizationCalibrationResult result = 8; // 结果内容 - repeated .agv.calibration.common.FileReference artifacts = 9; // 关联产物 + string message = 3; // 说明 + string job_id = 4; // 任务 ID + + ExternalLocalizationTaskRequest.TaskPurpose task_purpose = 5; // 任务目的 + + bool data_quality_passed = 6; // 数据质量是否达标 + bool suitable_for_commit = 7; // 是否适合写入 + string recommended_parameter_version = 8; // 推荐结果版本号 + + ExternalLocalizationValidationSummary validation_summary = 9; // 真值参考验证摘要 + ExternalLocalizationCalibrationResult result = 10; // 结果内容 + repeated .agv.calibration.common.FileReference artifacts = 11; // 关联产物 } \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/CancelExternalLocalizationJob.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/CancelExternalLocalizationJob.srv new file mode 100644 index 0000000..3d08e13 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/CancelExternalLocalizationJob.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 取消外部定位任务 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/StandardResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/CommitExternalLocalizationResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/CommitExternalLocalizationResult.srv new file mode 100644 index 0000000..0b7d770 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/CommitExternalLocalizationResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 写入外部定位标定结果 +# ========================================================= + +CommitExternalLocalizationResultRequest request +--- +calibration_common_interfaces/StandardResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/EmergencyStop.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/EmergencyStop.srv new file mode 100644 index 0000000..b1c0805 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/EmergencyStop.srv @@ -0,0 +1,13 @@ +# ========================================================= +# 紧急停止 +# 对应 proto： +# rpc EmergencyStop(.agv.calibration.common.Empty) +# returns (.agv.calibration.common.StandardResponse); +# 说明： +# 1) 这里不自定义 Empty.msg +# 2) 直接复用 ROS2 官方空消息 +# ========================================================= + +std_msgs/Empty request +--- +calibration_common_interfaces/StandardResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetAppliedExternalLocalizationResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetAppliedExternalLocalizationResult.srv new file mode 100644 index 0000000..b7b54db --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetAppliedExternalLocalizationResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询当前已生效外部定位结果 +# ========================================================= + +GetAppliedExternalLocalizationResultRequest request +--- +AppliedExternalLocalizationResultResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationCapability.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationCapability.srv new file mode 100644 index 0000000..b5b5380 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationCapability.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询外部定位能力 +# ========================================================= + +ExternalLocalizationCapabilityRequest request +--- +ExternalLocalizationCapabilityResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationJobResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationJobResult.srv new file mode 100644 index 0000000..f670a93 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationJobResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询外部定位任务结果 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +ExternalLocalizationJobResult response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationJobStatus.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationJobStatus.srv new file mode 100644 index 0000000..ed48029 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationJobStatus.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询外部定位任务状态 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/JobStatus response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationReadiness.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationReadiness.srv new file mode 100644 index 0000000..1acf2ec --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/GetExternalLocalizationReadiness.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询外部定位服务当前是否具备作为真值参考的条件 +# ========================================================= + +calibration_common_interfaces/AgentReadinessRequest request +--- +ExternalLocalizationReadinessResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/Heartbeat.srv new file mode 100644 index 0000000..77c06ad --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/Heartbeat.srv @@ -0,0 +1,9 @@ +# ========================================================= +# 心跳保活 +# 运行位置：Windows 车端代理 或 Linux 外部定位桥接服务 +# 调用方：Ubuntu 车间电脑 +# ========================================================= + +calibration_common_interfaces/HeartbeatRequest request +--- +calibration_common_interfaces/HeartbeatResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/SetExternalLocalizationWorkMode.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/SetExternalLocalizationWorkMode.srv new file mode 100644 index 0000000..6bc77b0 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/SetExternalLocalizationWorkMode.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 设置外部定位工作模式 +# ========================================================= + +ExternalLocalizationWorkModeRequest request +--- +calibration_common_interfaces/StandardResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/StartExternalLocalizationTask.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/StartExternalLocalizationTask.srv new file mode 100644 index 0000000..95d6e8a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/StartExternalLocalizationTask.srv @@ -0,0 +1,10 @@ +# ========================================================= +# 启动一个外部定位任务 +# 说明： +# 1) 这是对 proto 原始 RPC 的 1:1 保真映射 +# 2) 真正的 ROS2 长任务主通路建议优先使用 action/ExecuteExternalLocalizationTask.action +# ========================================================= + +ExternalLocalizationTaskRequest request +--- +calibration_common_interfaces/JobAccepted response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/StreamExternalLocalizationTelemetry.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/StreamExternalLocalizationTelemetry.srv new file mode 100644 index 0000000..a1db66c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_external_localization_interfaces/srv/StreamExternalLocalizationTelemetry.srv @@ -0,0 +1,11 @@ +# ========================================================= +# 打开外部定位遥测流 +# 说明： +# 1) proto 原始语义是 server streaming +# 2) ROS2 中真正的实时流请使用 ExternalLocalizationTelemetry topic +# 3) 该 srv 负责配置 / 开启 / 更新遥测推送策略 +# ========================================================= + +StreamExternalLocalizationTelemetryRequest request +--- +calibration_common_interfaces/StandardResponse response \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/CMakeLists.txt new file mode 100644 index 0000000..9abc3e4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/CMakeLists.txt @@ -0,0 +1,72 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_sensor_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(action_msgs REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) + +set(msg_files + "msg/AppliedSensorCalibrationParametersResponse.msg" + "msg/ArtifactType.msg" + "msg/CalibrationArtifact.msg" + "msg/CalibrationReferenceTarget.msg" + "msg/CameraIntrinsicCalibrationTask.msg" + "msg/CameraIntrinsics.msg" + "msg/CameraModelType.msg" + "msg/CaptureIntentType.msg" + "msg/CommitSensorCalibrationParametersRequest.msg" + "msg/GetAppliedSensorCalibrationParametersRequest.msg" + "msg/HandEyeCalibrationMode.msg" + "msg/HandEyeCalibrationTask.msg" + "msg/IMUIntrinsicCalibrationTask.msg" + "msg/IMUIntrinsics.msg" + "msg/SensorCalibrationCapabilityRequest.msg" + "msg/SensorCalibrationCapabilityResponse.msg" + "msg/SensorCalibrationJobResult.msg" + "msg/SensorCalibrationParameter.msg" + "msg/SensorCalibrationParameterSet.msg" + "msg/SensorCalibrationTaskPurpose.msg" + "msg/SensorCalibrationTaskRequest.msg" + "msg/SensorCalibrationTaskSubtype.msg" + "msg/SensorCalibrationTaskType.msg" + "msg/SensorCalibrationTelemetry.msg" + "msg/SensorCalibrationWorkMode.msg" + "msg/SensorCalibrationWorkModeRequest.msg" + "msg/SensorExtrinsics.msg" + "msg/SensorReadinessResponse.msg" + "msg/SensorToBaseExtrinsicCalibrationTask.msg" + "msg/SensorValidationSummary.msg" + "msg/StreamSensorCalibrationTelemetryRequest.msg" +) + +set(srv_files + "srv/CancelSensorCalibrationJob.srv" + "srv/CommitSensorCalibrationParameters.srv" + "srv/EmergencyStop.srv" + "srv/GetAppliedSensorCalibrationParameters.srv" + "srv/GetSensorCalibrationCapability.srv" + "srv/GetSensorCalibrationJobResult.srv" + "srv/GetSensorCalibrationJobStatus.srv" + "srv/GetSensorReadiness.srv" + "srv/Heartbeat.srv" + "srv/SetSensorCalibrationWorkMode.srv" + "srv/StartSensorCalibrationTask.srv" + "srv/StreamSensorCalibrationTelemetry.srv" +) + +set(action_files + "action/ExecuteSensorCalibrationTask.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES std_msgs action_msgs calibration_common_interfaces calibration_vehicle_profile_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/FILE_TREE.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/FILE_TREE.txt new file mode 100644 index 0000000..dd175c5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/FILE_TREE.txt @@ -0,0 +1,50 @@ +calibration_sensor_interfaces/ +├── msg/ +│ ├── AppliedSensorCalibrationParametersResponse.msg +│ ├── ArtifactType.msg +│ ├── CalibrationArtifact.msg +│ ├── CalibrationReferenceTarget.msg +│ ├── CameraIntrinsicCalibrationTask.msg +│ ├── CameraIntrinsics.msg +│ ├── CameraModelType.msg +│ ├── CaptureIntentType.msg +│ ├── CommitSensorCalibrationParametersRequest.msg +│ ├── GetAppliedSensorCalibrationParametersRequest.msg +│ ├── HandEyeCalibrationMode.msg +│ ├── HandEyeCalibrationTask.msg +│ ├── IMUIntrinsicCalibrationTask.msg +│ ├── IMUIntrinsics.msg +│ ├── SensorCalibrationCapabilityRequest.msg +│ ├── SensorCalibrationCapabilityResponse.msg +│ ├── SensorCalibrationJobResult.msg +│ ├── SensorCalibrationParameter.msg +│ ├── SensorCalibrationParameterSet.msg +│ ├── SensorCalibrationTaskPurpose.msg +│ ├── SensorCalibrationTaskRequest.msg +│ ├── SensorCalibrationTaskType.msg +│ ├── SensorCalibrationTelemetry.msg +│ ├── SensorCalibrationWorkMode.msg +│ ├── SensorCalibrationWorkModeRequest.msg +│ ├── SensorExtrinsics.msg +│ ├── SensorReadinessResponse.msg +│ ├── SensorToBaseExtrinsicCalibrationTask.msg +│ ├── SensorValidationSummary.msg +│ └── StreamSensorCalibrationTelemetryRequest.msg +├── srv/ +│ ├── CancelSensorCalibrationJob.srv +│ ├── CommitSensorCalibrationParameters.srv +│ ├── EmergencyStop.srv +│ ├── GetAppliedSensorCalibrationParameters.srv +│ ├── GetSensorCalibrationCapability.srv +│ ├── GetSensorCalibrationJobResult.srv +│ ├── GetSensorCalibrationJobStatus.srv +│ ├── GetSensorReadiness.srv +│ ├── Heartbeat.srv +│ ├── SetSensorCalibrationWorkMode.srv +│ ├── StartSensorCalibrationTask.srv +│ └── StreamSensorCalibrationTelemetry.srv +├── action/ +│ └── ExecuteSensorCalibrationTask.action +├── CMakeLists.txt +├── package.xml +└── FILE_TREE.txt diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/action/ExecuteSensorCalibrationTask.action b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/action/ExecuteSensorCalibrationTask.action new file mode 100644 index 0000000..0fbeffd --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/action/ExecuteSensorCalibrationTask.action @@ -0,0 +1,22 @@ +# ========================================================= +# 传感器标定任务执行 Action +# 作用：执行一个完整的传感器标定长任务 +# 来源： +# 1) StartSensorCalibrationTask +# 2) GetSensorCalibrationJobStatus +# 3) GetSensorCalibrationJobResult +# 4) CancelSensorCalibrationJob +# 说明： +# 1) goal 对应 SensorCalibrationTaskRequest +# 2) result 对应 SensorCalibrationJobResult +# 3) feedback 统一复用 common 的 JobStatus +# ========================================================= + +# Goal：传感器标定任务请求 +SensorCalibrationTaskRequest goal +--- +# Result：传感器标定任务最终结果 +SensorCalibrationJobResult result +--- +# Feedback：执行过程中的状态反馈 +calibration_common_interfaces/JobStatus feedback diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/AppliedSensorCalibrationParametersResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/AppliedSensorCalibrationParametersResponse.msg new file mode 100644 index 0000000..55c9947 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/AppliedSensorCalibrationParametersResponse.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 查询当前已生效传感器参数响应 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 当前版本 +string parameter_version +# 当前生效参数 +SensorCalibrationParameterSet params +# 生效时间 +int64 applied_timestamp_us diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/ArtifactType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/ArtifactType.msg new file mode 100644 index 0000000..598e43c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/ArtifactType.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 标定产物类型 +# 作用：用于区分不同采集任务产生的文件类型 +# ========================================================= + +uint8 ARTIFACT_TYPE_UNSPECIFIED=0 +uint8 IMAGE_FILE=1 +uint8 POINT_CLOUD_FILE=2 +uint8 LASER_SCAN_FILE=3 +uint8 IMU_SEQUENCE_FILE=4 +uint8 POSE_SEQUENCE_FILE=5 +uint8 REPORT_FILE=6 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CalibrationArtifact.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CalibrationArtifact.msg new file mode 100644 index 0000000..fbf390b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CalibrationArtifact.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 标定产物 +# 作用：记录一次任务关联的文件产物 +# ========================================================= + +# 产物类型 +ArtifactType artifact_type +# 文件引用 +calibration_common_interfaces/FileReference file +# 对应传感器 ID +string sensor_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CalibrationReferenceTarget.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CalibrationReferenceTarget.msg new file mode 100644 index 0000000..ac50a65 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CalibrationReferenceTarget.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 统一参考目标 +# 作用：描述标定任务中依赖的参考物体 +# ========================================================= + +# 参考目标唯一 ID +string reference_target_id +# 类型，例如 board / pole / wall / ground_plane +string reference_target_type +# 说明 +string description diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraIntrinsicCalibrationTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraIntrinsicCalibrationTask.msg new file mode 100644 index 0000000..eeef526 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraIntrinsicCalibrationTask.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 相机内参标定任务 +# 作用：采集图像并求解相机内参 +# ========================================================= + +# 目标相机 ID +string sensor_id +# 所需图像数 +uint32 required_image_count +# 超时时间 +float64 timeout_sec +# 标定板 ID +string target_board_id +# 统一参考目标 +CalibrationReferenceTarget reference_target diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraIntrinsics.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraIntrinsics.msg new file mode 100644 index 0000000..e9de968 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraIntrinsics.msg @@ -0,0 +1,23 @@ +# ========================================================= +# 相机内参 +# 作用：表达相机标定后的内参结果 +# ========================================================= + +# 图像宽度 +uint32 image_width +# 图像高度 +uint32 image_height +# 焦距 fx +float64 fx +# 焦距 fy +float64 fy +# 主点 cx +float64 cx +# 主点 cy +float64 cy +# 畸变系数 +float64[] distortion_coeffs +# 畸变模型名称 +string distortion_model +# 相机模型类型 +CameraModelType camera_model_type diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraModelType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraModelType.msg new file mode 100644 index 0000000..a12bc31 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CameraModelType.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 相机模型类型 +# 作用：表达相机内参使用的模型 +# ========================================================= + +uint8 CAMERA_MODEL_TYPE_UNSPECIFIED=0 +uint8 PINHOLE=1 +uint8 FISHEYE=2 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CaptureIntentType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CaptureIntentType.msg new file mode 100644 index 0000000..034348e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CaptureIntentType.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 采集意图类型 +# 作用：告诉采集服务“这次数据是为哪类标定而采” +# ========================================================= + +uint8 CAPTURE_INTENT_TYPE_UNSPECIFIED=0 +uint8 CAMERA_INTRINSIC_CAPTURE=1 +uint8 CAMERA_EXTRINSIC_CAPTURE=2 +uint8 LIDAR_3D_EXTRINSIC_CAPTURE=3 +uint8 LIDAR_2D_EXTRINSIC_CAPTURE=4 +uint8 IMU_STATIC_CAPTURE=5 +uint8 IMU_DYNAMIC_CAPTURE=6 +uint8 HAND_EYE_CAPTURE=7 +uint8 VALIDATION_CAPTURE=8 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CommitSensorCalibrationParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CommitSensorCalibrationParametersRequest.msg new file mode 100644 index 0000000..dfdc66e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/CommitSensorCalibrationParametersRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 写入传感器标定参数请求 +# 作用：将 Linux 求解出的传感器参数写入系统 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 参数版本号 +string parameter_version +# 参数总包 +SensorCalibrationParameterSet params +# 写入原因 +string commit_reason +# 摘要 +calibration_common_interfaces/FileDigest digest +# 是否持久化 +bool persistent_write diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/GetAppliedSensorCalibrationParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/GetAppliedSensorCalibrationParametersRequest.msg new file mode 100644 index 0000000..13a3fb9 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/GetAppliedSensorCalibrationParametersRequest.msg @@ -0,0 +1,6 @@ +# ========================================================= +# 查询当前已生效传感器参数请求 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/HandEyeCalibrationMode.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/HandEyeCalibrationMode.msg new file mode 100644 index 0000000..e86dd9b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/HandEyeCalibrationMode.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 手眼标定模式 +# 作用：区分眼在手上与眼在手外 +# ========================================================= + +uint8 HAND_EYE_CALIBRATION_MODE_UNSPECIFIED=0 +uint8 EYE_IN_HAND=1 +uint8 EYE_TO_HAND=2 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/HandEyeCalibrationTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/HandEyeCalibrationTask.msg new file mode 100644 index 0000000..4473ef9 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/HandEyeCalibrationTask.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 手眼标定任务 +# 作用：求解机械臂相机与机械臂 / 车体的关系 +# ========================================================= + +# 目标相机 ID +string sensor_id +# 机械臂 ID +string arm_id +# 手眼模式 +HandEyeCalibrationMode mode +# 所需位姿数 +uint32 required_pose_count +# 超时时间 +float64 timeout_sec +# 统一参考目标 +CalibrationReferenceTarget reference_target diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/IMUIntrinsicCalibrationTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/IMUIntrinsicCalibrationTask.msg new file mode 100644 index 0000000..eda6417 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/IMUIntrinsicCalibrationTask.msg @@ -0,0 +1,13 @@ +# ========================================================= +# IMU 内参标定任务 +# 作用：采集 IMU 静态 / 动态数据并求解偏置等参数 +# ========================================================= + +# 目标 IMU ID +string sensor_id +# 静止段数量 +uint32 required_static_segment_count +# 运动段数量 +uint32 required_motion_segment_count +# 超时时间 +float64 timeout_sec diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/IMUIntrinsics.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/IMUIntrinsics.msg new file mode 100644 index 0000000..b53d85a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/IMUIntrinsics.msg @@ -0,0 +1,13 @@ +# ========================================================= +# IMU 内参 +# 作用：表达 IMU 偏置等内参结果 +# 说明： +# 1) accel_bias 为加速度计三轴偏置 +# 2) gyro_bias 为陀螺仪三轴偏置 +# 3) 采用强约束三维向量，不再使用 repeated double +# ========================================================= + +# 加速度计偏置 +calibration_common_interfaces/Vector3D accel_bias +# 陀螺仪偏置 +calibration_common_interfaces/Vector3D gyro_bias diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationCapabilityRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationCapabilityRequest.msg new file mode 100644 index 0000000..28939c3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationCapabilityRequest.msg @@ -0,0 +1,7 @@ +# ========================================================= +# 查询传感器标定能力请求 +# 作用：查询当前车辆和工位支持的传感器标定能力 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationCapabilityResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationCapabilityResponse.msg new file mode 100644 index 0000000..ec456fc --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationCapabilityResponse.msg @@ -0,0 +1,29 @@ +# ========================================================= +# 查询传感器标定能力响应 +# 作用：返回支持的传感器标定类型 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message + +# 是否支持相机内参标定 +bool supports_camera_intrinsic +# 是否支持 IMU 内参标定 +bool supports_imu_intrinsic +# 是否支持传感器到 base_link 外参 +bool supports_sensor_to_base_extrinsic +# 是否支持手眼标定 +bool supports_hand_eye + +# 是否支持 3D 激光雷达外参 +bool supports_3d_lidar_extrinsic +# 是否支持 2D 激光雷达外参 +bool supports_2d_lidar_extrinsic +# 是否支持仅做相机内参 +bool supports_camera_only_intrinsic +# 是否支持仅做相机外参 +bool supports_camera_only_extrinsic diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationJobResult.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationJobResult.msg new file mode 100644 index 0000000..9bca23d --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationJobResult.msg @@ -0,0 +1,25 @@ +# ========================================================= +# 传感器标定任务结果 +# 作用：返回某次传感器标定 / 验证的结果摘要 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 任务 ID +string job_id +# 数据质量是否达标 +bool data_quality_passed +# 是否适合写入 +bool suitable_for_commit +# 推荐参数版本 +string recommended_parameter_version +# 验证摘要 +SensorValidationSummary validation_summary +# 本轮估计参数 +SensorCalibrationParameterSet estimated_params +# 关联产物 +CalibrationArtifact[] artifacts diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationParameter.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationParameter.msg new file mode 100644 index 0000000..3db71e5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationParameter.msg @@ -0,0 +1,20 @@ +# ========================================================= +# 单个传感器标定参数 +# 作用：统一表达一个传感器的内参和 / 或外参 +# 说明： +# 1) 对相机可只填 camera_intrinsics +# 2) 对相机也可只填 extrinsics +# 3) 对 IMU 可填 imu_intrinsics 和 / 或 extrinsics +# 4) 对激光雷达通常只填 extrinsics +# ========================================================= + +# 传感器 ID +string sensor_id +# 传感器类型 +calibration_vehicle_profile_interfaces/SensorType sensor_type +# 相机内参；仅相机适用 +CameraIntrinsics camera_intrinsics +# IMU 内参；仅 IMU 适用 +IMUIntrinsics imu_intrinsics +# 传感器外参 +SensorExtrinsics extrinsics diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationParameterSet.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationParameterSet.msg new file mode 100644 index 0000000..3146581 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationParameterSet.msg @@ -0,0 +1,7 @@ +# ========================================================= +# 传感器标定参数总包 +# 作用：统一表达整车所有传感器的标定结果 +# ========================================================= + +# 参数项列表 +SensorCalibrationParameter[] items diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskPurpose.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskPurpose.msg new file mode 100644 index 0000000..45f041a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskPurpose.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 传感器标定任务目的 +# 来源：SensorCalibrationTaskRequest.TaskPurpose +# 作用：区分数据采集、求解和验证 +# ========================================================= + +uint8 SENSOR_TASK_PURPOSE_UNSPECIFIED=0 +uint8 DATA_COLLECTION=1 +uint8 SOLVING=2 +uint8 VALIDATION=3 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskRequest.msg new file mode 100644 index 0000000..55e8a1d --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskRequest.msg @@ -0,0 +1,30 @@ +# ========================================================= +# 传感器标定任务请求 +# 作用：启动一次传感器标定任务 +# 发送方：Ubuntu 车间电脑 +# 接收方：Windows 车端代理 / Linux 采集服务 +# 说明： +# 1) proto 中使用 oneof task +# 2) ROS2 中通过 selected_task + 各 payload 字段共同表达 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 测试用例 ID +string test_case_id +# 任务目的 +SensorCalibrationTaskPurpose task_purpose +# 当前生效的任务载荷类型 +SensorCalibrationTaskType selected_task +# 当前生效的任务子类型：用于区分前视/下视相机、2D/3D 激光雷达、IMU 外参、手眼模式等 +SensorCalibrationTaskSubtype task_subtype +# 相机内参 +CameraIntrinsicCalibrationTask camera_intrinsic +# IMU 内参 +IMUIntrinsicCalibrationTask imu_intrinsic +# 外参 +SensorToBaseExtrinsicCalibrationTask sensor_to_base_extrinsic +# 手眼标定 +HandEyeCalibrationTask hand_eye +# 来源迭代号 +string source_iteration_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskSubtype.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskSubtype.msg new file mode 100644 index 0000000..12438b2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskSubtype.msg @@ -0,0 +1,29 @@ +# ========================================================= +# 传感器标定任务子类型 +# 作用：在顶层 task_type 之下，进一步区分具体算法分支 +# 说明： +# 1) 顶层 task_type 只区分内参 / 外参 / 手眼等大类 +# 2) task_subtype 负责把真正的算法分支说清楚 +# 3) 这样 orchestrator 仍保持较稳定，而算法侧可以细分到具体传感器与安装位 +# ========================================================= + +uint8 SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED=0 + +# 传感器内参 +uint8 FRONT_CAMERA_INTRINSIC=1 +uint8 DOWNWARD_CAMERA_INTRINSIC=2 +uint8 IMU_INTRINSIC=3 + +# 传感器外参 +uint8 FRONT_CAMERA_EXTRINSIC=4 +uint8 DOWNWARD_CAMERA_EXTRINSIC=5 +uint8 IMU_EXTRINSIC=6 +uint8 LIDAR_2D_EXTRINSIC=7 +uint8 LIDAR_3D_EXTRINSIC=8 + +# 手眼 +uint8 EYE_IN_HAND=9 +uint8 EYE_TO_HAND=10 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskType.msg new file mode 100644 index 0000000..8c65b99 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTaskType.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 传感器标定任务载荷类型 +# 作用：补足 proto oneof task 的 ROS2 表达 +# ========================================================= + +uint8 SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED=0 +uint8 CAMERA_INTRINSIC=1 +uint8 IMU_INTRINSIC=2 +uint8 SENSOR_TO_BASE_EXTRINSIC=3 +uint8 HAND_EYE=4 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTelemetry.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTelemetry.msg new file mode 100644 index 0000000..b7b6dc7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationTelemetry.msg @@ -0,0 +1,23 @@ +# ========================================================= +# 传感器标定遥测 +# 作用：回传采集进度、观测质量和任务执行状态 +# ========================================================= + +# 硬件时间戳 +int64 hardware_timestamp_us +# 传感器 ID +string sensor_id +# 传感器类型 +calibration_vehicle_profile_interfaces/SensorType sensor_type +# 已采集样本数 +uint32 collected_sample_count +# 目标样本数 +uint32 target_sample_count +# 当前是否检测到标定目标 +bool target_detected +# 当前观测质量评分 +float64 quality_score +# 当前任务 ID +string active_job_id +# 当前采集意图 +CaptureIntentType capture_intent diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationWorkMode.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationWorkMode.msg new file mode 100644 index 0000000..3d351dc --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationWorkMode.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 传感器标定工作模式 +# 来源：SensorCalibrationWorkModeRequest.Mode +# 作用：切换采集服务到不同模式 +# ========================================================= + +uint8 SENSOR_CALIBRATION_MODE_UNSPECIFIED=0 +uint8 NORMAL_MODE=1 +uint8 CALIBRATION_READY_MODE=2 +uint8 DATA_CAPTURE_MODE=3 +uint8 VALIDATION_MODE=4 + +# 当前模式取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationWorkModeRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationWorkModeRequest.msg new file mode 100644 index 0000000..e1339db --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorCalibrationWorkModeRequest.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 传感器标定工作模式请求 +# 作用：切换采集服务到不同模式 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 目标模式 +SensorCalibrationWorkMode target_mode +# 切换原因 +string reason diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorExtrinsics.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorExtrinsics.msg new file mode 100644 index 0000000..d478020 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorExtrinsics.msg @@ -0,0 +1,12 @@ +# ========================================================= +# 传感器外参 +# 作用：表达传感器与车辆 base_link 的位姿关系 +# 说明：旋转统一使用弧度 +# ========================================================= + +# 父坐标系，一般为 base_link +string parent_frame_id +# 子坐标系，一般为 sensor frame +string child_frame_id +# 父到子的位姿 +calibration_common_interfaces/Pose3D parent_to_child diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorReadinessResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorReadinessResponse.msg new file mode 100644 index 0000000..da28a66 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorReadinessResponse.msg @@ -0,0 +1,31 @@ +# ========================================================= +# 传感器服务就绪响应 +# 作用：返回当前传感器服务是否满足执行条件 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message + +# 服务本身是否就绪 +bool agent_ready +# 采集链路是否就绪 +bool capture_pipeline_ready +# 存储链路是否就绪 +bool storage_ready +# 遥测链路是否就绪 +bool telemetry_ready +# 若任务需移动车辆，当前是否允许移动 +bool vehicle_safe_to_move +# 若任务涉及机械臂，机械臂是否就绪 +bool arm_ready + +# 已就绪的传感器 ID +string[] ready_sensor_ids +# 不满足项 +calibration_common_interfaces/ReadinessIssue[] issues +# 检查时间 +int64 checked_timestamp_us diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorToBaseExtrinsicCalibrationTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorToBaseExtrinsicCalibrationTask.msg new file mode 100644 index 0000000..00723c1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorToBaseExtrinsicCalibrationTask.msg @@ -0,0 +1,21 @@ +# ========================================================= +# 传感器到 base_link 外参标定任务 +# 作用：求解单个传感器与车辆 base_link 的位姿关系 +# 说明： +# 1) 相机可只做外参 +# 2) 3D / 2D 激光雷达通常只做外参 +# 3) IMU 可做外参 +# ========================================================= + +# 目标传感器 ID +string sensor_id +# 车辆 base_link / 基准 frame +string base_frame_id +# 所需样本数 +uint32 required_sample_count +# 超时时间 +float64 timeout_sec +# 采集意图 +CaptureIntentType capture_intent +# 统一参考目标 +CalibrationReferenceTarget reference_target diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorValidationSummary.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorValidationSummary.msg new file mode 100644 index 0000000..f8f0235 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/SensorValidationSummary.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 传感器验证摘要 +# 作用：返回传感器专项自动验收的关键指标 +# ========================================================= + +# 相机重投影误差 +float64 reprojection_error_px +# 外参平移残差 +float64 translation_residual_m +# 外参旋转残差 +float64 rotation_residual_rad +# 平面 / 配准残差 +float64 plane_residual_m +# 重复性误差 +float64 repeatability_error_m +# 自动验收是否通过 +bool auto_acceptance_passed diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/StreamSensorCalibrationTelemetryRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/StreamSensorCalibrationTelemetryRequest.msg new file mode 100644 index 0000000..70e9027 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/msg/StreamSensorCalibrationTelemetryRequest.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 传感器遥测流请求 +# 作用：配置传感器标定过程中的状态上报 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 期望上报频率 +uint32 expected_hz +# 是否包含采集进度 +bool include_observation_progress +# 是否包含质量指标 +bool include_quality_metrics +# 是否包含采集意图 +bool include_capture_intent diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/package.xml new file mode 100644 index 0000000..61c7731 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/package.xml @@ -0,0 +1,25 @@ + + + calibration_sensor_interfaces + 0.0.1 + ROS 2 interface package generated from sensor_calibration.proto. + + user + Proprietary + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + action_msgs + calibration_common_interfaces + calibration_vehicle_profile_interfaces + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/sensor_calibration.proto b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/proto/sensor_calibration.proto similarity index 56% rename from agv_calib_brain/src/win_ubuntu_bridge/proto/sensor_calibration.proto rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/proto/sensor_calibration.proto index 06e52a9..38d0096 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/sensor_calibration.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/proto/sensor_calibration.proto @@ -10,9 +10,10 @@ import "vehicle_profile.proto"; // 运行关系： // Ubuntu 车间电脑(Linux) -> Windows 车端代理 / 采集服务 // 说明： -// 1) Linux 负责图像 / 点云 / IMU 数据分析与参数求解 +// 1) Linux 负责图像 / 点云 / 激光扫描 / IMU 数据分析与参数求解 // 2) 车端 / 采集服务只负责组织采集、回传观测状态、写入结果 // 3) 支持相机内参、IMU 内参、传感器外参、手眼标定 +// 4) 支持相机“只做内参 / 只做外参 / 内外参都做” // ========================================================= // ========================================================= @@ -25,6 +26,10 @@ service AgvCalibSensorService { rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) returns (.agv.calibration.common.HeartbeatResponse); + // 查询传感器标定服务当前是否具备执行条件 + rpc GetSensorReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (SensorReadinessResponse); + // 设置传感器标定工作模式 rpc SetSensorCalibrationWorkMode(SensorCalibrationWorkModeRequest) returns (.agv.calibration.common.StandardResponse); @@ -85,6 +90,27 @@ message SensorCalibrationWorkModeRequest { string reason = 3; // 切换原因 } +// ========================================================= +// 传感器服务就绪响应 +// 作用：返回当前传感器服务是否满足执行条件 +// ========================================================= +message SensorReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool capture_pipeline_ready = 5; // 采集链路是否就绪 + bool storage_ready = 6; // 存储链路是否就绪 + bool telemetry_ready = 7; // 遥测链路是否就绪 + bool vehicle_safe_to_move = 8; // 若任务需移动车辆，当前是否允许移动 + bool arm_ready = 9; // 若任务涉及机械臂，机械臂是否就绪 + + repeated string ready_sensor_ids = 10; // 已就绪的传感器 ID + repeated .agv.calibration.common.ReadinessIssue issues = 11; // 不满足项 + int64 checked_timestamp_us = 12; // 检查时间 +} + // ========================================================= // 查询传感器标定能力请求 // 作用：查询当前车辆和工位支持的传感器标定能力 @@ -101,10 +127,38 @@ message SensorCalibrationCapabilityResponse { bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 string message = 3; // 说明 - bool supports_camera_intrinsic = 4; // 是否支持相机内参标定 - bool supports_imu_intrinsic = 5; // 是否支持 IMU 内参标定 - bool supports_sensor_to_base_extrinsic = 6; // 是否支持传感器到 base_link 外参 - bool supports_hand_eye = 7; // 是否支持手眼标定 + + bool supports_camera_intrinsic = 4; // 是否支持相机内参标定 + bool supports_imu_intrinsic = 5; // 是否支持 IMU 内参标定 + bool supports_sensor_to_base_extrinsic = 6; // 是否支持传感器到 base_link 外参 + bool supports_hand_eye = 7; // 是否支持手眼标定 + + bool supports_3d_lidar_extrinsic = 8; // 是否支持 3D 激光雷达外参 + bool supports_2d_lidar_extrinsic = 9; // 是否支持 2D 激光雷达外参 + bool supports_camera_only_intrinsic = 10; // 是否支持仅做相机内参 + bool supports_camera_only_extrinsic = 11; // 是否支持仅做相机外参 +} + +// ========================================================= +// 采集意图类型 +// 作用：告诉采集服务“这次数据是为哪类标定而采” +// ========================================================= +enum CaptureIntentType { + CAPTURE_INTENT_TYPE_UNSPECIFIED = 0; // 未指定 + CAMERA_INTRINSIC_CAPTURE = 1; // 相机内参采集 + CAMERA_EXTRINSIC_CAPTURE = 2; // 相机外参采集 + LIDAR_3D_EXTRINSIC_CAPTURE = 3; // 3D 激光雷达外参采集 + LIDAR_2D_EXTRINSIC_CAPTURE = 4; // 2D 激光雷达外参采集 + IMU_STATIC_CAPTURE = 5; // IMU 静态采集 + IMU_DYNAMIC_CAPTURE = 6; // IMU 动态采集 + HAND_EYE_CAPTURE = 7; // 手眼标定采集 + VALIDATION_CAPTURE = 8; // 验证采集 +} + +message CalibrationReferenceTarget { + string reference_target_id = 1; // 参考目标唯一 ID + string reference_target_type = 2; // 类型，例如 board / pole / wall / ground_plane + string description = 3; // 说明 } // ========================================================= @@ -112,9 +166,11 @@ message SensorCalibrationCapabilityResponse { // 作用：采集图像并求解相机内参 // ========================================================= message CameraIntrinsicCalibrationTask { - string sensor_id = 1; // 目标相机 ID - uint32 required_image_count = 2;// 所需图像数 - double timeout_sec = 3; // 超时时间 + string sensor_id = 1; // 目标相机 ID + uint32 required_image_count = 2; // 所需图像数 + double timeout_sec = 3; // 超时时间 + string target_board_id = 4; // 标定板 ID + CalibrationReferenceTarget reference_target = 5; // 统一参考目标 } // ========================================================= @@ -122,21 +178,27 @@ message CameraIntrinsicCalibrationTask { // 作用：采集 IMU 静态 / 动态数据并求解偏置等参数 // ========================================================= message IMUIntrinsicCalibrationTask { - string sensor_id = 1; // 目标 IMU ID + string sensor_id = 1; // 目标 IMU ID uint32 required_static_segment_count = 2; // 静止段数量 uint32 required_motion_segment_count = 3; // 运动段数量 - double timeout_sec = 4; // 超时时间 + double timeout_sec = 4; // 超时时间 } // ========================================================= // 传感器到 base_link 外参标定任务 // 作用：求解单个传感器与车辆 base_link 的位姿关系 +// 说明： +// 1) 相机可只做外参 +// 2) 3D / 2D 激光雷达通常只做外参 +// 3) IMU 可做外参 // ========================================================= message SensorToBaseExtrinsicCalibrationTask { - string sensor_id = 1; // 目标传感器 ID - string base_frame_id = 2; // 车辆 base_link / 基准 frame - uint32 required_sample_count = 3;// 所需样本数 - double timeout_sec = 4; // 超时时间 + string sensor_id = 1; // 目标传感器 ID + string base_frame_id = 2; // 车辆 base_link / 基准 frame + uint32 required_sample_count = 3; // 所需样本数 + double timeout_sec = 4; // 超时时间 + CaptureIntentType capture_intent = 5; // 采集意图 + CalibrationReferenceTarget reference_target = 6; // 统一参考目标 } // ========================================================= @@ -154,11 +216,30 @@ enum HandEyeCalibrationMode { // 作用：求解机械臂相机与机械臂 / 车体的关系 // ========================================================= message HandEyeCalibrationTask { - string sensor_id = 1; // 目标相机 ID - string arm_id = 2; // 机械臂 ID - HandEyeCalibrationMode mode = 3; // 手眼模式 - uint32 required_pose_count = 4; // 所需位姿数 - double timeout_sec = 5; // 超时时间 + string sensor_id = 1; // 目标相机 ID + string arm_id = 2; // 机械臂 ID + HandEyeCalibrationMode mode = 3; // 手眼模式 + uint32 required_pose_count = 4; // 所需位姿数 + double timeout_sec = 5; // 超时时间 + CalibrationReferenceTarget reference_target = 6; // 统一参考目标 +} + +// ========================================================= +// 传感器标定任务子类型 +// 作用：在顶层 task_type 之下进一步区分具体算法分支 +// ========================================================= +enum SensorCalibrationTaskSubtype { + SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED = 0; + FRONT_CAMERA_INTRINSIC = 1; + DOWNWARD_CAMERA_INTRINSIC = 2; + IMU_INTRINSIC = 3; + FRONT_CAMERA_EXTRINSIC = 4; + DOWNWARD_CAMERA_EXTRINSIC = 5; + IMU_EXTRINSIC = 6; + LIDAR_2D_EXTRINSIC = 7; + LIDAR_3D_EXTRINSIC = 8; + EYE_IN_HAND = 9; + EYE_TO_HAND = 10; } // ========================================================= @@ -179,15 +260,16 @@ message SensorCalibrationTaskRequest { } TaskPurpose task_purpose = 3; // 任务目的 + SensorCalibrationTaskSubtype task_subtype = 4; // 任务子类型 oneof task { - CameraIntrinsicCalibrationTask camera_intrinsic = 4; // 相机内参 - IMUIntrinsicCalibrationTask imu_intrinsic = 5; // IMU 内参 - SensorToBaseExtrinsicCalibrationTask sensor_to_base_extrinsic = 6; // 外参 - HandEyeCalibrationTask hand_eye = 7; // 手眼标定 + CameraIntrinsicCalibrationTask camera_intrinsic = 5; // 相机内参 + IMUIntrinsicCalibrationTask imu_intrinsic = 6; // IMU 内参 + SensorToBaseExtrinsicCalibrationTask sensor_to_base_extrinsic = 7; // 外参 + HandEyeCalibrationTask hand_eye = 8; // 手眼标定 } - string source_iteration_id = 8; // 来源迭代号 + string source_iteration_id = 9; // 来源迭代号 } // ========================================================= @@ -199,6 +281,7 @@ message StreamSensorCalibrationTelemetryRequest { uint32 expected_hz = 2; // 期望上报频率 bool include_observation_progress = 3; // 是否包含采集进度 bool include_quality_metrics = 4; // 是否包含质量指标 + bool include_capture_intent = 5; // 是否包含采集意图 } // ========================================================= @@ -214,6 +297,17 @@ message SensorCalibrationTelemetry { bool target_detected = 6; // 当前是否检测到标定目标 double quality_score = 7; // 当前观测质量评分 string active_job_id = 8; // 当前任务 ID + CaptureIntentType capture_intent = 9; // 当前采集意图 +} + +// ========================================================= +// 相机模型类型 +// 作用：表达相机内参使用的模型 +// ========================================================= +enum CameraModelType { + CAMERA_MODEL_TYPE_UNSPECIFIED = 0; // 未指定 + PINHOLE = 1; // 针孔模型 + FISHEYE = 2; // 鱼眼模型 } // ========================================================= @@ -221,14 +315,15 @@ message SensorCalibrationTelemetry { // 作用：表达相机标定后的内参结果 // ========================================================= message CameraIntrinsics { - uint32 image_width = 1; // 图像宽度 - uint32 image_height = 2; // 图像高度 - double fx = 3; // 焦距 fx - double fy = 4; // 焦距 fy - double cx = 5; // 主点 cx - double cy = 6; // 主点 cy + uint32 image_width = 1; // 图像宽度 + uint32 image_height = 2; // 图像高度 + double fx = 3; // 焦距 fx + double fy = 4; // 焦距 fy + double cx = 5; // 主点 cx + double cy = 6; // 主点 cy repeated double distortion_coeffs = 7; // 畸变系数 - string distortion_model = 8; // 畸变模型名称 + string distortion_model = 8; // 畸变模型名称 + CameraModelType camera_model_type = 9; // 相机模型类型 } // ========================================================= @@ -237,25 +332,32 @@ message CameraIntrinsics { // 说明： // 1) accel_bias 为加速度计三轴偏置 // 2) gyro_bias 为陀螺仪三轴偏置 +// 3) 采用强约束三维向量，不再使用 repeated double // ========================================================= message IMUIntrinsics { - .agv.calibration.common.Vector3D accel_bias = 1; // 加速度计偏置 - .agv.calibration.common.Vector3D gyro_bias = 2; // 陀螺仪偏置 + .agv.calibration.common.Vector3d accel_bias = 1; // 加速度计偏置 + .agv.calibration.common.Vector3d gyro_bias = 2; // 陀螺仪偏置 } // ========================================================= // 传感器外参 // 作用：表达传感器与车辆 base_link 的位姿关系 +// 说明：旋转统一使用弧度 // ========================================================= message SensorExtrinsics { - string parent_frame_id = 1; // 父坐标系，一般为 base_link - string child_frame_id = 2; // 子坐标系，一般为 sensor frame - .agv.calibration.common.Pose3D parent_to_child = 3; // 父到子的位姿 + string parent_frame_id = 1; // 父坐标系，一般为 base_link + string child_frame_id = 2; // 子坐标系，一般为 sensor frame + .agv.calibration.common.Pose3dEuler parent_to_child = 3; // 父到子的位姿 } // ========================================================= // 单个传感器标定参数 // 作用：统一表达一个传感器的内参和 / 或外参 +// 说明： +// 1) 对相机可只填 camera_intrinsics +// 2) 对相机也可只填 extrinsics +// 3) 对 IMU 可填 imu_intrinsics 和 / 或 extrinsics +// 4) 对激光雷达通常只填 extrinsics // ========================================================= message SensorCalibrationParameter { string sensor_id = 1; // 传感器 ID @@ -273,6 +375,43 @@ message SensorCalibrationParameterSet { repeated SensorCalibrationParameter items = 1; // 参数项列表 } +// ========================================================= +// 标定产物类型 +// 作用：用于区分不同采集任务产生的文件类型 +// ========================================================= +enum ArtifactType { + ARTIFACT_TYPE_UNSPECIFIED = 0; // 未指定 + IMAGE_FILE = 1; // 图像文件 + POINT_CLOUD_FILE = 2; // 点云文件 + LASER_SCAN_FILE = 3; // 2D 激光雷达扫描文件 + IMU_SEQUENCE_FILE = 4; // IMU 序列文件 + POSE_SEQUENCE_FILE = 5; // 位姿序列文件 + REPORT_FILE = 6; // 报告文件 +} + +// ========================================================= +// 标定产物 +// 作用：记录一次任务关联的文件产物 +// ========================================================= +message CalibrationArtifact { + ArtifactType artifact_type = 1; // 产物类型 + .agv.calibration.common.FileReference file = 2; // 文件引用 + string sensor_id = 3; // 对应传感器 ID +} + +// ========================================================= +// 传感器验证摘要 +// 作用：返回传感器专项自动验收的关键指标 +// ========================================================= +message SensorValidationSummary { + double reprojection_error_px = 1; // 相机重投影误差 + double translation_residual_m = 2; // 外参平移残差 + double rotation_residual_rad = 3; // 外参旋转残差 + double plane_residual_m = 4; // 平面 / 配准残差 + double repeatability_error_m = 5; // 重复性误差 + bool auto_acceptance_passed = 6; // 自动验收是否通过 +} + // ========================================================= // 写入传感器标定参数请求 // 作用：将 Linux 求解出的传感器参数写入系统 @@ -314,11 +453,12 @@ message SensorCalibrationJobResult { .agv.calibration.common.ErrorCode error_code = 2; // 错误码 string message = 3; // 说明 string job_id = 4; // 任务 ID - bool data_quality_passed = 5; // 数据质量是否达标 - bool suitable_for_commit = 6; // 是否适合写入 - string recommended_parameter_version = 7; // 推荐参数版本 - double reprojection_error_px = 8; // 相机类任务的重投影误差 - double translation_residual_m = 9; // 外参平移残差 - double rotation_residual_deg = 10; // 外参旋转残差 - repeated .agv.calibration.common.FileReference artifacts = 11; // 关联产物 + + bool data_quality_passed = 5; // 数据质量是否达标 + bool suitable_for_commit = 6; // 是否适合写入 + string recommended_parameter_version = 7; // 推荐参数版本 + + SensorValidationSummary validation_summary = 8; // 验证摘要 + SensorCalibrationParameterSet estimated_params = 9; // 本轮估计参数 + repeated CalibrationArtifact artifacts = 10; // 关联产物 } \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/CancelSensorCalibrationJob.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/CancelSensorCalibrationJob.srv new file mode 100644 index 0000000..e767995 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/CancelSensorCalibrationJob.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 取消传感器标定任务 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/CommitSensorCalibrationParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/CommitSensorCalibrationParameters.srv new file mode 100644 index 0000000..a5488ef --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/CommitSensorCalibrationParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 写入传感器标定参数 +# ========================================================= + +CommitSensorCalibrationParametersRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/EmergencyStop.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/EmergencyStop.srv new file mode 100644 index 0000000..d0282a1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/EmergencyStop.srv @@ -0,0 +1,12 @@ +# ========================================================= +# 紧急停止 +# 对应 proto： +# rpc EmergencyStop(.agv.calibration.common.Empty) +# returns (.agv.calibration.common.StandardResponse); +# 说明： +# 1) 不自定义 Empty.msg +# 2) 这里使用空请求 section +# ========================================================= + +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetAppliedSensorCalibrationParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetAppliedSensorCalibrationParameters.srv new file mode 100644 index 0000000..9002728 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetAppliedSensorCalibrationParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询当前已生效传感器参数 +# ========================================================= + +GetAppliedSensorCalibrationParametersRequest request +--- +AppliedSensorCalibrationParametersResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationCapability.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationCapability.srv new file mode 100644 index 0000000..7beebd7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationCapability.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询传感器标定能力 +# ========================================================= + +SensorCalibrationCapabilityRequest request +--- +SensorCalibrationCapabilityResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationJobResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationJobResult.srv new file mode 100644 index 0000000..55d62dc --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationJobResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询传感器标定任务结果 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +SensorCalibrationJobResult response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationJobStatus.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationJobStatus.srv new file mode 100644 index 0000000..bb5c89b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorCalibrationJobStatus.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询传感器标定任务状态 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/JobStatus response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorReadiness.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorReadiness.srv new file mode 100644 index 0000000..d3d579c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/GetSensorReadiness.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询传感器标定服务当前是否具备执行条件 +# ========================================================= + +calibration_common_interfaces/AgentReadinessRequest request +--- +SensorReadinessResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/Heartbeat.srv new file mode 100644 index 0000000..e6bf2ad --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/Heartbeat.srv @@ -0,0 +1,9 @@ +# ========================================================= +# 心跳保活 +# 运行位置：Windows 车端代理 或 Linux 采集桥接服务 +# 调用方：Ubuntu 车间电脑 +# ========================================================= + +calibration_common_interfaces/HeartbeatRequest request +--- +calibration_common_interfaces/HeartbeatResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/SetSensorCalibrationWorkMode.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/SetSensorCalibrationWorkMode.srv new file mode 100644 index 0000000..54738d7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/SetSensorCalibrationWorkMode.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 设置传感器标定工作模式 +# ========================================================= + +SensorCalibrationWorkModeRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/StartSensorCalibrationTask.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/StartSensorCalibrationTask.srv new file mode 100644 index 0000000..a319df3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/StartSensorCalibrationTask.srv @@ -0,0 +1,10 @@ +# ========================================================= +# 启动一个传感器标定任务 +# 说明： +# 1) 这是对 proto 原始 RPC 的 1:1 保真映射 +# 2) 真正的 ROS2 长任务主通路建议优先使用 action/ExecuteSensorCalibrationTask.action +# ========================================================= + +SensorCalibrationTaskRequest request +--- +calibration_common_interfaces/JobAccepted response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/StreamSensorCalibrationTelemetry.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/StreamSensorCalibrationTelemetry.srv new file mode 100644 index 0000000..075b888 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_sensor_interfaces/srv/StreamSensorCalibrationTelemetry.srv @@ -0,0 +1,11 @@ +# ========================================================= +# 打开传感器标定遥测流 +# 说明： +# 1) proto 原始语义是 server streaming +# 2) ROS2 中真正的实时流请使用 SensorCalibrationTelemetry topic +# 3) 该 srv 负责配置 / 开启 / 更新遥测推送策略 +# ========================================================= + +StreamSensorCalibrationTelemetryRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/CMakeLists.txt new file mode 100644 index 0000000..d2e6f8f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_vehicle_profile_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(calibration_common_interfaces REQUIRED) + +set(msg_files + "msg/ChassisType.msg" + "msg/SensorType.msg" + "msg/CameraMountType.msg" + "msg/ControlAxisType.msg" + "msg/ControllerAlgorithmType.msg" + "msg/CalibrationAbilityType.msg" + "msg/CalibrationModuleType.msg" + "msg/WorkflowStageType.msg" + "msg/VehicleBaseInfo.msg" + "msg/MechanicalArmProfile.msg" + "msg/SensorProfile.msg" + "msg/ControllerProfile.msg" + "msg/ControllerSelection.msg" + "msg/CalibrationCapability.msg" + "msg/VehicleProfile.msg" + "msg/RegisterOrUpdateVehicleProfileRequest.msg" + "msg/VehicleProfileQuery.msg" + "msg/VehicleProfileResponse.msg" + "msg/EvaluateVehicleCalibrationApplicabilityRequest.msg" + "msg/ApplicabilityIssue.msg" + "msg/VehicleCalibrationApplicabilityResponse.msg" +) + +set(srv_files + "srv/Heartbeat.srv" + "srv/RegisterOrUpdateVehicleProfile.srv" + "srv/GetVehicleProfile.srv" + "srv/EvaluateVehicleCalibrationApplicability.srv" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES calibration_common_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/FILE_TREE.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/FILE_TREE.txt new file mode 100644 index 0000000..699a4f2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/FILE_TREE.txt @@ -0,0 +1,32 @@ +CMakeLists.txt + package.xml + msg/ + ApplicabilityIssue.msg + CalibrationAbilityType.msg + CalibrationCapability.msg + CalibrationModuleType.msg + CameraMountType.msg + ChassisType.msg + ControlAxisType.msg + ControllerAlgorithmType.msg + ControllerProfile.msg + ControllerSelection.msg + EvaluateVehicleCalibrationApplicabilityRequest.msg + MechanicalArmProfile.msg + RegisterOrUpdateVehicleProfileRequest.msg + SensorProfile.msg + SensorType.msg + VehicleBaseInfo.msg + VehicleCalibrationApplicabilityResponse.msg + VehicleProfile.msg + VehicleProfileQuery.msg + VehicleProfileResponse.msg + WorkflowStageType.msg + srv/ + EvaluateVehicleCalibrationApplicability.srv + GetVehicleProfile.srv + Heartbeat.srv + RegisterOrUpdateVehicleProfile.srv + action/ + proto/ + vehicle_profile.proto diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ApplicabilityIssue.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ApplicabilityIssue.msg new file mode 100644 index 0000000..90a16ff --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ApplicabilityIssue.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 适用性问题 +# 作用：描述不支持 / 风险 / 缺失项 +# ========================================================= + +# 对应能力 +CalibrationAbilityType ability_type + +# 是否为阻塞问题 +bool blocking + +# 问题说明 +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationAbilityType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationAbilityType.msg new file mode 100644 index 0000000..f8d9dc3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationAbilityType.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 标定能力类型 +# 作用：用于编排器判断某个专项是否适用于当前车辆 +# ========================================================= + +uint8 CALIBRATION_ABILITY_UNSPECIFIED=0 # 未指定 +uint8 EXTERNAL_LOCALIZATION_CALIBRATION=1 # 外部定位 / 真值参考接入 +uint8 CHASSIS_CALIBRATION=2 # 底盘标定 +uint8 CONTROL_CALIBRATION=3 # 运控参数标定 +uint8 SENSOR_CALIBRATION=4 # 传感器标定 +uint8 HAND_EYE_CALIBRATION=5 # 手眼标定 + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationCapability.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationCapability.msg new file mode 100644 index 0000000..f5a92f3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationCapability.msg @@ -0,0 +1,9 @@ +# ========================================================= +# 文件作用：标定能力项 +# 对应 proto：CalibrationCapability +# 作用：表达当前车辆是否支持某项标定 +# ========================================================= + +CalibrationAbilityType ability_type # 标定能力类型，取值参考 CalibrationAbilityType.msg +bool supported # 是否支持 +string message # 说明文字 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationModuleType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationModuleType.msg new file mode 100644 index 0000000..337d3a5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CalibrationModuleType.msg @@ -0,0 +1,22 @@ +# ========================================================= +# 标定模块类型 +# 作用：供总控编排、执行事件、报告汇总时标识任务属于哪个模块 +# 说明： +# 1) 该枚举会被 workshop_orchestration.proto 直接引用 +# 2) 这里不仅包含传统“标定模块”，也补充了预检、联合验收、归档等总控阶段 +# ========================================================= + +uint8 CALIBRATION_MODULE_UNSPECIFIED=0 # 未指定 +uint8 EXTERNAL_LOCALIZATION_MODULE=1 # 外部定位 / 真值参考模块 +uint8 CHASSIS_MODULE=2 # 底盘标定模块 +uint8 CONTROL_MODULE=3 # 运控标定模块 +uint8 SENSOR_INTRINSIC_MODULE=4 # 传感器内参标定模块 +uint8 SENSOR_EXTRINSIC_MODULE=5 # 传感器外参标定模块 +uint8 HAND_EYE_MODULE=6 # 手眼标定模块 +uint8 WORKSHOP_PRECHECK_MODULE=7 # 车间预检模块 +uint8 JOINT_ACCEPTANCE_MODULE=8 # 整车联合验收模块 +uint8 PARAMETER_COMMIT_MODULE=9 # 参数固化模块 +uint8 REPORT_ARCHIVE_MODULE=10 # 报告与归档模块 + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CameraMountType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CameraMountType.msg new file mode 100644 index 0000000..fdf7125 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/CameraMountType.msg @@ -0,0 +1,14 @@ +# ========================================================= +# 相机安装形式 +# 作用：描述相机与车体 / 机械臂的安装关系 +# ========================================================= + +uint8 CAMERA_MOUNT_TYPE_UNSPECIFIED=0 # 未指定 +uint8 FIXED_ON_BASE=1 # 固定在车体上 +uint8 DOWNWARD_MOUNTED=2 # 下视安装 +uint8 FRONT_MOUNTED=3 # 前视安装 +uint8 EYE_IN_HAND=4 # 眼在手上 +uint8 EYE_TO_HAND=5 # 眼在手外 + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ChassisType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ChassisType.msg new file mode 100644 index 0000000..25f6fa5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ChassisType.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 底盘类型 +# 作用：描述车辆底盘运动学形式 +# ========================================================= + +uint8 CHASSIS_TYPE_UNSPECIFIED=0 # 未指定 +uint8 ACKERMANN=1 # 阿克曼 +uint8 DIFFERENTIAL=2 # 差速 +uint8 SINGLE_STEER_WHEEL=3 # 单舵轮 +uint8 MULTI_STEER_WHEEL=4 # 多舵轮 + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControlAxisType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControlAxisType.msg new file mode 100644 index 0000000..131fb2a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControlAxisType.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 控制轴类型 +# 作用：将控制器分成横向控制器与纵向控制器 +# ========================================================= + +uint8 CONTROL_AXIS_UNSPECIFIED=0 # 未指定 +uint8 LATERAL_CONTROL=1 # 横向控制 +uint8 LONGITUDINAL_CONTROL=2 # 纵向控制 + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerAlgorithmType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerAlgorithmType.msg new file mode 100644 index 0000000..1e88997 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerAlgorithmType.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 控制算法类型 +# 作用：描述车辆当前支持的控制算法 +# ========================================================= + +uint8 CONTROLLER_ALGORITHM_UNSPECIFIED=0 # 未指定 +uint8 PID=1 # PID +uint8 MPC=2 # MPC +uint8 LQR=3 # LQR +uint8 PURE_PURSUIT=4 # Pure Pursuit + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerProfile.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerProfile.msg new file mode 100644 index 0000000..318309c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerProfile.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 控制器画像 +# 作用：描述某一控制轴支持的算法集合 +# 说明： +# 1) 该结构偏静态能力描述 +# 2) 表达“这个控制轴支持哪些算法、默认算法是什么” +# ========================================================= + +# 控制轴类型 +ControlAxisType control_axis + +# 支持的算法列表 +ControllerAlgorithmType[] supported_algorithms + +# 默认算法 +ControllerAlgorithmType default_algorithm \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerSelection.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerSelection.msg new file mode 100644 index 0000000..cf8e43c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/ControllerSelection.msg @@ -0,0 +1,22 @@ +# ========================================================= +# 控制器选择项 +# 作用：表达“本轮标定到底要启用哪个控制轴、哪个算法、是否需要调优” +# 说明： +# 1) 该结构偏会话/配方侧选择 +# 2) 用于解决“横向用 MPC，纵向用 PID”这类组合问题 +# ========================================================= + +# 控制轴类型 +ControlAxisType control_axis + +# 选中的算法类型 +ControllerAlgorithmType algorithm_type + +# 本轮是否启用该控制器 +bool enabled + +# 本轮是否需要对该控制器调优 +bool need_tuning + +# 是否为当前默认控制器 +bool is_default \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/EvaluateVehicleCalibrationApplicabilityRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/EvaluateVehicleCalibrationApplicabilityRequest.msg new file mode 100644 index 0000000..caf5651 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/EvaluateVehicleCalibrationApplicabilityRequest.msg @@ -0,0 +1,10 @@ +# ========================================================= +# 标定适用性评估请求 +# 作用：根据车辆画像分析能否执行各项标定 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header + +# 车辆画像快照 +VehicleProfile profile_snapshot \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/MechanicalArmProfile.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/MechanicalArmProfile.msg new file mode 100644 index 0000000..fad024a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/MechanicalArmProfile.msg @@ -0,0 +1,12 @@ +# ========================================================= +# 文件作用：机械臂画像 +# 对应 proto：MechanicalArmProfile +# 作用：描述车辆是否带机械臂以及机械臂的基本信息 +# ========================================================= + +bool has_mechanical_arm # 是否带机械臂 +string arm_id # 机械臂 ID +string arm_model # 机械臂型号 +uint32 dof # 自由度 +string arm_base_frame # 机械臂基座坐标系 +string tool_frame # 末端工具坐标系 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/RegisterOrUpdateVehicleProfileRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/RegisterOrUpdateVehicleProfileRequest.msg new file mode 100644 index 0000000..b95029f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/RegisterOrUpdateVehicleProfileRequest.msg @@ -0,0 +1,9 @@ +# ========================================================= +# 注册 / 更新车辆画像请求 +# 作用：向系统写入一份车辆画像 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 车辆画像 +VehicleProfile profile \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/SensorProfile.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/SensorProfile.msg new file mode 100644 index 0000000..6d0c8b9 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/SensorProfile.msg @@ -0,0 +1,44 @@ +# ========================================================= +# 传感器画像 +# 作用：描述单个传感器的安装与参与标定属性 +# 说明： +# 1) enabled 表示这台车物理上是否启用了该传感器 +# 2) selected_for_this_session 表示本轮标定是否纳入执行计划 +# 3) needs_intrinsic_calibration / needs_extrinsic_calibration 表示本轮是否需要做对应标定 +# ========================================================= + +# 传感器 ID +string sensor_id + +# 传感器类型 +SensorType sensor_type + +# 传感器显示名 +string sensor_name + +# 传感器 frame_id +string frame_id + +# 相机安装形式；非相机可忽略 +CameraMountType camera_mount_type + +# 当前是否启用 +bool enabled + +# 本轮是否需要做内参标定 +bool needs_intrinsic_calibration + +# 本轮是否需要做外参标定 +bool needs_extrinsic_calibration + +# 设备名 / topic / IP / 端口等提示信息 +string device_hint + +# 本轮是否纳入执行计划 +bool selected_for_this_session + +# 该传感器是否支持内参标定 +bool supports_intrinsic_calibration + +# 该传感器是否支持外参标定 +bool supports_extrinsic_calibration \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/SensorType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/SensorType.msg new file mode 100644 index 0000000..22ad7d9 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/SensorType.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 传感器类型 +# 作用：描述车辆上挂载的传感器种类 +# ========================================================= + +uint8 SENSOR_TYPE_UNSPECIFIED=0 # 未指定 +uint8 DOWNWARD_CAMERA=1 # 下视相机 +uint8 FRONT_CAMERA=2 # 前视相机 +uint8 ARM_CAMERA=3 # 机械臂相机 +uint8 LIDAR_3D=4 # 3D 激光雷达 +uint8 LIDAR_2D=5 # 2D 激光雷达 +uint8 IMU=6 # IMU + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleBaseInfo.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleBaseInfo.msg new file mode 100644 index 0000000..79992e1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleBaseInfo.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 车辆基础信息 +# 作用：表达车辆身份信息 +# ========================================================= + +# 车辆唯一 ID +string vehicle_id +# 车辆显示名 +string vehicle_name +# 车型 / 型号 +string model_name +# 出厂序列号 +string serial_number +# 厂商 +string manufacturer +# 备注说明 +string description \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleCalibrationApplicabilityResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleCalibrationApplicabilityResponse.msg new file mode 100644 index 0000000..293dfdd --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleCalibrationApplicabilityResponse.msg @@ -0,0 +1,25 @@ +# ========================================================= +# 标定适用性评估响应 +# 作用：返回支持项与阻塞项，供编排器生成计划 +# ========================================================= + +# 是否成功 +bool success + +# 错误码 +calibration_common_interfaces/ErrorCode error_code + +# 说明 +string message + +# 支持能力列表 +CalibrationCapability[] capabilities + +# 问题列表 +ApplicabilityIssue[] issues + +# 是否总体可开展标定 +bool overall_supported + +# 推荐执行阶段 +WorkflowStageType[] recommended_workflow_stages \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfile.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfile.msg new file mode 100644 index 0000000..85b1d6e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfile.msg @@ -0,0 +1,45 @@ +# ========================================================= +# 车辆画像总包 +# 作用：统一表达待标定车辆的全部静态配置 +# 发送方：配置界面 / 车间系统 +# 接收方：车间编排器 / 各专项服务 +# ========================================================= + +# 车辆基础信息 +VehicleBaseInfo base_info + +# 底盘类型 +ChassisType chassis_type + +# 机械臂画像 +MechanicalArmProfile arm_profile + +# 所有传感器画像 +SensorProfile[] sensors + +# 控制器能力画像 +ControllerProfile[] controllers + +# 车辆 URDF 文件引用 +calibration_common_interfaces/FileReference urdf_file + +# 车辆 base_link 名称 +string base_link_frame + +# 标定能力清单 +CalibrationCapability[] capabilities + +# 扩展元数据 +calibration_common_interfaces/KeyValuePair[] metadata + +# 本轮控制器选择配置 +ControllerSelection[] controller_selections + +# 本轮启用的流程阶段 +WorkflowStageType[] enabled_workflow_stages + +# 车辆画像版本号 +string profile_version + +# 车辆画像摘要 +calibration_common_interfaces/FileDigest profile_digest \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfileQuery.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfileQuery.msg new file mode 100644 index 0000000..1687664 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfileQuery.msg @@ -0,0 +1,10 @@ +# ========================================================= +# 车辆画像查询请求 +# 作用：按 vehicle_id 查询车辆画像 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header + +# 车辆 ID +string vehicle_id \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfileResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfileResponse.msg new file mode 100644 index 0000000..4545e31 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/VehicleProfileResponse.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 车辆画像查询响应 +# 作用：返回完整车辆画像 +# ========================================================= + +# 是否成功 +bool success + +# 错误码 +calibration_common_interfaces/ErrorCode error_code + +# 说明 +string message + +# 车辆画像 +VehicleProfile profile \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/WorkflowStageType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/WorkflowStageType.msg new file mode 100644 index 0000000..1a7f7ac --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/msg/WorkflowStageType.msg @@ -0,0 +1,23 @@ +# ========================================================= +# 工作流阶段类型 +# 作用：用于描述自动化标定车间完整流程中的阶段 +# 说明： +# 1) 该枚举比 CalibrationModuleType 更偏“流程阶段” +# 2) 可供后续编排器构建执行计划时直接使用 +# ========================================================= + +uint8 WORKFLOW_STAGE_UNSPECIFIED=0 # 未指定 +uint8 PROFILE_VALIDATION_STAGE=1 # 车辆画像校验阶段 +uint8 EXTERNAL_REFERENCE_READY_CHECK_STAGE=2 # 真值参考接入与确认阶段 +uint8 WORKSHOP_PRECHECK_STAGE=3 # 整车预检阶段 +uint8 CHASSIS_CALIBRATION_STAGE=4 # 底盘标定阶段 +uint8 CONTROL_CALIBRATION_STAGE=5 # 运控参数调优阶段 +uint8 SENSOR_INTRINSIC_CALIBRATION_STAGE=6 # 传感器内参标定阶段 +uint8 SENSOR_EXTRINSIC_CALIBRATION_STAGE=7 # 传感器外参标定阶段 +uint8 HAND_EYE_CALIBRATION_STAGE=8 # 手眼标定阶段 +uint8 JOINT_ACCEPTANCE_STAGE=9 # 整车联合验收阶段 +uint8 PARAMETER_COMMIT_STAGE=10 # 参数固化阶段 +uint8 REPORT_ARCHIVE_STAGE=11 # 报告归档阶段 + +# 当前枚举取值 +uint8 value \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/package.xml new file mode 100644 index 0000000..34b2af0 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/package.xml @@ -0,0 +1,22 @@ + + + calibration_vehicle_profile_interfaces + 0.0.1 + ROS 2 interfaces converted from vehicle_profile.proto. + user + Apache-2.0 + + ament_cmake + + rosidl_default_generators + calibration_common_interfaces + + rosidl_default_runtime + + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/vehicle_profile.proto b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/proto/vehicle_profile.proto similarity index 62% rename from agv_calib_brain/src/win_ubuntu_bridge/proto/vehicle_profile.proto rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/proto/vehicle_profile.proto index ae9a338..678fccd 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/vehicle_profile.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/proto/vehicle_profile.proto @@ -4,11 +4,41 @@ package agv.calibration.vehicle.profile; import "calibration_common.proto"; +// ├── msg/ +// │ ├── ChassisType.msg +// │ ├── SensorType.msg +// │ ├── CameraMountType.msg +// │ ├── ControlAxisType.msg +// │ ├── ControllerAlgorithmType.msg +// │ ├── CalibrationAbilityType.msg +// │ ├── CalibrationModuleType.msg +// │ ├── WorkflowStageType.msg +// │ ├── VehicleBaseInfo.msg +// │ ├── MechanicalArmProfile.msg +// │ ├── SensorProfile.msg +// │ ├── ControllerProfile.msg +// │ ├── ControllerSelection.msg +// │ ├── CalibrationCapability.msg +// │ ├── VehicleProfile.msg +// │ ├── RegisterOrUpdateVehicleProfileRequest.msg +// │ ├── VehicleProfileQuery.msg +// │ ├── VehicleProfileResponse.msg +// │ ├── EvaluateVehicleCalibrationApplicabilityRequest.msg +// │ ├── ApplicabilityIssue.msg +// │ └── VehicleCalibrationApplicabilityResponse.msg +// ├── srv/ +// │ ├── RegisterOrUpdateVehicleProfile.srv +// │ ├── GetVehicleProfile.srv +// │ └── EvaluateVehicleCalibrationApplicability.srv +// └── action/ +// └── （当前无需新增 action） + // ========================================================= // 文件作用：车辆画像与能力描述协议 // 使用范围： // 1) 车间在开始整车标定前，先描述“要标定的到底是什么车” // 2) 供车间编排器根据车辆画像自动生成执行计划 +// 3) 供各专项服务判断当前车辆是否支持对应标定任务 // ========================================================= // ========================================================= @@ -75,7 +105,7 @@ enum CameraMountType { } // ========================================================= -// 控制器类型 +// 控制轴类型 // 作用：将控制器分成横向控制器与纵向控制器 // ========================================================= enum ControlAxisType { @@ -101,12 +131,55 @@ enum ControllerAlgorithmType { // 作用：用于编排器判断某个专项是否适用于当前车辆 // ========================================================= enum CalibrationAbilityType { - CALIBRATION_ABILITY_UNSPECIFIED = 0; // 未指定 - EXTERNAL_LOCALIZATION_CALIBRATION = 1; // 外部定位 / 基准建立 - CHASSIS_CALIBRATION = 2; // 底盘标定 - CONTROL_CALIBRATION = 3; // 运控参数标定 - SENSOR_CALIBRATION = 4; // 传感器标定 - HAND_EYE_CALIBRATION = 5; // 手眼标定 + CALIBRATION_ABILITY_UNSPECIFIED = 0; // 未指定 + EXTERNAL_LOCALIZATION_CALIBRATION = 1; // 外部定位 / 真值参考接入 + CHASSIS_CALIBRATION = 2; // 底盘标定 + CONTROL_CALIBRATION = 3; // 运控参数标定 + SENSOR_CALIBRATION = 4; // 传感器标定 + HAND_EYE_CALIBRATION = 5; // 手眼标定 +} + +// ========================================================= +// 标定模块类型 +// 作用：供总控编排、执行事件、报告汇总时标识任务属于哪个模块 +// 说明： +// 1) 该枚举会被 workshop_orchestration.proto 直接引用 +// 2) 这里不仅包含传统“标定模块”，也补充了预检、联合验收、归档等总控阶段 +// ========================================================= +enum CalibrationModuleType { + CALIBRATION_MODULE_UNSPECIFIED = 0; // 未指定 + EXTERNAL_LOCALIZATION_MODULE = 1; // 外部定位 / 真值参考模块 + CHASSIS_MODULE = 2; // 底盘标定模块 + CONTROL_MODULE = 3; // 运控标定模块 + SENSOR_INTRINSIC_MODULE = 4; // 传感器内参标定模块 + SENSOR_EXTRINSIC_MODULE = 5; // 传感器外参标定模块 + HAND_EYE_MODULE = 6; // 手眼标定模块 + WORKSHOP_PRECHECK_MODULE = 7; // 车间预检模块 + JOINT_ACCEPTANCE_MODULE = 8; // 整车联合验收模块 + PARAMETER_COMMIT_MODULE = 9; // 参数固化模块 + REPORT_ARCHIVE_MODULE = 10; // 报告与归档模块 +} + +// ========================================================= +// 工作流阶段类型 +// 作用：用于描述自动化标定车间完整流程中的阶段 +// 说明： +// 1) 该枚举比 CalibrationModuleType 更偏“流程阶段” +// 2) 可供后续编排器构建执行计划时直接使用 +// ========================================================= +enum WorkflowStageType { + WORKFLOW_STAGE_UNSPECIFIED = 0; // 未指定 + PROFILE_VALIDATION_STAGE = 1; // 车辆画像校验阶段 + EXTERNAL_REFERENCE_READY_CHECK_STAGE = 2; // 真值参考接入与确认阶段 + WORKSHOP_PRECHECK_STAGE = 3; // 整车预检阶段 + CHASSIS_CALIBRATION_STAGE = 4; // 底盘标定阶段 + CONTROL_CALIBRATION_STAGE = 5; // 运控参数调优阶段 + SENSOR_INTRINSIC_CALIBRATION_STAGE = 6; // 传感器内参标定阶段 + SENSOR_EXTRINSIC_CALIBRATION_STAGE = 7; // 传感器外参标定阶段 + HAND_EYE_CALIBRATION_STAGE = 8; // 手眼标定阶段 + JOINT_ACCEPTANCE_STAGE = 9; // 整车联合验收阶段 + PARAMETER_COMMIT_STAGE = 10; // 参数固化阶段 + REPORT_ARCHIVE_STAGE = 11; // 报告归档阶段 } // ========================================================= @@ -138,6 +211,10 @@ message MechanicalArmProfile { // ========================================================= // 传感器画像 // 作用：描述单个传感器的安装与参与标定属性 +// 说明： +// 1) enabled 表示这台车物理上是否启用了该传感器 +// 2) selected_for_this_session 表示本轮标定是否纳入执行计划 +// 3) needs_intrinsic_calibration / needs_extrinsic_calibration 表示本轮是否需要做对应标定 // ========================================================= message SensorProfile { string sensor_id = 1; // 传感器 ID @@ -146,19 +223,40 @@ message SensorProfile { string frame_id = 4; // 传感器 frame_id CameraMountType camera_mount_type = 5;// 相机安装形式；非相机可忽略 bool enabled = 6; // 当前是否启用 - bool needs_intrinsic_calibration = 7; // 是否需要做内参标定 - bool needs_extrinsic_calibration = 8; // 是否需要做外参标定 + bool needs_intrinsic_calibration = 7; // 本轮是否需要做内参标定 + bool needs_extrinsic_calibration = 8; // 本轮是否需要做外参标定 string device_hint = 9; // 设备名 / topic / IP / 端口等提示信息 + bool selected_for_this_session = 10; // 本轮是否纳入执行计划 + bool supports_intrinsic_calibration = 11; // 该传感器是否支持内参标定 + bool supports_extrinsic_calibration = 12; // 该传感器是否支持外参标定 } // ========================================================= // 控制器画像 // 作用：描述某一控制轴支持的算法集合 +// 说明： +// 1) 该结构偏静态能力描述 +// 2) 表达“这个控制轴支持哪些算法、默认算法是什么” // ========================================================= message ControllerProfile { - ControlAxisType control_axis = 1; // 控制轴类型 + ControlAxisType control_axis = 1; // 控制轴类型 repeated ControllerAlgorithmType supported_algorithms = 2; // 支持的算法列表 - ControllerAlgorithmType default_algorithm = 3; // 默认算法 + ControllerAlgorithmType default_algorithm = 3; // 默认算法 +} + +// ========================================================= +// 控制器选择项 +// 作用：表达“本轮标定到底要启用哪个控制轴、哪个算法、是否需要调优” +// 说明： +// 1) 该结构偏会话/配方侧选择 +// 2) 用于解决“横向用 MPC，纵向用 PID”这类组合问题 +// ========================================================= +message ControllerSelection { + ControlAxisType control_axis = 1; // 控制轴类型 + ControllerAlgorithmType algorithm_type = 2; // 选中的算法类型 + bool enabled = 3; // 本轮是否启用该控制器 + bool need_tuning = 4; // 本轮是否需要对该控制器调优 + bool is_default = 5; // 是否为当前默认控制器 } // ========================================================= @@ -182,11 +280,16 @@ message VehicleProfile { ChassisType chassis_type = 2; // 底盘类型 MechanicalArmProfile arm_profile = 3; // 机械臂画像 repeated SensorProfile sensors = 4; // 所有传感器画像 - repeated ControllerProfile controllers = 5; // 控制器画像 + repeated ControllerProfile controllers = 5; // 控制器能力画像 .agv.calibration.common.FileReference urdf_file = 6; // 车辆 URDF 文件引用 string base_link_frame = 7; // 车辆 base_link 名称 repeated CalibrationCapability capabilities = 8; // 标定能力清单 repeated .agv.calibration.common.KeyValuePair metadata = 9; // 扩展元数据 + + repeated ControllerSelection controller_selections = 10; // 本轮控制器选择配置 + repeated WorkflowStageType enabled_workflow_stages = 11; // 本轮启用的流程阶段 + string profile_version = 12; // 车辆画像版本号 + .agv.calibration.common.FileDigest profile_digest = 13; // 车辆画像摘要 } // ========================================================= @@ -248,4 +351,5 @@ message VehicleCalibrationApplicabilityResponse { repeated CalibrationCapability capabilities = 4; // 支持能力列表 repeated ApplicabilityIssue issues = 5; // 问题列表 bool overall_supported = 6; // 是否总体可开展标定 + repeated WorkflowStageType recommended_workflow_stages = 7; // 推荐执行阶段 } \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/EvaluateVehicleCalibrationApplicability.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/EvaluateVehicleCalibrationApplicability.srv new file mode 100644 index 0000000..0cfbf9b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/EvaluateVehicleCalibrationApplicability.srv @@ -0,0 +1,4 @@ +# 评估当前车辆画像支持哪些标定能力 +EvaluateVehicleCalibrationApplicabilityRequest request +--- +VehicleCalibrationApplicabilityResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/GetVehicleProfile.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/GetVehicleProfile.srv new file mode 100644 index 0000000..54c32ab --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/GetVehicleProfile.srv @@ -0,0 +1,4 @@ +# 查询车辆画像 +VehicleProfileQuery request +--- +VehicleProfileResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/Heartbeat.srv new file mode 100644 index 0000000..dd025ed --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/Heartbeat.srv @@ -0,0 +1,4 @@ +# 心跳保活 +calibration_common_interfaces/HeartbeatRequest request +--- +calibration_common_interfaces/HeartbeatResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/RegisterOrUpdateVehicleProfile.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/RegisterOrUpdateVehicleProfile.srv new file mode 100644 index 0000000..620620b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_vehicle_profile_interfaces/srv/RegisterOrUpdateVehicleProfile.srv @@ -0,0 +1,4 @@ +# 注册 / 更新一份车辆画像 +RegisterOrUpdateVehicleProfileRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/CMakeLists.txt new file mode 100644 index 0000000..34cff5b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.8) +project(calibration_workshop_orchestration_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(action_msgs REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_vehicle_profile_interfaces REQUIRED) + +set(msg_files + "msg/WorkshopSessionState.msg" + "msg/StageExecutionPolicy.msg" + "msg/ApprovalState.msg" + "msg/ManualActionType.msg" + "msg/WorkshopEventType.msg" + "msg/WorkshopOperatorInfo.msg" + "msg/RequestedCalibrationTask.msg" + "msg/WorkshopSessionConfig.msg" + "msg/StagePlan.msg" + "msg/ExecutionPlanValidationIssue.msg" + "msg/WorkshopSession.msg" + "msg/CreateWorkshopSessionRequest.msg" + "msg/CreateWorkshopSessionResponse.msg" + "msg/WorkshopSessionQuery.msg" + "msg/WorkshopSessionResponse.msg" + "msg/BuildExecutionPlanRequest.msg" + "msg/RebuildExecutionPlanRequest.msg" + "msg/BuildExecutionPlanResponse.msg" + "msg/ValidateExecutionPlanRequest.msg" + "msg/ValidateExecutionPlanResponse.msg" + "msg/RunWorkshopPrecheckRequest.msg" + "msg/PrecheckItem.msg" + "msg/WorkshopPrecheckResponse.msg" + "msg/StartWorkshopSessionRequest.msg" + "msg/PauseWorkshopSessionRequest.msg" + "msg/ResumeWorkshopSessionRequest.msg" + "msg/CancelWorkshopSessionRequest.msg" + "msg/WorkshopEvent.msg" + "msg/ManualStepAckRequest.msg" + "msg/ApproveStageResultRequest.msg" + "msg/RollbackStageParametersRequest.msg" + "msg/StageResultSummary.msg" + "msg/WorkshopReport.msg" + "msg/WorkshopReportResponse.msg" +) + +set(srv_files + "srv/Heartbeat.srv" + "srv/CreateWorkshopSession.srv" + "srv/GetWorkshopSession.srv" + "srv/BuildExecutionPlan.srv" + "srv/RebuildExecutionPlan.srv" + "srv/ValidateExecutionPlan.srv" + "srv/RunWorkshopPrecheck.srv" + "srv/GetLastWorkshopPrecheckResult.srv" + "srv/StartWorkshopSession.srv" + "srv/GetWorkshopJobStatus.srv" + "srv/PauseWorkshopSession.srv" + "srv/ResumeWorkshopSession.srv" + "srv/CancelWorkshopSession.srv" + "srv/StreamWorkshopEvent.srv" + "srv/AcknowledgeManualStep.srv" + "srv/ApproveStageResult.srv" + "srv/RollbackStageParameters.srv" + "srv/GetWorkshopReport.srv" +) + +set(action_files + "action/RunWorkshopPrecheck.action" + "action/ExecuteWorkshopSession.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES action_msgs calibration_common_interfaces calibration_vehicle_profile_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/FILE_TREE.txt b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/FILE_TREE.txt new file mode 100644 index 0000000..6fdb797 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/FILE_TREE.txt @@ -0,0 +1,61 @@ +CMakeLists.txt +action/ +action/ExecuteWorkshopSession.action +action/RunWorkshopPrecheck.action +msg/ +msg/ApprovalState.msg +msg/ApproveStageResultRequest.msg +msg/BuildExecutionPlanRequest.msg +msg/BuildExecutionPlanResponse.msg +msg/CancelWorkshopSessionRequest.msg +msg/CreateWorkshopSessionRequest.msg +msg/CreateWorkshopSessionResponse.msg +msg/ExecutionPlanValidationIssue.msg +msg/ManualActionType.msg +msg/ManualStepAckRequest.msg +msg/PauseWorkshopSessionRequest.msg +msg/PrecheckItem.msg +msg/RebuildExecutionPlanRequest.msg +msg/RequestedCalibrationTask.msg +msg/ResumeWorkshopSessionRequest.msg +msg/RollbackStageParametersRequest.msg +msg/RunWorkshopPrecheckRequest.msg +msg/StageExecutionPolicy.msg +msg/StagePlan.msg +msg/StageResultSummary.msg +msg/StartWorkshopSessionRequest.msg +msg/ValidateExecutionPlanRequest.msg +msg/ValidateExecutionPlanResponse.msg +msg/WorkshopEvent.msg +msg/WorkshopEventType.msg +msg/WorkshopOperatorInfo.msg +msg/WorkshopPrecheckResponse.msg +msg/WorkshopReport.msg +msg/WorkshopReportResponse.msg +msg/WorkshopSession.msg +msg/WorkshopSessionConfig.msg +msg/WorkshopSessionQuery.msg +msg/WorkshopSessionResponse.msg +msg/WorkshopSessionState.msg +package.xml +proto/ +proto/workshop_orchestration.proto +srv/ +srv/AcknowledgeManualStep.srv +srv/ApproveStageResult.srv +srv/BuildExecutionPlan.srv +srv/CancelWorkshopSession.srv +srv/CreateWorkshopSession.srv +srv/GetLastWorkshopPrecheckResult.srv +srv/GetWorkshopJobStatus.srv +srv/GetWorkshopReport.srv +srv/GetWorkshopSession.srv +srv/Heartbeat.srv +srv/PauseWorkshopSession.srv +srv/RebuildExecutionPlan.srv +srv/ResumeWorkshopSession.srv +srv/RollbackStageParameters.srv +srv/RunWorkshopPrecheck.srv +srv/StartWorkshopSession.srv +srv/StreamWorkshopEvent.srv +srv/ValidateExecutionPlan.srv \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/action/ExecuteWorkshopSession.action b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/action/ExecuteWorkshopSession.action new file mode 100644 index 0000000..dff3402 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/action/ExecuteWorkshopSession.action @@ -0,0 +1,22 @@ +# ========================================================= +# 整场车间会话执行 Action +# 作用：执行一场完整的自动化标定会话 +# 来源： +# 1) StartWorkshopSession +# 2) GetWorkshopJobStatus +# 3) CancelWorkshopSession +# 4) GetWorkshopReport +# 说明： +# 1) goal 对应 StartWorkshopSessionRequest +# 2) result 对应 WorkshopReportResponse +# 3) feedback 使用 WorkshopEvent，便于持续反馈阶段推进、人工确认、审批等待等事件 +# ========================================================= + +# Goal：启动整场自动化标定 +StartWorkshopSessionRequest goal +--- +# Result：整场会话完成后的最终报告 +WorkshopReportResponse result +--- +# Feedback：执行过程中的事件反馈 +WorkshopEvent feedback diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/action/RunWorkshopPrecheck.action b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/action/RunWorkshopPrecheck.action new file mode 100644 index 0000000..fc1062a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/action/RunWorkshopPrecheck.action @@ -0,0 +1,20 @@ +# ========================================================= +# 车间预检 Action +# 作用：执行一次完整的车间预检 +# 来源： +# 1) RunWorkshopPrecheck +# 2) GetLastWorkshopPrecheckResult +# 说明： +# 1) goal 对应 RunWorkshopPrecheckRequest +# 2) result 对应 WorkshopPrecheckResponse +# 3) feedback 统一复用 common 的 JobStatus +# ========================================================= + +# Goal：车间预检请求 +RunWorkshopPrecheckRequest goal +--- +# Result：预检最终结果 +WorkshopPrecheckResponse result +--- +# Feedback：预检过程中的状态反馈 +calibration_common_interfaces/JobStatus feedback diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ApprovalState.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ApprovalState.msg new file mode 100644 index 0000000..661f386 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ApprovalState.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 审批状态 +# 作用：描述某一阶段结果是否需要人工审核 +# ========================================================= + +uint8 APPROVAL_STATE_UNSPECIFIED=0 # 未指定 +uint8 APPROVAL_NOT_REQUIRED=1 # 不需要审批 +uint8 APPROVAL_PENDING=2 # 待审批 +uint8 APPROVED=3 # 已通过 +uint8 REJECTED=4 # 已拒绝 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ApproveStageResultRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ApproveStageResultRequest.msg new file mode 100644 index 0000000..81e0f24 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ApproveStageResultRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 阶段结果审批请求 +# 作用：对某阶段结果进行通过 / 驳回 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 阶段 ID +string stage_id +# 审批结论 +ApprovalState decision +# 审批人 ID +string reviewer_id +# 审批说明 +string comment diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/BuildExecutionPlanRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/BuildExecutionPlanRequest.msg new file mode 100644 index 0000000..928f27a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/BuildExecutionPlanRequest.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 生成执行计划请求 +# 作用：根据车辆画像与配置生成阶段计划 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 生成前是否先校验车辆画像 +bool validate_profile_before_build diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/BuildExecutionPlanResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/BuildExecutionPlanResponse.msg new file mode 100644 index 0000000..cd449fa --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/BuildExecutionPlanResponse.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 生成执行计划响应 +# 作用：返回生成后的阶段计划 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 阶段计划 +StagePlan[] stage_plan +# 警告信息 +string[] warnings diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CancelWorkshopSessionRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CancelWorkshopSessionRequest.msg new file mode 100644 index 0000000..835c167 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CancelWorkshopSessionRequest.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 取消会话请求 +# 作用：终止整场标定 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 取消原因 +string reason diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CreateWorkshopSessionRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CreateWorkshopSessionRequest.msg new file mode 100644 index 0000000..574c0f3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CreateWorkshopSessionRequest.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 创建会话请求 +# 作用：创建一场新的整车标定会话 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 车辆画像快照 +calibration_vehicle_profile_interfaces/VehicleProfile vehicle_profile_snapshot +# 操作员信息 +WorkshopOperatorInfo operator_info +# 会话配置 +WorkshopSessionConfig config +# 产线 / 工位线 ID +string workshop_line_id +# 创建后是否自动生成计划 +bool auto_build_execution_plan diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CreateWorkshopSessionResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CreateWorkshopSessionResponse.msg new file mode 100644 index 0000000..1aa9fbb --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/CreateWorkshopSessionResponse.msg @@ -0,0 +1,17 @@ +# ========================================================= +# 创建会话响应 +# 作用：返回新建会话结果 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 新建会话 ID +string session_id +# 初始状态 +WorkshopSessionState state +# 完整会话内容 +WorkshopSession session diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ExecutionPlanValidationIssue.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ExecutionPlanValidationIssue.msg new file mode 100644 index 0000000..431bed3 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ExecutionPlanValidationIssue.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 执行计划校验问题 +# 作用：描述当前阶段计划中的冲突、缺项或风险 +# ========================================================= + +# 是否为阻塞问题 +bool blocking +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 问题代码 +string issue_code +# 问题说明 +string message +# 关联阶段 ID +string related_stage_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ManualActionType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ManualActionType.msg new file mode 100644 index 0000000..391a3a0 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ManualActionType.msg @@ -0,0 +1,16 @@ +# ========================================================= +# 人工动作类型 +# 作用：表达需要人工介入的节点类型 +# ========================================================= + +uint8 MANUAL_ACTION_TYPE_UNSPECIFIED=0 # 未指定 +uint8 PLACE_TARGET_BOARD=1 # 放置标靶 / 标定板 +uint8 CONFIRM_WORKCELL_CLEAR=2 # 确认工位区域安全 +uint8 CONFIRM_VEHICLE_POSE=3 # 确认车辆姿态已就位 +uint8 CONFIRM_ARM_HOME=4 # 确认机械臂已回零 +uint8 CONFIRM_SENSOR_INSTALLATION=5 # 确认传感器安装状态 +uint8 CONFIRM_READY_TO_CONTINUE=6 # 确认可继续执行 +uint8 OTHER_MANUAL_ACTION=7 # 其他人工动作 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ManualStepAckRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ManualStepAckRequest.msg new file mode 100644 index 0000000..fb6dcb9 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ManualStepAckRequest.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 人工步骤确认请求 +# 作用：当系统停在人工步骤时，由操作员确认完成 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 阶段 ID +string stage_id +# 是否确认完成 +bool confirmed +# 备注 +string note diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/PauseWorkshopSessionRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/PauseWorkshopSessionRequest.msg new file mode 100644 index 0000000..6903c2e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/PauseWorkshopSessionRequest.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 暂停会话请求 +# 作用：暂停整场标定 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 暂停原因 +string reason diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/PrecheckItem.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/PrecheckItem.msg new file mode 100644 index 0000000..bda979c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/PrecheckItem.msg @@ -0,0 +1,21 @@ +# ========================================================= +# 单个预检项 +# 作用：表达一条 readiness / 安全 / 资源检查结果 +# ========================================================= + +# 检查项编码 +string item_code +# 检查项名称 +string display_name +# 是否通过 +bool passed +# 是否为阻塞项 +bool blocking +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 结果说明 +string message +# 关联阶段 +calibration_vehicle_profile_interfaces/WorkflowStageType related_stage_type +# 关联模块 +calibration_vehicle_profile_interfaces/CalibrationModuleType related_module_type diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RebuildExecutionPlanRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RebuildExecutionPlanRequest.msg new file mode 100644 index 0000000..970da91 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RebuildExecutionPlanRequest.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 重建执行计划请求 +# 作用：在修改配置 / 修正依赖关系后重建计划 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 重建原因 +string rebuild_reason +# 是否保留已完成阶段结果 +bool keep_finished_stage_results diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RequestedCalibrationTask.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RequestedCalibrationTask.msg new file mode 100644 index 0000000..c10cb98 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RequestedCalibrationTask.msg @@ -0,0 +1,19 @@ +# ========================================================= +# 单项标定任务配置 +# 作用：表达本次会话中某类流程阶段是否启用 +# ========================================================= + +# 阶段类型 +calibration_vehicle_profile_interfaces/WorkflowStageType stage_type +# 是否启用 +bool enabled +# 是否要求人工审批 +bool require_manual_approval +# 执行策略 +StageExecutionPolicy execution_policy +# 启用 / 禁用原因 +string reason + +string task_code +string target_id +calibration_common_interfaces/KeyValuePair[] task_params diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ResumeWorkshopSessionRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ResumeWorkshopSessionRequest.msg new file mode 100644 index 0000000..716dc73 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ResumeWorkshopSessionRequest.msg @@ -0,0 +1,11 @@ +# ========================================================= +# 恢复会话请求 +# 作用：从暂停态恢复整场标定 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 恢复原因 +string reason diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RollbackStageParametersRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RollbackStageParametersRequest.msg new file mode 100644 index 0000000..96c6b11 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RollbackStageParametersRequest.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 参数回滚请求 +# 作用：对某阶段参数回滚到指定版本 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id +# 阶段 ID +string stage_id +# 回滚目标版本 +string target_parameter_version +# 回滚原因 +string reason diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RunWorkshopPrecheckRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RunWorkshopPrecheckRequest.msg new file mode 100644 index 0000000..6e34fa7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/RunWorkshopPrecheckRequest.msg @@ -0,0 +1,9 @@ +# ========================================================= +# 车间预检请求 +# 作用：在正式启动前检查车辆、工位、服务、传感器、资源锁等是否就绪 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StageExecutionPolicy.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StageExecutionPolicy.msg new file mode 100644 index 0000000..4ddbddc --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StageExecutionPolicy.msg @@ -0,0 +1,12 @@ +# ========================================================= +# 阶段执行策略 +# 作用：控制某个阶段失败时系统该如何处理 +# ========================================================= + +uint8 STAGE_EXECUTION_POLICY_UNSPECIFIED=0 # 未指定 +uint8 REQUIRED=1 # 必须执行，失败即整场失败 +uint8 OPTIONAL=2 # 可选执行，失败后可继续 +uint8 SKIP_IF_UNSUPPORTED=3 # 若车辆不支持则自动跳过 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StagePlan.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StagePlan.msg new file mode 100644 index 0000000..9d1cb7e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StagePlan.msg @@ -0,0 +1,38 @@ +# ========================================================= +# 阶段计划 +# 作用：描述车间编排器生成的一步执行计划 +# ========================================================= + +# 阶段 ID +string stage_id +# 流程阶段类型 +calibration_vehicle_profile_interfaces/WorkflowStageType stage_type +# 所属模块类型 +calibration_vehicle_profile_interfaces/CalibrationModuleType module_type +# 阶段显示名 +string display_name +# 阶段顺序号 +uint32 order_index + +# 执行策略 +StageExecutionPolicy execution_policy +# 依赖的前置阶段 ID +string[] depends_on_stage_ids +# 开始前是否要求人工确认 +bool requires_manual_confirmation_before_start +# 参数提交前是否要求审批 +bool requires_approval_before_commit +# 若为人工阶段，对应的人工动作类型 +ManualActionType manual_action_type +# 实际执行的端点名（可对应 action / service） +string executor_endpoint_name +# 阶段说明 +string description +# 最大重试次数 +uint32 retry_limit +# 是否由系统自动生成 +bool auto_generated +# 阶段关联的资源 ID 列表 +string[] target_resource_ids +# 扩展元数据 +calibration_common_interfaces/KeyValuePair[] metadata diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StageResultSummary.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StageResultSummary.msg new file mode 100644 index 0000000..6cb6752 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StageResultSummary.msg @@ -0,0 +1,34 @@ +# ========================================================= +# 阶段结果摘要 +# 作用：用于最终报告中概述每个阶段的结果 +# ========================================================= + +# 阶段 ID +string stage_id +# 阶段类型 +calibration_vehicle_profile_interfaces/WorkflowStageType stage_type +# 模块类型 +calibration_vehicle_profile_interfaces/CalibrationModuleType module_type + +# 是否成功 +bool success +# 自动验收是否通过 +bool auto_acceptance_passed +# 最终任务状态 +calibration_common_interfaces/JobState final_job_state +# 审批状态 +ApprovalState approval_state +# 该阶段产出的参数版本 +string parameter_version + +# 关联产物 +calibration_common_interfaces/FileReference[] artifacts +# 摘要说明 +string summary + +# 是否发生回滚 +bool rolled_back +# 失败根因说明 +string failure_root_cause +# 阶段结果扩展信息 +calibration_common_interfaces/KeyValuePair[] metadata diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StartWorkshopSessionRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StartWorkshopSessionRequest.msg new file mode 100644 index 0000000..5d800fa --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/StartWorkshopSessionRequest.msg @@ -0,0 +1,9 @@ +# ========================================================= +# 启动会话请求 +# 作用：正式启动整场自动化标定 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ValidateExecutionPlanRequest.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ValidateExecutionPlanRequest.msg new file mode 100644 index 0000000..83a9fbd --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ValidateExecutionPlanRequest.msg @@ -0,0 +1,9 @@ +# ========================================================= +# 校验执行计划请求 +# 作用：检查当前阶段计划是否闭合、依赖是否正确、资源是否冲突 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ValidateExecutionPlanResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ValidateExecutionPlanResponse.msg new file mode 100644 index 0000000..2c95256 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/ValidateExecutionPlanResponse.msg @@ -0,0 +1,15 @@ +# ========================================================= +# 校验执行计划响应 +# 作用：返回当前执行计划的校验结果 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 计划是否有效 +bool valid +# 校验问题列表 +ExecutionPlanValidationIssue[] issues diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopEvent.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopEvent.msg new file mode 100644 index 0000000..2b49cd5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopEvent.msg @@ -0,0 +1,31 @@ +# ========================================================= +# 车间事件 +# 作用：向界面 / 上层系统异步推送状态变化 +# 说明：该消息既可用于事件 topic，也可用于 action feedback +# ========================================================= + +# 服务端时间戳 +int64 server_timestamp_us +# 会话 ID +string session_id +# 阶段 ID +string stage_id + +# 事件类型 +WorkshopEventType event_type +# 当前会话状态 +WorkshopSessionState session_state +# 当前阶段任务状态 +calibration_common_interfaces/JobState stage_job_state + +# 流程阶段类型 +calibration_vehicle_profile_interfaces/WorkflowStageType stage_type +# 模块类型 +calibration_vehicle_profile_interfaces/CalibrationModuleType module_type + +# 是否需要人工确认 +bool requires_manual_ack +# 审批状态 +ApprovalState approval_state +# 事件说明 +string message diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopEventType.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopEventType.msg new file mode 100644 index 0000000..b0f2db0 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopEventType.msg @@ -0,0 +1,21 @@ +# ========================================================= +# 车间事件类型 +# 作用：用于异步通知界面 / 上层系统 +# ========================================================= + +uint8 WORKSHOP_EVENT_TYPE_UNSPECIFIED=0 # 未指定 +uint8 SESSION_STATE_CHANGED=1 # 会话状态变化 +uint8 EXECUTION_PLAN_BUILT=2 # 执行计划已生成 +uint8 PRECHECK_STARTED=3 # 预检开始 +uint8 PRECHECK_COMPLETED=4 # 预检完成 +uint8 STAGE_STARTED=5 # 阶段开始 +uint8 STAGE_COMPLETED=6 # 阶段完成 +uint8 STAGE_FAILED=7 # 阶段失败 +uint8 MANUAL_ACTION_REQUIRED=8 # 需要人工操作 +uint8 APPROVAL_REQUIRED=9 # 需要审批 +uint8 SAFETY_TRIGGERED=10 # 安全保护触发 +uint8 ROLLBACK_EXECUTED=11 # 已执行回滚 +uint8 REPORT_READY=12 # 报告已生成 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopOperatorInfo.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopOperatorInfo.msg new file mode 100644 index 0000000..fafae0f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopOperatorInfo.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 操作员信息 +# 作用：记录本次会话的人工责任归属 +# ========================================================= + +# 操作员工号 +string operator_id +# 操作员姓名 +string operator_name +# 工位 ID +string workstation_id +# 班次 ID +string shift_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopPrecheckResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopPrecheckResponse.msg new file mode 100644 index 0000000..8683488 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopPrecheckResponse.msg @@ -0,0 +1,21 @@ +# ========================================================= +# 车间预检结果 +# 作用：返回最近一次预检详细结果 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 是否全部通过 +bool all_passed +# 检查项明细 +PrecheckItem[] items +# 检查时间 +int64 checked_timestamp_us +# 阻塞问题数量 +uint32 blocking_issue_count +# 是否可进入正式执行 +bool ready_for_start diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopReport.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopReport.msg new file mode 100644 index 0000000..7904b60 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopReport.msg @@ -0,0 +1,23 @@ +# ========================================================= +# 车间最终报告 +# 作用：归档整场会话的总体结果与所有阶段结果 +# ========================================================= + +# 会话 ID +string session_id +# 是否总体成功 +bool overall_success +# 开始时间 +int64 started_timestamp_us +# 结束时间 +int64 finished_timestamp_us +# 各阶段结果 +StageResultSummary[] stage_results +# 报告文件 +calibration_common_interfaces/FileReference[] report_files +# 总结说明 +string summary +# 最终生效参数包版本 +string active_parameter_bundle_version +# 报告扩展元数据 +calibration_common_interfaces/KeyValuePair[] metadata diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopReportResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopReportResponse.msg new file mode 100644 index 0000000..ab36e36 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopReportResponse.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 车间报告响应 +# 作用：返回最终归档报告 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 报告内容 +WorkshopReport report diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSession.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSession.msg new file mode 100644 index 0000000..96433a0 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSession.msg @@ -0,0 +1,32 @@ +# ========================================================= +# 车间会话 +# 作用：统一表达整场标定会话的核心状态 +# ========================================================= + +# 会话 ID +string session_id +# 会话状态 +WorkshopSessionState state + +# 车辆画像快照 +calibration_vehicle_profile_interfaces/VehicleProfile vehicle_profile_snapshot +# 操作员信息 +WorkshopOperatorInfo operator_info +# 会话配置 +WorkshopSessionConfig config +# 阶段计划 +StagePlan[] stage_plan + +# 创建时间 +int64 created_timestamp_us +# 最近更新时间 +int64 updated_timestamp_us +# 当前活跃阶段 ID +string active_stage_id + +# 所属工位 / 产线 ID +string workshop_line_id +# 当前整场执行的 job_id +string active_job_id +# 车辆画像摘要 +calibration_common_interfaces/FileDigest vehicle_profile_digest diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionConfig.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionConfig.msg new file mode 100644 index 0000000..048c4f7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionConfig.msg @@ -0,0 +1,25 @@ +# ========================================================= +# 会话配置 +# 作用：描述整场自动化标定运行策略 +# ========================================================= + +# 请求执行的阶段列表 +RequestedCalibrationTask[] requested_tasks +# 是否自动写入参数 +bool auto_commit_parameters +# 写入前是否要求审批 +bool require_manual_approval_before_commit +# 每阶段后是否自动复测 +bool run_validation_after_each_stage +# 是否首错即停 +bool stop_on_first_failure +# 是否允许可选阶段跳过 +bool allow_optional_stage_skip +# 验证失败是否自动回滚 +bool enable_auto_rollback_on_validation_failure +# 是否允许运行前重建计划 +bool allow_rebuild_execution_plan + +string localization_source_id +string workcell_zone_id +string reference_target_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionQuery.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionQuery.msg new file mode 100644 index 0000000..4b52c13 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionQuery.msg @@ -0,0 +1,9 @@ +# ========================================================= +# 会话查询请求 +# 作用：按 session_id 查询会话 +# ========================================================= + +# 请求头 +calibration_common_interfaces/RequestHeader header +# 会话 ID +string session_id diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionResponse.msg new file mode 100644 index 0000000..4636651 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionResponse.msg @@ -0,0 +1,13 @@ +# ========================================================= +# 会话查询响应 +# 作用：返回整场会话信息 +# ========================================================= + +# 是否成功 +bool success +# 错误码 +calibration_common_interfaces/ErrorCode error_code +# 说明 +string message +# 会话详情 +WorkshopSession session diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionState.msg b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionState.msg new file mode 100644 index 0000000..6c5eebd --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/msg/WorkshopSessionState.msg @@ -0,0 +1,21 @@ +# ========================================================= +# 会话状态 +# 作用：描述整场整车标定会话当前所处阶段 +# ========================================================= + +uint8 WORKSHOP_SESSION_STATE_UNSPECIFIED=0 # 未指定 +uint8 DRAFT=1 # 草稿，尚未规划 +uint8 PLANNING=2 # 正在规划 +uint8 READY=3 # 已规划完成，可启动 +uint8 PRECHECK_RUNNING=4 # 预检执行中 +uint8 RUNNING=5 # 运行中 +uint8 PAUSED=6 # 已暂停 +uint8 WAITING_MANUAL_ACTION=7 # 等待人工动作 +uint8 WAITING_APPROVAL=8 # 等待审批 +uint8 SUCCEEDED=9 # 整场成功 +uint8 FAILED=10 # 整场失败 +uint8 CANCELED=11 # 已取消 +uint8 ROLLED_BACK=12 # 已回滚 + +# 当前枚举取值 +uint8 value diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/package.xml new file mode 100644 index 0000000..1166301 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/package.xml @@ -0,0 +1,22 @@ + + + calibration_workshop_orchestration_interfaces + 0.0.1 + ROS 2 interfaces converted from workshop_orchestration.proto for the automated calibration workshop. + user + Apache-2.0 + + ament_cmake + rosidl_default_generators + + action_msgs + calibration_common_interfaces + calibration_vehicle_profile_interfaces + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/workshop_orchestration.proto b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/proto/workshop_orchestration.proto similarity index 56% rename from agv_calib_brain/src/win_ubuntu_bridge/proto/workshop_orchestration.proto rename to agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/proto/workshop_orchestration.proto index 02e44df..48e2d0a 100644 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/workshop_orchestration.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/proto/workshop_orchestration.proto @@ -11,12 +11,13 @@ import "vehicle_profile.proto"; // 1) 负责整车标定会话的创建、规划、执行、暂停、恢复、取消 // 2) 负责人工确认节点、审批节点、回滚、报告归档 // 3) 负责把外部定位 / 底盘 / 运控 / 传感器标定串成完整闭环 +// 4) 负责根据车辆画像与阶段配置自动生成执行计划 // ========================================================= // ========================================================= // 服务：车间总编排服务 // 运行位置：Ubuntu 车间电脑 -// 调用方：上位机界面、自动化车间入口程序 +// 调用方：上位机界面、自动化车间入口程序、调度系统 // ========================================================= service WorkshopOrchestrationService { // 心跳保活 @@ -35,6 +36,14 @@ service WorkshopOrchestrationService { rpc BuildExecutionPlan(BuildExecutionPlanRequest) returns (BuildExecutionPlanResponse); + // 重建执行计划 + rpc RebuildExecutionPlan(RebuildExecutionPlanRequest) + returns (BuildExecutionPlanResponse); + + // 校验当前执行计划 + rpc ValidateExecutionPlan(ValidateExecutionPlanRequest) + returns (ValidateExecutionPlanResponse); + // 执行车间预检 / readiness 检查 rpc RunWorkshopPrecheck(RunWorkshopPrecheckRequest) returns (.agv.calibration.common.JobAccepted); @@ -93,29 +102,15 @@ enum WorkshopSessionState { DRAFT = 1; // 草稿，尚未规划 PLANNING = 2; // 正在规划 READY = 3; // 已规划完成，可启动 - RUNNING = 4; // 运行中 - PAUSED = 5; // 已暂停 - WAITING_MANUAL_ACTION = 6; // 等待人工动作 - WAITING_APPROVAL = 7; // 等待审批 - SUCCEEDED = 8; // 整场成功 - FAILED = 9; // 整场失败 - CANCELED = 10; // 已取消 - ROLLED_BACK = 11; // 已回滚 -} - -// ========================================================= -// 车间阶段类型 -// 作用：描述执行计划中的阶段类别 -// ========================================================= -enum WorkshopStageType { - WORKSHOP_STAGE_TYPE_UNSPECIFIED = 0; // 未指定 - STAGE_EXTERNAL_LOCALIZATION = 1; // 外部定位 / 基准建立阶段 - STAGE_CHASSIS_CALIBRATION = 2; // 底盘标定阶段 - STAGE_CONTROL_CALIBRATION = 3; // 运控参数标定阶段 - STAGE_SENSOR_CALIBRATION = 4; // 传感器标定阶段 - STAGE_VALIDATION = 5; // 验证复测阶段 - STAGE_MANUAL_CHECK = 6; // 人工检查 / 人工放置 / 人工确认阶段 - STAGE_REPORT_FINALIZATION = 7; // 报告归档阶段 + PRECHECK_RUNNING = 4; // 预检执行中 + RUNNING = 5; // 运行中 + PAUSED = 6; // 已暂停 + WAITING_MANUAL_ACTION = 7; // 等待人工动作 + WAITING_APPROVAL = 8; // 等待审批 + SUCCEEDED = 9; // 整场成功 + FAILED = 10; // 整场失败 + CANCELED = 11; // 已取消 + ROLLED_BACK = 12; // 已回滚 } // ========================================================= @@ -163,13 +158,17 @@ enum ManualActionType { enum WorkshopEventType { WORKSHOP_EVENT_TYPE_UNSPECIFIED = 0; // 未指定 SESSION_STATE_CHANGED = 1; // 会话状态变化 - STAGE_STARTED = 2; // 阶段开始 - STAGE_COMPLETED = 3; // 阶段完成 - STAGE_FAILED = 4; // 阶段失败 - MANUAL_ACTION_REQUIRED = 5; // 需要人工操作 - APPROVAL_REQUIRED = 6; // 需要审批 - SAFETY_TRIGGERED = 7; // 安全保护触发 - REPORT_READY = 8; // 报告已生成 + EXECUTION_PLAN_BUILT = 2; // 执行计划已生成 + PRECHECK_STARTED = 3; // 预检开始 + PRECHECK_COMPLETED = 4; // 预检完成 + STAGE_STARTED = 5; // 阶段开始 + STAGE_COMPLETED = 6; // 阶段完成 + STAGE_FAILED = 7; // 阶段失败 + MANUAL_ACTION_REQUIRED = 8; // 需要人工操作 + APPROVAL_REQUIRED = 9; // 需要审批 + SAFETY_TRIGGERED = 10; // 安全保护触发 + ROLLBACK_EXECUTED = 11; // 已执行回滚 + REPORT_READY = 12; // 报告已生成 } // ========================================================= @@ -177,21 +176,22 @@ enum WorkshopEventType { // 作用：记录本次会话的人工责任归属 // ========================================================= message WorkshopOperatorInfo { - string operator_id = 1; // 操作员工号 - string operator_name = 2; // 操作员姓名 - string workstation_id = 3;// 工位 ID - string shift_id = 4; // 班次 ID + string operator_id = 1; // 操作员工号 + string operator_name = 2; // 操作员姓名 + string workstation_id = 3; // 工位 ID + string shift_id = 4; // 班次 ID } // ========================================================= // 单项标定任务配置 -// 作用：表达本次会话中某类专项是否启用 +// 作用：表达本次会话中某类流程阶段是否启用 // ========================================================= message RequestedCalibrationTask { - WorkshopStageType stage_type = 1; // 阶段类型 - bool enabled = 2; // 是否启用 - bool require_manual_approval = 3; // 是否要求人工审批 - string reason = 4; // 启用 / 禁用原因 + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 1; // 阶段类型 + bool enabled = 2; // 是否启用 + bool require_manual_approval = 3; // 是否要求人工审批 + StageExecutionPolicy execution_policy = 4; // 执行策略 + string reason = 5; // 启用 / 禁用原因 } // ========================================================= @@ -199,13 +199,14 @@ message RequestedCalibrationTask { // 作用：描述整场自动化标定运行策略 // ========================================================= message WorkshopSessionConfig { - repeated RequestedCalibrationTask requested_tasks = 1; // 请求执行的专项列表 + repeated RequestedCalibrationTask requested_tasks = 1; // 请求执行的阶段列表 bool auto_commit_parameters = 2; // 是否自动写入参数 bool require_manual_approval_before_commit = 3; // 写入前是否要求审批 bool run_validation_after_each_stage = 4; // 每阶段后是否自动复测 bool stop_on_first_failure = 5; // 是否首错即停 bool allow_optional_stage_skip = 6; // 是否允许可选阶段跳过 bool enable_auto_rollback_on_validation_failure = 7; // 验证失败是否自动回滚 + bool allow_rebuild_execution_plan = 8; // 是否允许运行前重建计划 } // ========================================================= @@ -213,18 +214,35 @@ message WorkshopSessionConfig { // 作用：描述车间编排器生成的一步执行计划 // ========================================================= message StagePlan { - string stage_id = 1; // 阶段 ID - WorkshopStageType stage_type = 2; // 阶段类型 - string display_name = 3; // 阶段显示名 - uint32 order_index = 4; // 阶段顺序号 - StageExecutionPolicy execution_policy = 5; // 执行策略 - repeated string depends_on_stage_ids = 6; // 依赖的前置阶段 ID - bool requires_manual_confirmation_before_start = 7; // 开始前是否要求人工确认 - bool requires_approval_before_commit = 8; // 参数提交前是否要求审批 - ManualActionType manual_action_type = 9; // 若为人工阶段，对应的人工动作类型 - string executor_service_name = 10; // 实际执行的服务名 - string description = 11; // 阶段说明 - uint32 retry_limit = 12; // 最大重试次数 + string stage_id = 1; // 阶段 ID + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 2; // 流程阶段类型 + .agv.calibration.vehicle.profile.CalibrationModuleType module_type = 3; // 所属模块类型 + string display_name = 4; // 阶段显示名 + uint32 order_index = 5; // 阶段顺序号 + + StageExecutionPolicy execution_policy = 6; // 执行策略 + repeated string depends_on_stage_ids = 7; // 依赖的前置阶段 ID + bool requires_manual_confirmation_before_start = 8; // 开始前是否要求人工确认 + bool requires_approval_before_commit = 9; // 参数提交前是否要求审批 + ManualActionType manual_action_type = 10; // 若为人工阶段，对应的人工动作类型 + string executor_endpoint_name = 11; // 实际执行的端点名（可对应 action / service） + string description = 12; // 阶段说明 + uint32 retry_limit = 13; // 最大重试次数 + bool auto_generated = 14; // 是否由系统自动生成 + repeated string target_resource_ids = 15; // 阶段关联的资源 ID 列表 + repeated .agv.calibration.common.KeyValuePair metadata = 16; // 扩展元数据 +} + +// ========================================================= +// 执行计划校验问题 +// 作用：描述当前阶段计划中的冲突、缺项或风险 +// ========================================================= +message ExecutionPlanValidationIssue { + bool blocking = 1; // 是否为阻塞问题 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string issue_code = 3; // 问题代码 + string message = 4; // 问题说明 + string related_stage_id = 5; // 关联阶段 ID } // ========================================================= @@ -232,15 +250,21 @@ message StagePlan { // 作用：统一表达整场标定会话的核心状态 // ========================================================= message WorkshopSession { - string session_id = 1; // 会话 ID - WorkshopSessionState state = 2; // 会话状态 + string session_id = 1; // 会话 ID + WorkshopSessionState state = 2; // 会话状态 + .agv.calibration.vehicle.profile.VehicleProfile vehicle_profile_snapshot = 3; // 车辆画像快照 - WorkshopOperatorInfo operator_info = 4; // 操作员信息 - WorkshopSessionConfig config = 5; // 会话配置 - repeated StagePlan stage_plan = 6; // 阶段计划 - int64 created_timestamp_us = 7; // 创建时间 - int64 updated_timestamp_us = 8; // 最近更新时间 - string active_stage_id = 9; // 当前活跃阶段 ID + WorkshopOperatorInfo operator_info = 4; // 操作员信息 + WorkshopSessionConfig config = 5; // 会话配置 + repeated StagePlan stage_plan = 6; // 阶段计划 + + int64 created_timestamp_us = 7; // 创建时间 + int64 updated_timestamp_us = 8; // 最近更新时间 + string active_stage_id = 9; // 当前活跃阶段 ID + + string workshop_line_id = 10; // 所属工位 / 产线 ID + string active_job_id = 11; // 当前整场执行的 job_id + .agv.calibration.common.FileDigest vehicle_profile_digest = 12; // 车辆画像摘要 } // ========================================================= @@ -250,9 +274,10 @@ message WorkshopSession { message CreateWorkshopSessionRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 .agv.calibration.vehicle.profile.VehicleProfile vehicle_profile_snapshot = 2; // 车辆画像快照 - WorkshopOperatorInfo operator_info = 3; // 操作员信息 - WorkshopSessionConfig config = 4; // 会话配置 - string workshop_line_id = 5; // 产线 / 工位线 ID + WorkshopOperatorInfo operator_info = 3; // 操作员信息 + WorkshopSessionConfig config = 4; // 会话配置 + string workshop_line_id = 5; // 产线 / 工位线 ID + bool auto_build_execution_plan = 6; // 创建后是否自动生成计划 } // ========================================================= @@ -260,11 +285,12 @@ message CreateWorkshopSessionRequest { // 作用：返回新建会话结果 // ========================================================= message CreateWorkshopSessionResponse { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - string session_id = 4; // 新建会话 ID - WorkshopSessionState state = 5; // 初始状态 + string message = 3; // 说明 + string session_id = 4; // 新建会话 ID + WorkshopSessionState state = 5; // 初始状态 + WorkshopSession session = 6; // 完整会话内容 } // ========================================================= @@ -273,7 +299,7 @@ message CreateWorkshopSessionResponse { // ========================================================= message WorkshopSessionQuery { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID + string session_id = 2; // 会话 ID } // ========================================================= @@ -281,10 +307,10 @@ message WorkshopSessionQuery { // 作用：返回整场会话信息 // ========================================================= message WorkshopSessionResponse { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - WorkshopSession session = 4; // 会话详情 + string message = 3; // 说明 + WorkshopSession session = 4; // 会话详情 } // ========================================================= @@ -293,7 +319,19 @@ message WorkshopSessionResponse { // ========================================================= message BuildExecutionPlanRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID + string session_id = 2; // 会话 ID + bool validate_profile_before_build = 3; // 生成前是否先校验车辆画像 +} + +// ========================================================= +// 重建执行计划请求 +// 作用：在修改配置 / 修正依赖关系后重建计划 +// ========================================================= +message RebuildExecutionPlanRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string rebuild_reason = 3; // 重建原因 + bool keep_finished_stage_results = 4; // 是否保留已完成阶段结果 } // ========================================================= @@ -301,11 +339,32 @@ message BuildExecutionPlanRequest { // 作用：返回生成后的阶段计划 // ========================================================= message BuildExecutionPlanResponse { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - repeated StagePlan stage_plan = 4; // 阶段计划 - repeated string warnings = 5; // 警告信息 + string message = 3; // 说明 + repeated StagePlan stage_plan = 4; // 阶段计划 + repeated string warnings = 5; // 警告信息 +} + +// ========================================================= +// 校验执行计划请求 +// 作用：检查当前阶段计划是否闭合、依赖是否正确、资源是否冲突 +// ========================================================= +message ValidateExecutionPlanRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID +} + +// ========================================================= +// 校验执行计划响应 +// 作用：返回当前执行计划的校验结果 +// ========================================================= +message ValidateExecutionPlanResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + bool valid = 4; // 计划是否有效 + repeated ExecutionPlanValidationIssue issues = 5; // 校验问题列表 } // ========================================================= @@ -314,7 +373,7 @@ message BuildExecutionPlanResponse { // ========================================================= message RunWorkshopPrecheckRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID + string session_id = 2; // 会话 ID } // ========================================================= @@ -322,12 +381,14 @@ message RunWorkshopPrecheckRequest { // 作用：表达一条 readiness / 安全 / 资源检查结果 // ========================================================= message PrecheckItem { - string item_code = 1; // 检查项编码 - string display_name = 2; // 检查项名称 - bool passed = 3; // 是否通过 - bool blocking = 4; // 是否为阻塞项 + string item_code = 1; // 检查项编码 + string display_name = 2; // 检查项名称 + bool passed = 3; // 是否通过 + bool blocking = 4; // 是否为阻塞项 .agv.calibration.common.ErrorCode error_code = 5; // 错误码 - string message = 6; // 结果说明 + string message = 6; // 结果说明 + .agv.calibration.vehicle.profile.WorkflowStageType related_stage_type = 7; // 关联阶段 + .agv.calibration.vehicle.profile.CalibrationModuleType related_module_type = 8; // 关联模块 } // ========================================================= @@ -335,12 +396,14 @@ message PrecheckItem { // 作用：返回最近一次预检详细结果 // ========================================================= message WorkshopPrecheckResponse { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - bool all_passed = 4; // 是否全部通过 - repeated PrecheckItem items = 5; // 检查项明细 - int64 checked_timestamp_us = 6; // 检查时间 + string message = 3; // 说明 + bool all_passed = 4; // 是否全部通过 + repeated PrecheckItem items = 5; // 检查项明细 + int64 checked_timestamp_us = 6; // 检查时间 + uint32 blocking_issue_count = 7; // 阻塞问题数量 + bool ready_for_start = 8; // 是否可进入正式执行 } // ========================================================= @@ -349,7 +412,7 @@ message WorkshopPrecheckResponse { // ========================================================= message StartWorkshopSessionRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID + string session_id = 2; // 会话 ID } // ========================================================= @@ -358,8 +421,8 @@ message StartWorkshopSessionRequest { // ========================================================= message PauseWorkshopSessionRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID - string reason = 3; // 暂停原因 + string session_id = 2; // 会话 ID + string reason = 3; // 暂停原因 } // ========================================================= @@ -368,8 +431,8 @@ message PauseWorkshopSessionRequest { // ========================================================= message ResumeWorkshopSessionRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID - string reason = 3; // 恢复原因 + string session_id = 2; // 会话 ID + string reason = 3; // 恢复原因 } // ========================================================= @@ -378,8 +441,8 @@ message ResumeWorkshopSessionRequest { // ========================================================= message CancelWorkshopSessionRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID - string reason = 3; // 取消原因 + string session_id = 2; // 会话 ID + string reason = 3; // 取消原因 } // ========================================================= @@ -387,14 +450,20 @@ message CancelWorkshopSessionRequest { // 作用：向界面 / 上层系统异步推送状态变化 // ========================================================= message WorkshopEvent { - int64 server_timestamp_us = 1; // 服务端时间戳 - string session_id = 2; // 会话 ID - string stage_id = 3; // 阶段 ID - WorkshopEventType event_type = 4; // 事件类型 - WorkshopSessionState session_state = 5; // 当前会话状态 + int64 server_timestamp_us = 1; // 服务端时间戳 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + + WorkshopEventType event_type = 4; // 事件类型 + WorkshopSessionState session_state = 5; // 当前会话状态 .agv.calibration.common.JobState stage_job_state = 6; // 当前阶段任务状态 - bool requires_manual_ack = 7; // 是否需要人工确认 - string message = 8; // 事件说明 + + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 7; // 流程阶段类型 + .agv.calibration.vehicle.profile.CalibrationModuleType module_type = 8; // 模块类型 + + bool requires_manual_ack = 9; // 是否需要人工确认 + ApprovalState approval_state = 10; // 审批状态 + string message = 11; // 事件说明 } // ========================================================= @@ -403,10 +472,10 @@ message WorkshopEvent { // ========================================================= message ManualStepAckRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID - string stage_id = 3; // 阶段 ID - bool confirmed = 4; // 是否确认完成 - string note = 5; // 备注 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + bool confirmed = 4; // 是否确认完成 + string note = 5; // 备注 } // ========================================================= @@ -415,11 +484,11 @@ message ManualStepAckRequest { // ========================================================= message ApproveStageResultRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID - string stage_id = 3; // 阶段 ID - ApprovalState decision = 4; // 审批结论 - string reviewer_id = 5; // 审批人 ID - string comment = 6; // 审批说明 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + ApprovalState decision = 4; // 审批结论 + string reviewer_id = 5; // 审批人 ID + string comment = 6; // 审批说明 } // ========================================================= @@ -428,10 +497,10 @@ message ApproveStageResultRequest { // ========================================================= message RollbackStageParametersRequest { .agv.calibration.common.RequestHeader header = 1; // 请求头 - string session_id = 2; // 会话 ID - string stage_id = 3; // 阶段 ID - string target_parameter_version = 4; // 回滚目标版本 - string reason = 5; // 回滚原因 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + string target_parameter_version = 4; // 回滚目标版本 + string reason = 5; // 回滚原因 } // ========================================================= @@ -439,14 +508,22 @@ message RollbackStageParametersRequest { // 作用：用于最终报告中概述每个阶段的结果 // ========================================================= message StageResultSummary { - string stage_id = 1; // 阶段 ID - WorkshopStageType stage_type = 2; // 阶段类型 - bool success = 3; // 是否成功 - .agv.calibration.common.JobState final_job_state = 4; // 最终任务状态 - ApprovalState approval_state = 5; // 审批状态 - string parameter_version = 6; // 该阶段产出的参数版本 - repeated .agv.calibration.common.FileReference artifacts = 7; // 关联产物 - string summary = 8; // 摘要说明 + string stage_id = 1; // 阶段 ID + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 2; // 阶段类型 + .agv.calibration.vehicle.profile.CalibrationModuleType module_type = 3; // 模块类型 + + bool success = 4; // 是否成功 + bool auto_acceptance_passed = 5; // 自动验收是否通过 + .agv.calibration.common.JobState final_job_state = 6; // 最终任务状态 + ApprovalState approval_state = 7; // 审批状态 + string parameter_version = 8; // 该阶段产出的参数版本 + + repeated .agv.calibration.common.FileReference artifacts = 9; // 关联产物 + string summary = 10; // 摘要说明 + + bool rolled_back = 11; // 是否发生回滚 + string failure_root_cause = 12; // 失败根因说明 + repeated .agv.calibration.common.KeyValuePair metadata = 13; // 阶段结果扩展信息 } // ========================================================= @@ -454,13 +531,15 @@ message StageResultSummary { // 作用：归档整场会话的总体结果与所有阶段结果 // ========================================================= message WorkshopReport { - string session_id = 1; // 会话 ID - bool overall_success = 2; // 是否总体成功 - int64 started_timestamp_us = 3; // 开始时间 - int64 finished_timestamp_us = 4; // 结束时间 - repeated StageResultSummary stage_results = 5; // 各阶段结果 + string session_id = 1; // 会话 ID + bool overall_success = 2; // 是否总体成功 + int64 started_timestamp_us = 3; // 开始时间 + int64 finished_timestamp_us = 4; // 结束时间 + repeated StageResultSummary stage_results = 5; // 各阶段结果 repeated .agv.calibration.common.FileReference report_files = 6; // 报告文件 - string summary = 7; // 总结说明 + string summary = 7; // 总结说明 + string active_parameter_bundle_version = 8; // 最终生效参数包版本 + repeated .agv.calibration.common.KeyValuePair metadata = 9; // 报告扩展元数据 } // ========================================================= @@ -468,8 +547,8 @@ message WorkshopReport { // 作用：返回最终归档报告 // ========================================================= message WorkshopReportResponse { - bool success = 1; // 是否成功 + bool success = 1; // 是否成功 .agv.calibration.common.ErrorCode error_code = 2; // 错误码 - string message = 3; // 说明 - WorkshopReport report = 4; // 报告内容 + string message = 3; // 说明 + WorkshopReport report = 4; // 报告内容 } \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/AcknowledgeManualStep.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/AcknowledgeManualStep.srv new file mode 100644 index 0000000..a9b93a1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/AcknowledgeManualStep.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 人工确认某个等待中的手动步骤 +# ========================================================= + +ManualStepAckRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ApproveStageResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ApproveStageResult.srv new file mode 100644 index 0000000..ca33902 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ApproveStageResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 审批某个阶段的结果 +# ========================================================= + +ApproveStageResultRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/BuildExecutionPlan.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/BuildExecutionPlan.srv new file mode 100644 index 0000000..62b6a17 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/BuildExecutionPlan.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 生成本次会话的执行计划 +# ========================================================= + +BuildExecutionPlanRequest request +--- +BuildExecutionPlanResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/CancelWorkshopSession.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/CancelWorkshopSession.srv new file mode 100644 index 0000000..3790140 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/CancelWorkshopSession.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 取消整场会话 +# ========================================================= + +CancelWorkshopSessionRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/CreateWorkshopSession.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/CreateWorkshopSession.srv new file mode 100644 index 0000000..051081a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/CreateWorkshopSession.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 创建一场新的整车标定会话 +# ========================================================= + +CreateWorkshopSessionRequest request +--- +CreateWorkshopSessionResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetLastWorkshopPrecheckResult.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetLastWorkshopPrecheckResult.srv new file mode 100644 index 0000000..1782d6a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetLastWorkshopPrecheckResult.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询最近一次预检结果 +# ========================================================= + +WorkshopSessionQuery request +--- +WorkshopPrecheckResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopJobStatus.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopJobStatus.srv new file mode 100644 index 0000000..a394b44 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopJobStatus.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询整场会话对应长任务状态 +# ========================================================= + +calibration_common_interfaces/JobQuery request +--- +calibration_common_interfaces/JobStatus response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopReport.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopReport.srv new file mode 100644 index 0000000..0668fd5 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopReport.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 获取最终车间报告 +# ========================================================= + +WorkshopSessionQuery request +--- +WorkshopReportResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopSession.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopSession.srv new file mode 100644 index 0000000..df9b857 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/GetWorkshopSession.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 查询会话信息 +# ========================================================= + +WorkshopSessionQuery request +--- +WorkshopSessionResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/Heartbeat.srv new file mode 100644 index 0000000..3500da2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/Heartbeat.srv @@ -0,0 +1,9 @@ +# ========================================================= +# 心跳保活 +# 运行位置：Ubuntu 车间电脑 +# 调用方：上位机界面、自动化车间入口程序、调度系统 +# ========================================================= + +calibration_common_interfaces/HeartbeatRequest request +--- +calibration_common_interfaces/HeartbeatResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/PauseWorkshopSession.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/PauseWorkshopSession.srv new file mode 100644 index 0000000..1068da4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/PauseWorkshopSession.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 暂停整场会话 +# ========================================================= + +PauseWorkshopSessionRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RebuildExecutionPlan.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RebuildExecutionPlan.srv new file mode 100644 index 0000000..1ed7b6c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RebuildExecutionPlan.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 重建执行计划 +# ========================================================= + +RebuildExecutionPlanRequest request +--- +BuildExecutionPlanResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ResumeWorkshopSession.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ResumeWorkshopSession.srv new file mode 100644 index 0000000..126957c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ResumeWorkshopSession.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 恢复整场会话 +# ========================================================= + +ResumeWorkshopSessionRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RollbackStageParameters.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RollbackStageParameters.srv new file mode 100644 index 0000000..3cd7f8b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RollbackStageParameters.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 对某阶段参数进行回滚 +# ========================================================= + +RollbackStageParametersRequest request +--- +calibration_common_interfaces/StandardResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RunWorkshopPrecheck.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RunWorkshopPrecheck.srv new file mode 100644 index 0000000..bf36407 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/RunWorkshopPrecheck.srv @@ -0,0 +1,10 @@ +# ========================================================= +# 执行车间预检 / readiness 检查 +# 说明： +# 1) 这是对 proto 原始 RPC 的 1:1 保真映射 +# 2) 真正的 ROS2 长任务主通路建议优先使用 action/RunWorkshopPrecheck.action +# ========================================================= + +RunWorkshopPrecheckRequest request +--- +calibration_common_interfaces/JobAccepted response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/StartWorkshopSession.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/StartWorkshopSession.srv new file mode 100644 index 0000000..44e879b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/StartWorkshopSession.srv @@ -0,0 +1,10 @@ +# ========================================================= +# 启动整场自动化标定 +# 说明： +# 1) 这是对 proto 原始 RPC 的 1:1 保真映射 +# 2) 真正的 ROS2 长任务主通路建议优先使用 action/ExecuteWorkshopSession.action +# ========================================================= + +StartWorkshopSessionRequest request +--- +calibration_common_interfaces/JobAccepted response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/StreamWorkshopEvent.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/StreamWorkshopEvent.srv new file mode 100644 index 0000000..a8ae0ba --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/StreamWorkshopEvent.srv @@ -0,0 +1,12 @@ +# ========================================================= +# 打开会话事件流 +# 说明： +# 1) proto 原始语义是 server streaming +# 2) ROS2 中真正的持续事件流请使用 WorkshopEvent topic +# 3) 该 srv 负责声明 / 打开 / 订阅某个会话的事件流 +# ========================================================= + +WorkshopSessionQuery request +--- +calibration_common_interfaces/StandardResponse response +string topic_name diff --git a/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ValidateExecutionPlan.srv b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ValidateExecutionPlan.srv new file mode 100644 index 0000000..bb60aa4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/calibration_workshop_orchestration_interfaces/srv/ValidateExecutionPlan.srv @@ -0,0 +1,7 @@ +# ========================================================= +# 校验当前执行计划 +# ========================================================= + +ValidateExecutionPlanRequest request +--- +ValidateExecutionPlanResponse response diff --git a/agv_calib_brain/src/win_ubuntu_bridge/gateways/chassis_gateway_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/gateways/chassis_gateway_node.cpp deleted file mode 100644 index 74e9bae..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/gateways/chassis_gateway_node.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include -#include - -// 🚨 引入自定义的 ROS 2 接口头文件 (由 win_ubuntu_bridge 包生成) -#include "win_ubuntu_bridge/msg/hardware_state.hpp" -#include "win_ubuntu_bridge/srv/set_diagnostic_mode.hpp" -#include "win_ubuntu_bridge/srv/execute_raw_drive.hpp" -#include "win_ubuntu_bridge/srv/execute_raw_steer.hpp" -#include "win_ubuntu_bridge/srv/commit_kinematics.hpp" -#include // 紧急刹车可以直接用标准库的 Trigger - -// 引入 gRPC 自动生成的契约头文件 -#include -#include "agv_calib_chassis.grpc.pb.h" - -#include -#include - -using namespace agv::calibration::chassis; - -namespace win_ubuntu_bridge { - -class ChassisGatewayNode : public rclcpp::Node { -public: - explicit ChassisGatewayNode(const rclcpp::NodeOptions & options) - : Node("chassis_gateway", options), stream_running_(false) { - - RCLCPP_INFO(this->get_logger(), "🔌 底盘 gRPC 网关插件启动中... 正在挂载 6 大完整接口！"); - - // 1. 拨号连接 Windows 车端 - std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051"); - channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials()); - stub_ = AgvCalibChassisService::NewStub(channel_); - - // ========================================================== - // 2. 注册 5 个 ROS 2 服务 (拦截内部行为树的请求，翻译给车端) - // ========================================================== - - // 接口 1：夺权 - srv_set_mode_ = this->create_service( - "~/set_diagnostic_mode", std::bind(&ChassisGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2)); - - // 接口 2：最高优急停 (无参数，用 Trigger) - srv_estop_ = this->create_service( - "~/hardware_emergency_brake", std::bind(&ChassisGatewayNode::cb_emergency_brake, this, std::placeholders::_1, std::placeholders::_2)); - - // 接口 3：开环直行 (测轮径) - srv_raw_drive_ = this->create_service( - "~/execute_raw_drive", std::bind(&ChassisGatewayNode::cb_raw_drive, this, std::placeholders::_1, std::placeholders::_2)); - - // 接口 4：开环转向 (测死区/零位) - srv_raw_steer_ = this->create_service( - "~/execute_raw_steer", std::bind(&ChassisGatewayNode::cb_raw_steer, this, std::placeholders::_1, std::placeholders::_2)); - - // 接口 5：物理参数定稿固化 - srv_commit_kinematics_ = this->create_service( - "~/commit_kinematics", std::bind(&ChassisGatewayNode::cb_commit_kinematics, this, std::placeholders::_1, std::placeholders::_2)); - - // ========================================================== - // 3. 注册 1 个 ROS 2 话题 (发布从车端抽上来的 50Hz 流数据) - // ========================================================== - pub_telemetry_ = this->create_publisher("~/hardware_telemetry", 50); - - // 4. 启动异步抽水机线程 - stream_running_ = true; - stream_thread_ = std::thread(&ChassisGatewayNode::grpc_stream_to_ros2_topic, this); - } - - ~ChassisGatewayNode() { - stream_running_ = false; - if (stream_context_) stream_context_->TryCancel(); // 唤醒阻塞的 gRPC - if (stream_thread_.joinable()) stream_thread_.join(); - } - -private: - std::shared_ptr channel_; - std::unique_ptr stub_; - std::unique_ptr stream_context_; - - // ROS 2 接口句柄 - rclcpp::Service::SharedPtr srv_set_mode_; - rclcpp::Service::SharedPtr srv_estop_; - rclcpp::Service::SharedPtr srv_raw_drive_; - rclcpp::Service::SharedPtr srv_raw_steer_; - rclcpp::Service::SharedPtr srv_commit_kinematics_; - rclcpp::Publisher::SharedPtr pub_telemetry_; - - std::thread stream_thread_; - std::atomic stream_running_; - - // ---------------------------------------------------------------------- - // ⬇️ 以下全是完美的 1:1 翻译逻辑 (ROS Request -> gRPC -> ROS Response) ⬇️ - // ---------------------------------------------------------------------- - - // [翻译] 接口 1: SetDiagnosticMode - void cb_set_mode(const std::shared_ptr req, - std::shared_ptr res) { - DiagnosticModeRequest grpc_req; - grpc_req.set_target_mode(req->target_mode == 1 ? DiagnosticModeRequest::DIRECT_RAW_DRIVE : DiagnosticModeRequest::NORMAL_KINEMATICS); - StandardResponse grpc_reply; - grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2)); - grpc::Status status = stub_->SetDiagnosticMode(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); - res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // [翻译] 接口 2: HardwareEmergencyBrake - void cb_emergency_brake(const std::shared_ptr req, - std::shared_ptr res) { - (void)req; // 忽略未使用变量 - Empty grpc_req; StandardResponse grpc_reply; - grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(1)); // 急停超时设为1秒 - grpc::Status status = stub_->HardwareEmergencyBrake(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); - res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // [翻译] 接口 3: ExecuteRawDriveCommand - void cb_raw_drive(const std::shared_ptr req, - std::shared_ptr res) { - RawDriveRequest grpc_req; - grpc_req.set_test_case_id(req->test_case_id); - grpc_req.set_fl_motor_rpm(req->fl_motor_rpm); - grpc_req.set_fr_motor_rpm(req->fr_motor_rpm); - grpc_req.set_rl_motor_rpm(req->rl_motor_rpm); - grpc_req.set_rr_motor_rpm(req->rr_motor_rpm); - grpc_req.set_duration_sec(req->duration_sec); - - StandardResponse grpc_reply; - grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2)); - grpc::Status status = stub_->ExecuteRawDriveCommand(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); - res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // [翻译] 接口 4: ExecuteRawSteerCommand - void cb_raw_steer(const std::shared_ptr req, - std::shared_ptr res) { - RawSteerRequest grpc_req; - grpc_req.set_test_case_id(req->test_case_id); - grpc_req.set_front_steer_angle_deg(req->front_steer_angle_deg); - grpc_req.set_rear_steer_angle_deg(req->rear_steer_angle_deg); - grpc_req.set_duration_sec(req->duration_sec); - - // 处理 proto 的 optional 字段 (依靠 ROS 2 传来的 bool 判断) - if (req->has_sweep) { - grpc_req.set_sweep_amplitude_deg(req->sweep_amplitude_deg); - grpc_req.set_sweep_frequency_hz(req->sweep_frequency_hz); - } - - StandardResponse grpc_reply; - grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2)); - grpc::Status status = stub_->ExecuteRawSteerCommand(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); - res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // [翻译] 接口 5: CommitKinematicParameters - void cb_commit_kinematics(const std::shared_ptr req, - std::shared_ptr res) { - KinematicParams grpc_req; - // ROS 2 请求中依靠 bool 标志位来判断是否 set 进 grpc (实现真正的 optional) - if (req->has_wheel_radius_fl) grpc_req.set_wheel_radius_fl_m(req->wheel_radius_fl_m); - if (req->has_wheel_radius_fr) grpc_req.set_wheel_radius_fr_m(req->wheel_radius_fr_m); - if (req->has_wheel_radius_rl) grpc_req.set_wheel_radius_rl_m(req->wheel_radius_rl_m); - if (req->has_wheel_radius_rr) grpc_req.set_wheel_radius_rr_m(req->wheel_radius_rr_m); - if (req->has_steer_zero_offset_front) grpc_req.set_steer_zero_offset_front_deg(req->steer_zero_offset_front_deg); - if (req->has_steer_zero_offset_rear) grpc_req.set_steer_zero_offset_rear_deg(req->steer_zero_offset_rear_deg); - if (req->has_effective_track_width) grpc_req.set_effective_track_width_m(req->effective_track_width_m); - if (req->has_effective_wheel_base) grpc_req.set_effective_wheel_base_m(req->effective_wheel_base_m); - if (req->has_icr_offset_x) grpc_req.set_icr_offset_x_m(req->icr_offset_x_m); - if (req->has_icr_offset_y) grpc_req.set_icr_offset_y_m(req->icr_offset_y_m); - - StandardResponse grpc_reply; - grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3)); - grpc::Status status = stub_->CommitKinematicParameters(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); - res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // ---------------------------------------------------------------------- - // 🌊 接口 6 [异步水龙头]: 后台读取 50Hz 流并发布 ROS 2 Topic - // ---------------------------------------------------------------------- - void grpc_stream_to_ros2_topic() { - while (stream_running_ && rclcpp::ok()) { - stream_context_ = std::make_unique(); - Empty grpc_req; - - auto reader = stub_->StreamHardwareTelemetry(stream_context_.get(), grpc_req); - win_ubuntu_bridge::msg::HardwareState ros_msg; - HardwareState grpc_state; - - RCLCPP_INFO(this->get_logger(), "🌊 车端底盘 50Hz 裸数据流连接已建立，开始广播 ROS 2 Topic..."); - - // 只要网不断，Read() 就会以 50Hz 的频率一直被触发 - while (stream_running_ && reader->Read(&grpc_state) && rclcpp::ok()) { - // 完美 1:1 结构体拷贝 - ros_msg.hardware_timestamp_us = grpc_state.hardware_timestamp_us(); - ros_msg.encoder_ticks_fl = grpc_state.encoder_ticks_fl(); - ros_msg.encoder_ticks_fr = grpc_state.encoder_ticks_fr(); - ros_msg.encoder_ticks_rl = grpc_state.encoder_ticks_rl(); - ros_msg.encoder_ticks_rr = grpc_state.encoder_ticks_rr(); - ros_msg.actual_steer_angle_front_deg = grpc_state.actual_steer_angle_front_deg(); - ros_msg.actual_steer_angle_rear_deg = grpc_state.actual_steer_angle_rear_deg(); - ros_msg.current_fl_amp = grpc_state.current_fl_amp(); - ros_msg.current_fr_amp = grpc_state.current_fr_amp(); - ros_msg.current_rl_amp = grpc_state.current_rl_amp(); - ros_msg.current_rr_amp = grpc_state.current_rr_amp(); - ros_msg.current_steer_front_amp = grpc_state.current_steer_front_amp(); - ros_msg.driver_error_code = grpc_state.driver_error_code(); - - // 发布到 ROS 2 网络中！ - pub_telemetry_->publish(ros_msg); - } - - // 如果走到这里，说明网络断了或者发生了异常，稍等 1 秒后自动重试 - if (stream_running_) { - RCLCPP_WARN(this->get_logger(), "⚠️ gRPC 到底端的流意外中断！尝试重连..."); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - } - } -}; - -} // namespace agv_calib_core - -RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::ChassisGatewayNode) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/gateways/control_gateway_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/gateways/control_gateway_node.cpp deleted file mode 100644 index 1e45e90..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/gateways/control_gateway_node.cpp +++ /dev/null @@ -1,235 +0,0 @@ -#include -#include - -// 引入自定义的 ROS 2 接口头文件 -#include "win_ubuntu_bridge/msg/trajectory_point.hpp" -#include "win_ubuntu_bridge/msg/control_telemetry.hpp" -#include "win_ubuntu_bridge/srv/set_control_mode.hpp" -#include "win_ubuntu_bridge/srv/execute_open_loop_cmd.hpp" -#include "win_ubuntu_bridge/srv/follow_test_trajectory.hpp" -#include "win_ubuntu_bridge/srv/execute_step_response.hpp" -#include "win_ubuntu_bridge/srv/inject_tuning_params.hpp" -#include - -// 引入 gRPC 网络契约 -#include -#include "agv_calib_control.grpc.pb.h" - -#include -#include - -using namespace agv::calibration::control; - -namespace win_ubuntu_bridge { - -class ControlGatewayNode : public rclcpp::Node { -public: - explicit ControlGatewayNode(const rclcpp::NodeOptions & options) - : Node("control_gateway", options), stream_running_(false) { - - RCLCPP_INFO(this->get_logger(), "🧠 运控 gRPC 网关插件启动中... 正在挂载闭环调优 7 大接口！"); - - std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051"); - channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials()); - stub_ = AgvCalibControlService::NewStub(channel_); - - // ========================================================== - // 1. 挂载 7 个 ROS 2 翻译服务 - // ========================================================== - srv_set_mode_ = this->create_service( - "~/set_control_mode", std::bind(&ControlGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2)); - - srv_estop_ = this->create_service( - "~/emergency_stop", std::bind(&ControlGatewayNode::cb_estop, this, std::placeholders::_1, std::placeholders::_2)); - - srv_open_loop_ = this->create_service( - "~/execute_open_loop_cmd", std::bind(&ControlGatewayNode::cb_open_loop, this, std::placeholders::_1, std::placeholders::_2)); - - srv_follow_traj_ = this->create_service( - "~/follow_test_trajectory", std::bind(&ControlGatewayNode::cb_follow_traj, this, std::placeholders::_1, std::placeholders::_2)); - - srv_step_resp_ = this->create_service( - "~/execute_step_response", std::bind(&ControlGatewayNode::cb_step_resp, this, std::placeholders::_1, std::placeholders::_2)); - - srv_inject_params_ = this->create_service( - "~/inject_tuning_params", std::bind(&ControlGatewayNode::cb_inject_params, this, std::placeholders::_1, std::placeholders::_2)); - - srv_commit_params_ = this->create_service( - "~/commit_control_parameters", std::bind(&ControlGatewayNode::cb_commit_params, this, std::placeholders::_1, std::placeholders::_2)); - - // ========================================================== - // 2. 挂载 1 个 ROS 2 遥测发布者 (50Hz) - // ========================================================== - pub_telemetry_ = this->create_publisher("~/control_telemetry", 50); - - // 3. 启动独立推流抽水机线程 - stream_running_ = true; - stream_thread_ = std::thread(&ControlGatewayNode::grpc_stream_to_ros2_topic, this); - } - - ~ControlGatewayNode() { - stream_running_ = false; - if (stream_context_) stream_context_->TryCancel(); - if (stream_thread_.joinable()) stream_thread_.join(); - } - -private: - std::shared_ptr channel_; - std::unique_ptr stub_; - std::unique_ptr stream_context_; - - // Service & Publisher 句柄 - rclcpp::Service::SharedPtr srv_set_mode_; - rclcpp::Service::SharedPtr srv_estop_; - rclcpp::Service::SharedPtr srv_open_loop_; - rclcpp::Service::SharedPtr srv_follow_traj_; - rclcpp::Service::SharedPtr srv_step_resp_; - rclcpp::Service::SharedPtr srv_inject_params_; - rclcpp::Service::SharedPtr srv_commit_params_; - - rclcpp::Publisher::SharedPtr pub_telemetry_; - - std::thread stream_thread_; - std::atomic stream_running_; - - // ---------------------------------------------------------------------- - // ⬇️ ROS 2 to gRPC 核心翻译逻辑 ⬇️ - // ---------------------------------------------------------------------- - - void cb_set_mode(const std::shared_ptr req, - std::shared_ptr res) { - ModeRequest grpc_req; - grpc_req.set_target_mode(static_cast(req->target_mode)); - StandardResponse grpc_reply; - grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2)); - grpc::Status status = stub_->SetControlMode(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - void cb_estop(const std::shared_ptr req, std::shared_ptr res) { - (void)req; Empty grpc_req; StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(1)); - grpc::Status status = stub_->EmergencyStop(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - void cb_open_loop(const std::shared_ptr req, std::shared_ptr res) { - OpenLoopRequest grpc_req; - grpc_req.set_left_motor_cmd(req->left_motor_cmd); grpc_req.set_right_motor_cmd(req->right_motor_cmd); - grpc_req.set_steering_angle(req->steering_angle); grpc_req.set_duration_sec(req->duration_sec); - StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2)); - grpc::Status status = stub_->ExecuteOpenLoopCmd(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // 🚨 核心难点 1：翻译不定长数组 (轨迹考卷下发) + 超长超时时间 - void cb_follow_traj(const std::shared_ptr req, - std::shared_ptr res) { - TrajectoryRequest grpc_req; - grpc_req.set_test_case_id(req->test_case_id); - - // 遍历 ROS 2 的变长数组，依次 Add 进 gRPC 结构体 (使用 proto 的 add_ 方法分配内存) - for (const auto& ros_pt : req->path) { - TrajectoryPoint* grpc_pt = grpc_req.add_path(); - grpc_pt->set_x_m(ros_pt.x_m); - grpc_pt->set_y_m(ros_pt.y_m); - grpc_pt->set_yaw_rad(ros_pt.yaw_rad); - grpc_pt->set_target_speed_ms(ros_pt.target_speed_ms); - grpc_pt->set_curvature(ros_pt.curvature); - } - - RCLCPP_INFO(this->get_logger(), "正在下发包含 %zu 个轨迹点的考卷...", req->path.size()); - - StandardResponse grpc_reply; - grpc::ClientContext context; - - // 🚨 [架构师级防线]：车辆跑一圈可能需要 120 秒！这里的超时时间决不能像其他指令那样设为 2 秒，否则必死锁报错！ - context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(180)); - - grpc::Status status = stub_->FollowTestTrajectory(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - void cb_step_resp(const std::shared_ptr req, std::shared_ptr res) { - StepResponseRequest grpc_req; - grpc_req.set_target_velocity_ms(req->target_velocity_ms); grpc_req.set_duration_sec(req->duration_sec); - StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(10)); // 阶跃可能耗时几秒 - grpc::Status status = stub_->ExecuteStepResponse(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // 🚨 核心难点 2：解决 Optional 热更新映射 - void cb_inject_params(const std::shared_ptr req, std::shared_ptr res) { - ControlParams grpc_req; - - // 只有当 ROS 2 里的 has_ 标志位为 true 时，才向 gRPC 里 set 值 - if (req->has_wheel_radius_left_ratio) grpc_req.set_wheel_radius_left_ratio(req->wheel_radius_left_ratio); - if (req->has_wheel_radius_right_ratio) grpc_req.set_wheel_radius_right_ratio(req->wheel_radius_right_ratio); - if (req->has_effective_track_width_m) grpc_req.set_effective_track_width_m(req->effective_track_width_m); - if (req->has_steering_zero_offset_deg) grpc_req.set_steering_zero_offset_deg(req->steering_zero_offset_deg); - - if (req->has_pid_kp_lateral) grpc_req.set_pid_kp_lateral(req->pid_kp_lateral); - if (req->has_pid_ki_lateral) grpc_req.set_pid_ki_lateral(req->pid_ki_lateral); - if (req->has_pid_kd_lateral) grpc_req.set_pid_kd_lateral(req->pid_kd_lateral); - - if (req->has_pid_kp_heading) grpc_req.set_pid_kp_heading(req->pid_kp_heading); - if (req->has_pid_ki_heading) grpc_req.set_pid_ki_heading(req->pid_ki_heading); - if (req->has_pid_kd_heading) grpc_req.set_pid_kd_heading(req->pid_kd_heading); - - if (req->has_pure_pursuit_lookahead_m) grpc_req.set_pure_pursuit_lookahead_m(req->pure_pursuit_lookahead_m); - if (req->has_mpc_weight_q_lateral) grpc_req.set_mpc_weight_q_lateral(req->mpc_weight_q_lateral); - if (req->has_mpc_weight_r_steering) grpc_req.set_mpc_weight_r_steering(req->mpc_weight_r_steering); - - StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2)); - grpc::Status status = stub_->InjectTuningParameters(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - void cb_commit_params(const std::shared_ptr req, std::shared_ptr res) { - (void)req; Empty grpc_req; StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3)); - grpc::Status status = stub_->CommitControlParameters(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // ========================================================== - // 🌊 异步抽水机：读取 50Hz 运控状态 -> 转化为 ROS 2 Topic - // ========================================================== - void grpc_stream_to_ros2_topic() { - while (stream_running_ && rclcpp::ok()) { - stream_context_ = std::make_unique(); - Empty grpc_req; - auto reader = stub_->StreamTelemetry(stream_context_.get(), grpc_req); - - win_ubuntu_bridge::msg::ControlTelemetry ros_msg; - TelemetryData grpc_state; - - RCLCPP_INFO(this->get_logger(), "🌊 车端运控 50Hz ODOM 与体感数据流已建立，开始广播..."); - - while (stream_running_ && reader->Read(&grpc_state) && rclcpp::ok()) { - // 将 gRPC Odom 与速度赋值给 ROS msg - ros_msg.hardware_timestamp_us = grpc_state.hardware_timestamp_us(); - ros_msg.odom_x_m = grpc_state.odom_x_m(); - ros_msg.odom_y_m = grpc_state.odom_y_m(); - ros_msg.odom_yaw_rad = grpc_state.odom_yaw_rad(); - ros_msg.feedback_linear_vel_ms = grpc_state.feedback_linear_vel_ms(); - ros_msg.feedback_angular_vel_rads = grpc_state.feedback_angular_vel_rads(); - ros_msg.left_motor_current_amp = grpc_state.left_motor_current_amp(); - ros_msg.right_motor_current_amp = grpc_state.right_motor_current_amp(); - ros_msg.steering_motor_current_amp = grpc_state.steering_motor_current_amp(); - ros_msg.cmd_steering_output = grpc_state.cmd_steering_output(); - - // 广播供行为树和 AI 节点白嫖！ - pub_telemetry_->publish(ros_msg); - } - - if (stream_running_) { - RCLCPP_WARN(this->get_logger(), "⚠️ 运控推流中断！1秒后重连..."); - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - } - } -}; - -} // namespace agv_calib_core - -// 🚨 注册为 ROS 2 组件 -RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::ControlGatewayNode) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/gateways/sensor_gateway_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/gateways/sensor_gateway_node.cpp deleted file mode 100644 index ee936f7..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/gateways/sensor_gateway_node.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include -#include -#include - -// 引入自定义 ROS 2 接口 -#include "win_ubuntu_bridge/msg/camera_intrinsic.hpp" -#include "win_ubuntu_bridge/msg/sensor_extrinsic.hpp" -#include "win_ubuntu_bridge/srv/move_to_observation_pose.hpp" -#include "win_ubuntu_bridge/srv/trigger_sync_capture.hpp" -#include "win_ubuntu_bridge/srv/commit_calibration_results.hpp" -#include "win_ubuntu_bridge/action/download_sensor_data.hpp" - -// 引入 gRPC 契约 -#include -#include "agv_calib_sensor.grpc.pb.h" - -// 文件 I/O 与多线程 -#include -#include -#include - -using namespace agv::calibration::sensor; -namespace fs = std::filesystem; - -namespace win_ubuntu_bridge { - -class SensorGatewayNode : public rclcpp::Node { -public: - using DownloadAction = win_ubuntu_bridge::action::DownloadSensorData; - using GoalHandleDownload = rclcpp_action::ServerGoalHandle; - - explicit SensorGatewayNode(const rclcpp::NodeOptions & options) - : Node("sensor_gateway", options) { - - RCLCPP_INFO(this->get_logger(), "📷 传感器 gRPC 网关插件启动中... 准备接管大文件流式拉取引擎！"); - - std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051"); - channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials()); - stub_ = SensorCalibrationService::NewStub(channel_); - - // 1. 挂载 3 个瞬间动作服务 (Service) - srv_move_ = this->create_service( - "~/move_to_pose", std::bind(&SensorGatewayNode::cb_move, this, std::placeholders::_1, std::placeholders::_2)); - - srv_trigger_ = this->create_service( - "~/trigger_sync_capture", std::bind(&SensorGatewayNode::cb_trigger, this, std::placeholders::_1, std::placeholders::_2)); - - srv_commit_ = this->create_service( - "~/commit_calibration_results", std::bind(&SensorGatewayNode::cb_commit, this, std::placeholders::_1, std::placeholders::_2)); - - // 2. 🌟 挂载 1 个耗时动作服务器 (Action Server) —— 专治大文件流 - action_download_ = rclcpp_action::create_server( - this, - "~/download_sensor_data", - std::bind(&SensorGatewayNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&SensorGatewayNode::handle_cancel, this, std::placeholders::_1), - std::bind(&SensorGatewayNode::handle_accepted, this, std::placeholders::_1)); - } - -private: - std::shared_ptr channel_; - std::unique_ptr stub_; - - rclcpp::Service::SharedPtr srv_move_; - rclcpp::Service::SharedPtr srv_trigger_; - rclcpp::Service::SharedPtr srv_commit_; - rclcpp_action::Server::SharedPtr action_download_; - - // ========================================================== - // 瞬时服务翻译 (Service) - // ========================================================== - void cb_move(const std::shared_ptr req, - std::shared_ptr res) { - PoseRequest grpc_req; - grpc_req.set_target_x_m(req->target_x_m); grpc_req.set_target_y_m(req->target_y_m); - grpc_req.set_target_yaw_deg(req->target_yaw_deg); grpc_req.set_is_relative(req->is_relative); - StandardResponse grpc_reply; grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(60)); // 走位可能需要较长时间 - grpc::Status status = stub_->MoveToObservationPose(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - void cb_trigger(const std::shared_ptr req, - std::shared_ptr res) { - CaptureRequest grpc_req; - for (const auto& id : req->sensor_ids) { grpc_req.add_sensor_ids(id); } - CaptureResponse grpc_reply; grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3)); - grpc::Status status = stub_->TriggerSyncCapture(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); - res->capture_timestamp_us = grpc_reply.capture_timestamp_us(); // 🚨 取回取件码 - res->error_message = status.ok() ? grpc_reply.error_message() : status.error_message(); - } - - void cb_commit(const std::shared_ptr req, - std::shared_ptr res) { - CalibrationPayload grpc_req; - grpc_req.set_task_id(req->task_id); - - // 映射结构体数组 - for (const auto& ros_in : req->updated_intrinsics) { - auto* grpc_in = grpc_req.add_updated_intrinsics(); - grpc_in->set_camera_id(ros_in.camera_id); grpc_in->set_fx(ros_in.fx); grpc_in->set_fy(ros_in.fy); - grpc_in->set_cx(ros_in.cx); grpc_in->set_cy(ros_in.cy); - for (auto d : ros_in.dist_coeffs) grpc_in->add_dist_coeffs(d); - } - for (const auto& ros_ex : req->updated_extrinsics) { - auto* grpc_ex = grpc_req.add_updated_extrinsics(); - grpc_ex->set_source_frame(ros_ex.source_frame); grpc_ex->set_target_frame(ros_ex.target_frame); - grpc_ex->set_trans_x_mm(ros_ex.trans_x_mm); grpc_ex->set_trans_y_mm(ros_ex.trans_y_mm); grpc_ex->set_trans_z_mm(ros_ex.trans_z_mm); - grpc_ex->set_roll_deg(ros_ex.roll_deg); grpc_ex->set_pitch_deg(ros_ex.pitch_deg); grpc_ex->set_yaw_deg(ros_ex.yaw_deg); - } - - StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(5)); - grpc::Status status = stub_->CommitCalibrationResults(&context, grpc_req, &grpc_reply); - res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message(); - } - - // ========================================================== - // 🚨 灵魂逻辑：大文件流式下载 转换为 ROS 2 Action 落盘 - // ========================================================== - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { - (void)uuid; - RCLCPP_INFO(this->get_logger(), "📥 收到大文件下载工单! 凭证: %ld, 传感器: %s", goal->capture_timestamp_us, goal->sensor_id.c_str()); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { - (void)goal_handle; - RCLCPP_WARN(this->get_logger(), "🛑 行为树请求强行取消下载"); - return rclcpp_action::CancelResponse::ACCEPT; - } - - void handle_accepted(const std::shared_ptr goal_handle) { - // 🚨 必须在独立线程中执行阻塞的网络流读取，否则会死锁整个 ROS 2 容器！ - std::thread{std::bind(&SensorGatewayNode::execute_download, this, std::placeholders::_1), goal_handle}.detach(); - } - - // 后台真实下载线程 - void execute_download(const std::shared_ptr goal_handle) { - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - auto feedback = std::make_shared(); - - DataFetchRequest grpc_req; - grpc_req.set_capture_timestamp_us(goal->capture_timestamp_us); - grpc_req.set_sensor_id(goal->sensor_id); - - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(60)); // 大文件给足 60 秒传输时间 - - std::unique_ptr> reader; - if (goal->data_type == DownloadAction::Goal::DATA_TYPE_IMAGE) { - reader = stub_->DownloadImage(&context, grpc_req); - } else { - reader = stub_->DownloadPointCloud(&context, grpc_req); - } - - FileChunk chunk; - std::ofstream outfile; - std::string final_file_path; - uint64_t total_bytes = 0; - bool is_first_chunk = true; - - RCLCPP_INFO(this->get_logger(), "🚀 开始从车端流式拉取传感器大文件..."); - - // 🌊 像蚂蚁搬家一样，收到一块，就用二进制追加写入硬盘一块！ - while (reader->Read(&chunk) && rclcpp::ok()) { - - // 1. 检查行为树是否发出了取消指令 - if (goal_handle->is_canceling()) { - context.TryCancel(); // 物理打断 gRPC 传输 - if (outfile.is_open()) outfile.close(); - if (!final_file_path.empty() && fs::exists(final_file_path)) fs::remove(final_file_path); // 删掉下了一半的残次品 - result->success = false; result->error_message = "下载被系统强制中止"; - goal_handle->canceled(result); - return; - } - - // 2. 如果是第一包数据，创建目录并打开硬盘文件 - if (is_first_chunk) { - if (!fs::exists(goal->save_directory)) { fs::create_directories(goal->save_directory); } - - std::string ext = chunk.format_ext().empty() ? "bin" : chunk.format_ext(); // 动态获取文件后缀 - final_file_path = goal->save_directory + "/" + goal->sensor_id + "_" + std::to_string(goal->capture_timestamp_us) + "." + ext; - - // 🚨 以纯二进制 (binary) 和截断清空 (trunc) 模式打开硬盘文件！ - outfile.open(final_file_path, std::ios::out | std::ios::binary | std::ios::trunc); - if (!outfile.is_open()) { - result->success = false; result->error_message = "无法在 Ubuntu 硬盘创建文件！权限不足？"; - goal_handle->abort(result); - return; - } - is_first_chunk = false; - } - - // 3. 把网络收到的内存碎块直接写入硬盘！(内存占用极低，绝对不会爆内存) - outfile.write(chunk.chunk_data().data(), chunk.chunk_data().size()); - total_bytes += chunk.chunk_data().size(); - - // 4. 每收到一块，通过 Action Feedback 汇报一次进度 - feedback->downloaded_bytes = total_bytes; - goal_handle->publish_feedback(feedback); - - if (chunk.is_last_chunk()) break; - } - - grpc::Status status = reader->Finish(); - if (outfile.is_open()) outfile.close(); - - // 5. 善后汇报 - if (status.ok() && !is_first_chunk) { - RCLCPP_INFO(this->get_logger(), "✅ 下载落盘成功！共计 %.2f MB -> %s", total_bytes / 1024.0 / 1024.0, final_file_path.c_str()); - result->success = true; - result->saved_file_path = final_file_path; // 🚨 核心：将绝对路径交还给行为树！ - goal_handle->succeed(result); - } else { - if (!final_file_path.empty() && fs::exists(final_file_path)) fs::remove(final_file_path); // 失败清理垃圾 - RCLCPP_ERROR(this->get_logger(), "❌ 下载大文件失败: %s", status.error_message().c_str()); - result->success = false; - result->error_message = "网络传输中断: " + status.error_message(); - goal_handle->abort(result); - } - } -}; - -} // namespace agv_calib_core - -// 注册为 ROS 2 组件 -RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::SensorGatewayNode) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/launch/gateways_bringup.launch.py b/agv_calib_brain/src/win_ubuntu_bridge/launch/gateways_bringup.launch.py deleted file mode 100644 index 652419b..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/launch/gateways_bringup.launch.py +++ /dev/null @@ -1,47 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -def generate_launch_description(): - # 1. 声明一个外部参数 agv_ip，默认值是本地 - agv_ip_arg = DeclareLaunchArgument( - 'agv_ip', - default_value='127.0.0.1:50051', - description='Windows 车端在 Wi-Fi 下的 IP 和端口' - ) - - agv_ip = LaunchConfiguration('agv_ip') - - # 2. 把参数动态注入给三个网关组件 - container = ComposableNodeContainer( - name='agv_gateway_container', - namespace='', - package='rclcpp_components', - executable='component_container_mt', - composable_node_descriptions=[ - ComposableNode( - package='win_ubuntu_bridge', - plugin='win_ubuntu_bridge::ChassisGatewayNode', - name='chassis_gateway', - parameters=[{'agv_ip': agv_ip}] # 👈 动态注入真实 IP - ), - - ComposableNode( - package='win_ubuntu_bridge', - plugin='win_ubuntu_bridge::ControlGatewayNode', - name='control_gateway', - parameters=[{'agv_ip': agv_ip}] # 👈 动态注入真实 IP - ), - - ComposableNode( - package='win_ubuntu_bridge', - plugin='win_ubuntu_bridge::SensorGatewayNode', - name='sensor_gateway', - parameters=[{'agv_ip': agv_ip}] # 👈 动态注入真实 IP - ), - ], - output='screen', - ) - return LaunchDescription([agv_ip_arg, container]) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/JobAccepted.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/JobAccepted.msg deleted file mode 100644 index fe49ce4..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/msg/JobAccepted.msg +++ /dev/null @@ -1,12 +0,0 @@ -# ========================================================= -# 文件作用：长任务受理响应 -# 对应 proto：JobAccepted -# 作用：耗时任务先返回 job_id，后续再查询状态 -# 发送方：服务端 -# 接收方：Ubuntu 车间电脑 -# ========================================================= - -bool accepted # 是否受理成功 -uint16 error_code # 未受理时的错误码，取值参考 ErrorCode.msg -string message # 说明 -string job_id # 长任务 ID \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/JobStatus.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/JobStatus.msg deleted file mode 100644 index 571c1ea..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/msg/JobStatus.msg +++ /dev/null @@ -1,15 +0,0 @@ -# ========================================================= -# 文件作用：长任务状态响应 -# 对应 proto：JobStatus -# 作用：返回任务执行状态、进度、错误信息 -# 发送方：服务端 -# 接收方：Ubuntu 车间电脑 -# ========================================================= - -string job_id # 任务 ID -uint8 state # 当前状态，取值参考 JobState.msg -float64 progress # 进度，建议范围 0.0 ~ 1.0 -uint16 error_code # 当前错误码，取值参考 ErrorCode.msg -string message # 状态说明 -int64 server_timestamp_us # 服务端时间戳 -bool safe_to_retry # 是否适合自动重试 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/StandardResponse.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/StandardResponse.msg deleted file mode 100644 index ea6f18c..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/msg/StandardResponse.msg +++ /dev/null @@ -1,14 +0,0 @@ -# ========================================================= -# 文件作用：通用短响应 -# 对应 proto：StandardResponse -# 作用：用于同步服务调用的标准响应 -# 发送方：服务端 -# 接收方：调用方 -# 说明： -# 1) error_code 使用 uint16 -# 2) 取值参考 ErrorCode.msg 中定义的常量 -# ========================================================= - -bool success # 是否成功 -uint16 error_code # 错误码，取值参考 ErrorCode.msg -string message # 说明文字 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/package.xml deleted file mode 100644 index 9e57938..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - win_ubuntu_bridge - 1.0.0 - AGV 自动化标定车间跨平台网关与总控核心 - nvidia - Apache-2.0 - - ament_cmake - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - rclcpp - rclcpp_components - rclcpp_action - std_msgs - std_srvs - action_msgs - behaviortree_cpp_v3 ament_lint_auto - ament_lint_common - - - ament_cmake - - \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/proto/README.md b/agv_calib_brain/src/win_ubuntu_bridge/proto/README.md deleted file mode 100644 index 4a3e6e9..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/proto/README.md +++ /dev/null @@ -1,949 +0,0 @@ -# 自动化标定车间 Proto 协同工作说明 - -[![Protocol](https://img.shields.io/badge/Protocol-Protocol%20Buffers-blue)](https://developers.google.com/protocol-buffers) -[![Platform](https://img.shields.io/badge/Platform-Ubuntu%20%7C%20Linux%20%7C%20Windows-green)](https://ubuntu.com) -[![Status](https://img.shields.io/badge/Status-Design%20Phase-orange)](./) - -> **核心定位**：面向自动化标定车间的分层协议设计，实现车间总控、专业域服务与车端代理的协同工作。 - ---- - -## 📋 目录 - -- [1. 项目目标](#1-项目目标) -- [2. 七个 Proto 文件的职责划分](#2-七个-proto-文件的职责划分) - - [2.1 calibration_common.proto](#21-calibration_commonproto) - - [2.2 vehicle_profile.proto](#22-vehicle_profileproto) - - [2.3 external_localization.proto](#23-external_localizationproto) - - [2.4 chassis_calibration.proto](#24-chassis_calibrationproto) - - [2.5 sensor_calibration.proto](#25-sensor_calibrationproto) - - [2.6 control_calibration.proto](#26-control_calibrationproto) - - [2.7 workshop_orchestration.proto](#27-workshop_orchestrationproto) -- [3. 七个 Proto 的依赖关系](#3-七个-proto-的依赖关系) -- [4. 协同架构图](#4-协同架构图) -- [5. 自动化标定车间的完整逻辑流程](#5-自动化标定车间的完整逻辑流程) -- [6. 为什么一定要拆成这 7 个 Proto](#6-为什么一定要拆成这-7-个-proto) -- [7. 从通信形态角度看，这 7 个 Proto 如何使用](#7-从通信形态角度看这-7-个-proto-如何使用) -- [8. 七个 Proto 在一次完整会话中的协同时序](#8-七个-proto-在一次完整会话中的协同时序) -- [9. 七个 Proto 的工程分层建议](#9-七个-proto-的工程分层建议) -- [10. 当前这套 Proto 的核心协同原则](#10-当前这套-proto-的核心协同原则) -- [11. 落地实现时还必须注意的点](#11-落地实现时还必须注意的点) -- [12. 推荐的理解方式](#12-推荐的理解方式) -- [13. 总结](#13-总结) -- [14. 后续建议](#14-后续建议) - ---- - -## 1. 项目目标 - -本项目面向**自动化标定车间**场景，目标是通过一组分层设计的 `.proto` 协议文件，定义： - -- 车间上位机（Ubuntu / Linux）内部各服务之间的通信接口 -- 车间上位机与车端执行代理（Windows）之间的通信接口 -- 自动化调度、车辆建模、外部定位、底盘标定、传感器标定、运控参数标定等模块之间的职责边界 -- 整车自动化标定流程的统一任务组织方式、状态表达方式、结果归档方式 - -这一套协议的核心设计思想不是"把所有逻辑塞到一个服务里"，而是： - -1. **车间总控负责流程编排** -2. **各标定域服务负责专业能力** -3. **车端代理只负责动作执行、数据采集、参数落盘** -4. **所有流程都围绕统一的会话、任务、车辆画像、作业状态进行协同** - ---- - -## 2. 七个 Proto 文件的职责划分 - -本工程共包含 **7** 个核心 `.proto` 文件，它们共同构成自动化标定车间的通信协议层。 - ---- - -### 2.1 calibration_common.proto - -**作用：公共基础协议定义** - -这是整个系统的底层公共协议文件，所有其他 proto 都会依赖它。 - -**主要定义内容：** - -- 通用请求头 `RequestHeader` -- 空消息、标准响应、错误码 -- 长任务状态表达 `JobAccepted` / `JobQuery` / `JobStatus` -- 心跳请求与响应 `HeartbeatRequest` / `HeartbeatResponse` -- 文件摘要 `FileDigest` -- 统一错误码 `ErrorCode` -- 统一任务状态 `JobState` - -**解决的共性问题：** - -- 一次请求是谁发起的 -- 属于哪个标定会话 -- 当前针对哪台车 -- 这个任务现在处于什么状态 -- 返回的是不是成功 -- 错误是否可重试 -- 文件、参数包如何做摘要校验 - -> **一句话理解**：`calibration_common.proto` 是整个自动化标定车间协议体系的"地基"。 - ---- - -### 2.2 vehicle_profile.proto - -**作用：车辆画像与能力建模** - -这个文件负责表达"待标定对象到底是什么"。 - -**主要定义内容：** - -- 车辆底盘类型 -- 是否带机械臂 -- 传感器配置类型 -- 控制器算法配置 -- 车辆基础几何与命名信息 -- URDF 导入关联信息 -- 车辆能力声明 - -**解决的对象建模问题：** - -- 当前进站的是哪种车 -- 是阿克曼、差速、单舵轮还是多舵轮 -- 车上有哪些传感器 -- 机械臂有没有 -- 需要做哪些标定任务 -- 底盘标定该走哪套动作原语 -- 运控参数标定应该面向哪些控制器 - -> **一句话理解**：`vehicle_profile.proto` 负责回答"这台车是谁、长什么样、具备什么能力"。 - ---- - -### 2.3 external_localization.proto - -**作用：外部定位能力与测量结果接口** - -这个文件负责定义自动化车间中"外部定位系统"相关的协议。 - -这里的外部定位通常不是车上自带定位，而是车间里的外部测量系统，例如： - -- 动捕系统 -- 激光跟踪仪 -- 高精度全站仪 -- 外部相机阵列 -- 标定工装测量系统 - -**主要职责：** - -- 请求外部定位系统开始测量 -- 获取目标位姿 / 轨迹 -- 获取参考基准坐标 -- 记录测量结果质量 -- 提供给底盘、传感器、运控标定使用的真值参考 - -它在整个车间流程中是一个非常关键的"**真值来源模块**"。 - -> **一句话理解**：`external_localization.proto` 负责给整个自动化标定车间提供高精度外部参考。 - ---- - -### 2.4 chassis_calibration.proto - -**作用：底盘标定执行代理协议** - -这个文件主要描述**车间上位机 → Windows 车端代理**之间，底盘标定相关的执行接口。 - -**主要内容：** - -- 底盘工作模式切换 -- 车端底盘能力查询 -- 动作原语执行请求 -- 底盘遥测流 -- 底盘参数写入 -- 当前生效参数查询 -- 紧急刹停 - -**典型动作原语：** - -- 直线行驶 -- 圆弧行驶 -- 原地旋转 -- 舵角扫动 - -**典型遥测：** - -- 里程计 -- 轮速 -- 编码器 -- 舵角 -- 驱动器状态 -- 急停状态 - -这个文件本质上不是"求解底盘参数"，而是定义： - -> Linux 上位机如何命令车端去做底盘动作，并拿回原始数据，再由 Linux 进行参数求解。 - -> **一句话理解**：`chassis_calibration.proto` 是底盘标定的"执行与数据回传协议"。 - ---- - -### 2.5 sensor_calibration.proto - -**作用：传感器标定任务与结果接口** - -这个文件负责定义传感器标定域相关协议。 - -**覆盖范围：** - -- 下视相机 -- 前视相机 -- 机械臂相机（眼在手上 / 眼在手外） -- 2D 激光雷达 -- 3D 激光雷达 -- IMU - -**典型标定内容：** - -- 相机内参 -- IMU 内参（如 `accel_bias` / `gyro_bias`） -- 传感器外参（相对于 `base_link`） -- 标定数据采集任务 -- 标定结果质量评估 -- 标定结果写入与查询 - -这个文件通常会与 `vehicle_profile.proto`、`external_localization.proto`、`workshop_orchestration.proto` 有很强协作关系。 - -因为是否要做某种传感器标定、该使用哪种工装、是否需要机械臂配合、外部真值从哪里来，都取决于车辆画像和整体流程编排。 - -> **一句话理解**：`sensor_calibration.proto` 负责定义传感器标定的任务、数据和结果。 - ---- - -### 2.6 control_calibration.proto - -**作用：运控参数调优标定执行代理协议** - -这个文件负责定义车辆控制参数调优相关接口。 - -这里的"运控参数标定"一般不是底盘几何标定，而是： - -- 横向控制器参数标定 -- 纵向控制器参数标定 - -**典型控制算法：** - -- PID -- MPC -- LQR -- PP（Pure Pursuit） - -并且要明确拆分为： - -- **横向控制器** -- **纵向控制器** - -因为这两类控制目标不同，参数含义不同，评估指标也不同。 - -**主要功能：** - -- 控制工作模式切换 -- 参数热加载 -- 控制评估任务启动 -- 遥测流回传 -- 参数固化 -- 当前参数查询 -- 紧急停车 - -**常见评估任务：** - -- 轨迹跟踪任务 -- 速度阶跃任务 - -**常见回传指标：** - -- 横向误差 -- 航向误差 -- 速度误差 -- 转向输出 -- 油门 / 驱动输出 -- 饱和状态 - -> **一句话理解**：`control_calibration.proto` 负责让 Linux 调参服务可以驱动车端执行控制测试并闭环评估。 - ---- - -### 2.7 workshop_orchestration.proto - -**作用：车间总控编排协议** - -这是整个自动化标定车间里最核心的"总控协议"。 - -它不是具体做底盘求解，也不是具体做传感器求解，而是负责编排整条流程： - -- 创建整车标定会话 -- 加载车辆画像 -- 校验前置条件 -- 决定本次需要执行哪些任务 -- 按阶段推进流程 -- 跟踪每个子任务状态 -- 统一处理失败、重试、跳过、中止 -- 归档全部标定结果 -- 输出最终标定报告 - -它会与其余 6 个 proto 对应的服务产生协作关系。 - -你可以把它理解成： - -> 自动化标定车间的"总导演"和"状态机主控器" - -> **一句话理解**：`workshop_orchestration.proto` 决定"先做什么、后做什么、谁调用谁、失败了怎么办"。 - ---- - -## 3. 七个 Proto 的依赖关系 - -整体依赖关系可以概括为： - -| Proto 文件 | 被依赖方 | 作用 | -|:-----------|:---------|:-----| -| `calibration_common.proto` | 所有其他 proto | 公共基础能力 | -| `vehicle_profile.proto` | orchestration、底盘、传感器、运控标定 | 车辆建模依据 | -| `external_localization.proto` | 底盘、传感器、运控标定 | 外部真值/参考测量 | -| `chassis_calibration.proto` | `workshop_orchestration.proto` | 被总控调度 | -| `sensor_calibration.proto` | `workshop_orchestration.proto` | 被总控调度 | -| `control_calibration.proto` | `workshop_orchestration.proto` | 被总控调度 | -| `workshop_orchestration.proto` | - | 作为总控层，组织全部流程 | - ---- - -## 4. 协同架构图 - -下面是一个推荐的逻辑协同图： - -```mermaid -flowchart TD - A[工作人员在 Ubuntu 车间电脑配置车辆信息] --> B[vehicle_profile.proto -生成车辆画像] - B --> C[workshop_orchestration.proto -创建整车标定会话] - C --> D[前置检查 / 能力校验 / 安全检查] - - D --> E[external_localization.proto -初始化外部定位系统] - E --> F[chassis_calibration.proto -执行底盘动作与回传遥测] - E --> G[sensor_calibration.proto -执行传感器采集与求解] - E --> H[control_calibration.proto -执行轨迹/速度测试并评估] - - F --> C - G --> C - H --> C - - C --> I[统一汇总结果] - I --> J[参数写入 / 版本归档 / 生成报告] -``` - ---- - -## 5. 自动化标定车间的完整逻辑流程 - -下面从工程落地角度梳理一遍完整的自动化流程。 - ---- - -### 阶段 1：车辆入站与基础配置 - -工作人员在 Ubuntu 车间电脑上完成基础配置： - -- 输入车辆 ID -- 选择底盘类型 -- 选择是否带机械臂 -- 选择搭载的传感器类型 -- 选择控制器类型（横向 / 纵向） -- 导入 URDF（可选但强烈建议） -- 选择本次需要执行的标定项目 - -此时由 `vehicle_profile.proto` 完成车辆画像建模。 - -**输出结果：** - -- 一个完整的车辆画像对象 -- 当前标定会话的基础上下文 - ---- - -### 阶段 2：创建整车标定会话 - -由 `workshop_orchestration.proto` 创建一轮完整的标定会话。 - -这里通常会生成： - -- `session_id` -- 标定计划 -- 子任务列表 -- 初始状态机节点 - -同时将统一的 `RequestHeader` 注入后续所有子任务请求。 - -**输出结果：** - -- 一次完整的整车标定上下文 -- 后续所有模块共享同一会话 ID - ---- - -### 阶段 3：前置检查 - -总控模块调用各子服务检查前置条件： - -- 车辆是否在线 -- 车端代理是否在线 -- 外部定位系统是否在线 -- 传感器是否可通信 -- 驱动器是否健康 -- 急停状态是否释放 -- 当前车辆能力是否满足目标任务 - -这里会大量使用： - -- `Heartbeat` -- `StandardResponse` -- `JobStatus` -- 车辆能力查询 -- 外部定位能力查询 - -若前置检查失败，则流程不能继续。 - ---- - -### 阶段 4：外部定位系统建立参考 - -由 `external_localization.proto` 驱动外部定位系统建立参考坐标框架。 - -例如： - -- 建立车间世界坐标系 -- 识别待标定车辆 -- 获取基准工装位姿 -- 连续输出外部真值位姿 -- 检查测量质量是否达标 - -这一阶段输出的是后续标定需要依赖的"真值"或"参考值"。 - ---- - -### 阶段 5：底盘标定 - -由 `workshop_orchestration.proto` 调度 `chassis_calibration.proto` 所对应的服务进行底盘标定。 - -**基本流程：** - -1. 切换到底盘标定模式 -2. 查询底盘能力 -3. 按底盘类型下发动作原语 -4. 车端执行动作 -5. 车端持续回传遥测 -6. Linux 求解底盘参数 -7. 参数写入车端 -8. 查询写入后的生效结果 - -**不同底盘类型的动作组合：** - -| 底盘类型 | 动作组合 | -|:---------|:---------| -| 阿克曼 | 直线、圆弧、舵角扫动 | -| 差速 | 直线、原地旋转、圆弧 | -| 单舵轮 | 直线、圆弧、舵角扫动 | -| 多舵轮 | 直线、旋转、单模块扫动或联动测试 | - -**底盘标定解决的问题：** - -- 直线跑偏 -- 曲率误差 -- 轮径补偿 -- 轴距/轮距有效值 -- 编码器比例 -- 舵角零偏 -- 模块安装误差 - ---- - -### 阶段 6：传感器标定 - -由 `workshop_orchestration.proto` 调度 `sensor_calibration.proto` 进行传感器标定。 - -**常见流程：** - -1. 根据车辆画像，识别本车有哪些传感器需要标定 -2. 检查工装和采集条件 -3. 触发采集任务 -4. 回传采集状态和原始数据引用 -5. Linux 侧完成求解 -6. 输出标定参数 -7. 参数写入配置系统 -8. 校验结果质量是否达标 - -**两大类参数：** - -#### 6.1 内参 - -例如： - -- 相机内参矩阵 -- 畸变参数 -- IMU 的 `accel_bias` -- IMU 的 `gyro_bias` - -#### 6.2 外参 - -例如： - -- 各传感器相对 `base_link` 的位姿 -- 手眼关系 -- 机械臂末端与相机的位姿关系 -- 外部定位参考坐标系到车体系的关系 - ---- - -### 阶段 7：运控参数标定 - -由 `workshop_orchestration.proto` 调度 `control_calibration.proto` 进行运控参数调优。 - -> **注意**：控制器必须拆成横向与纵向两大类来表达。 - -#### 横向控制器 - -例如： - -- PID（横向误差 / 航向误差） -- MPC -- LQR -- Pure Pursuit - -#### 纵向控制器 - -例如： - -- PID（速度环） -- MPC 速度规划控制 -- 其他速度控制算法 - -**典型流程：** - -1. 切换到调参模式 -2. 下发一版候选参数 -3. 启动车端评估任务 -4. 执行轨迹跟踪或速度阶跃测试 -5. 回传遥测数据 -6. Linux 计算性能指标 -7. 决定继续迭代还是固化参数 -8. 固化最终参数版本 - -**评估指标：** - -- 横向误差 -- 航向误差 -- 速度误差 -- 超调量 -- 稳态误差 -- 控制量抖动 -- 饱和比例 -- 收敛时间 - ---- - -### 阶段 8：统一收敛判定与结果归档 - -当底盘标定、传感器标定、运控标定都完成后，由总控模块统一判定： - -- 是否全部成功 -- 是否存在部分成功 -- 是否允许跳过某项后完成整体会话 -- 是否需要人工复核 -- 是否需要重试某一子项 - -然后进行： - -- 参数版本归档 -- 文件摘要记录 -- 标定结果持久化 -- 会话状态闭环 -- 生成最终报告 - ---- - -## 6. 为什么一定要拆成这 7 个 Proto - -从自动化标定车间落地的角度，这样拆分有几个核心优点。 - ---- - -### 6.1 降低耦合 - -如果把所有内容塞到一个 proto 或一个服务里，会导致： - -- 车辆建模与执行耦合 -- 专业算法与流程编排耦合 -- 车端执行与求解逻辑耦合 -- 协议难维护 -- 后续扩展困难 - -拆分后，每个 proto 只负责一个明确领域。 - ---- - -### 6.2 便于多人协作开发 - -你这个项目本身就已经是多人分工场景： - -- 你负责自动化车间运行逻辑 -- 别人负责外部定位 -- 别人负责底盘标定算法 -- 别人负责运控调参算法 -- 别人负责传感器标定算法 - -在这种情况下，proto 分层能让每个人在清晰边界内开发。 - ---- - -### 6.3 便于后续接 ROS2 - -你后面明确希望把 proto 内容进一步映射为： - -- ROS2 msg -- ROS2 srv -- ROS2 action - -如果 proto 设计本身就已经分层清晰，那么后续转换到 ROS2 的结构也会更自然： - -- 公共消息 → 基础 msg -- 短请求/响应 → srv -- 长任务 → action -- 流式遥测 → topic - ---- - -### 6.4 更符合真实车间运行逻辑 - -真实车间不是"一个函数跑到底"，而是： - -- 一个总控状态机 -- 多个专业子服务 -- 一个或多个车端执行代理 -- 多个外部设备 -- 多阶段流程推进 -- 随时可能失败、重试、中断、恢复 - -所以协议设计也必须体现真实的系统边界。 - ---- - -## 7. 从通信形态角度看，这 7 个 Proto 如何使用 - -自动化标定车间里并不是所有消息都属于同一种通信形式。 - ---- - -### 7.1 适合短同步调用的内容 - -典型如： - -- 工作模式切换 -- 能力查询 -- 当前参数查询 -- 心跳 -- 参数写入确认 - -这类内容通常是**请求-响应式**的。 - ---- - -### 7.2 适合长任务异步处理的内容 - -典型如： - -- 启动底盘测试任务 -- 启动传感器采集任务 -- 启动控制评估任务 -- 启动外部定位测量任务 - -这些任务通常不会立刻完成，因此需要： - -- 先返回 `JobAccepted` -- 后续通过 `JobQuery` / `JobStatus` 查询 -- 或进一步映射为 action - ---- - -### 7.3 适合流式数据输出的内容 - -典型如： - -- 底盘遥测 -- 控制遥测 -- 外部定位连续位姿流 -- 传感器采集状态流 - -这类数据天然是持续输出的，更适合 topic / stream。 - ---- - -## 8. 七个 Proto 在一次完整会话中的协同时序 - -![自动化标定车间协同时序图](../docs/images/sequence_diagram.png) - ---- - -## 9. 七个 Proto 的工程分层建议 - -为了后续工程实现更清晰，建议代码目录也按这 7 个 proto 的职责进行分层。 - -**参考结构：** - -```text -proto/ -├── calibration_common.proto -├── vehicle_profile.proto -├── external_localization.proto -├── chassis_calibration.proto -├── sensor_calibration.proto -├── control_calibration.proto -└── workshop_orchestration.proto - -services/ -├── workshop_orchestrator/ -├── vehicle_profile_manager/ -├── external_localization_service/ -├── chassis_calibration_service/ -├── sensor_calibration_service/ -├── control_calibration_service/ -└── vehicle_agent_windows/ - -algorithms/ -├── chassis_solver/ -├── sensor_solver/ -├── control_tuner/ -└── external_localization_backend/ - -configs/ -├── vehicles/ -├── sensors/ -├── controllers/ -└── workshop/ -``` - ---- - -## 10. 当前这套 Proto 的核心协同原则 - -这 7 个 proto 在设计上应始终遵循以下原则。 - ---- - -### 原则 1：总控只编排，不求解 - -`workshop_orchestration.proto` 负责： - -- 编排 -- 状态推进 -- 失败处理 -- 结果汇总 - -但不要在总控里塞入底盘求解、传感器求解、控制调参等专业算法细节。 - ---- - -### 原则 2：车端只执行，不决策 - -Windows 车端代理负责： - -- 接收命令 -- 执行动作 -- 采集遥测 -- 写入参数 -- 回传状态 - -它不应该承载复杂的标定求解逻辑。 - ---- - -### 原则 3：参数求解在 Linux 侧完成 - -不论是： - -- 底盘参数求解 -- 传感器参数求解 -- 运控参数优化 - -都应优先放在 Ubuntu / Linux 车间电脑侧完成。 - -这与你之前确定的原则完全一致。 - ---- - -### 原则 4：车辆画像先行 - -任何标定任务开始之前，都必须先有完整的车辆画像。 - -否则： - -- 不知道该做哪些任务 -- 不知道该下发哪些动作 -- 不知道哪些传感器存在 -- 不知道是否涉及机械臂 -- 不知道使用哪套控制参数模板 - ---- - -### 原则 5：统一任务状态表达 - -所有耗时任务都应尽量统一为： - -- 受理 -- 执行中 -- 成功 -- 失败 -- 取消 - -并配合统一错误码和统一重试语义。 - -这样总控状态机才容易实现。 - ---- - -## 11. 落地实现时还必须注意的点 - -虽然这 7 个 proto 已经能覆盖主干流程，但真正落地时，还必须继续注意以下内容。 - ---- - -### 11.1 安全状态一定要贯穿所有流程 - -例如： - -- 急停是否触发 -- 防撞区域是否占用 -- 机械臂是否处于安全姿态 -- 外部定位工装是否到位 -- 当前车速是否允许切模式 -- 参数写入时是否禁止车辆运动 - -这些要么在公共协议中补状态字段，要么在各业务协议中明确体现。 - ---- - -### 11.2 所有结果都要有版本和摘要 - -尤其是： - -- 底盘参数 -- 传感器外参 -- 相机内参 -- IMU 偏置 -- 运控参数 -- URDF 引用版本 - -都建议具备： - -- `parameter_version` -- `digest` -- `applied_timestamp` -- `source_session_id` - ---- - -### 11.3 所有长任务最好支持中止与恢复 - -真实车间里很常见： - -- 人员临时介入 -- 车辆断电 -- 网络闪断 -- 外部定位丢失 -- 工装被碰撞 -- 某个子任务失败 - -所以流程设计最好支持： - -- 取消任务 -- 安全中止 -- 从阶段恢复 -- 局部重跑 - ---- - -### 11.4 结果质量不能只给成功/失败 - -例如： - -- 传感器标定应给重投影误差、残差、覆盖率 -- 底盘标定应给直线误差、角度误差、曲率误差 -- 运控调参应给超调、稳态误差、收敛时间 -- 外部定位应给测量质量等级、可见性状态 - -也就是说，最终不应该只有 `success=true/false`，而应该尽可能有"质量指标"。 - ---- - -## 12. 推荐的理解方式 - -如果把整个自动化标定车间看成一个工厂流水线，那么： - -| Proto 文件 | 类比角色 | -|:-----------|:---------| -| `calibration_common.proto` | 通用工单格式、错误码、状态码、追踪信息 | -| `vehicle_profile.proto` | 产品型号定义表 | -| `external_localization.proto` | 高精度测量工位 | -| `chassis_calibration.proto` | 底盘调校工位 | -| `sensor_calibration.proto` | 传感器标定工位 | -| `control_calibration.proto` | 运控调参工位 | -| `workshop_orchestration.proto` | 整条产线的总调度系统 | - ---- - -## 13. 总结 - -这 7 个 proto 不是彼此独立的零散文件，而是共同组成了自动化标定车间的完整协议体系： - -- `calibration_common.proto` 提供统一基础能力 -- `vehicle_profile.proto` 描述待标定车辆对象 -- `external_localization.proto` 提供外部真值参考 -- `chassis_calibration.proto` 负责底盘标定执行与数据回传 -- `sensor_calibration.proto` 负责传感器标定任务与结果表达 -- `control_calibration.proto` 负责横纵向控制参数调优执行链路 -- `workshop_orchestration.proto` 负责全流程编排、状态推进与结果汇总 - -它们共同支撑起一条完整的自动化标定车间流程： - -**车辆入站 → 车辆画像 → 会话创建 → 前置检查 → 外部定位建立参考 → 底盘标定 → 传感器标定 → 运控调参 → 参数写入 → 结果归档 → 会话闭环** - -这也是后续继续落地到： - -- ROS2 接口映射 -- C++ 服务实现 -- 状态机编排 -- 车端代理实现 -- 参数管理系统 -- 标定报告系统 - -的协议基础。 - ---- - -## 📄 附录 - -### 相关文档 - -- [Protocol Buffers 官方文档](https://developers.google.com/protocol-buffers) -- [ROS2 接口设计指南](https://docs.ros.org/) - -### 修订记录 - -| 版本 | 日期 | 说明 | -|:-----|:-----|:-----| -| v1.0 | 2026-03-10 | 初始版本，定义 7 个核心 proto 职责 | - ---- - -*本文档遵循 [Markdown 最佳实践](https://www.markdownguide.org/basic-syntax/) 编写。* \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/src/brain_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/src/brain_node.cpp deleted file mode 100644 index d6a9e1f..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/src/brain_node.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#include -#include -// 包含刚才 CMake 自动生成的头文件 -#include "agv_calib_control.grpc.pb.h" - -using namespace agv::calibration::control; - -class BrainNode : public rclcpp::Node { -public: - BrainNode() : Node("calib_brain_node") { - // 1. 拨号连接到 Windows 车端 (请替换为车端在 Wi-Fi 6 下的真实 IP) - std::string target_ip = "192.168.1.100:50051"; - - // 🚨 Wi-Fi 防断联核心：强制开启 gRPC 保活机制 (KeepAlive) - grpc::ChannelArguments args; - args.SetInt(GRPC_ARG_KEEPALIVE_TIME_MS, 2000); // 每 2 秒发一次 TCP 心跳探活 - args.SetInt(GRPC_ARG_KEEPALIVE_TIMEOUT_MS, 1000); // 1 秒没回音即认为掉线 - args.SetInt(GRPC_ARG_KEEPALIVE_PERMIT_WITHOUT_CALLS, 1); - - auto channel = grpc::CreateCustomChannel(target_ip, grpc::InsecureChannelCredentials(), args); - stub_ = AgvCalibControlService::NewStub(channel); - - RCLCPP_INFO(this->get_logger(), "连接车端 %s 中...", target_ip.c_str()); - - // 2. 发起夺权指令测试 - send_control_mode_request(); - } - -private: - std::unique_ptr stub_; - - void send_control_mode_request() { - ModeRequest request; - request.set_target_mode(ModeRequest::OPEN_LOOP_MODE); // 下发开环提线木偶模式 - - StandardResponse response; - grpc::ClientContext context; - - // 【网络防卡死】设置单次通信的超时时间为 2 秒，防止死锁 ROS 2 主线程 - context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2)); - - grpc::Status status = stub_->SetControlMode(&context, request, &response); - - if (status.ok() && response.success()) { - RCLCPP_INFO(this->get_logger(), "✅ 夺权成功！车端回复: %s", response.message().c_str()); - } else { - RCLCPP_ERROR(this->get_logger(), "❌ 调用失败: %d - %s", status.error_code(), status.error_message().c_str()); - } - } -}; - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/src/master_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/src/master_node.cpp deleted file mode 100644 index f637f03..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/src/master_node.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("master_node"); - - RCLCPP_INFO(node->get_logger(), "🚀 AGV 标定主控大脑编译成功，gRPC 契约已挂载！"); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/Heartbeat.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/Heartbeat.srv deleted file mode 100644 index b6ca9cf..0000000 --- a/agv_calib_brain/src/win_ubuntu_bridge/srv/Heartbeat.srv +++ /dev/null @@ -1,24 +0,0 @@ -# ========================================================= -# 文件作用：心跳服务 -# 对应 proto：HeartbeatRequest + HeartbeatResponse -# 作用：保持 Linux 与 Windows 车端代理 / Linux 子服务之间的在线状态 -# 调用方：Ubuntu 车间电脑 或 上位服务 -# 服务方：Windows 车端代理 / Linux 子服务 -# ========================================================= - -# ========================= -# 请求部分 -# ========================= -RequestHeader header # 请求头 -string agent_name # 服务名 / 代理名 -int32 expect_next_heartbeat_ms # 下次期望心跳间隔（ms） - ---- -# ========================= -# 响应部分 -# ========================= -bool success # 是否正常 -uint16 error_code # 错误码，取值参考 ErrorCode.msg -string message # 说明文字 -int64 server_timestamp_us # 响应时间戳 -bool vehicle_ready # 车辆 / 服务是否准备好接任务 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/CMakeLists.txt new file mode 100644 index 0000000..95848f1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.8) +project(vehicle_agent_gateway) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(calibration_common_interfaces REQUIRED) +find_package(calibration_chassis_interfaces REQUIRED) +find_package(calibration_control_interfaces REQUIRED) +find_package(nlohmann_json REQUIRED) + +# 共享库：包含帧协议层、TCP 客户端、两个网关节点组件 +add_library(vehicle_agent_gateway_component SHARED + src/proto_frame.cpp + src/tcp_client.cpp + src/chassis_bridge_node.cpp + src/control_bridge_node.cpp +) + +target_include_directories(vehicle_agent_gateway_component PUBLIC + $ + $ +) + +ament_target_dependencies(vehicle_agent_gateway_component + rclcpp + rclcpp_action + rclcpp_components + calibration_common_interfaces + calibration_chassis_interfaces + calibration_control_interfaces +) + +target_link_libraries(vehicle_agent_gateway_component + nlohmann_json::nlohmann_json +) + +rclcpp_components_register_nodes(vehicle_agent_gateway_component + "vehicle_agent_gateway::ChassisBridgeNode" + "vehicle_agent_gateway::ControlBridgeNode" +) + +# 可执行：同进程启动两个节点 +add_executable(vehicle_agent_gateway_node src/main.cpp) +ament_target_dependencies(vehicle_agent_gateway_node + rclcpp + rclcpp_components +) +target_link_libraries(vehicle_agent_gateway_node + vehicle_agent_gateway_component +) + +install(TARGETS + vehicle_agent_gateway_component + vehicle_agent_gateway_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +install(DIRECTORY scripts/ + DESTINATION share/${PROJECT_NAME}/scripts +) + +ament_package() diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/chassis_bridge_node.hpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/chassis_bridge_node.hpp new file mode 100644 index 0000000..08e8849 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/chassis_bridge_node.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_chassis_interfaces/action/execute_motion_primitive.hpp" +#include "calibration_chassis_interfaces/srv/get_chassis_readiness.hpp" + +#include "vehicle_agent_gateway/tcp_client.hpp" + +namespace vehicle_agent_gateway +{ + +// 底盘网关节点： +// 1. 暴露与 chassis_calibration_service 完全相同的 ROS2 接口，orchestrator 零感知。 +// 2. 收到请求后通过 TcpClient 发帧到 Windows 车端，等待响应后回填 ROS2 结果。 +// 3. 当前 payload 为空占位；后续接入 protobuf 序列化时只需修改本文件的序列化/反序列化部分。 +class ChassisBridgeNode : public rclcpp::Node +{ +public: + explicit ChassisBridgeNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using ReadinessSrv = calibration_chassis_interfaces::srv::GetChassisReadiness; + using ExecuteTask = calibration_chassis_interfaces::action::ExecuteMotionPrimitive; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + rclcpp::Service::SharedPtr readiness_service_; + rclcpp_action::Server::SharedPtr execute_task_server_; + + std::string chassis_host_; + uint16_t chassis_port_; + int timeout_ms_; + + void handle_readiness( + const std::shared_ptr request, + std::shared_ptr response); + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle); + + void handle_accepted(const std::shared_ptr goal_handle); + + void execute_goal(const std::shared_ptr goal_handle); +}; + +} // namespace vehicle_agent_gateway diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/control_bridge_node.hpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/control_bridge_node.hpp new file mode 100644 index 0000000..14eec14 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/control_bridge_node.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "calibration_control_interfaces/action/execute_controller_evaluation.hpp" +#include "calibration_control_interfaces/srv/get_control_readiness.hpp" + +#include "vehicle_agent_gateway/tcp_client.hpp" + +namespace vehicle_agent_gateway +{ + +// 运控网关节点： +// 1. 暴露与 control_calibration_service 完全相同的 ROS2 接口，orchestrator 零感知。 +// 2. 收到请求后通过 TcpClient 发帧到 Windows 车端，等待响应后回填 ROS2 结果。 +// 3. 当前 payload 为空占位；后续接入 protobuf 序列化时只需修改序列化/反序列化部分。 +class ControlBridgeNode : public rclcpp::Node +{ +public: + explicit ControlBridgeNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using ReadinessSrv = calibration_control_interfaces::srv::GetControlReadiness; + using ExecuteTask = calibration_control_interfaces::action::ExecuteControllerEvaluation; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + rclcpp::Service::SharedPtr readiness_service_; + rclcpp_action::Server::SharedPtr execute_task_server_; + + std::string control_host_; + uint16_t control_port_; + int timeout_ms_; + + void handle_readiness( + const std::shared_ptr request, + std::shared_ptr response); + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle); + + void handle_accepted(const std::shared_ptr goal_handle); + + void execute_goal(const std::shared_ptr goal_handle); +}; + +} // namespace vehicle_agent_gateway diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/gateway_codec.hpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/gateway_codec.hpp new file mode 100644 index 0000000..7d9feb1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/gateway_codec.hpp @@ -0,0 +1,383 @@ +#pragma once + +// gateway_codec.hpp +// 作用：vehicle_agent_gateway 的 JSON 序列化/反序列化工具函数。 +// +// 设计原则： +// 1. 只依赖 nlohmann/json 和 ROS2 消息头，不引入 protobuf 运行时。 +// 2. JSON key 命名完全对齐 proto 字段名，方便未来直接切换 protobuf 二进制。 +// 3. 每个函数都是纯函数，不持有状态。 +// +// 帧 payload 格式：UTF-8 JSON 字符串，不含 BOM。 + +#include +#include + +#include "calibration_chassis_interfaces/msg/motion_primitive_request.hpp" +#include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" +#include "calibration_chassis_interfaces/msg/chassis_job_result.hpp" +#include "calibration_control_interfaces/msg/controller_evaluation_request.hpp" +#include "calibration_control_interfaces/msg/control_readiness_response.hpp" +#include "calibration_control_interfaces/msg/control_job_result.hpp" +#include "calibration_common_interfaces/msg/agent_readiness_request.hpp" + +namespace vehicle_agent_gateway +{ +namespace codec +{ + +// ========================================================= +// 公共辅助：ErrorCode +// ========================================================= +inline nlohmann::json encode_error_code( + const calibration_common_interfaces::msg::ErrorCode & ec) +{ + return nlohmann::json{{"code", ec.code}}; +} + +inline void decode_error_code( + const nlohmann::json & j, + calibration_common_interfaces::msg::ErrorCode & ec) +{ + ec.code = j.value("code", static_cast(0)); +} + +// ========================================================= +// 公共辅助：RequestHeader +// ========================================================= +inline nlohmann::json encode_request_header( + const calibration_common_interfaces::msg::RequestHeader & h) +{ + return nlohmann::json{ + {"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} + }; +} + +// ========================================================= +// 公共辅助：FileReference +// ========================================================= +inline calibration_common_interfaces::msg::FileReference decode_file_reference( + const nlohmann::json & j) +{ + calibration_common_interfaces::msg::FileReference ref; + ref.file_name = j.value("file_name", std::string{}); + ref.file_uri = j.value("file_uri", std::string{}); + ref.size_bytes = j.value("size_bytes", int64_t{0}); + ref.description = j.value("description", std::string{}); + return ref; +} + +// ========================================================= +// 底盘：MotionPrimitiveRequest → JSON +// ========================================================= +inline std::string encode_motion_primitive_request( + const calibration_chassis_interfaces::msg::MotionPrimitiveRequest & req) +{ + using namespace calibration_chassis_interfaces::msg; + + nlohmann::json j; + j["header"] = encode_request_header(req.header); + j["test_case_id"] = req.test_case_id; + j["task_purpose"] = req.task_purpose.value; + j["selected_primitive"] = req.selected_primitive.value; + j["brake_when_finished"] = req.brake_when_finished; + j["timeout_sec"] = req.timeout_sec; + j["source_iteration_id"] = req.source_iteration_id; + + // 根据实际选择的原语类型只序列化对应字段，其余省略。 + switch (req.selected_primitive.value) { + case ChassisMotionPrimitiveType::STRAIGHT_LINE: + j["straight_line"] = { + {"target_speed_ms", req.straight_line.target_speed_ms}, + {"target_distance_m", req.straight_line.target_distance_m}, + {"reverse", req.straight_line.reverse} + }; + break; + case ChassisMotionPrimitiveType::ARC: + j["arc"] = { + {"target_speed_ms", req.arc.target_speed_ms}, + {"radius_m", req.arc.radius_m}, + {"sweep_angle_deg", req.arc.sweep_angle_deg}, + {"clockwise", req.arc.clockwise} + }; + break; + case ChassisMotionPrimitiveType::IN_PLACE_ROTATION: + j["in_place_rotation"] = { + {"target_yaw_deg", req.in_place_rotation.target_yaw_deg}, + {"target_angular_vel_deg_s", req.in_place_rotation.target_angular_vel_deg_s} + }; + break; + case ChassisMotionPrimitiveType::STEERING_SWEEP: + j["steering_sweep"] = { + {"target_angle_deg", req.steering_sweep.target_angle_deg}, + {"sweep_amplitude_deg", req.steering_sweep.sweep_amplitude_deg}, + {"sweep_frequency_hz", req.steering_sweep.sweep_frequency_hz}, + {"duration_sec", req.steering_sweep.duration_sec} + }; + break; + case ChassisMotionPrimitiveType::LATERAL_TRANSLATION: + j["lateral_translation"] = { + {"target_speed_ms", req.lateral_translation.target_speed_ms}, + {"target_distance_m", req.lateral_translation.target_distance_m}, + {"move_left", req.lateral_translation.move_left} + }; + break; + case ChassisMotionPrimitiveType::DIAGONAL_MOTION: + j["diagonal_motion"] = { + {"target_speed_ms", req.diagonal_motion.target_speed_ms}, + {"target_distance_m", req.diagonal_motion.target_distance_m}, + {"heading_deg", req.diagonal_motion.heading_deg} + }; + break; + case ChassisMotionPrimitiveType::MODULE_ALIGNMENT: + j["module_alignment"] = { + {"module_ids", req.module_alignment.module_ids}, + {"target_zero_deg", req.module_alignment.target_zero_deg}, + {"tolerance_deg", req.module_alignment.tolerance_deg} + }; + break; + case ChassisMotionPrimitiveType::COORDINATED_STEERING: + j["coordinated_steering"] = { + {"module_ids", req.coordinated_steering.module_ids}, + {"target_angle_deg", req.coordinated_steering.target_angle_deg}, + {"hold_time_sec", req.coordinated_steering.hold_time_sec} + }; + break; + default: + break; + } + + return j.dump(); +} + +// ========================================================= +// 底盘：AgentReadinessRequest → JSON +// ========================================================= +inline std::string encode_chassis_readiness_request( + const calibration_common_interfaces::msg::AgentReadinessRequest & req) +{ + nlohmann::json j; + j["agent_name"] = req.agent_name; + j["include_details"] = req.include_details; + nlohmann::json ids = nlohmann::json::array(); + for (const auto & id : req.target_resource_ids) { ids.push_back(id); } + j["target_resource_ids"] = ids; + return j.dump(); +} + +// ========================================================= +// 运控：AgentReadinessRequest → JSON +// ========================================================= +inline std::string encode_control_readiness_request( + const calibration_common_interfaces::msg::AgentReadinessRequest & req) +{ + nlohmann::json j; + j["agent_name"] = req.agent_name; + j["include_details"] = req.include_details; + nlohmann::json ids = nlohmann::json::array(); + for (const auto & id : req.target_resource_ids) { ids.push_back(id); } + j["target_resource_ids"] = ids; + return j.dump(); +} + +// ========================================================= +// 底盘：JSON → ChassisReadinessResponse +// ========================================================= +inline void decode_chassis_readiness_response( + const std::string & payload, + calibration_chassis_interfaces::msg::ChassisReadinessResponse & rsp) +{ + if (payload.empty()) return; + try { + auto j = nlohmann::json::parse(payload); + rsp.success = j.value("success", false); + rsp.message = j.value("message", std::string{}); + rsp.agent_ready = j.value("agent_ready", false); + rsp.chassis_driver_online = j.value("chassis_driver_online", false); + rsp.motion_control_ready = j.value("motion_control_ready", false); + rsp.estop_released = j.value("estop_released", false); + rsp.vehicle_safe_to_move = j.value("vehicle_safe_to_move", false); + rsp.telemetry_available = j.value("telemetry_available", false); + rsp.checked_timestamp_us = j.value("checked_timestamp_us", int64_t{0}); + if (j.contains("error_code")) { + decode_error_code(j["error_code"], rsp.error_code); + } + } catch (...) { + // payload 解析失败保持调用方已设置的默认值不变 + } +} + +// ========================================================= +// 底盘：JSON → ChassisJobResult +// ========================================================= +inline void decode_chassis_job_result( + const std::string & payload, + calibration_chassis_interfaces::msg::ChassisJobResult & result) +{ + if (payload.empty()) return; + try { + auto j = nlohmann::json::parse(payload); + result.success = j.value("success", false); + result.message = j.value("message", std::string{}); + result.job_id = j.value("job_id", std::string{}); + result.data_quality_passed = j.value("data_quality_passed", false); + result.suitable_for_commit = j.value("suitable_for_commit", false); + result.recommended_parameter_version = j.value("recommended_parameter_version", std::string{}); + result.estimated_straight_line_bias = j.value("estimated_straight_line_bias", 0.0); + if (j.contains("error_code")) { + decode_error_code(j["error_code"], result.error_code); + } + if (j.contains("validation_summary")) { + auto & vs = j["validation_summary"]; + result.validation_summary.max_lateral_error_m = vs.value("max_lateral_error_m", 0.0); + result.validation_summary.max_yaw_error_rad = vs.value("max_yaw_error_rad", 0.0); + result.validation_summary.rms_lateral_error_m = vs.value("rms_lateral_error_m", 0.0); + result.validation_summary.rms_yaw_error_rad = vs.value("rms_yaw_error_rad", 0.0); + result.validation_summary.repeatability_error_m = vs.value("repeatability_error_m", 0.0); + result.validation_summary.curvature_error = vs.value("curvature_error", 0.0); + result.validation_summary.module_consistency_error = vs.value("module_consistency_error", 0.0); + result.validation_summary.auto_acceptance_passed = vs.value("auto_acceptance_passed", false); + } + if (j.contains("artifacts") && j["artifacts"].is_array()) { + for (const auto & a : j["artifacts"]) { + result.artifacts.push_back(decode_file_reference(a)); + } + } + } catch (...) {} +} + +// ========================================================= +// 运控：ControllerEvaluationRequest → JSON +// ========================================================= +inline std::string encode_controller_evaluation_request( + const calibration_control_interfaces::msg::ControllerEvaluationRequest & req) +{ + using namespace calibration_control_interfaces::msg; + + nlohmann::json j; + j["header"] = encode_request_header(req.header); + j["test_case_id"] = req.test_case_id; + j["task_purpose"] = req.task_purpose.value; + j["selected_task"] = req.selected_task.value; + j["source_iteration_id"] = req.source_iteration_id; + + switch (req.selected_task.value) { + case ControllerEvaluationTaskType::TRAJECTORY_TRACKING: { + nlohmann::json path_arr = nlohmann::json::array(); + for (const auto & pt : req.trajectory_tracking.path) { + path_arr.push_back({ + {"x_m", pt.x_m}, + {"y_m", pt.y_m}, + {"yaw_rad", pt.yaw_rad}, + {"target_speed_ms", pt.target_speed_ms} + }); + } + j["trajectory_tracking"] = { + {"path", path_arr}, + {"stop_at_end", req.trajectory_tracking.stop_at_end}, + {"timeout_sec", req.trajectory_tracking.timeout_sec} + }; + break; + } + case ControllerEvaluationTaskType::VELOCITY_STEP: + j["velocity_step"] = { + {"target_velocity_ms", req.velocity_step.target_velocity_ms}, + {"hold_time_sec", req.velocity_step.hold_time_sec}, + {"settle_before_step_sec", req.velocity_step.settle_before_step_sec} + }; + break; + case ControllerEvaluationTaskType::ACCELERATION_DECELERATION: + j["accel_decel"] = { + {"start_velocity_ms", req.accel_decel.start_velocity_ms}, + {"target_velocity_ms", req.accel_decel.target_velocity_ms}, + {"target_accel_ms2", req.accel_decel.target_accel_ms2}, + {"hold_time_sec", req.accel_decel.hold_time_sec} + }; + break; + case ControllerEvaluationTaskType::STOP_ACCURACY: + j["stop_accuracy"] = { + {"target_stop_x_m", req.stop_accuracy.target_stop_x_m}, + {"target_stop_y_m", req.stop_accuracy.target_stop_y_m}, + {"target_stop_yaw_rad", req.stop_accuracy.target_stop_yaw_rad}, + {"timeout_sec", req.stop_accuracy.timeout_sec} + }; + break; + default: + break; + } + + return j.dump(); +} + +// ========================================================= +// 运控：JSON → ControlReadinessResponse +// ========================================================= +inline void decode_control_readiness_response( + const std::string & payload, + calibration_control_interfaces::msg::ControlReadinessResponse & rsp) +{ + if (payload.empty()) return; + try { + auto j = nlohmann::json::parse(payload); + rsp.success = j.value("success", false); + rsp.message = j.value("message", std::string{}); + rsp.agent_ready = j.value("agent_ready", false); + rsp.trajectory_executor_ready = j.value("trajectory_executor_ready", false); + rsp.vehicle_feedback_ready = j.value("vehicle_feedback_ready", false); + rsp.control_output_ready = j.value("control_output_ready", false); + rsp.estop_released = j.value("estop_released", false); + rsp.vehicle_safe_to_move = j.value("vehicle_safe_to_move", false); + rsp.checked_timestamp_us = j.value("checked_timestamp_us", int64_t{0}); + if (j.contains("error_code")) { + decode_error_code(j["error_code"], rsp.error_code); + } + } catch (...) {} +} + +// ========================================================= +// 运控：JSON → ControlJobResult +// ========================================================= +inline void decode_control_job_result( + const std::string & payload, + calibration_control_interfaces::msg::ControlJobResult & result) +{ + if (payload.empty()) return; + try { + auto j = nlohmann::json::parse(payload); + result.success = j.value("success", false); + result.message = j.value("message", std::string{}); + result.job_id = j.value("job_id", std::string{}); + result.data_quality_passed = j.value("data_quality_passed", false); + result.suitable_for_commit = j.value("suitable_for_commit", false); + result.recommended_parameter_version = j.value("recommended_parameter_version", std::string{}); + if (j.contains("error_code")) { + decode_error_code(j["error_code"], result.error_code); + } + if (j.contains("validation_summary")) { + auto & vs = j["validation_summary"]; + result.validation_summary.rms_lateral_error_m = vs.value("rms_lateral_error_m", 0.0); + result.validation_summary.rms_heading_error_rad = vs.value("rms_heading_error_rad", 0.0); + result.validation_summary.rms_speed_error_ms = vs.value("rms_speed_error_ms", 0.0); + result.validation_summary.overshoot_ratio = vs.value("overshoot_ratio", 0.0); + result.validation_summary.settle_time_sec = vs.value("settle_time_sec", 0.0); + result.validation_summary.stop_position_error_m = vs.value("stop_position_error_m", 0.0); + result.validation_summary.max_jerk = vs.value("max_jerk", 0.0); + result.validation_summary.saturation_ratio = vs.value("saturation_ratio", 0.0); + result.validation_summary.auto_acceptance_passed = vs.value("auto_acceptance_passed", false); + } + if (j.contains("artifacts") && j["artifacts"].is_array()) { + for (const auto & a : j["artifacts"]) { + result.artifacts.push_back(decode_file_reference(a)); + } + } + } catch (...) {} +} + +} // namespace codec +} // namespace vehicle_agent_gateway diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/proto_frame.hpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/proto_frame.hpp new file mode 100644 index 0000000..d2c20d0 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/proto_frame.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +namespace vehicle_agent_gateway +{ + +// 消息帧类型枚举 +// 帧格式：[4字节LE: msg_type][4字节LE: payload_len][payload_len字节: payload] +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, +}; + +// 向 fd 发送一帧。payload 可以为空（长度字段写 0）。 +// 返回 true 表示发送成功，false 表示 write 失败。 +bool send_frame(int fd, MsgType type, const std::string & payload); + +// 从 fd 读取一帧，结果写入 out_type 和 out_payload。 +// 返回 true 表示读取成功，false 表示读取失败（连接断开或超时）。 +bool recv_frame(int fd, MsgType & out_type, std::string & out_payload); + +} // namespace vehicle_agent_gateway diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/tcp_client.hpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/tcp_client.hpp new file mode 100644 index 0000000..2130adf --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/include/vehicle_agent_gateway/tcp_client.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include "vehicle_agent_gateway/proto_frame.hpp" + +namespace vehicle_agent_gateway +{ + +// 同步阻塞 TCP 客户端。 +// 每次调用 send_recv 都在调用线程内完成：connect → send REQ帧 → recv RSP帧 → disconnect。 +// 设计为短连接，每次请求独立建连，避免连接状态管理复杂度。 +class TcpClient +{ +public: + TcpClient(const std::string & host, uint16_t port, int timeout_ms = 5000); + + // 发送一个 REQ 帧并等待对应的 RSP 帧。 + // req_type：请求帧类型；req_payload：请求 payload（可为空）。 + // rsp_type：期望的响应帧类型；rsp_payload：响应 payload 输出。 + // 返回 false 表示连接失败、发送失败、超时或帧类型不符。 + bool send_recv( + MsgType req_type, + const std::string & req_payload, + MsgType rsp_type, + std::string & rsp_payload); + +private: + std::string host_; + uint16_t port_; + int timeout_ms_; + + // 建立 TCP 连接，返回 socket fd，失败返回 -1。 + int connect_to_server() const; +}; + +} // namespace vehicle_agent_gateway diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/launch/vehicle_agent_gateway.launch.py b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/launch/vehicle_agent_gateway.launch.py new file mode 100644 index 0000000..abe3952 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/launch/vehicle_agent_gateway.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="vehicle_agent_gateway", + executable="vehicle_agent_gateway_node", + name="vehicle_agent_gateway", + output="screen", + parameters=[{ + # Windows 车端 IP，实际部署时通过命令行覆盖： + # ros2 launch vehicle_agent_gateway vehicle_agent_gateway.launch.py chassis_host:=192.168.x.x + "chassis_host": "192.168.1.100", + "chassis_port": 9001, + "chassis_timeout_ms": 5000, + "control_host": "192.168.1.100", + "control_port": 9002, + "control_timeout_ms": 5000, + }], + ), + ]) diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/package.xml new file mode 100644 index 0000000..47c99c7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/package.xml @@ -0,0 +1,24 @@ + + + vehicle_agent_gateway + 0.0.1 + Ubuntu 侧 WiFi 网关节点：把 ROS2 底盘/运控接口通过 TCP 帧转发到 Windows 车端代理。 + you + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + calibration_common_interfaces + calibration_chassis_interfaces + calibration_control_interfaces + + launch + launch_ros + + + ament_cmake + + 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 new file mode 100644 index 0000000..68797ff --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/scripts/windows_stub_server.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 +# Windows 侧 TCP stub server（本机联调用） +# 用途：模拟 Windows 车端代理，收到任意帧后回复对应的 RSP 帧（payload 为空，表示成功）。 +# 启动方式：python3 windows_stub_server.py +# 默认监听： +# 9001 端口 —— 底盘域 +# 9002 端口 —— 运控域 + +import socket +import struct +import threading +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 +} + + +def read_exactly(conn, n): + """从 conn 读取恰好 n 个字节，不足则抛出 ConnectionError。""" + buf = b"" + while len(buf) < n: + chunk = conn.recv(n - len(buf)) + if not chunk: + raise ConnectionError("连接已关闭") + buf += chunk + return buf + + +def handle_connection(conn, addr): + logging.info(f"新连接来自 {addr}") + try: + # 读取帧头：8字节（4字节LE类型 + 4字节LE长度） + header = read_exactly(conn, 8) + msg_type, payload_len = struct.unpack_from(" 0 else b"" + logging.info(f"收到帧 type={msg_type} payload_len={payload_len}") + + rsp_type = MSG_TYPE.get(msg_type) + if rsp_type is None: + logging.warning(f"未知帧类型 {msg_type}，忽略") + return + + # 回复 RSP 帧，payload 为空（stub 固定返回成功） + rsp_header = struct.pack(" + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "vehicle_agent_gateway/gateway_codec.hpp" + +namespace vehicle_agent_gateway +{ + +using calibration_common_interfaces::msg::ErrorCode; +using calibration_common_interfaces::msg::JobState; + +ChassisBridgeNode::ChassisBridgeNode(const rclcpp::NodeOptions & options) +: Node("chassis_bridge_node", options) +{ + // 节点参数：Windows 车端 IP 与端口，方便通过 launch 文件或命令行覆盖。 + chassis_host_ = declare_parameter("chassis_host", "192.168.1.100"); + chassis_port_ = static_cast(declare_parameter("chassis_port", 9001)); + timeout_ms_ = declare_parameter("chassis_timeout_ms", 5000); + + readiness_service_ = create_service( + "/chassis/get_readiness", + std::bind( + &ChassisBridgeNode::handle_readiness, this, + std::placeholders::_1, std::placeholders::_2)); + + execute_task_server_ = rclcpp_action::create_server( + this, + "/chassis/execute_motion_primitive", + std::bind(&ChassisBridgeNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ChassisBridgeNode::handle_cancel, this, std::placeholders::_1), + std::bind(&ChassisBridgeNode::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "ChassisBridgeNode 启动，目标: %s:%u", chassis_host_.c_str(), chassis_port_); +} + +void ChassisBridgeNode::handle_readiness( + const std::shared_ptr request, + std::shared_ptr response) +{ + // 序列化 readiness 请求 + const std::string req_payload = codec::encode_chassis_readiness_request(request->request); + + TcpClient client(chassis_host_, chassis_port_, timeout_ms_); + std::string rsp_payload; + const bool ok = client.send_recv( + MsgType::CHASSIS_GET_READINESS_REQ, req_payload, + MsgType::CHASSIS_GET_READINESS_RSP, rsp_payload); + + if (!ok || rsp_payload.empty()) { + response->response.success = false; + response->response.error_code.code = ErrorCode::NETWORK_LOSS; + response->response.message = "底盘网关：无法连接 Windows 车端。"; + response->response.agent_ready = false; + response->response.chassis_driver_online = false; + response->response.motion_control_ready = false; + response->response.estop_released = false; + response->response.vehicle_safe_to_move = false; + response->response.telemetry_available = false; + return; + } + + // 反序列化响应 + codec::decode_chassis_readiness_response(rsp_payload, response->response); +} + +rclcpp_action::GoalResponse ChassisBridgeNode::handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + if (goal->goal.header.request_id.empty()) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ChassisBridgeNode::handle_cancel( + const std::shared_ptr /*goal_handle*/) +{ + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ChassisBridgeNode::handle_accepted(const std::shared_ptr goal_handle) +{ + std::thread(std::bind(&ChassisBridgeNode::execute_goal, this, goal_handle)).detach(); +} + +void ChassisBridgeNode::execute_goal(const std::shared_ptr goal_handle) +{ + // 先回一帧 RUNNING feedback，与 chassis_calibration_service 行为保持一致。 + auto feedback = std::make_shared(); + feedback->feedback.state.state = JobState::RUNNING; + goal_handle->publish_feedback(feedback); + + TcpClient client(chassis_host_, chassis_port_, timeout_ms_); + std::string rsp_payload; + // 序列化 goal → JSON payload 发送，反序列化响应回填 result。 + const std::string req_payload = + codec::encode_motion_primitive_request(goal_handle->get_goal()->goal); + const bool ok = client.send_recv( + MsgType::CHASSIS_MOTION_PRIMITIVE_REQ, req_payload, + MsgType::CHASSIS_MOTION_PRIMITIVE_RSP, rsp_payload); + + auto result = std::make_shared(); + result->result.job_id = goal_handle->get_goal()->goal.header.request_id; + if (ok && !rsp_payload.empty()) { + codec::decode_chassis_job_result(rsp_payload, result->result); + if (result->result.success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } else { + result->result.success = false; + result->result.error_code.code = ErrorCode::NETWORK_LOSS; + result->result.message = "底盘网关：无法连接 Windows 车端，动作原语执行失败。"; + result->result.data_quality_passed = false; + result->result.suitable_for_commit = false; + goal_handle->abort(result); + } +} + +} // namespace vehicle_agent_gateway + +RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_agent_gateway::ChassisBridgeNode) diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/control_bridge_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/control_bridge_node.cpp new file mode 100644 index 0000000..450e8f8 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/control_bridge_node.cpp @@ -0,0 +1,126 @@ +#include "vehicle_agent_gateway/control_bridge_node.hpp" + +#include + +#include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_common_interfaces/msg/job_state.hpp" +#include "calibration_common_interfaces/msg/job_status.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "vehicle_agent_gateway/gateway_codec.hpp" + +namespace vehicle_agent_gateway +{ + +using calibration_common_interfaces::msg::ErrorCode; +using calibration_common_interfaces::msg::JobState; + +ControlBridgeNode::ControlBridgeNode(const rclcpp::NodeOptions & options) +: Node("control_bridge_node", options) +{ + // 节点参数：Windows 车端 IP 与端口，方便通过 launch 文件或命令行覆盖。 + control_host_ = declare_parameter("control_host", "192.168.1.100"); + control_port_ = static_cast(declare_parameter("control_port", 9002)); + timeout_ms_ = declare_parameter("control_timeout_ms", 5000); + + readiness_service_ = create_service( + "/control/get_readiness", + std::bind( + &ControlBridgeNode::handle_readiness, this, + std::placeholders::_1, std::placeholders::_2)); + + execute_task_server_ = rclcpp_action::create_server( + this, + "/control/execute_controller_evaluation", + std::bind(&ControlBridgeNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&ControlBridgeNode::handle_cancel, this, std::placeholders::_1), + std::bind(&ControlBridgeNode::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "ControlBridgeNode 启动，目标: %s:%u", control_host_.c_str(), control_port_); +} + +void ControlBridgeNode::handle_readiness( + const std::shared_ptr request, + std::shared_ptr response) +{ + const std::string req_payload = codec::encode_control_readiness_request(request->request); + + TcpClient client(control_host_, control_port_, timeout_ms_); + std::string rsp_payload; + const bool ok = client.send_recv( + MsgType::CONTROL_GET_READINESS_REQ, req_payload, + MsgType::CONTROL_GET_READINESS_RSP, rsp_payload); + + if (!ok || rsp_payload.empty()) { + response->response.success = false; + response->response.error_code.code = ErrorCode::NETWORK_LOSS; + response->response.message = "运控网关：无法连接 Windows 车端。"; + response->response.agent_ready = false; + response->response.trajectory_executor_ready = false; + response->response.vehicle_feedback_ready = false; + response->response.control_output_ready = false; + response->response.estop_released = false; + response->response.vehicle_safe_to_move = false; + return; + } + + codec::decode_control_readiness_response(rsp_payload, response->response); +} + +rclcpp_action::GoalResponse ControlBridgeNode::handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + if (goal->goal.header.request_id.empty()) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ControlBridgeNode::handle_cancel( + const std::shared_ptr /*goal_handle*/) +{ + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ControlBridgeNode::handle_accepted(const std::shared_ptr goal_handle) +{ + std::thread(std::bind(&ControlBridgeNode::execute_goal, this, goal_handle)).detach(); +} + +void ControlBridgeNode::execute_goal(const std::shared_ptr goal_handle) +{ + // 先回一帧 RUNNING feedback，与 control_calibration_service 行为保持一致。 + auto feedback = std::make_shared(); + feedback->feedback.state.state = JobState::RUNNING; + goal_handle->publish_feedback(feedback); + + TcpClient client(control_host_, control_port_, timeout_ms_); + std::string rsp_payload; + const std::string req_payload = + codec::encode_controller_evaluation_request(goal_handle->get_goal()->goal); + const bool ok = client.send_recv( + MsgType::CONTROL_EVALUATION_REQ, req_payload, + MsgType::CONTROL_EVALUATION_RSP, rsp_payload); + + auto result = std::make_shared(); + result->result.job_id = goal_handle->get_goal()->goal.header.request_id; + if (ok && !rsp_payload.empty()) { + codec::decode_control_job_result(rsp_payload, result->result); + if (result->result.success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } else { + result->result.success = false; + result->result.error_code.code = ErrorCode::NETWORK_LOSS; + result->result.message = "运控网关：无法连接 Windows 车端，控制评估执行失败。"; + result->result.data_quality_passed = false; + result->result.suitable_for_commit = false; + goal_handle->abort(result); + } +} + +} // namespace vehicle_agent_gateway + +RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_agent_gateway::ControlBridgeNode) diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/main.cpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/main.cpp new file mode 100644 index 0000000..30877e2 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/main.cpp @@ -0,0 +1,25 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/component_manager.hpp" + +#include "vehicle_agent_gateway/chassis_bridge_node.hpp" +#include "vehicle_agent_gateway/control_bridge_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + // 同一进程内同时运行底盘和运控网关节点，共享线程池。 + rclcpp::executors::MultiThreadedExecutor executor; + + auto chassis_node = std::make_shared(); + auto control_node = std::make_shared(); + + executor.add_node(chassis_node); + executor.add_node(control_node); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/proto_frame.cpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/proto_frame.cpp new file mode 100644 index 0000000..5ec1e04 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/proto_frame.cpp @@ -0,0 +1,94 @@ +#include "vehicle_agent_gateway/proto_frame.hpp" + +#include +#include + +#include + +namespace vehicle_agent_gateway +{ + +// 以小端字节序把 uint32_t 写入 buf。 +static void write_le32(uint8_t * buf, uint32_t v) +{ + buf[0] = static_cast(v & 0xFF); + buf[1] = static_cast((v >> 8) & 0xFF); + buf[2] = static_cast((v >> 16) & 0xFF); + buf[3] = static_cast((v >> 24) & 0xFF); +} + +// 以小端字节序从 buf 读取 uint32_t。 +static uint32_t read_le32(const uint8_t * buf) +{ + return static_cast(buf[0]) | + (static_cast(buf[1]) << 8) | + (static_cast(buf[2]) << 16) | + (static_cast(buf[3]) << 24); +} + +// 确保把 buf 里所有字节都写到 fd，处理 EINTR 重试。 +static bool write_all(int fd, const uint8_t * buf, size_t len) +{ + size_t written = 0; + while (written < len) { + ssize_t n = ::write(fd, buf + written, len - written); + if (n <= 0) { + return false; + } + written += static_cast(n); + } + return true; +} + +// 确保从 fd 读满 len 字节，处理 EINTR 重试。 +static bool read_all(int fd, uint8_t * buf, size_t len) +{ + size_t got = 0; + while (got < len) { + ssize_t n = ::read(fd, buf + got, len - got); + if (n <= 0) { + return false; + } + got += static_cast(n); + } + return true; +} + +bool send_frame(int fd, MsgType type, const std::string & payload) +{ + // 帧头：8字节（4字节 msg_type + 4字节 payload_len），均小端。 + uint8_t header[8]; + write_le32(header, static_cast(type)); + write_le32(header + 4, static_cast(payload.size())); + + if (!write_all(fd, header, 8)) { + return false; + } + if (!payload.empty()) { + if (!write_all(fd, reinterpret_cast(payload.data()), payload.size())) { + return false; + } + } + return true; +} + +bool recv_frame(int fd, MsgType & out_type, std::string & out_payload) +{ + uint8_t header[8]; + if (!read_all(fd, header, 8)) { + return false; + } + + out_type = static_cast(read_le32(header)); + const uint32_t payload_len = read_le32(header + 4); + + out_payload.resize(payload_len); + if (payload_len > 0) { + if (!read_all(fd, reinterpret_cast(out_payload.data()), payload_len)) { + return false; + } + } + return true; +} + +} // namespace vehicle_agent_gateway diff --git a/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/tcp_client.cpp b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/tcp_client.cpp new file mode 100644 index 0000000..595249e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/vehicle_agent_gateway/src/tcp_client.cpp @@ -0,0 +1,75 @@ +#include "vehicle_agent_gateway/tcp_client.hpp" + +#include +#include +#include +#include + +#include + +namespace vehicle_agent_gateway +{ + +TcpClient::TcpClient(const std::string & host, uint16_t port, int timeout_ms) +: host_(host), port_(port), timeout_ms_(timeout_ms) +{ +} + +int TcpClient::connect_to_server() const +{ + int fd = ::socket(AF_INET, SOCK_STREAM, 0); + if (fd < 0) { + return -1; + } + + // 设置接收超时，避免 recv_frame 永久阻塞。 + struct timeval tv; + tv.tv_sec = timeout_ms_ / 1000; + tv.tv_usec = (timeout_ms_ % 1000) * 1000; + ::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + ::setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)); + + struct sockaddr_in addr; + std::memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + + if (::inet_pton(AF_INET, host_.c_str(), &addr.sin_addr) <= 0) { + ::close(fd); + return -1; + } + + if (::connect(fd, reinterpret_cast(&addr), sizeof(addr)) < 0) { + ::close(fd); + return -1; + } + + return fd; +} + +bool TcpClient::send_recv( + MsgType req_type, + const std::string & req_payload, + MsgType rsp_type, + std::string & rsp_payload) +{ + int fd = connect_to_server(); + if (fd < 0) { + return false; + } + + bool ok = send_frame(fd, req_type, req_payload); + if (ok) { + MsgType recv_type; + ok = recv_frame(fd, recv_type, rsp_payload); + // 校验响应帧类型是否符合预期。 + if (ok && recv_type != rsp_type) { + ok = false; + } + } + + ::close(fd); + return ok; +} + +} // namespace vehicle_agent_gateway diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/CMakeLists.txt new file mode 100644 index 0000000..a4a26f4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(win_ubuntu_bridge) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +# 安装 launch 文件 +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +# 安装文档（可选） +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/docs") + install(DIRECTORY docs/ + DESTINATION share/${PROJECT_NAME}/docs + ) +endif() + +# 安装 proto 归档（可选） +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/proto") + install(DIRECTORY proto/ + DESTINATION share/${PROJECT_NAME}/proto + ) +endif() + + +ament_package() \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/docs/images/sequence_diagram.png b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/images/sequence_diagram.png similarity index 100% rename from agv_calib_brain/src/win_ubuntu_bridge/docs/images/sequence_diagram.png rename to agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/images/sequence_diagram.png diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/proto 驱动任务选择图.png b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/proto 驱动任务选择图.png new file mode 100644 index 0000000..f485220 Binary files /dev/null and b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/proto 驱动任务选择图.png differ diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/传感器标定详细流程图.png b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/传感器标定详细流程图.png new file mode 100644 index 0000000..7480b47 Binary files /dev/null and b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/传感器标定详细流程图.png differ diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/底盘标定详细流程图.png b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/底盘标定详细流程图.png new file mode 100644 index 0000000..2844f22 Binary files /dev/null and b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/底盘标定详细流程图.png differ diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/自动化标定车间总控完整流程图.png b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/自动化标定车间总控完整流程图.png new file mode 100644 index 0000000..dd0275d Binary files /dev/null and b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/自动化标定车间总控完整流程图.png differ diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/运控参数调优详细流程图.png b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/运控参数调优详细流程图.png new file mode 100644 index 0000000..dc29b5d Binary files /dev/null and b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/docs/运控参数调优详细流程图.png differ diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/launch/minimal_workshop_demo.launch.py b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/launch/minimal_workshop_demo.launch.py new file mode 100644 index 0000000..5efa949 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/launch/minimal_workshop_demo.launch.py @@ -0,0 +1,100 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +# 最小 demo 拉起完整闭环：车辆画像 + 外部定位 + 传感器 + 底盘/运控（stub 或 WiFi 网关）+ 车间编排。 +# 参数： +# use_gateway (bool, 默认 false)：true 时用 vehicle_agent_gateway 替换本地底盘/运控 stub 节点。 +# chassis_host / control_host：Windows 车端 IP（仅 use_gateway=true 时生效）。 +def generate_launch_description(): + use_gateway_arg = DeclareLaunchArgument( + "use_gateway", + default_value="false", + description="true: 使用 WiFi 网关节点连接 Windows 车端；false: 使用本地 stub 节点", + ) + chassis_host_arg = DeclareLaunchArgument( + "chassis_host", default_value="192.168.1.100", + description="Windows 车端 IP（use_gateway=true 时生效）", + ) + control_host_arg = DeclareLaunchArgument( + "control_host", default_value="192.168.1.100", + description="Windows 车端 IP（use_gateway=true 时生效）", + ) + + def make_nodes(context): + use_gateway = LaunchConfiguration("use_gateway").perform(context).lower() == "true" + chassis_host = LaunchConfiguration("chassis_host").perform(context) + control_host = LaunchConfiguration("control_host").perform(context) + + common_nodes = [ + Node( + package="vehicle_profile_manager", + executable="vehicle_profile_manager_node", + name="vehicle_profile_manager", + output="screen", + ), + Node( + package="external_localization_service", + executable="external_localization_service_node", + name="external_localization_service", + output="screen", + ), + Node( + package="sensor_calibration_service", + executable="sensor_calibration_service_node", + name="sensor_calibration_service", + output="screen", + ), + Node( + package="workshop_orchestrator_v2", + executable="workshop_orchestrator_v2_node", + name="workshop_orchestrator_v2", + output="screen", + ), + ] + + if use_gateway: + # WiFi 网关模式：底盘和运控通过 TCP 转发到 Windows 车端。 + chassis_control_nodes = [ + Node( + package="vehicle_agent_gateway", + executable="vehicle_agent_gateway_node", + name="vehicle_agent_gateway", + output="screen", + parameters=[{ + "chassis_host": chassis_host, + "chassis_port": 9001, + "chassis_timeout_ms": 5000, + "control_host": control_host, + "control_port": 9002, + "control_timeout_ms": 5000, + }], + ), + ] + else: + # 本地 stub 模式：底盘和运控使用本地 stub 节点，无需 Windows 连接。 + chassis_control_nodes = [ + Node( + package="chassis_calibration_service", + executable="chassis_calibration_service_node", + name="chassis_calibration_service", + output="screen", + ), + Node( + package="control_calibration_service", + executable="control_calibration_service_node", + name="control_calibration_service", + output="screen", + ), + ] + + return common_nodes + chassis_control_nodes + + return LaunchDescription([ + use_gateway_arg, + chassis_host_arg, + control_host_arg, + OpaqueFunction(function=make_nodes), + ]) diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/package.xml new file mode 100644 index 0000000..c287c01 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/package.xml @@ -0,0 +1,27 @@ + + + win_ubuntu_bridge + 0.0.1 + Launch, docs and integration package for AutoCalib Workshop. + + you + Apache-2.0 + + ament_cmake + + + launch + launch_ros + + + vehicle_profile_manager + external_localization_service + chassis_calibration_service + control_calibration_service + sensor_calibration_service + workshop_orchestrator_v2 + + + ament_cmake + + \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/README.md b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/README.md new file mode 100644 index 0000000..0999a49 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/README.md @@ -0,0 +1,749 @@ +# 自动化标定车间 Proto 协同工作说明 + +[![Protocol](https://img.shields.io/badge/Protocol-Protocol%20Buffers-blue)](https://developers.google.com/protocol-buffers) +[![Platform](https://img.shields.io/badge/Platform-Ubuntu%20%7C%20Linux%20%7C%20Windows-green)](https://ubuntu.com) +[![Status](https://img.shields.io/badge/Status-Proto%20Frozen%20Candidate-orange)](./) + +> **核心定位**：面向自动化标定车间的分层协议设计，用 7 个 `.proto` 把“车间总控、车辆画像、真值参考、底盘标定、传感器标定、运控调优、参数归档”串成一条可执行、可追溯、可扩展的自动化流程。 + +--- + +## 目录 + +- [1. 文档目的](#1-文档目的) +- [2. 系统边界与角色分工](#2-系统边界与角色分工) +- [3. 七个 Proto 文件的职责划分](#3-七个-proto-文件的职责划分) +- [4. 七个 Proto 的依赖关系](#4-七个-proto-的依赖关系) +- [5. 自动化标定车间完整流程](#5-自动化标定车间完整流程) +- [6. 七个 Proto 在主流程中的协同方式](#6-七个-proto-在主流程中的协同方式) +- [7. 当前协议设计已经明确的关键原则](#7-当前协议设计已经明确的关键原则) +- [8. 冻结前建议补齐的小修补项](#8-冻结前建议补齐的小修补项) +- [9. 从 Proto 到 ROS2 的映射建议](#9-从-proto-到-ros2-的映射建议) +- [10. 工程实现建议](#10-工程实现建议) +- [11. 总结](#11-总结) + +--- + +## 1. 文档目的 + +本文档用于说明当前自动化标定车间中 **7 个核心 `.proto` 文件** 的职责边界、依赖关系、协同流程和工程落地方式。 + +这套协议不是为了把所有逻辑堆到一个服务里，而是为了把整个系统拆成清晰的几层： + +- **车间总控层**：只负责编排、状态推进、审批、回滚、归档 +- **专业域服务层**：分别处理真值参考、底盘、传感器、运控调优 +- **车端代理层**：只负责动作执行、数据采集、参数写入、状态回传 +- **公共协议层**：统一请求头、错误码、心跳、长任务状态、文件引用、位姿/向量类型 + +这份 README 的目标是让后续做 ROS2 映射、节点架构设计、C++ 服务实现时，所有人对协议语义有同一个理解。 + +--- + +## 2. 系统边界与角色分工 + +### 2.1 Ubuntu / Linux 车间电脑 + +负责： + +- 读取车辆画像与工位配置 +- 创建整车标定会话 +- 生成执行计划与阶段顺序 +- 调用各专项服务完成数据采集和参数求解 +- 做自动验收、审批流、失败回退、结果归档 + +### 2.2 Windows 车端代理 + +负责： + +- 接收车间电脑下发的动作命令 +- 执行底盘动作、轨迹跟踪、停车、走位等任务 +- 采集车辆状态、传感器状态与原始数据 +- 写入已经求解好的参数 +- 回传执行状态与遥测数据 + +### 2.3 车间基础设施 + +包括但不限于： + +- 外部定位 / 真值参考系统 +- 标定板、标靶、地面参考目标 +- 安全联锁、急停、区域清场机制 +- 车间网络、存储、数据库、日志系统 + +### 2.4 协议设计原则 + +- **Linux 负责求解和编排，不负责直接驱动硬件细节** +- **Windows 负责执行和采集，不负责复杂求解与全局决策** +- **所有阶段都围绕同一个 `session_id`、`vehicle_id`、`job_id` 协同** +- **所有长任务都统一使用 `JobAccepted / JobStatus / JobResult` 语义** + +--- + +## 3. 七个 Proto 文件的职责划分 + +### 3.1 calibration_common.proto + +**作用：公共基础协议定义。** + +它是整个协议体系的地基，所有其他 proto 都依赖它。 + +当前主要承担： + +- `RequestHeader`：统一会话、任务、车辆、请求追踪信息 +- `ErrorCode`：统一错误码 +- `StandardResponse`：统一短响应 +- `JobAccepted / JobQuery / JobStatus`：统一长任务状态表达 +- `HeartbeatRequest / HeartbeatResponse`：统一保活机制 +- `AgentReadinessRequest / ReadinessIssue`：统一 readiness 检查模式 +- `FileDigest / FileReference`：统一文件摘要与文件引用 +- `Vector3D / Vector3d / Pose3D / Pose3dEuler`：统一三维向量与位姿表达 +- `KeyValuePair`：为后续扩展保留元数据入口 + +**一句话理解**：它不描述任何专项算法，但所有专项服务都要先站在这块地基上。 + +--- + +### 3.2 vehicle_profile.proto + +**作用：车辆画像、能力描述、流程启用配置。** + +它回答的问题不是“怎么标”，而是： + +- 这台车是什么底盘 +- 有哪些传感器 +- 有没有机械臂 +- 当前支持哪些控制器 +- 本轮会话到底启用哪些阶段 +- 本轮横向 / 纵向分别使用什么控制算法 + +当前重点内容包括： + +- `ChassisType` +- `SensorType` +- `CameraMountType` +- `ControlAxisType` +- `ControllerAlgorithmType` +- `CalibrationModuleType` +- `WorkflowStageType` +- `SensorProfile` +- `ControllerProfile` +- `ControllerSelection` +- `VehicleProfile` + +**这一版最关键的升级点**： + +- 已明确拆出 **横向控制器** 与 **纵向控制器** +- 已明确加入 **流程阶段类型 `WorkflowStageType`** +- 已支持 **相机是否做内参 / 是否做外参** 的会话级选择 + +**一句话理解**：它决定“这台车是什么、支持什么、这次打算标什么”。 + +--- + +### 3.3 external_localization.proto + +**作用：外部定位 / 真值参考接入与验证协议。** + +它负责的不是“车上定位算法”，而是车间里的真值参考来源，例如： + +- 动捕系统 +- 外部相机阵列 +- 激光跟踪仪 +- 全站仪 +- 工位级高精度测量系统 + +当前协议已经覆盖： + +- readiness 查询 +- 工作模式切换 +- 能力查询 +- 参考位姿采集 +- 标靶对齐 +- 真值参考验证 +- 外部定位遥测 +- 外部定位结果写入与查询 + +**这一版最关键的升级点**： + +- 已不再只是“能采位姿” +- 已支持 **阶段 0：真值参考接入与确认** +- 已支持验证指标，例如时间同步、覆盖范围、观测质量、跟踪稳定性、丢失比例 + +**一句话理解**：它给整个自动化车间提供“可信真值输入”。 + +--- + +### 3.4 chassis_calibration.proto + +**作用：底盘标定执行、遥测回传、参数写入。** + +它定义的是 Linux 与车端代理之间，关于底盘动作执行和结果回传的协议，而不是底盘参数求解算法本身。 + +当前协议已经覆盖： + +- readiness 查询 +- 工作模式切换 +- 底盘能力查询 +- 动作原语执行 +- 遥测流回传 +- 参数写入与查询 +- 验证摘要与任务结果 + +支持的动作原语包括： + +- 直线 +- 圆弧 +- 原地旋转 +- 舵角扫动 +- 横移 +- 斜移 +- 模块零位检查 +- 模块协同转向 + +**这一版最关键的升级点**： + +- 已覆盖 **阿克曼 / 差速 / 单舵轮 / 多舵轮** +- 已补齐多舵轮细化原语 +- 已增加 `ChassisValidationSummary` +- 已增加 `ChassisJobResult` + +**一句话理解**：它让 Linux 可以命令车辆完成底盘测试，并拿回足够做求解与验收的数据。 + +--- + +### 3.5 control_calibration.proto + +**作用：运控参数调优执行与评估协议。** + +这里的“运控标定”指的是车辆控制器参数调优，而不是底盘几何参数。 + +当前协议已经覆盖： + +- readiness 查询 +- 工作模式切换 +- 参数热加载 +- 评估任务启动 +- 遥测流回传 +- 参数固化 +- 当前参数查询 +- 任务结果与验证摘要 + +支持的评估任务包括： + +- 轨迹跟踪 +- 速度阶跃 +- 加减速 +- 停车精度 + +**这一版最关键的升级点**： + +- 已明确拆分 **横向控制器** 与 **纵向控制器** +- 已将 MPC 参数模型拆为： + - `LateralMPCParams` + - `LongitudinalMPCParams` +- 已支持“横向 MPC + 纵向 PID”这类组合 +- 已增加 `ControlValidationSummary` +- 已增加 `ControlJobResult` + +**一句话理解**：它负责“加载参数、执行测试、回传误差、评估收敛、固化最终控制器参数”。 + +--- + +### 3.6 sensor_calibration.proto + +**作用：传感器标定任务、采集过程、参数结果与验证摘要。** + +覆盖范围包括： + +- 下视相机 +- 前视相机 +- 机械臂相机（眼在手上 / 眼在手外） +- 3D 激光雷达 +- 2D 激光雷达 +- IMU + +当前协议已经覆盖： + +- readiness 查询 +- 工作模式切换 +- 能力查询 +- 相机内参任务 +- IMU 内参任务 +- 传感器到 `base_link` 外参任务 +- 手眼标定任务 +- 遥测流 +- 参数写入与查询 +- 任务结果与验证摘要 + +**这一版最关键的升级点**： + +- 已支持相机 **只做内参 / 只做外参 / 内外参都做** +- 已将 IMU 内参改为三轴强约束： + - `Vector3d accel_bias` + - `Vector3d gyro_bias` +- 已增加 `LASER_SCAN_FILE`，单独表达 2D 激光雷达产物 +- 已增加 `SensorValidationSummary` +- 已增加 `SensorCalibrationJobResult` + +**一句话理解**：它负责把“采什么、怎么采、产出什么参数、怎么判断是否通过”描述完整。 + +--- + +### 3.7 workshop_orchestration.proto + +**作用：整车自动化标定总编排协议。** + +这是整套系统的总控层，不做具体求解，只做流程推进与状态管理。 + +当前协议已经覆盖： + +- 创建会话 +- 查询会话 +- 构建执行计划 +- 重建执行计划 +- 校验执行计划 +- 运行车间预检 +- 启动 / 暂停 / 恢复 / 取消整场会话 +- 事件流推送 +- 人工确认 +- 审批阶段结果 +- 回滚阶段参数 +- 获取最终报告 + +**这一版最关键的升级点**： + +- 已不再只是“创建会话 + 获取状态” +- 已支持 **阶段计划**、**预检**、**人工确认**、**审批**、**回滚**、**最终报告** +- 已通过 `WorkflowStageType` 直接对齐自动化车间主流程 + +**一句话理解**：它是整个自动化标定车间的总导演和状态机主控器。 + +--- + +## 4. 七个 Proto 的依赖关系 + +依赖关系可以概括如下： + +| Proto 文件 | 依赖 | 核心作用 | +|:--|:--|:--| +| `calibration_common.proto` | 无 | 公共消息、错误码、长任务、文件、位姿、readiness | +| `vehicle_profile.proto` | `calibration_common.proto` | 车辆画像、能力声明、控制器选择、流程阶段配置 | +| `external_localization.proto` | `calibration_common.proto` | 真值参考接入、外部定位验证、结果写入 | +| `chassis_calibration.proto` | `calibration_common.proto`、`vehicle_profile.proto` | 底盘动作执行、遥测、验证、参数写入 | +| `control_calibration.proto` | `calibration_common.proto`、`vehicle_profile.proto` | 运控调优执行、遥测、验证、参数写入 | +| `sensor_calibration.proto` | `calibration_common.proto`、`vehicle_profile.proto` | 传感器任务、采集、参数、验证 | +| `workshop_orchestration.proto` | `calibration_common.proto`、`vehicle_profile.proto` | 整场会话编排、计划、审批、回滚、报告 | + +总体依赖方向是： + +```mermaid +flowchart LR + A[calibration_common.proto] --> B[vehicle_profile.proto] + A --> C[external_localization.proto] + A --> D[chassis_calibration.proto] + A --> E[control_calibration.proto] + A --> F[sensor_calibration.proto] + A --> G[workshop_orchestration.proto] + B --> D + B --> E + B --> F + B --> G +``` + +--- + +## 5. 自动化标定车间完整流程 + +### 5.1 车间级前置条件 + +这一部分不属于“每台车都重复执行的主流程”，而是车间基础设施准备： + +- 外部定位系统安装与调试完成 +- 工位网络、时钟同步、数据库、文件存储就绪 +- 标定板 / 标靶 / 地面参考目标 / 安全设施就绪 +- Windows 车端代理和 Linux 侧各服务部署完成 + +### 5.2 单车级自动化标定主流程 + +```mermaid +flowchart TB + A[车辆入场] --> B[创建 Session] + B --> C[加载车辆画像与历史参数] + C --> D[生成执行计划] + D --> E[阶段0 真值参考接入与确认] + E --> F[阶段1 整车预检] + F --> G[阶段2 底盘标定] + G --> H[阶段3 运控参数调优] + H --> I[阶段4 传感器标定] + I --> J[阶段5 整车联合验收] + J --> K[阶段6 参数固化与归档] + K --> L[车辆出场] +``` + +### 5.3 各阶段要点 + +#### 阶段 0：真值参考接入与确认 + +由 `external_localization.proto` 负责： + +- 查询 readiness +- 切换工作模式 +- 做参考位姿采集 +- 执行真值源验证 +- 给出验证摘要 + +这一步的本质不是“重新搭建外部定位系统”，而是： + +> 确认车间里已经搭好的外部定位系统，当前这次会话能不能作为可信真值源使用。 + +#### 阶段 1：整车预检 + +由 `workshop_orchestration.proto` 统一编排： + +- 服务在线检查 +- 资源锁检查 +- 工位安全检查 +- 车辆 readiness 检查 +- 传感器 / 底盘 / 运控 / 外部定位 readiness 检查 + +#### 阶段 2：底盘标定 + +由 `chassis_calibration.proto` 承担执行链路，Linux 做求解： + +- 根据底盘类型选择动作原语 +- 执行动作 +- 采集遥测 +- 求解底盘参数 +- 做自动验收 + +#### 阶段 3：运控参数调优 + +由 `control_calibration.proto` 承担执行链路，Linux 做评估和迭代： + +- 根据 `ControllerSelection` 决定横向 / 纵向控制器组合 +- 加载一版参数 +- 运行轨迹跟踪 / 速度阶跃 / 加减速 / 停车测试 +- 回传遥测 +- 计算误差与收敛指标 +- 判断是否继续迭代或固化参数 + +#### 阶段 4：传感器标定 + +由 `sensor_calibration.proto` 承担采集与结果表达： + +- 根据 `SensorProfile` 决定本轮要标哪些传感器 +- 相机可只做内参、只做外参、或内外参都做 +- 机械臂相机需区分 `EYE_IN_HAND` / `EYE_TO_HAND` +- 求解内参与外参 +- 做自动验收 + +#### 阶段 5：整车联合验收 + +由 `workshop_orchestration.proto` 负责总控，联合各专项结果进行判断： + +- 底盘参数是否有效 +- 运控参数是否有效 +- 传感器参数是否有效 +- 真值参考是否可靠 +- 若失败，回退到对应阶段重跑 + +#### 阶段 6：参数固化与归档 + +- 写入底盘参数 +- 写入控制参数 +- 写入传感器参数 +- 记录文件摘要、版本号、时间戳 +- 生成最终报告 +- 归档所有产物与日志 + +--- + +## 6. 七个 Proto 在主流程中的协同方式 + +### 6.1 协同主图 + +```mermaid +flowchart LR + A[vehicle_profile.proto\n车辆画像] --> B[workshop_orchestration.proto\n生成执行计划] + B --> C[external_localization.proto\n真值参考接入] + B --> D[chassis_calibration.proto\n底盘动作执行] + B --> E[control_calibration.proto\n控制测试执行] + B --> F[sensor_calibration.proto\n传感器采集执行] + + C --> B + D --> B + E --> B + F --> B + + G[calibration_common.proto\n公共消息/状态/错误码] --> A + G --> B + G --> C + G --> D + G --> E + G --> F +``` + +### 6.2 典型协同链路 + +一场完整会话通常按下面这个顺序协同： + +1. `vehicle_profile.proto` 生成车辆画像 +2. `workshop_orchestration.proto` 创建会话并生成阶段计划 +3. `external_localization.proto` 完成真值参考接入与验证 +4. `workshop_orchestration.proto` 运行车间预检 +5. `chassis_calibration.proto` 执行底盘动作与回传遥测 +6. `control_calibration.proto` 执行控制测试与回传遥测 +7. `sensor_calibration.proto` 执行采集与提交参数结果 +8. `workshop_orchestration.proto` 汇总结果，审批、回滚、归档 + +--- + +## 7. 当前协议设计已经明确的关键原则 + +### 7.1 总控只编排，不求解 + +`workshop_orchestration.proto` 不应该塞进底盘求解、相机求解、控制优化算法细节。 + +### 7.2 车端只执行，不决策 + +Windows 代理只负责： + +- 接命令 +- 执行动作 +- 采集数据 +- 写入参数 +- 回传状态 + +不负责全局调度和复杂求解。 + +### 7.3 车辆画像先行 + +没有完整 `VehicleProfile`，就不应该进入任何专项标定任务。 + +### 7.4 流程阶段和模块类型分开表达 + +- `WorkflowStageType` 负责表达流程阶段 +- `CalibrationModuleType` 负责表达模块归属 + +这样总控既能表达“当前在哪个阶段”，又能表达“这个阶段属于哪个模块”。 + +### 7.5 控制器必须区分横向与纵向 + +运控参数调优中，不能再把所有控制器混在一起。当前协议已经明确区分： + +- `LATERAL_CONTROL` +- `LONGITUDINAL_CONTROL` + +### 7.6 相机必须支持内参 / 外参可独立配置 + +当前协议已经支持： + +- 只做相机内参 +- 只做相机外参 +- 内参与外参同时做 + +### 7.7 结果不能只给成功 / 失败 + +当前各专项都已经增加了： + +- `ValidationSummary` +- `JobResult` + +这意味着后续自动验收、报告、回滚都可以基于质量指标，而不是只看一个布尔值。 + +--- + +## 8. 冻结前建议补齐的小修补项 + +当前 7 个 proto 的主结构已经可以冻结，但为了减少后续返工，建议在冻结前补 4 类小修补。 + +### 8.1 给计划 / 结果 / 报告补统一 `metadata` + +建议优先补到： + +- `StagePlan` +- `StageResultSummary` +- `WorkshopReport` +- 各类 `ValidationSummary` +- 各类 `JobResult` + +统一形式： + +```proto +repeated .agv.calibration.common.KeyValuePair metadata = N; +``` + +这样后面想扩展指标、展示信息、附加标签时，不用再改结构。 + +### 8.2 给 external_localization.proto 补 `localization_source_id` / `workcell_zone_id` + +这样未来扩展到多工位、多外部定位源时，不需要再改协议主结构。 + +### 8.3 给 sensor_calibration.proto 补统一 `reference_target` + +建议增加一个通用参考目标消息，例如： + +```proto +message CalibrationReferenceTarget { + string reference_target_id = 1; + string reference_target_type = 2; + string description = 3; +} +``` + +然后让相机内参、传感器外参、手眼标定任务都使用统一的参考目标表达。 + +### 8.4 把角度单位策略写死 + +建议统一约定： + +- **状态量 / 结果量 / 误差量 / 位姿量统一使用 rad** +- **人工输入更直观的命令量 / 限幅量可使用 deg** +- **字段名必须带单位后缀**，例如 `_rad`、`_deg` + +--- + +## 9. 从 Proto 到 ROS2 的映射建议 + +这一步不是立即实现代码，而是给后续 ROS2 转换定规则。 + +### 9.1 msg + +适合映射为 ROS2 `msg` 的内容： + +- 公共结构体 +- 遥测消息 +- 参数结果消息 +- 位姿 / 向量 / 摘要结构 + +例如： + +- `RequestHeader` +- `JobStatus` +- `Pose3dEuler` +- `ChassisTelemetry` +- `ControlTelemetry` +- `SensorCalibrationTelemetry` + +### 9.2 srv + +适合映射为 ROS2 `srv` 的内容： + +- 短同步调用 +- 模式切换 +- 能力查询 +- readiness 查询 +- 当前生效参数查询 + +例如： + +- `SetChassisWorkMode` +- `GetControlReadiness` +- `GetSensorCalibrationCapability` + +### 9.3 action + +适合映射为 ROS2 `action` 的内容： + +- 底盘动作原语执行 +- 外部定位验证任务 +- 控制评估任务 +- 传感器标定任务 +- 整场车间会话执行 + +因为这些任务天然具备： + +- 启动 +- 持续反馈 +- 查询进度 +- 取消 +- 完成结果 + +### 9.4 topic + +适合映射为 ROS2 `topic` 的内容： + +- 各类遥测流 +- 各类事件流 +- 外部定位连续位姿流 +- 标定过程状态广播 + +--- + +## 10. 工程实现建议 + +### 10.1 目录建议 + +```text +proto/ +├── calibration_common.proto +├── vehicle_profile.proto +├── external_localization.proto +├── chassis_calibration.proto +├── control_calibration.proto +├── sensor_calibration.proto +└── workshop_orchestration.proto + +services/ +├── workshop_orchestrator/ +├── vehicle_profile_manager/ +├── external_localization_service/ +├── chassis_calibration_service/ +├── control_calibration_service/ +├── sensor_calibration_service/ +└── vehicle_agent_windows/ + +algorithms/ +├── external_reference_solver/ +├── chassis_solver/ +├── control_tuner/ +└── sensor_solver/ + +reports/ +configs/ +logs/ +``` + +### 10.2 实现优先级建议 + +建议按下面顺序推进： + +1. 先冻结 7 个 proto +2. 再设计 ROS2 的 msg / srv / action 对应关系 +3. 再画节点架构图和 topic / service / action 拓扑 +4. 再实现总控状态机 +5. 再逐个接入专项服务和车端代理 + +### 10.3 最容易踩坑的地方 + +- 把总控写成求解器 +- 把 Windows 代理写得过重 +- 不先做车辆画像，直接开始下发任务 +- 相机内外参逻辑混在一起 +- 横向 / 纵向控制器不分开处理 +- 角度单位在不同层乱用 +- 报告只记录成功 / 失败，不记录质量指标和版本号 + +--- + +## 11. 总结 + +当前这套 7 个 proto 已经不再是“概念方案”，而是一套接近可冻结的自动化标定车间协议主干。 + +它们共同支撑的不是单个算法模块，而是完整的一条自动化流程： + +**车辆画像 → 会话创建 → 真值参考接入 → 车间预检 → 底盘标定 → 运控调优 → 传感器标定 → 整车联合验收 → 参数固化 → 结果归档** + +其中： + +- `calibration_common.proto` 提供统一地基 +- `vehicle_profile.proto` 决定“这台车是谁、支持什么、这次做什么” +- `external_localization.proto` 提供可信真值参考 +- `chassis_calibration.proto` 负责底盘动作执行与参数验证 +- `control_calibration.proto` 负责横纵向控制器调优执行 +- `sensor_calibration.proto` 负责多传感器采集、求解与参数表达 +- `workshop_orchestration.proto` 负责总流程编排、审批、回滚与归档 + +只要把冻结前的小修补项补齐，这套协议就可以正式进入下一阶段： + +**ROS2 接口映射、节点架构设计、C++ 服务实现。** \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/calibration_common.proto b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/calibration_common.proto new file mode 100644 index 0000000..4d44308 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/calibration_common.proto @@ -0,0 +1,266 @@ +syntax = "proto3"; + +package agv.calibration.common; + +// ├── msg/ +// │ ├── Vector3d.msg +// │ ├── Pose3dEuler.msg +// │ ├── RequestHeader.msg +// │ ├── ErrorCode.msg +// │ ├── StandardResponse.msg +// │ ├── JobState.msg +// │ ├── JobAccepted.msg +// │ ├── JobQuery.msg +// │ ├── JobStatus.msg +// │ ├── HeartbeatRequest.msg +// │ ├── HeartbeatResponse.msg +// │ ├── AgentReadinessRequest.msg +// │ ├── ReadinessIssue.msg +// │ ├── FileDigest.msg +// │ ├── FileReference.msg +// │ └── KeyValuePair.msg +// ├── srv/ +// │ ├── Heartbeat.srv +// │ ├── QueryJobStatus.srv +// │ └── GetAgentReadiness.srv +// └── action/ +// └── MonitorJob.action + +// ========================================================= +// 文件作用：自动化标定车间公共协议定义 +// 使用范围： +// 1) Ubuntu 车间电脑内部服务之间 +// 2) Ubuntu 车间电脑 与 Windows 车端代理服务之间 +// 3) 所有专项标定服务的公共基础消息 +// ========================================================= + +// ========================================================= +// 空消息 +// 作用：无参数 RPC 使用 +// ========================================================= +message Empty {} + +// ========================================================= +// 三维向量（兼容命名 1） +// 作用：用于表达三轴偏置、平移量、加速度等三维数据 +// ========================================================= +message Vector3D { + double x = 1; // X 轴分量 + double y = 2; // Y 轴分量 + double z = 3; // Z 轴分量 +} + + +// ========================================================= +// 六自由度位姿（兼容命名 1） +// 作用：用于表达 base_link、传感器、外部定位坐标系之间的位姿关系 +// 说明： +// 1) 平移单位为米 +// 2) 旋转单位为弧度 +// 3) 采用 xyz + rpy 表达，便于工程实现与人工检查 +// ========================================================= +message Pose3D { + double x_m = 1; // X 方向平移（m） + double y_m = 2; // Y 方向平移（m） + double z_m = 3; // Z 方向平移（m） + double roll_rad = 4; // 绕 X 轴旋转（rad） + double pitch_rad = 5; // 绕 Y 轴旋转（rad） + double yaw_rad = 6; // 绕 Z 轴旋转（rad） +} + +// ========================================================= +// 通用请求头 +// 作用：带齐一次会话、任务、车辆、请求追踪信息 +// 发送方：通常为 Ubuntu 车间电脑 +// 接收方：Linux 内部服务 或 Windows 车端代理 +// ========================================================= +message RequestHeader { + string session_id = 1; // 本次整车标定会话 ID + string task_id = 2; // 当前任务 ID / 阶段 ID + string vehicle_id = 3; // 车辆 ID + string request_id = 4; // 本次请求唯一 ID + int64 client_send_timestamp_us = 5; // 请求发送时间戳（微秒） + string operator_id = 6; // 操作员工号 / 工位号 + string workshop_host = 7; // Ubuntu 车间电脑主机名 +} + +// ========================================================= +// 统一错误码 +// 作用：所有服务统一返回，便于编排器与状态机处理 +// ========================================================= +enum ErrorCode { + ERROR_CODE_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; // 数据质量不足 +} + +// ========================================================= +// 通用短响应 +// 作用：用于同步 RPC 的标准响应 +// 发送方：服务端 +// 接收方：调用方 +// ========================================================= +message StandardResponse { + bool success = 1; // 是否成功 + ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明文字 +} + +// ========================================================= +// 长任务状态 +// 作用：用于异步任务状态表达 +// ========================================================= +enum JobState { + JOB_STATE_UNSPECIFIED = 0; // 未指定 + PENDING = 1; // 已受理，等待执行 + RUNNING = 2; // 执行中 + WAITING_MANUAL = 3; // 等待人工操作 / 确认 + WAITING_APPROVAL = 4; // 等待审批 + SUCCEEDED = 5; // 成功 + FAILED = 6; // 失败 + CANCELED = 7; // 已取消 + ROLLED_BACK = 8; // 已回滚 +} + +// ========================================================= +// 长任务受理响应 +// 作用：耗时任务先返回 job_id，后续轮询状态 +// 发送方：服务端 +// 接收方：Ubuntu 车间电脑 +// ========================================================= +message JobAccepted { + bool accepted = 1; // 是否受理成功 + ErrorCode error_code = 2; // 未受理时的错误码 + string message = 3; // 说明 + string job_id = 4; // 长任务 ID +} + +// ========================================================= +// 长任务查询请求 +// 作用：通过 job_id 查询执行状态 +// 发送方：Ubuntu 车间电脑 +// 接收方：Linux 服务 或 Windows 车端代理 +// ========================================================= +message JobQuery { + RequestHeader header = 1; // 请求头 + string job_id = 2; // 任务 ID +} + +// ========================================================= +// 长任务状态响应 +// 作用：返回任务执行状态、进度、错误信息 +// 发送方：服务端 +// 接收方：Ubuntu 车间电脑 +// ========================================================= +message JobStatus { + string job_id = 1; // 任务 ID + JobState state = 2; // 当前状态 + double progress = 3; // 进度，建议范围 0.0 ~ 1.0 + ErrorCode error_code = 4; // 当前错误码 + string message = 5; // 状态说明 + int64 server_timestamp_us = 6; // 服务端时间戳 + bool safe_to_retry = 7; // 是否适合自动重试 +} + +// ========================================================= +// 心跳请求 +// 作用：保持 Linux 与 Windows 车端之间的在线状态 +// 发送方：Ubuntu 车间电脑 +// 接收方：Windows 车端代理 / Linux 子服务 +// ========================================================= +message HeartbeatRequest { + RequestHeader header = 1; // 请求头 + string agent_name = 2; // 服务名 / 代理名 + int32 expect_next_heartbeat_ms = 3; // 下次期望心跳间隔（ms） +} + +// ========================================================= +// 心跳响应 +// 作用：反馈当前是否在线、是否可接任务 +// 发送方：服务端 +// 接收方：调用方 +// ========================================================= +message HeartbeatResponse { + bool success = 1; // 是否正常 + ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明文字 + int64 server_timestamp_us = 4; // 响应时间戳 + bool vehicle_ready = 5; // 车辆 / 服务是否准备好接任务 +} + +// ========================================================= +// Agent 就绪查询请求 +// 作用：用于查询某个服务 / 代理 / 资源集合当前是否具备执行条件 +// 使用场景： +// 1) 外部定位就绪检查 +// 2) 车端控制服务就绪检查 +// 3) 底盘服务就绪检查 +// 4) 传感器服务就绪检查 +// ========================================================= +message AgentReadinessRequest { + RequestHeader header = 1; // 请求头 + string agent_name = 2; // 目标服务 / 代理名称 + repeated string target_resource_ids = 3; // 指定资源 ID 列表，例如 sensor_id + bool include_details = 4; // 是否返回详细问题列表 +} + +// ========================================================= +// 就绪问题项 +// 作用：表达“当前为什么还不能执行任务” +// 使用场景：各专项服务 readiness 响应中的 issues 列表 +// ========================================================= +message ReadinessIssue { + string issue_code = 1; // 问题代码，便于程序识别 + string message = 2; // 问题说明 + ErrorCode mapped_error_code = 3; // 映射到统一错误码 + bool blocking = 4; // 是否为阻塞问题 + string related_resource_id = 5; // 关联资源 ID，例如某个传感器 ID + repeated KeyValuePair metadata = 6; // 扩展元数据 +} + +// ========================================================= +// 文件摘要 +// 作用：用于参数包、URDF、数据文件校验 +// ========================================================= +message FileDigest { + string checksum_type = 1; // 摘要算法，例如 sha256 + string checksum_value = 2; // 摘要值 +} + +// ========================================================= +// 文件引用 +// 作用：用于报告、参数包、数据集、日志包等产物追溯 +// ========================================================= +message FileReference { + string file_name = 1; // 文件名 + string file_uri = 2; // 文件路径 / URI + int64 size_bytes = 3; // 文件大小（字节） + string description = 4; // 文件说明 + FileDigest digest = 5; // 文件摘要 +} + +// ========================================================= +// 键值对 +// 作用：用于扩展元数据，避免每次新增少量字段都改协议 +// ========================================================= +message KeyValuePair { + string key = 1; // 键 + string value = 2; // 值 +} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/chassis_calibration.proto b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/chassis_calibration.proto new file mode 100644 index 0000000..c7cb854 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/chassis_calibration.proto @@ -0,0 +1,465 @@ +syntax = "proto3"; + +package agv.calibration.chassis; + +import "calibration_common.proto"; +import "vehicle_profile.proto"; + +// ========================================================= +// 单位约定： +// 1) 所有“状态量、结果量、误差量、位姿量”中的角度统一使用 rad +// 2) 所有“人工输入更直观的命令量、限幅量、目标角度”允许使用 deg +// 3) 字段名必须显式带单位后缀，例如 _rad / _deg / _m / _ms +// 4) 服务端与客户端都必须严格按字段后缀做单位转换，禁止隐式假定 +// ========================================================= + +// ========================================================= +// 文件作用：底盘标定执行代理协议 +// 运行关系： +// Ubuntu 车间电脑(Linux) -> Windows 车端代理 +// 说明： +// 1) Linux 负责底盘参数求解、误差分析、自动验收、验证判断 +// 2) Windows 车端只负责执行动作、回传原始状态、写入参数 +// 3) 该协议同时覆盖：底盘预检 / 数据采集 / 参数验证 / 参数写入 +// ========================================================= + +// ========================================================= +// 服务：底盘标定服务 +// 运行位置：Windows 车端电脑 +// 调用方：Ubuntu 车间电脑 +// ========================================================= +service AgvCalibChassisService { + // 心跳保活 + rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) + returns (.agv.calibration.common.HeartbeatResponse); + + // 查询底盘服务当前是否具备执行条件 + rpc GetChassisReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (ChassisReadinessResponse); + + // 设置底盘标定工作模式 + rpc SetChassisWorkMode(ChassisWorkModeRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询车端底盘能力 + rpc GetChassisCapability(ChassisCapabilityRequest) + returns (ChassisCapabilityResponse); + + // 启动一个底盘动作原语任务 + rpc StartMotionPrimitive(MotionPrimitiveRequest) + returns (.agv.calibration.common.JobAccepted); + + // 查询底盘任务状态 + rpc GetChassisJobStatus(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.JobStatus); + + // 查询底盘任务结果 + rpc GetChassisJobResult(.agv.calibration.common.JobQuery) + returns (ChassisJobResult); + + // 取消底盘任务 + rpc CancelChassisJob(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.StandardResponse); + + // 打开底盘遥测流 + rpc StreamChassisTelemetry(StreamChassisTelemetryRequest) + returns (stream ChassisTelemetry); + + // 写入底盘标定结果 + rpc CommitChassisCalibrationParameters(CommitChassisCalibrationParametersRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询当前已生效底盘标定参数 + rpc GetAppliedChassisCalibrationParameters(GetAppliedChassisCalibrationParametersRequest) + returns (AppliedChassisCalibrationParametersResponse); + + // 紧急刹停 + rpc EmergencyBrake(.agv.calibration.common.Empty) + returns (.agv.calibration.common.StandardResponse); +} + +// ========================================================= +// 底盘工作模式 +// 作用：切换底盘到标定准备、直接执行、验证等模式 +// ========================================================= +message ChassisWorkModeRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + + enum Mode { + CHASSIS_MODE_UNSPECIFIED = 0; // 未指定 + NORMAL_MODE = 1; // 正常业务模式 + CALIBRATION_READY_MODE = 2; // 标定准备模式 + DIRECT_EXECUTION_MODE = 3; // 允许直接执行原始动作命令 + VALIDATION_MODE = 4; // 底盘参数验证模式 + } + + Mode target_mode = 2; // 目标模式 + string reason = 3; // 切换原因 +} + +// ========================================================= +// 底盘服务就绪响应 +// 作用：返回当前底盘服务是否满足执行条件 +// ========================================================= +message ChassisReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool chassis_driver_online = 5; // 底盘驱动是否在线 + bool motion_control_ready = 6; // 运控链路是否可执行动作 + bool estop_released = 7; // 急停是否释放 + bool vehicle_safe_to_move = 8; // 当前车辆是否允许移动 + bool telemetry_available = 9; // 是否可回传遥测 + repeated .agv.calibration.common.ReadinessIssue issues = 10; // 不满足项 + int64 checked_timestamp_us = 11; // 检查时间 +} + +// ========================================================= +// 查询底盘能力请求 +// 作用：运行前确认当前车辆支持哪些动作 +// ========================================================= +message ChassisCapabilityRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 +} + +// ========================================================= +// 查询底盘能力响应 +// 作用：返回当前车辆底盘形式和支持的动作原语 +// ========================================================= +message ChassisCapabilityResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + .agv.calibration.vehicle.profile.ChassisType chassis_type = 4; // 底盘类型 + + bool supports_straight_line = 5; // 是否支持直线动作 + bool supports_arc = 6; // 是否支持圆弧动作 + bool supports_in_place_rotation = 7; // 是否支持原地旋转 + bool supports_steer_sweep = 8; // 是否支持舵角扫动 + bool supports_reverse_motion = 9; // 是否支持倒车动作 + + bool supports_lateral_translation = 10; // 是否支持横移动作 + bool supports_diagonal_motion = 11; // 是否支持斜移动作 + bool supports_module_alignment_check = 12; // 是否支持模块零位检查 + bool supports_coordinated_steering = 13; // 是否支持模块协同转向 +} + +// ========================================================= +// 直线动作命令 +// 作用：用于验证直线行驶能力和线速度误差 +// ========================================================= +message StraightLineCommand { + double target_speed_ms = 1; // 目标线速度（m/s） + double target_distance_m = 2; // 目标距离（m） + bool reverse = 3; // 是否倒车 +} + +// ========================================================= +// 圆弧动作命令 +// 作用：用于验证曲线行驶能力和曲率误差 +// ========================================================= +message ArcCommand { + double target_speed_ms = 1; // 目标线速度（m/s） + double radius_m = 2; // 目标半径（m） + double sweep_angle_deg = 3; // 圆弧扫角（度） + bool clockwise = 4; // 是否顺时针 +} + +// ========================================================= +// 原地旋转动作命令 +// 作用：用于差速、多舵轮等底盘的旋转能力测试 +// ========================================================= +message InPlaceRotationCommand { + double target_yaw_deg = 1; // 目标旋转角（度） + double target_angular_vel_deg_s = 2; // 目标角速度（度/秒） +} + +// ========================================================= +// 舵角扫动命令 +// 作用：用于阿克曼、单舵轮、多舵轮的转向零位和转向响应测试 +// ========================================================= +message SteeringSweepCommand { + double target_angle_deg = 1; // 目标舵角（度） + double sweep_amplitude_deg = 2; // 扫动幅值（度） + double sweep_frequency_hz = 3; // 扫动频率（Hz） + double duration_sec = 4; // 持续时长（秒） +} + +// ========================================================= +// 横移动作命令 +// 作用：用于多舵轮底盘横向平移能力测试 +// ========================================================= +message LateralTranslationCommand { + double target_speed_ms = 1; // 目标横移速度（m/s） + double target_distance_m = 2; // 目标横移距离（m） + bool move_left = 3; // 是否向左移动 +} + +// ========================================================= +// 斜移动作命令 +// 作用：用于多舵轮底盘斜向运动能力测试 +// ========================================================= +message DiagonalMotionCommand { + double target_speed_ms = 1; // 目标速度（m/s） + double target_distance_m = 2; // 目标距离（m） + double heading_deg = 3; // 目标运动方向角（度） +} + +// ========================================================= +// 模块零位检查命令 +// 作用：用于多舵轮每个舵轮模块零位与安装状态检查 +// ========================================================= +message ModuleAlignmentCheckCommand { + repeated string module_ids = 1; // 待检查模块 ID 列表 + double target_zero_deg = 2; // 目标零位角（度） + double tolerance_deg = 3; // 允许误差（度） +} + +// ========================================================= +// 模块协同转向命令 +// 作用：用于多舵轮模块协同转向与一致性测试 +// ========================================================= +message CoordinatedSteeringCommand { + repeated string module_ids = 1; // 参与测试的模块 ID 列表 + double target_angle_deg = 2; // 目标舵角（度） + double hold_time_sec = 3; // 保持时长（秒） +} + +// ========================================================= +// 底盘动作原语请求 +// 作用：让车端执行一个标准化动作 +// 发送方：Ubuntu 车间电脑 +// 接收方：Windows 车端代理 +// ========================================================= +message MotionPrimitiveRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string test_case_id = 2; // 测试用例 ID + + enum TaskPurpose { + CHASSIS_TASK_PURPOSE_UNSPECIFIED = 0; // 未指定 + DATA_COLLECTION = 1; // 数据采集 + VALIDATION = 2; // 参数验证 + DIRECT_CHECK = 3; // 直接检查 + } + + TaskPurpose task_purpose = 3; // 任务目的 + + oneof primitive { + StraightLineCommand straight_line = 4; // 直线动作 + ArcCommand arc = 5; // 圆弧动作 + InPlaceRotationCommand in_place_rotation = 6; // 原地旋转 + SteeringSweepCommand steering_sweep = 7; // 舵角扫动 + LateralTranslationCommand lateral_translation = 8; // 横移 + DiagonalMotionCommand diagonal_motion = 9; // 斜移 + ModuleAlignmentCheckCommand module_alignment = 10; // 模块零位检查 + CoordinatedSteeringCommand coordinated_steering = 11; // 模块协同转向 + } + + bool brake_when_finished = 12; // 结束后是否刹停 + double timeout_sec = 13; // 任务超时时间 + string source_iteration_id = 14; // 来源迭代号 +} + +// ========================================================= +// 底盘遥测流请求 +// 作用：指定底盘遥测内容和频率 +// ========================================================= +message StreamChassisTelemetryRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + uint32 expected_hz = 2; // 期望上报频率 + bool include_odom = 3; // 是否包含里程计 + bool include_wheel_state = 4; // 是否包含轮组状态 + bool include_driver_state = 5; // 是否包含驱动器状态 + bool include_module_state = 6; // 是否包含模块级状态 +} + +// ========================================================= +// 单个轮 / 舵模块状态 +// 作用：兼容差速、单舵轮、多舵轮、阿克曼等形式 +// ========================================================= +message WheelModuleState { + string module_id = 1; // 模块 ID，例如 fl / fr / drive_center + int64 encoder_ticks = 2; // 编码器累计脉冲 + double wheel_speed_rpm = 3; // 当前轮速（RPM） + double steer_angle_deg = 4; // 当前舵角（度），无舵角则可置 0 + double motor_current_amp = 5; // 电机电流（A） +} + +// ========================================================= +// 底盘遥测数据 +// 作用：回传原始底盘状态供 Linux 做误差分析 +// 发送方：Windows 车端代理 +// 接收方：Ubuntu 车间电脑 +// ========================================================= +message ChassisTelemetry { + int64 hardware_timestamp_us = 1; // 硬件时间戳 + .agv.calibration.vehicle.profile.ChassisType chassis_type = 2; // 当前底盘类型 + + double odom_x_m = 3; // 车端里程计 X（m） + double odom_y_m = 4; // 车端里程计 Y（m） + double odom_yaw_rad = 5; // 车端里程计偏航角（rad） + double linear_velocity_ms = 6; // 实际线速度（m/s） + double angular_velocity_rads = 7; // 实际角速度（rad/s） + + repeated WheelModuleState modules = 8; // 所有轮 / 舵模块状态 + + bool estop_engaged = 9; // 是否急停 + uint32 driver_error_code = 10; // 驱动器错误码 + string active_job_id = 11; // 当前任务 ID + + double lateral_slip_estimate = 12; // 估计横向滑移 + double curvature_estimate = 13; // 估计曲率 +} + +// ========================================================= +// 通用底盘参数 +// 作用：不同底盘形式都可能共用的几何 / 补偿参数 +// ========================================================= +message CommonChassisCalibrationParams { + optional double effective_wheel_base_m = 1; // 有效轴距（m） + optional double effective_track_width_m = 2; // 有效轮距（m） + optional double longitudinal_scale = 3; // 纵向里程比例补偿 + optional double lateral_scale = 4; // 横向里程比例补偿 + optional double yaw_scale = 5; // 航向比例补偿 + optional double straight_line_bias = 6; // 直线跑偏补偿 +} + +// ========================================================= +// 阿克曼专属参数 +// 作用：适用于阿克曼车 +// ========================================================= +message AckermannCalibrationParams { + optional double front_left_steer_zero_offset_deg = 1; // 左前舵角零偏 + optional double front_right_steer_zero_offset_deg = 2; // 右前舵角零偏 + optional double rear_left_wheel_radius_m = 3; // 左后轮有效半径 + optional double rear_right_wheel_radius_m = 4; // 右后轮有效半径 + optional double steering_ratio = 5; // 转向传动比 +} + +// ========================================================= +// 差速专属参数 +// 作用：适用于差速 AGV +// ========================================================= +message DifferentialCalibrationParams { + optional double left_wheel_radius_m = 1; // 左轮有效半径 + optional double right_wheel_radius_m = 2; // 右轮有效半径 + optional double axle_track_width_m = 3; // 驱动轮间距 + optional double left_encoder_scale = 4; // 左编码器比例系数 + optional double right_encoder_scale = 5; // 右编码器比例系数 +} + +// ========================================================= +// 单舵轮专属参数 +// 作用：适用于单舵轮 AGV +// ========================================================= +message SingleSteerWheelCalibrationParams { + optional double drive_wheel_radius_m = 1; // 驱动轮有效半径 + optional double steer_zero_offset_deg = 2; // 舵角零偏 + optional double steering_ratio = 3; // 转向比 + optional double drive_encoder_scale = 4; // 驱动编码器比例 +} + +// ========================================================= +// 多舵轮单模块参数 +// 作用：用于多舵轮车每个模块的独立参数 +// ========================================================= +message SteeringModuleCalibrationParam { + string module_id = 1; // 模块 ID + optional double wheel_radius_m = 2; // 模块轮半径 + optional double steer_zero_offset_deg = 3; // 模块舵角零偏 + optional double module_pos_x_m = 4; // 模块在 base_link 下的 X + optional double module_pos_y_m = 5; // 模块在 base_link 下的 Y +} + +// ========================================================= +// 多舵轮专属参数 +// 作用：适用于多舵轮 AGV +// ========================================================= +message MultiSteerWheelCalibrationParams { + repeated SteeringModuleCalibrationParam modules = 1; // 各舵轮模块参数 +} + +// ========================================================= +// 底盘标定参数总包 +// 作用：统一表达不同底盘类型的标定结果 +// ========================================================= +message ChassisCalibrationParameterSet { + .agv.calibration.vehicle.profile.ChassisType chassis_type = 1; // 底盘类型 + CommonChassisCalibrationParams common = 2; // 通用参数 + + oneof specific_params { + AckermannCalibrationParams ackermann = 3; // 阿克曼参数 + DifferentialCalibrationParams differential = 4; // 差速参数 + SingleSteerWheelCalibrationParams single_steer = 5; // 单舵轮参数 + MultiSteerWheelCalibrationParams multi_steer = 6; // 多舵轮参数 + } +} + +// ========================================================= +// 底盘验证摘要 +// 作用：返回底盘专项自动验收的关键指标 +// ========================================================= +message ChassisValidationSummary { + double max_lateral_error_m = 1; // 最大横向误差 + double max_yaw_error_rad = 2; // 最大航向误差 + double rms_lateral_error_m = 3; // 横向误差均方根 + double rms_yaw_error_rad = 4; // 航向误差均方根 + double repeatability_error_m = 5; // 重复性误差 + double curvature_error = 6; // 曲率误差 + double module_consistency_error = 7; // 模块一致性误差 + bool auto_acceptance_passed = 8; // 自动验收是否通过 +} + +// ========================================================= +// 写入底盘标定参数请求 +// 作用：将 Linux 求解结果写入车端 +// ========================================================= +message CommitChassisCalibrationParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string parameter_version = 2; // 参数版本号 + ChassisCalibrationParameterSet params = 3; // 参数总包 + string commit_reason = 4; // 写入原因 + .agv.calibration.common.FileDigest digest = 5; // 参数摘要 + bool persistent_write = 6; // 是否持久化 +} + +// ========================================================= +// 查询当前已生效底盘参数请求 +// ========================================================= +message GetAppliedChassisCalibrationParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 +} + +// ========================================================= +// 查询当前已生效底盘参数响应 +// ========================================================= +message AppliedChassisCalibrationParametersResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string parameter_version = 4; // 当前版本 + ChassisCalibrationParameterSet params = 5; // 当前生效参数 + int64 applied_timestamp_us = 6; // 生效时间 +} + +// ========================================================= +// 底盘任务结果 +// 作用：返回底盘动作执行后的分析摘要，供编排器判断是否可进入下一步 +// ========================================================= +message ChassisJobResult { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string job_id = 4; // 任务 ID + + bool data_quality_passed = 5; // 数据质量是否达标 + bool suitable_for_commit = 6; // 是否适合写入参数 + string recommended_parameter_version = 7; // 推荐参数版本 + + double estimated_straight_line_bias = 8; // 估计的直线跑偏 + ChassisValidationSummary validation_summary = 9; // 验证摘要 + ChassisCalibrationParameterSet estimated_params = 10; // 本轮估计参数 + repeated .agv.calibration.common.FileReference artifacts = 11; // 关联产物 +} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/control_calibration.proto b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/control_calibration.proto new file mode 100644 index 0000000..61a04c4 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/control_calibration.proto @@ -0,0 +1,423 @@ +syntax = "proto3"; + +package agv.calibration.control; + +import "calibration_common.proto"; +import "vehicle_profile.proto"; + +// ========================================================= +// 单位约定： +// 1) 所有“状态量、结果量、误差量、位姿量”中的角度统一使用 rad +// 2) 所有“人工输入更直观的命令量、限幅量、目标角度”允许使用 deg +// 3) 字段名必须显式带单位后缀，例如 _rad / _deg / _m / _ms +// 4) 服务端与客户端都必须严格按字段后缀做单位转换，禁止隐式假定 +// ========================================================= + +// ========================================================= +// 文件作用：运控参数调优执行代理协议 +// 运行关系： +// Ubuntu 车间电脑(Linux) -> Windows 车端代理 +// 说明： +// 1) Linux 负责控制参数搜索、收敛判断、验证分析 +// 2) Windows 负责执行轨迹 / 速度测试并回传遥测 +// 3) 控制器明确区分为“横向控制器”和“纵向控制器” +// 4) 本文件已将横向 MPC 与纵向 MPC 参数模型拆开 +// ========================================================= + +// ========================================================= +// 服务：运控调参服务 +// 运行位置：Windows 车端电脑 +// 调用方：Ubuntu 车间电脑 +// ========================================================= +service AgvCalibControlService { + // 心跳保活 + rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) + returns (.agv.calibration.common.HeartbeatResponse); + + // 查询控制服务当前是否具备执行条件 + rpc GetControlReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (ControlReadinessResponse); + + // 设置调参工作模式 + rpc SetControlWorkMode(ControlWorkModeRequest) + returns (.agv.calibration.common.StandardResponse); + + // 热加载一版控制参数 + rpc InjectControllerParameters(InjectControllerParametersRequest) + returns (.agv.calibration.common.StandardResponse); + + // 启动一个控制评估任务 + rpc StartControllerEvaluation(ControllerEvaluationRequest) + returns (.agv.calibration.common.JobAccepted); + + // 查询控制评估任务状态 + rpc GetControlJobStatus(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.JobStatus); + + // 查询控制评估任务结果 + rpc GetControlJobResult(.agv.calibration.common.JobQuery) + returns (ControlJobResult); + + // 取消控制评估任务 + rpc CancelControlJob(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.StandardResponse); + + // 打开控制遥测流 + rpc StreamControlTelemetry(StreamControlTelemetryRequest) + returns (stream ControlTelemetry); + + // 固化最终控制参数 + rpc CommitControllerParameters(CommitControllerParametersRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询当前已生效控制参数 + rpc GetActiveControllerParameters(GetActiveControllerParametersRequest) + returns (ActiveControllerParametersResponse); + + // 紧急停车 + rpc EmergencyStop(.agv.calibration.common.Empty) + returns (.agv.calibration.common.StandardResponse); +} + +// ========================================================= +// 运控工作模式 +// 作用：切换控制系统到调试、评估、验证等状态 +// ========================================================= +message ControlWorkModeRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + + enum Mode { + CONTROL_MODE_UNSPECIFIED = 0; // 未指定 + NORMAL_MODE = 1; // 正常模式 + TUNING_READY_MODE = 2; // 调参准备模式 + EVALUATION_MODE = 3; // 参数评估模式 + VALIDATION_MODE = 4; // 参数验证模式 + } + + Mode target_mode = 2; // 目标模式 + string reason = 3; // 切换原因 +} + +// ========================================================= +// 控制服务就绪响应 +// 作用：返回当前控制服务是否满足执行条件 +// ========================================================= +message ControlReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool trajectory_executor_ready = 5; // 轨迹执行器是否就绪 + bool vehicle_feedback_ready = 6; // 车辆反馈链路是否就绪 + bool control_output_ready = 7; // 控制输出链路是否就绪 + bool estop_released = 8; // 急停是否释放 + bool vehicle_safe_to_move = 9; // 当前车辆是否允许移动 + repeated .agv.calibration.common.ReadinessIssue issues = 10; // 不满足项 + int64 checked_timestamp_us = 11; // 检查时间 +} + +// ========================================================= +// PID 参数 +// 作用：PID 控制器参数 +// 说明：适用于横向或纵向，但需由 control_axis 配合解释 +// ========================================================= +message PIDParams { + string loop_name = 1; // 回路名，例如 lateral / heading / speed + double kp = 2; // 比例系数 + double ki = 3; // 积分系数 + double kd = 4; // 微分系数 + optional double integral_limit = 5; // 积分限幅 + optional double output_limit = 6; // 输出限幅 +} + +// ========================================================= +// 横向 MPC 参数 +// 作用：适用于横向控制器的 MPC 参数 +// ========================================================= +message LateralMPCParams { + uint32 prediction_horizon = 1; // 预测步长 + uint32 control_horizon = 2; // 控制步长 + double model_dt_s = 3; // 离散时间步长 + double q_lateral = 4; // 横向误差权重 + double q_heading = 5; // 航向误差权重 + double r_steering = 6; // 转向输出权重 + double r_steering_rate = 7; // 转向变化率权重 + optional double steering_limit_deg = 8; // 转向限幅 +} + +// ========================================================= +// 纵向 MPC 参数 +// 作用：适用于纵向控制器的 MPC 参数 +// ========================================================= +message LongitudinalMPCParams { + uint32 prediction_horizon = 1; // 预测步长 + uint32 control_horizon = 2; // 控制步长 + double model_dt_s = 3; // 离散时间步长 + double q_speed = 4; // 速度误差权重 + double q_accel = 5; // 加速度权重 + double r_throttle = 6; // 油门 / 驱动输出权重 + double r_brake = 7; // 制动输出权重 + double r_jerk = 8; // jerk 权重 + optional double throttle_limit = 9; // 驱动输出限幅 + optional double brake_limit = 10; // 制动输出限幅 +} + +// ========================================================= +// LQR 参数 +// 作用：LQR 控制器参数 +// 说明：通常用于横向控制器 +// ========================================================= +message LQRParams { + repeated double q_state_weights = 1; // 状态权重向量 Q + repeated double r_input_weights = 2; // 输入权重向量 R + optional double preview_time_s = 3; // 前视时间 +} + +// ========================================================= +// Pure Pursuit 参数 +// 作用：Pure Pursuit 控制器参数 +// 说明：仅适用于横向控制器 +// ========================================================= +message PurePursuitParams { + double lookahead_m = 1; // 前瞻距离 + optional double min_lookahead_m = 2; // 最小前瞻距离 + optional double max_lookahead_m = 3; // 最大前瞻距离 + optional double curvature_gain = 4; // 曲率增益 + optional double steering_limit_deg = 5; // 转向限幅 +} + +// ========================================================= +// 单个控制器参数包 +// 作用：统一表达“某个控制轴上的某个控制器”的参数 +// 说明： +// 1) control_axis 用来明确这是横向控制器还是纵向控制器 +// 2) algorithm_type 用来明确该控制器采用哪类算法 +// ========================================================= +message ControllerParameterPack { + .agv.calibration.vehicle.profile.ControlAxisType control_axis = 1; // 控制轴类型 + .agv.calibration.vehicle.profile.ControllerAlgorithmType algorithm_type = 2; // 算法类型 + + oneof params { + PIDParams pid = 3; // PID 参数 + LateralMPCParams lateral_mpc = 4; // 横向 MPC 参数 + LongitudinalMPCParams longitudinal_mpc = 5; // 纵向 MPC 参数 + LQRParams lqr = 6; // LQR 参数 + PurePursuitParams pure_pursuit = 7; // Pure Pursuit 参数 + } +} + +// ========================================================= +// 控制参数集合 +// 作用：一版参数里可以同时包含多个控制器参数 +// 说明： +// 1) 例如可同时包含“横向 MPC + 纵向 PID” +// 2) 也可包含“横向 PP + 纵向 PID” +// ========================================================= +message ControllerParameterSet { + repeated ControllerParameterPack items = 1; // 参数项列表 +} + +// ========================================================= +// 热注入参数请求 +// 作用：将 Linux 计算出的一版参数加载到车端运行内存 +// ========================================================= +message InjectControllerParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string parameter_version = 2; // 参数版本号 + ControllerParameterSet parameter_set = 3; // 参数集合 + bool apply_immediately = 4; // 是否立即生效 + string source_iteration_id = 5; // 来源迭代号 +} + +// ========================================================= +// 轨迹点 +// 作用：控制评估任务中使用的测试轨迹点 +// ========================================================= +message TrajectoryPoint { + double x_m = 1; // 目标 X + double y_m = 2; // 目标 Y + double yaw_rad = 3; // 目标偏航角 + double target_speed_ms = 4; // 目标速度 +} + +// ========================================================= +// 轨迹跟踪评估任务 +// 作用：让车端按当前控制参数跑一条测试轨迹 +// ========================================================= +message TrajectoryTrackingTask { + repeated TrajectoryPoint path = 1; // 轨迹点序列 + bool stop_at_end = 2; // 结束后是否停车 + double timeout_sec = 3; // 超时时间 +} + +// ========================================================= +// 速度阶跃任务 +// 作用：用于评估纵向动态响应 +// ========================================================= +message VelocityStepTask { + double target_velocity_ms = 1; // 目标速度 + double hold_time_sec = 2; // 保持时长 + double settle_before_step_sec = 3; // 阶跃前静稳时间 +} + +// ========================================================= +// 加减速任务 +// 作用：用于评估纵向平顺性、加减速响应和 jerk 表现 +// ========================================================= +message AccelerationDecelerationTask { + double start_velocity_ms = 1; // 起始速度 + double target_velocity_ms = 2; // 目标速度 + double target_accel_ms2 = 3; // 目标加速度 + double hold_time_sec = 4; // 保持时间 +} + +// ========================================================= +// 停车精度任务 +// 作用：用于评估停车位置误差与速度收敛情况 +// ========================================================= +message StopAccuracyTask { + double target_stop_x_m = 1; // 目标停车点 X + double target_stop_y_m = 2; // 目标停车点 Y + double target_stop_yaw_rad = 3; // 目标停车姿态 + double timeout_sec = 4; // 超时时间 +} + +// ========================================================= +// 控制评估任务请求 +// 作用：启动一次参数评估任务 +// 发送方：Ubuntu 车间电脑 +// 接收方：Windows 车端代理 +// ========================================================= +message ControllerEvaluationRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string test_case_id = 2; // 测试用例 ID + + enum TaskPurpose { + CONTROL_TASK_PURPOSE_UNSPECIFIED = 0; // 未指定 + DATA_COLLECTION = 1; // 数据采集 + TUNING_EVALUATION = 2; // 调参评估 + VALIDATION = 3; // 最终验证 + } + + TaskPurpose task_purpose = 3; // 任务目的 + + oneof task { + TrajectoryTrackingTask trajectory_tracking = 4; // 轨迹跟踪任务 + VelocityStepTask velocity_step = 5; // 速度阶跃任务 + AccelerationDecelerationTask accel_decel = 6; // 加减速任务 + StopAccuracyTask stop_accuracy = 7; // 停车精度任务 + } + + string source_iteration_id = 8; // 来源迭代号 +} + +// ========================================================= +// 控制遥测流请求 +// 作用：配置遥测频率和内容 +// ========================================================= +message StreamControlTelemetryRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + uint32 expected_hz = 2; // 上报频率 + bool include_tracking_error = 3; // 是否包含跟踪误差 + bool include_control_output = 4; // 是否包含控制器输出 + bool include_vehicle_feedback = 5; // 是否包含车辆反馈 + bool include_controller_identity = 6; // 是否包含当前控制器信息 +} + +// ========================================================= +// 控制遥测 +// 作用：回传调参评估时的关键数据 +// 发送方：Windows 车端代理 +// 接收方：Ubuntu 车间电脑 +// ========================================================= +message ControlTelemetry { + int64 hardware_timestamp_us = 1; // 时间戳 + + double odom_x_m = 2; // 位置 X + double odom_y_m = 3; // 位置 Y + double odom_yaw_rad = 4; // 偏航角 + double linear_velocity_ms = 5; // 实际线速度 + double angular_velocity_rads = 6; // 实际角速度 + + double lateral_error_m = 7; // 横向误差 + double heading_error_rad = 8; // 航向误差 + double speed_error_ms = 9; // 速度误差 + + double steering_output = 10; // 横向控制输出 + double throttle_output = 11; // 纵向驱动输出 + double brake_output = 12; // 制动输出 + bool saturation_flag = 13; // 是否发生饱和 + string active_job_id = 14; // 当前任务 ID + + string parameter_version = 15; // 当前生效参数版本 + .agv.calibration.vehicle.profile.ControllerAlgorithmType active_lateral_algorithm = 16; // 当前横向算法 + .agv.calibration.vehicle.profile.ControllerAlgorithmType active_longitudinal_algorithm = 17; // 当前纵向算法 +} + +// ========================================================= +// 控制验证摘要 +// 作用：返回运控专项自动验收的关键指标 +// ========================================================= +message ControlValidationSummary { + double rms_lateral_error_m = 1; // 横向误差均方根 + double rms_heading_error_rad = 2; // 航向误差均方根 + double rms_speed_error_ms = 3; // 速度误差均方根 + double overshoot_ratio = 4; // 超调比例 + double settle_time_sec = 5; // 收敛时间 + double stop_position_error_m = 6; // 停车位置误差 + double max_jerk = 7; // 最大 jerk + double saturation_ratio = 8; // 饱和占比 + bool auto_acceptance_passed = 9; // 自动验收是否通过 +} + +// ========================================================= +// 固化控制参数请求 +// 作用：将最终收敛的一版参数写入车端 +// ========================================================= +message CommitControllerParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string parameter_version = 2; // 参数版本号 + ControllerParameterSet parameter_set = 3; // 参数集合 + string commit_reason = 4; // 写入原因 + .agv.calibration.common.FileDigest digest = 5; // 摘要 + bool persistent_write = 6; // 是否持久化 +} + +// ========================================================= +// 查询当前控制参数请求 +// ========================================================= +message GetActiveControllerParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 +} + +// ========================================================= +// 查询当前控制参数响应 +// ========================================================= +message ActiveControllerParametersResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string parameter_version = 4; // 当前参数版本 + ControllerParameterSet parameter_set = 5; // 当前生效参数 + int64 applied_timestamp_us = 6; // 生效时间 +} + +// ========================================================= +// 控制任务结果 +// 作用：返回一次调参评估 / 验证后的结果摘要 +// ========================================================= +message ControlJobResult { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string job_id = 4; // 任务 ID + + bool data_quality_passed = 5; // 数据质量是否达标 + bool suitable_for_commit = 6; // 是否适合写入 + string recommended_parameter_version = 7; // 推荐参数版本 + + ControlValidationSummary validation_summary = 8; // 验证摘要 + ControllerParameterSet estimated_parameter_set = 9; // 本轮估计参数集 + repeated .agv.calibration.common.FileReference artifacts = 10; // 关联产物 +} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/external_localization.proto b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/external_localization.proto new file mode 100644 index 0000000..35749b7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/external_localization.proto @@ -0,0 +1,337 @@ +syntax = "proto3"; + +package agv.calibration.localization.external; + +import "calibration_common.proto"; + +// ========================================================= +// 文件作用：外部定位 / 真值参考接入执行代理协议 +// 运行关系： +// Ubuntu 车间电脑(Linux) -> Windows 车端代理 / Linux 外部定位桥接服务 +// 说明： +// 1) Linux 负责任务编排、基准求解、重复性分析、真值参考验收判断 +// 2) 车端代理 / 桥接服务只负责采集数据、提供位姿流、执行必要检查 +// 3) 本文件既支持“外部定位结果求解”，也支持“阶段0真值参考接入与确认” +// ========================================================= + +// ========================================================= +// 服务：外部定位标定服务 +// 运行位置：Windows 车端代理 或 Linux 外部定位桥接服务 +// 调用方：Ubuntu 车间电脑 +// ========================================================= +service AgvCalibExternalLocalizationService { + // 心跳保活 + rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) + returns (.agv.calibration.common.HeartbeatResponse); + + // 查询外部定位服务当前是否具备作为真值参考的条件 + rpc GetExternalLocalizationReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (ExternalLocalizationReadinessResponse); + + // 设置外部定位工作模式 + rpc SetExternalLocalizationWorkMode(ExternalLocalizationWorkModeRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询外部定位能力 + rpc GetExternalLocalizationCapability(ExternalLocalizationCapabilityRequest) + returns (ExternalLocalizationCapabilityResponse); + + // 启动一个外部定位任务 + rpc StartExternalLocalizationTask(ExternalLocalizationTaskRequest) + returns (.agv.calibration.common.JobAccepted); + + // 查询外部定位任务状态 + rpc GetExternalLocalizationJobStatus(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.JobStatus); + + // 查询外部定位任务结果 + rpc GetExternalLocalizationJobResult(.agv.calibration.common.JobQuery) + returns (ExternalLocalizationJobResult); + + // 取消外部定位任务 + rpc CancelExternalLocalizationJob(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.StandardResponse); + + // 打开外部定位遥测流 + rpc StreamExternalLocalizationTelemetry(StreamExternalLocalizationTelemetryRequest) + returns (stream ExternalLocalizationTelemetry); + + // 写入外部定位标定结果 + rpc CommitExternalLocalizationResult(CommitExternalLocalizationResultRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询当前已生效外部定位结果 + rpc GetAppliedExternalLocalizationResult(GetAppliedExternalLocalizationResultRequest) + returns (AppliedExternalLocalizationResultResponse); + + // 紧急停止 + rpc EmergencyStop(.agv.calibration.common.Empty) + returns (.agv.calibration.common.StandardResponse); +} + +// ========================================================= +// 外部定位工作模式请求 +// 作用：切换外部定位桥接服务到不同模式 +// ========================================================= +message ExternalLocalizationWorkModeRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + + enum Mode { + EXTERNAL_LOCALIZATION_MODE_UNSPECIFIED = 0; // 未指定 + NORMAL_MODE = 1; // 正常模式 + CALIBRATION_READY_MODE = 2; // 标定准备模式 + REFERENCE_COLLECTION_MODE = 3; // 参考数据采集模式 + VALIDATION_MODE = 4; // 真值参考验证模式 + } + + Mode target_mode = 2; // 目标模式 + string reason = 3; // 切换原因 +} + +// ========================================================= +// 查询外部定位能力请求 +// 作用：查询当前工位 / 服务支持的外部定位能力 +// ========================================================= +message ExternalLocalizationCapabilityRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 +} + +// ========================================================= +// 查询外部定位能力响应 +// 作用：返回当前支持的数据源与功能 +// ========================================================= +message ExternalLocalizationCapabilityResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool supports_truth_source_readiness_check = 4; // 是否支持真值源就绪检查 + bool supports_marker_alignment = 5; // 是否支持基于标靶对齐 + bool supports_reference_pose_collection = 6; // 是否支持参考位姿采集 + bool supports_truth_source_validation = 7; // 是否支持真值参考验证 + bool supports_persistent_commit = 8; // 是否支持持久化写入 + + string localization_source_name = 9; // 外部定位源名称 + string localization_source_id = 10; // 外部定位源唯一 ID + string workcell_zone_id = 11; // 当前工位 / 区域 ID +} + +// ========================================================= +// 外部定位验证摘要 +// 作用：表达当前外部定位是否适合作为真值参考输入 +// 使用场景： +// 1) readiness 响应 +// 2) 外部定位任务结果 +// 3) 阶段0自动验收判断 +// ========================================================= +message ExternalLocalizationValidationSummary { + bool time_sync_ok = 1; // 时间同步是否达标 + bool coverage_ok = 2; // 覆盖范围是否达标 + bool quality_ok = 3; // 观测质量是否达标 + bool tracking_stable = 4; // 跟踪是否稳定 + bool recommended_as_truth_source = 5; // 是否推荐作为真值参考源 + + double position_stddev_m = 6; // 位置标准差 + double yaw_stddev_rad = 7; // 航向标准差 + double tracking_loss_ratio = 8; // 跟踪丢失比例 + double time_sync_offset_ms = 9; // 时间同步偏差（ms） + + repeated .agv.calibration.common.ReadinessIssue issues = 10; // 不满足项列表 +} + +// ========================================================= +// 外部定位就绪响应 +// 作用：返回当前是否适合作为真值参考源 +// 发送方：外部定位桥接服务 +// 接收方：Ubuntu 车间电脑 +// ========================================================= +message ExternalLocalizationReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool ready_for_reference_validation = 5; // 是否可进入真值参考验证 + ExternalLocalizationValidationSummary validation_summary = 6; // 验证摘要 + int64 checked_timestamp_us = 7; // 检查时间 +} + +// ========================================================= +// 参考位姿采集任务 +// 作用：采集一批车辆静止位姿用于重复性分析或基准建立 +// ========================================================= +message ReferencePoseCollectionTask { + uint32 sample_count = 1; // 需要采集的样本数 + bool require_vehicle_static = 2; // 是否要求车辆静止 + double timeout_sec = 3; // 超时时间 + double min_sample_interval_sec = 4; // 最小采样间隔 +} + +// ========================================================= +// 标靶对齐任务 +// 作用：通过标靶 / 标定板建立外部参考坐标关系 +// ========================================================= +message MarkerAlignmentTask { + string target_board_id = 1; // 标靶 / 标定板 ID + uint32 min_valid_observation_count = 2; // 最少有效观测数 + double timeout_sec = 3; // 超时时间 +} + +// ========================================================= +// 真值参考验证任务 +// 作用：验证外部定位输出的重复性、稳定性、时间同步与可用性 +// ========================================================= +message TruthSourceValidationTask { + uint32 static_sample_count = 1; // 静态样本数 + uint32 dynamic_sample_count = 2; // 动态样本数 + bool require_short_motion_segment = 3; // 是否要求短距离运动段验证 + + double max_position_stddev_m = 4; // 允许的最大位置标准差 + double max_yaw_stddev_rad = 5; // 允许的最大航向标准差 + double max_tracking_loss_ratio = 6; // 允许的最大跟踪丢失比例 + double max_time_sync_offset_ms = 7; // 允许的最大时间同步偏差 + + double timeout_sec = 8; // 超时时间 +} + +// ========================================================= +// 外部定位任务请求 +// 作用：启动一类外部定位任务 +// 发送方：Ubuntu 车间电脑 +// 接收方：Windows 车端代理 / Linux 桥接服务 +// ========================================================= +message ExternalLocalizationTaskRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string test_case_id = 2; // 测试用例 ID + + enum TaskPurpose { + EXTERNAL_LOCALIZATION_TASK_PURPOSE_UNSPECIFIED = 0; // 未指定 + REFERENCE_READY_CHECK = 1; // 真值参考可用性检查 + DATA_COLLECTION = 2; // 数据采集 + SOLVING = 3; // 求解 + VALIDATION = 4; // 验证 + } + + TaskPurpose task_purpose = 3; // 任务目的 + + oneof task { + ReferencePoseCollectionTask reference_pose_collection = 4; // 参考位姿采集 + MarkerAlignmentTask marker_alignment = 5; // 标靶对齐 + TruthSourceValidationTask truth_source_validation = 6; // 真值参考验证 + } + + string source_iteration_id = 7; // 来源迭代号 + string localization_source_id = 8; // 本次任务指定使用的外部定位源 ID + string workcell_zone_id = 9; // 本次任务所属工位 / 区域 ID +} + +// ========================================================= +// 外部定位遥测流请求 +// 作用：指定外部定位数据上报内容 +// ========================================================= +message StreamExternalLocalizationTelemetryRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + uint32 expected_hz = 2; // 期望上报频率 + bool include_pose = 3; // 是否包含位姿 + bool include_quality_metrics = 4; // 是否包含质量指标 + bool include_time_sync_metrics = 5; // 是否包含时间同步指标 + bool include_tracking_state = 6; // 是否包含跟踪状态 +} + +// ========================================================= +// 外部定位遥测 +// 作用：回传外部定位实时观测结果 +// 发送方：Windows 车端代理 / Linux 桥接服务 +// 接收方：Ubuntu 车间电脑 +// ========================================================= +message ExternalLocalizationTelemetry { + int64 hardware_timestamp_us = 1; // 硬件时间戳 + bool pose_valid = 2; // 位姿是否有效 + + .agv.calibration.common.Pose3dEuler workshop_pose = 3; // 在车间参考系下的位姿 + + double position_stddev_m = 4; // 位置标准差 + double yaw_stddev_rad = 5; // 航向标准差 + double tracking_loss_ratio = 6; // 跟踪丢失比例 + double time_sync_offset_ms = 7; // 时间同步偏差（ms） + double quality_score = 8; // 质量分数，数值越高越好 + + uint32 observed_target_count = 9; // 当前观测到的目标数量 + string reference_source_name = 10; // 外部定位源名称 + string active_job_id = 11; // 当前任务 ID +} + +// ========================================================= +// 外部定位标定结果 +// 作用：表达车间参考系与外部定位参考系之间的结果关系 +// ========================================================= +message ExternalLocalizationCalibrationResult { + string workshop_frame_id = 1; // 车间参考坐标系 ID + string localization_frame_id = 2; // 外部定位坐标系 ID + + .agv.calibration.common.Pose3dEuler workshop_to_localization = 3; // 车间系到定位系的位姿 + + double position_repeatability_m = 4; // 位置重复性 + double yaw_repeatability_rad = 5; // 航向重复性 + double residual_error_m = 6; // 残差位置误差 + double residual_error_rad = 7; // 残差姿态误差 + + double tracking_loss_ratio = 8; // 跟踪丢失比例 + double time_sync_offset_ms = 9; // 时间同步偏差（ms） + bool validated_as_truth_source = 10; // 是否已验证可作为真值参考源 + string localization_source_id = 11; // 该结果对应的外部定位源 ID + string workcell_zone_id = 12; // 该结果对应的工位 / 区域 ID +} + +// ========================================================= +// 外部定位结果写入请求 +// 作用：将 Linux 求解出的外部定位结果写入系统 +// ========================================================= +message CommitExternalLocalizationResultRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string parameter_version = 2; // 结果版本号 + ExternalLocalizationCalibrationResult result = 3; // 外部定位结果 + string commit_reason = 4; // 写入原因 + .agv.calibration.common.FileDigest digest = 5; // 摘要 + bool persistent_write = 6; // 是否持久化 +} + +// ========================================================= +// 查询当前已生效外部定位结果请求 +// ========================================================= +message GetAppliedExternalLocalizationResultRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 +} + +// ========================================================= +// 查询当前已生效外部定位结果响应 +// ========================================================= +message AppliedExternalLocalizationResultResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string parameter_version = 4; // 当前版本 + ExternalLocalizationCalibrationResult result = 5; // 当前生效结果 + int64 applied_timestamp_us = 6; // 生效时间 +} + +// ========================================================= +// 外部定位任务结果 +// 作用：返回求解 / 验证后的结果摘要，供编排器判断是否进入下一阶段 +// ========================================================= +message ExternalLocalizationJobResult { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string job_id = 4; // 任务 ID + + ExternalLocalizationTaskRequest.TaskPurpose task_purpose = 5; // 任务目的 + + bool data_quality_passed = 6; // 数据质量是否达标 + bool suitable_for_commit = 7; // 是否适合写入 + string recommended_parameter_version = 8; // 推荐结果版本号 + + ExternalLocalizationValidationSummary validation_summary = 9; // 真值参考验证摘要 + ExternalLocalizationCalibrationResult result = 10; // 结果内容 + repeated .agv.calibration.common.FileReference artifacts = 11; // 关联产物 +} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/sensor_calibration.proto b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/sensor_calibration.proto new file mode 100644 index 0000000..38d0096 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/sensor_calibration.proto @@ -0,0 +1,464 @@ +syntax = "proto3"; + +package agv.calibration.sensor; + +import "calibration_common.proto"; +import "vehicle_profile.proto"; + +// ========================================================= +// 文件作用：传感器标定执行代理协议 +// 运行关系： +// Ubuntu 车间电脑(Linux) -> Windows 车端代理 / 采集服务 +// 说明： +// 1) Linux 负责图像 / 点云 / 激光扫描 / IMU 数据分析与参数求解 +// 2) 车端 / 采集服务只负责组织采集、回传观测状态、写入结果 +// 3) 支持相机内参、IMU 内参、传感器外参、手眼标定 +// 4) 支持相机“只做内参 / 只做外参 / 内外参都做” +// ========================================================= + +// ========================================================= +// 服务：传感器标定服务 +// 运行位置：Windows 车端代理 或 Linux 采集桥接服务 +// 调用方：Ubuntu 车间电脑 +// ========================================================= +service AgvCalibSensorService { + // 心跳保活 + rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) + returns (.agv.calibration.common.HeartbeatResponse); + + // 查询传感器标定服务当前是否具备执行条件 + rpc GetSensorReadiness(.agv.calibration.common.AgentReadinessRequest) + returns (SensorReadinessResponse); + + // 设置传感器标定工作模式 + rpc SetSensorCalibrationWorkMode(SensorCalibrationWorkModeRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询传感器标定能力 + rpc GetSensorCalibrationCapability(SensorCalibrationCapabilityRequest) + returns (SensorCalibrationCapabilityResponse); + + // 启动一个传感器标定任务 + rpc StartSensorCalibrationTask(SensorCalibrationTaskRequest) + returns (.agv.calibration.common.JobAccepted); + + // 查询传感器标定任务状态 + rpc GetSensorCalibrationJobStatus(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.JobStatus); + + // 查询传感器标定任务结果 + rpc GetSensorCalibrationJobResult(.agv.calibration.common.JobQuery) + returns (SensorCalibrationJobResult); + + // 取消传感器标定任务 + rpc CancelSensorCalibrationJob(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.StandardResponse); + + // 打开传感器标定遥测流 + rpc StreamSensorCalibrationTelemetry(StreamSensorCalibrationTelemetryRequest) + returns (stream SensorCalibrationTelemetry); + + // 写入传感器标定参数 + rpc CommitSensorCalibrationParameters(CommitSensorCalibrationParametersRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询当前已生效传感器参数 + rpc GetAppliedSensorCalibrationParameters(GetAppliedSensorCalibrationParametersRequest) + returns (AppliedSensorCalibrationParametersResponse); + + // 紧急停止 + rpc EmergencyStop(.agv.calibration.common.Empty) + returns (.agv.calibration.common.StandardResponse); +} + +// ========================================================= +// 传感器标定工作模式 +// 作用：切换采集服务到不同模式 +// ========================================================= +message SensorCalibrationWorkModeRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + + enum Mode { + SENSOR_CALIBRATION_MODE_UNSPECIFIED = 0; // 未指定 + NORMAL_MODE = 1; // 正常模式 + CALIBRATION_READY_MODE = 2; // 标定准备模式 + DATA_CAPTURE_MODE = 3; // 采集模式 + VALIDATION_MODE = 4; // 验证模式 + } + + Mode target_mode = 2; // 目标模式 + string reason = 3; // 切换原因 +} + +// ========================================================= +// 传感器服务就绪响应 +// 作用：返回当前传感器服务是否满足执行条件 +// ========================================================= +message SensorReadinessResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool agent_ready = 4; // 服务本身是否就绪 + bool capture_pipeline_ready = 5; // 采集链路是否就绪 + bool storage_ready = 6; // 存储链路是否就绪 + bool telemetry_ready = 7; // 遥测链路是否就绪 + bool vehicle_safe_to_move = 8; // 若任务需移动车辆，当前是否允许移动 + bool arm_ready = 9; // 若任务涉及机械臂，机械臂是否就绪 + + repeated string ready_sensor_ids = 10; // 已就绪的传感器 ID + repeated .agv.calibration.common.ReadinessIssue issues = 11; // 不满足项 + int64 checked_timestamp_us = 12; // 检查时间 +} + +// ========================================================= +// 查询传感器标定能力请求 +// 作用：查询当前车辆和工位支持的传感器标定能力 +// ========================================================= +message SensorCalibrationCapabilityRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 +} + +// ========================================================= +// 查询传感器标定能力响应 +// 作用：返回支持的传感器标定类型 +// ========================================================= +message SensorCalibrationCapabilityResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + + bool supports_camera_intrinsic = 4; // 是否支持相机内参标定 + bool supports_imu_intrinsic = 5; // 是否支持 IMU 内参标定 + bool supports_sensor_to_base_extrinsic = 6; // 是否支持传感器到 base_link 外参 + bool supports_hand_eye = 7; // 是否支持手眼标定 + + bool supports_3d_lidar_extrinsic = 8; // 是否支持 3D 激光雷达外参 + bool supports_2d_lidar_extrinsic = 9; // 是否支持 2D 激光雷达外参 + bool supports_camera_only_intrinsic = 10; // 是否支持仅做相机内参 + bool supports_camera_only_extrinsic = 11; // 是否支持仅做相机外参 +} + +// ========================================================= +// 采集意图类型 +// 作用：告诉采集服务“这次数据是为哪类标定而采” +// ========================================================= +enum CaptureIntentType { + CAPTURE_INTENT_TYPE_UNSPECIFIED = 0; // 未指定 + CAMERA_INTRINSIC_CAPTURE = 1; // 相机内参采集 + CAMERA_EXTRINSIC_CAPTURE = 2; // 相机外参采集 + LIDAR_3D_EXTRINSIC_CAPTURE = 3; // 3D 激光雷达外参采集 + LIDAR_2D_EXTRINSIC_CAPTURE = 4; // 2D 激光雷达外参采集 + IMU_STATIC_CAPTURE = 5; // IMU 静态采集 + IMU_DYNAMIC_CAPTURE = 6; // IMU 动态采集 + HAND_EYE_CAPTURE = 7; // 手眼标定采集 + VALIDATION_CAPTURE = 8; // 验证采集 +} + +message CalibrationReferenceTarget { + string reference_target_id = 1; // 参考目标唯一 ID + string reference_target_type = 2; // 类型，例如 board / pole / wall / ground_plane + string description = 3; // 说明 +} + +// ========================================================= +// 相机内参标定任务 +// 作用：采集图像并求解相机内参 +// ========================================================= +message CameraIntrinsicCalibrationTask { + string sensor_id = 1; // 目标相机 ID + uint32 required_image_count = 2; // 所需图像数 + double timeout_sec = 3; // 超时时间 + string target_board_id = 4; // 标定板 ID + CalibrationReferenceTarget reference_target = 5; // 统一参考目标 +} + +// ========================================================= +// IMU 内参标定任务 +// 作用：采集 IMU 静态 / 动态数据并求解偏置等参数 +// ========================================================= +message IMUIntrinsicCalibrationTask { + string sensor_id = 1; // 目标 IMU ID + uint32 required_static_segment_count = 2; // 静止段数量 + uint32 required_motion_segment_count = 3; // 运动段数量 + double timeout_sec = 4; // 超时时间 +} + +// ========================================================= +// 传感器到 base_link 外参标定任务 +// 作用：求解单个传感器与车辆 base_link 的位姿关系 +// 说明： +// 1) 相机可只做外参 +// 2) 3D / 2D 激光雷达通常只做外参 +// 3) IMU 可做外参 +// ========================================================= +message SensorToBaseExtrinsicCalibrationTask { + string sensor_id = 1; // 目标传感器 ID + string base_frame_id = 2; // 车辆 base_link / 基准 frame + uint32 required_sample_count = 3; // 所需样本数 + double timeout_sec = 4; // 超时时间 + CaptureIntentType capture_intent = 5; // 采集意图 + CalibrationReferenceTarget reference_target = 6; // 统一参考目标 +} + +// ========================================================= +// 手眼标定模式 +// 作用：区分眼在手上与眼在手外 +// ========================================================= +enum HandEyeCalibrationMode { + HAND_EYE_CALIBRATION_MODE_UNSPECIFIED = 0; // 未指定 + EYE_IN_HAND = 1; // 眼在手上 + EYE_TO_HAND = 2; // 眼在手外 +} + +// ========================================================= +// 手眼标定任务 +// 作用：求解机械臂相机与机械臂 / 车体的关系 +// ========================================================= +message HandEyeCalibrationTask { + string sensor_id = 1; // 目标相机 ID + string arm_id = 2; // 机械臂 ID + HandEyeCalibrationMode mode = 3; // 手眼模式 + uint32 required_pose_count = 4; // 所需位姿数 + double timeout_sec = 5; // 超时时间 + CalibrationReferenceTarget reference_target = 6; // 统一参考目标 +} + +// ========================================================= +// 传感器标定任务子类型 +// 作用：在顶层 task_type 之下进一步区分具体算法分支 +// ========================================================= +enum SensorCalibrationTaskSubtype { + SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED = 0; + FRONT_CAMERA_INTRINSIC = 1; + DOWNWARD_CAMERA_INTRINSIC = 2; + IMU_INTRINSIC = 3; + FRONT_CAMERA_EXTRINSIC = 4; + DOWNWARD_CAMERA_EXTRINSIC = 5; + IMU_EXTRINSIC = 6; + LIDAR_2D_EXTRINSIC = 7; + LIDAR_3D_EXTRINSIC = 8; + EYE_IN_HAND = 9; + EYE_TO_HAND = 10; +} + +// ========================================================= +// 传感器标定任务请求 +// 作用：启动一次传感器标定任务 +// 发送方：Ubuntu 车间电脑 +// 接收方：Windows 车端代理 / Linux 采集服务 +// ========================================================= +message SensorCalibrationTaskRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string test_case_id = 2; // 测试用例 ID + + enum TaskPurpose { + SENSOR_TASK_PURPOSE_UNSPECIFIED = 0; // 未指定 + DATA_COLLECTION = 1; // 数据采集 + SOLVING = 2; // 求解 + VALIDATION = 3; // 验证 + } + + TaskPurpose task_purpose = 3; // 任务目的 + SensorCalibrationTaskSubtype task_subtype = 4; // 任务子类型 + + oneof task { + CameraIntrinsicCalibrationTask camera_intrinsic = 5; // 相机内参 + IMUIntrinsicCalibrationTask imu_intrinsic = 6; // IMU 内参 + SensorToBaseExtrinsicCalibrationTask sensor_to_base_extrinsic = 7; // 外参 + HandEyeCalibrationTask hand_eye = 8; // 手眼标定 + } + + string source_iteration_id = 9; // 来源迭代号 +} + +// ========================================================= +// 传感器遥测流请求 +// 作用：配置传感器标定过程中的状态上报 +// ========================================================= +message StreamSensorCalibrationTelemetryRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + uint32 expected_hz = 2; // 期望上报频率 + bool include_observation_progress = 3; // 是否包含采集进度 + bool include_quality_metrics = 4; // 是否包含质量指标 + bool include_capture_intent = 5; // 是否包含采集意图 +} + +// ========================================================= +// 传感器标定遥测 +// 作用：回传采集进度、观测质量和任务执行状态 +// ========================================================= +message SensorCalibrationTelemetry { + int64 hardware_timestamp_us = 1; // 硬件时间戳 + string sensor_id = 2; // 传感器 ID + .agv.calibration.vehicle.profile.SensorType sensor_type = 3; // 传感器类型 + uint32 collected_sample_count = 4; // 已采集样本数 + uint32 target_sample_count = 5; // 目标样本数 + bool target_detected = 6; // 当前是否检测到标定目标 + double quality_score = 7; // 当前观测质量评分 + string active_job_id = 8; // 当前任务 ID + CaptureIntentType capture_intent = 9; // 当前采集意图 +} + +// ========================================================= +// 相机模型类型 +// 作用：表达相机内参使用的模型 +// ========================================================= +enum CameraModelType { + CAMERA_MODEL_TYPE_UNSPECIFIED = 0; // 未指定 + PINHOLE = 1; // 针孔模型 + FISHEYE = 2; // 鱼眼模型 +} + +// ========================================================= +// 相机内参 +// 作用：表达相机标定后的内参结果 +// ========================================================= +message CameraIntrinsics { + uint32 image_width = 1; // 图像宽度 + uint32 image_height = 2; // 图像高度 + double fx = 3; // 焦距 fx + double fy = 4; // 焦距 fy + double cx = 5; // 主点 cx + double cy = 6; // 主点 cy + repeated double distortion_coeffs = 7; // 畸变系数 + string distortion_model = 8; // 畸变模型名称 + CameraModelType camera_model_type = 9; // 相机模型类型 +} + +// ========================================================= +// IMU 内参 +// 作用：表达 IMU 偏置等内参结果 +// 说明： +// 1) accel_bias 为加速度计三轴偏置 +// 2) gyro_bias 为陀螺仪三轴偏置 +// 3) 采用强约束三维向量，不再使用 repeated double +// ========================================================= +message IMUIntrinsics { + .agv.calibration.common.Vector3d accel_bias = 1; // 加速度计偏置 + .agv.calibration.common.Vector3d gyro_bias = 2; // 陀螺仪偏置 +} + +// ========================================================= +// 传感器外参 +// 作用：表达传感器与车辆 base_link 的位姿关系 +// 说明：旋转统一使用弧度 +// ========================================================= +message SensorExtrinsics { + string parent_frame_id = 1; // 父坐标系，一般为 base_link + string child_frame_id = 2; // 子坐标系，一般为 sensor frame + .agv.calibration.common.Pose3dEuler parent_to_child = 3; // 父到子的位姿 +} + +// ========================================================= +// 单个传感器标定参数 +// 作用：统一表达一个传感器的内参和 / 或外参 +// 说明： +// 1) 对相机可只填 camera_intrinsics +// 2) 对相机也可只填 extrinsics +// 3) 对 IMU 可填 imu_intrinsics 和 / 或 extrinsics +// 4) 对激光雷达通常只填 extrinsics +// ========================================================= +message SensorCalibrationParameter { + string sensor_id = 1; // 传感器 ID + .agv.calibration.vehicle.profile.SensorType sensor_type = 2; // 传感器类型 + CameraIntrinsics camera_intrinsics = 3; // 相机内参；仅相机适用 + IMUIntrinsics imu_intrinsics = 4; // IMU 内参；仅 IMU 适用 + SensorExtrinsics extrinsics = 5; // 传感器外参 +} + +// ========================================================= +// 传感器标定参数总包 +// 作用：统一表达整车所有传感器的标定结果 +// ========================================================= +message SensorCalibrationParameterSet { + repeated SensorCalibrationParameter items = 1; // 参数项列表 +} + +// ========================================================= +// 标定产物类型 +// 作用：用于区分不同采集任务产生的文件类型 +// ========================================================= +enum ArtifactType { + ARTIFACT_TYPE_UNSPECIFIED = 0; // 未指定 + IMAGE_FILE = 1; // 图像文件 + POINT_CLOUD_FILE = 2; // 点云文件 + LASER_SCAN_FILE = 3; // 2D 激光雷达扫描文件 + IMU_SEQUENCE_FILE = 4; // IMU 序列文件 + POSE_SEQUENCE_FILE = 5; // 位姿序列文件 + REPORT_FILE = 6; // 报告文件 +} + +// ========================================================= +// 标定产物 +// 作用：记录一次任务关联的文件产物 +// ========================================================= +message CalibrationArtifact { + ArtifactType artifact_type = 1; // 产物类型 + .agv.calibration.common.FileReference file = 2; // 文件引用 + string sensor_id = 3; // 对应传感器 ID +} + +// ========================================================= +// 传感器验证摘要 +// 作用：返回传感器专项自动验收的关键指标 +// ========================================================= +message SensorValidationSummary { + double reprojection_error_px = 1; // 相机重投影误差 + double translation_residual_m = 2; // 外参平移残差 + double rotation_residual_rad = 3; // 外参旋转残差 + double plane_residual_m = 4; // 平面 / 配准残差 + double repeatability_error_m = 5; // 重复性误差 + bool auto_acceptance_passed = 6; // 自动验收是否通过 +} + +// ========================================================= +// 写入传感器标定参数请求 +// 作用：将 Linux 求解出的传感器参数写入系统 +// ========================================================= +message CommitSensorCalibrationParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string parameter_version = 2; // 参数版本号 + SensorCalibrationParameterSet params = 3; // 参数总包 + string commit_reason = 4; // 写入原因 + .agv.calibration.common.FileDigest digest = 5; // 摘要 + bool persistent_write = 6; // 是否持久化 +} + +// ========================================================= +// 查询当前已生效传感器参数请求 +// ========================================================= +message GetAppliedSensorCalibrationParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 +} + +// ========================================================= +// 查询当前已生效传感器参数响应 +// ========================================================= +message AppliedSensorCalibrationParametersResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string parameter_version = 4; // 当前版本 + SensorCalibrationParameterSet params = 5; // 当前生效参数 + int64 applied_timestamp_us = 6; // 生效时间 +} + +// ========================================================= +// 传感器标定任务结果 +// 作用：返回某次传感器标定 / 验证的结果摘要 +// ========================================================= +message SensorCalibrationJobResult { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string job_id = 4; // 任务 ID + + bool data_quality_passed = 5; // 数据质量是否达标 + bool suitable_for_commit = 6; // 是否适合写入 + string recommended_parameter_version = 7; // 推荐参数版本 + + SensorValidationSummary validation_summary = 8; // 验证摘要 + SensorCalibrationParameterSet estimated_params = 9; // 本轮估计参数 + repeated CalibrationArtifact artifacts = 10; // 关联产物 +} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/vehicle_profile.proto b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/vehicle_profile.proto new file mode 100644 index 0000000..678fccd --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/vehicle_profile.proto @@ -0,0 +1,355 @@ +syntax = "proto3"; + +package agv.calibration.vehicle.profile; + +import "calibration_common.proto"; + +// ├── msg/ +// │ ├── ChassisType.msg +// │ ├── SensorType.msg +// │ ├── CameraMountType.msg +// │ ├── ControlAxisType.msg +// │ ├── ControllerAlgorithmType.msg +// │ ├── CalibrationAbilityType.msg +// │ ├── CalibrationModuleType.msg +// │ ├── WorkflowStageType.msg +// │ ├── VehicleBaseInfo.msg +// │ ├── MechanicalArmProfile.msg +// │ ├── SensorProfile.msg +// │ ├── ControllerProfile.msg +// │ ├── ControllerSelection.msg +// │ ├── CalibrationCapability.msg +// │ ├── VehicleProfile.msg +// │ ├── RegisterOrUpdateVehicleProfileRequest.msg +// │ ├── VehicleProfileQuery.msg +// │ ├── VehicleProfileResponse.msg +// │ ├── EvaluateVehicleCalibrationApplicabilityRequest.msg +// │ ├── ApplicabilityIssue.msg +// │ └── VehicleCalibrationApplicabilityResponse.msg +// ├── srv/ +// │ ├── RegisterOrUpdateVehicleProfile.srv +// │ ├── GetVehicleProfile.srv +// │ └── EvaluateVehicleCalibrationApplicability.srv +// └── action/ +// └── （当前无需新增 action） + +// ========================================================= +// 文件作用：车辆画像与能力描述协议 +// 使用范围： +// 1) 车间在开始整车标定前，先描述“要标定的到底是什么车” +// 2) 供车间编排器根据车辆画像自动生成执行计划 +// 3) 供各专项服务判断当前车辆是否支持对应标定任务 +// ========================================================= + +// ========================================================= +// 服务：车辆画像服务 +// 运行位置：Ubuntu 车间电脑 +// 调用方：车间编排器、配置界面、专项服务 +// ========================================================= +service VehicleProfileService { + // 心跳保活 + rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) + returns (.agv.calibration.common.HeartbeatResponse); + + // 注册 / 更新一份车辆画像 + rpc RegisterOrUpdateVehicleProfile(RegisterOrUpdateVehicleProfileRequest) + returns (.agv.calibration.common.StandardResponse); + + // 查询车辆画像 + rpc GetVehicleProfile(VehicleProfileQuery) + returns (VehicleProfileResponse); + + // 评估当前车辆画像支持哪些标定能力 + rpc EvaluateVehicleCalibrationApplicability( + EvaluateVehicleCalibrationApplicabilityRequest) + returns (VehicleCalibrationApplicabilityResponse); +} + +// ========================================================= +// 底盘类型 +// 作用：描述车辆底盘运动学形式 +// ========================================================= +enum ChassisType { + CHASSIS_TYPE_UNSPECIFIED = 0; // 未指定 + ACKERMANN = 1; // 阿克曼 + DIFFERENTIAL = 2; // 差速 + SINGLE_STEER_WHEEL = 3; // 单舵轮 + MULTI_STEER_WHEEL = 4; // 多舵轮 +} + +// ========================================================= +// 传感器类型 +// 作用：描述车辆上挂载的传感器种类 +// ========================================================= +enum SensorType { + SENSOR_TYPE_UNSPECIFIED = 0; // 未指定 + DOWNWARD_CAMERA = 1; // 下视相机 + FRONT_CAMERA = 2; // 前视相机 + ARM_CAMERA = 3; // 机械臂相机 + LIDAR_3D = 4; // 3D 激光雷达 + LIDAR_2D = 5; // 2D 激光雷达 + IMU = 6; // IMU +} + +// ========================================================= +// 相机安装形式 +// 作用：描述相机与车体 / 机械臂的安装关系 +// ========================================================= +enum CameraMountType { + CAMERA_MOUNT_TYPE_UNSPECIFIED = 0; // 未指定 + FIXED_ON_BASE = 1; // 固定在车体上 + DOWNWARD_MOUNTED = 2; // 下视安装 + FRONT_MOUNTED = 3; // 前视安装 + EYE_IN_HAND = 4; // 眼在手上 + EYE_TO_HAND = 5; // 眼在手外 +} + +// ========================================================= +// 控制轴类型 +// 作用：将控制器分成横向控制器与纵向控制器 +// ========================================================= +enum ControlAxisType { + CONTROL_AXIS_UNSPECIFIED = 0; // 未指定 + LATERAL_CONTROL = 1; // 横向控制 + LONGITUDINAL_CONTROL = 2; // 纵向控制 +} + +// ========================================================= +// 控制算法类型 +// 作用：描述车辆当前支持的控制算法 +// ========================================================= +enum ControllerAlgorithmType { + CONTROLLER_ALGORITHM_UNSPECIFIED = 0; // 未指定 + PID = 1; // PID + MPC = 2; // MPC + LQR = 3; // LQR + PURE_PURSUIT = 4; // Pure Pursuit +} + +// ========================================================= +// 标定能力类型 +// 作用：用于编排器判断某个专项是否适用于当前车辆 +// ========================================================= +enum CalibrationAbilityType { + CALIBRATION_ABILITY_UNSPECIFIED = 0; // 未指定 + EXTERNAL_LOCALIZATION_CALIBRATION = 1; // 外部定位 / 真值参考接入 + CHASSIS_CALIBRATION = 2; // 底盘标定 + CONTROL_CALIBRATION = 3; // 运控参数标定 + SENSOR_CALIBRATION = 4; // 传感器标定 + HAND_EYE_CALIBRATION = 5; // 手眼标定 +} + +// ========================================================= +// 标定模块类型 +// 作用：供总控编排、执行事件、报告汇总时标识任务属于哪个模块 +// 说明： +// 1) 该枚举会被 workshop_orchestration.proto 直接引用 +// 2) 这里不仅包含传统“标定模块”，也补充了预检、联合验收、归档等总控阶段 +// ========================================================= +enum CalibrationModuleType { + CALIBRATION_MODULE_UNSPECIFIED = 0; // 未指定 + EXTERNAL_LOCALIZATION_MODULE = 1; // 外部定位 / 真值参考模块 + CHASSIS_MODULE = 2; // 底盘标定模块 + CONTROL_MODULE = 3; // 运控标定模块 + SENSOR_INTRINSIC_MODULE = 4; // 传感器内参标定模块 + SENSOR_EXTRINSIC_MODULE = 5; // 传感器外参标定模块 + HAND_EYE_MODULE = 6; // 手眼标定模块 + WORKSHOP_PRECHECK_MODULE = 7; // 车间预检模块 + JOINT_ACCEPTANCE_MODULE = 8; // 整车联合验收模块 + PARAMETER_COMMIT_MODULE = 9; // 参数固化模块 + REPORT_ARCHIVE_MODULE = 10; // 报告与归档模块 +} + +// ========================================================= +// 工作流阶段类型 +// 作用：用于描述自动化标定车间完整流程中的阶段 +// 说明： +// 1) 该枚举比 CalibrationModuleType 更偏“流程阶段” +// 2) 可供后续编排器构建执行计划时直接使用 +// ========================================================= +enum WorkflowStageType { + WORKFLOW_STAGE_UNSPECIFIED = 0; // 未指定 + PROFILE_VALIDATION_STAGE = 1; // 车辆画像校验阶段 + EXTERNAL_REFERENCE_READY_CHECK_STAGE = 2; // 真值参考接入与确认阶段 + WORKSHOP_PRECHECK_STAGE = 3; // 整车预检阶段 + CHASSIS_CALIBRATION_STAGE = 4; // 底盘标定阶段 + CONTROL_CALIBRATION_STAGE = 5; // 运控参数调优阶段 + SENSOR_INTRINSIC_CALIBRATION_STAGE = 6; // 传感器内参标定阶段 + SENSOR_EXTRINSIC_CALIBRATION_STAGE = 7; // 传感器外参标定阶段 + HAND_EYE_CALIBRATION_STAGE = 8; // 手眼标定阶段 + JOINT_ACCEPTANCE_STAGE = 9; // 整车联合验收阶段 + PARAMETER_COMMIT_STAGE = 10; // 参数固化阶段 + REPORT_ARCHIVE_STAGE = 11; // 报告归档阶段 +} + +// ========================================================= +// 车辆基础信息 +// 作用：表达车辆身份信息 +// ========================================================= +message VehicleBaseInfo { + string vehicle_id = 1; // 车辆唯一 ID + string vehicle_name = 2; // 车辆显示名 + string model_name = 3; // 车型 / 型号 + string serial_number = 4; // 出厂序列号 + string manufacturer = 5; // 厂商 + string description = 6; // 备注说明 +} + +// ========================================================= +// 机械臂画像 +// 作用：描述车辆是否带机械臂以及机械臂的基本信息 +// ========================================================= +message MechanicalArmProfile { + bool has_mechanical_arm = 1; // 是否带机械臂 + string arm_id = 2; // 机械臂 ID + string arm_model = 3; // 机械臂型号 + uint32 dof = 4; // 自由度 + string arm_base_frame = 5; // 机械臂基座坐标系 + string tool_frame = 6; // 末端工具坐标系 +} + +// ========================================================= +// 传感器画像 +// 作用：描述单个传感器的安装与参与标定属性 +// 说明： +// 1) enabled 表示这台车物理上是否启用了该传感器 +// 2) selected_for_this_session 表示本轮标定是否纳入执行计划 +// 3) needs_intrinsic_calibration / needs_extrinsic_calibration 表示本轮是否需要做对应标定 +// ========================================================= +message SensorProfile { + string sensor_id = 1; // 传感器 ID + SensorType sensor_type = 2; // 传感器类型 + string sensor_name = 3; // 传感器显示名 + string frame_id = 4; // 传感器 frame_id + CameraMountType camera_mount_type = 5;// 相机安装形式；非相机可忽略 + bool enabled = 6; // 当前是否启用 + bool needs_intrinsic_calibration = 7; // 本轮是否需要做内参标定 + bool needs_extrinsic_calibration = 8; // 本轮是否需要做外参标定 + string device_hint = 9; // 设备名 / topic / IP / 端口等提示信息 + bool selected_for_this_session = 10; // 本轮是否纳入执行计划 + bool supports_intrinsic_calibration = 11; // 该传感器是否支持内参标定 + bool supports_extrinsic_calibration = 12; // 该传感器是否支持外参标定 +} + +// ========================================================= +// 控制器画像 +// 作用：描述某一控制轴支持的算法集合 +// 说明： +// 1) 该结构偏静态能力描述 +// 2) 表达“这个控制轴支持哪些算法、默认算法是什么” +// ========================================================= +message ControllerProfile { + ControlAxisType control_axis = 1; // 控制轴类型 + repeated ControllerAlgorithmType supported_algorithms = 2; // 支持的算法列表 + ControllerAlgorithmType default_algorithm = 3; // 默认算法 +} + +// ========================================================= +// 控制器选择项 +// 作用：表达“本轮标定到底要启用哪个控制轴、哪个算法、是否需要调优” +// 说明： +// 1) 该结构偏会话/配方侧选择 +// 2) 用于解决“横向用 MPC，纵向用 PID”这类组合问题 +// ========================================================= +message ControllerSelection { + ControlAxisType control_axis = 1; // 控制轴类型 + ControllerAlgorithmType algorithm_type = 2; // 选中的算法类型 + bool enabled = 3; // 本轮是否启用该控制器 + bool need_tuning = 4; // 本轮是否需要对该控制器调优 + bool is_default = 5; // 是否为当前默认控制器 +} + +// ========================================================= +// 标定能力项 +// 作用：表达当前车辆是否支持某项标定 +// ========================================================= +message CalibrationCapability { + CalibrationAbilityType ability_type = 1; // 标定能力类型 + bool supported = 2; // 是否支持 + string message = 3; // 说明文字 +} + +// ========================================================= +// 车辆画像总包 +// 作用：统一表达待标定车辆的全部静态配置 +// 发送方：配置界面 / 车间系统 +// 接收方：车间编排器 / 各专项服务 +// ========================================================= +message VehicleProfile { + VehicleBaseInfo base_info = 1; // 车辆基础信息 + ChassisType chassis_type = 2; // 底盘类型 + MechanicalArmProfile arm_profile = 3; // 机械臂画像 + repeated SensorProfile sensors = 4; // 所有传感器画像 + repeated ControllerProfile controllers = 5; // 控制器能力画像 + .agv.calibration.common.FileReference urdf_file = 6; // 车辆 URDF 文件引用 + string base_link_frame = 7; // 车辆 base_link 名称 + repeated CalibrationCapability capabilities = 8; // 标定能力清单 + repeated .agv.calibration.common.KeyValuePair metadata = 9; // 扩展元数据 + + repeated ControllerSelection controller_selections = 10; // 本轮控制器选择配置 + repeated WorkflowStageType enabled_workflow_stages = 11; // 本轮启用的流程阶段 + string profile_version = 12; // 车辆画像版本号 + .agv.calibration.common.FileDigest profile_digest = 13; // 车辆画像摘要 +} + +// ========================================================= +// 注册 / 更新车辆画像请求 +// 作用：向系统写入一份车辆画像 +// ========================================================= +message RegisterOrUpdateVehicleProfileRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + VehicleProfile profile = 2; // 车辆画像 +} + +// ========================================================= +// 车辆画像查询请求 +// 作用：按 vehicle_id 查询车辆画像 +// ========================================================= +message VehicleProfileQuery { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string vehicle_id = 2; // 车辆 ID +} + +// ========================================================= +// 车辆画像查询响应 +// 作用：返回完整车辆画像 +// ========================================================= +message VehicleProfileResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + VehicleProfile profile = 4; // 车辆画像 +} + +// ========================================================= +// 标定适用性评估请求 +// 作用：根据车辆画像分析能否执行各项标定 +// ========================================================= +message EvaluateVehicleCalibrationApplicabilityRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + VehicleProfile profile_snapshot = 2; // 车辆画像快照 +} + +// ========================================================= +// 适用性问题 +// 作用：描述不支持 / 风险 / 缺失项 +// ========================================================= +message ApplicabilityIssue { + CalibrationAbilityType ability_type = 1; // 对应能力 + bool blocking = 2; // 是否为阻塞问题 + string message = 3; // 问题说明 +} + +// ========================================================= +// 标定适用性评估响应 +// 作用：返回支持项与阻塞项，供编排器生成计划 +// ========================================================= +message VehicleCalibrationApplicabilityResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + repeated CalibrationCapability capabilities = 4; // 支持能力列表 + repeated ApplicabilityIssue issues = 5; // 问题列表 + bool overall_supported = 6; // 是否总体可开展标定 + repeated WorkflowStageType recommended_workflow_stages = 7; // 推荐执行阶段 +} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/workshop_orchestration.proto b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/workshop_orchestration.proto new file mode 100644 index 0000000..48e2d0a --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/win_ubuntu_bridge/proto/workshop_orchestration.proto @@ -0,0 +1,554 @@ +syntax = "proto3"; + +package agv.calibration.workshop; + +import "calibration_common.proto"; +import "vehicle_profile.proto"; + +// ========================================================= +// 文件作用：自动化标定车间总编排协议 +// 使用范围： +// 1) 负责整车标定会话的创建、规划、执行、暂停、恢复、取消 +// 2) 负责人工确认节点、审批节点、回滚、报告归档 +// 3) 负责把外部定位 / 底盘 / 运控 / 传感器标定串成完整闭环 +// 4) 负责根据车辆画像与阶段配置自动生成执行计划 +// ========================================================= + +// ========================================================= +// 服务：车间总编排服务 +// 运行位置：Ubuntu 车间电脑 +// 调用方：上位机界面、自动化车间入口程序、调度系统 +// ========================================================= +service WorkshopOrchestrationService { + // 心跳保活 + rpc Heartbeat(.agv.calibration.common.HeartbeatRequest) + returns (.agv.calibration.common.HeartbeatResponse); + + // 创建一场新的整车标定会话 + rpc CreateWorkshopSession(CreateWorkshopSessionRequest) + returns (CreateWorkshopSessionResponse); + + // 查询会话信息 + rpc GetWorkshopSession(WorkshopSessionQuery) + returns (WorkshopSessionResponse); + + // 生成本次会话的执行计划 + rpc BuildExecutionPlan(BuildExecutionPlanRequest) + returns (BuildExecutionPlanResponse); + + // 重建执行计划 + rpc RebuildExecutionPlan(RebuildExecutionPlanRequest) + returns (BuildExecutionPlanResponse); + + // 校验当前执行计划 + rpc ValidateExecutionPlan(ValidateExecutionPlanRequest) + returns (ValidateExecutionPlanResponse); + + // 执行车间预检 / readiness 检查 + rpc RunWorkshopPrecheck(RunWorkshopPrecheckRequest) + returns (.agv.calibration.common.JobAccepted); + + // 查询最近一次预检结果 + rpc GetLastWorkshopPrecheckResult(WorkshopSessionQuery) + returns (WorkshopPrecheckResponse); + + // 启动整场自动化标定 + rpc StartWorkshopSession(StartWorkshopSessionRequest) + returns (.agv.calibration.common.JobAccepted); + + // 查询整场会话对应长任务状态 + rpc GetWorkshopJobStatus(.agv.calibration.common.JobQuery) + returns (.agv.calibration.common.JobStatus); + + // 暂停整场会话 + rpc PauseWorkshopSession(PauseWorkshopSessionRequest) + returns (.agv.calibration.common.StandardResponse); + + // 恢复整场会话 + rpc ResumeWorkshopSession(ResumeWorkshopSessionRequest) + returns (.agv.calibration.common.StandardResponse); + + // 取消整场会话 + rpc CancelWorkshopSession(CancelWorkshopSessionRequest) + returns (.agv.calibration.common.StandardResponse); + + // 打开会话事件流 + rpc StreamWorkshopEvent(WorkshopSessionQuery) + returns (stream WorkshopEvent); + + // 人工确认某个等待中的手动步骤 + rpc AcknowledgeManualStep(ManualStepAckRequest) + returns (.agv.calibration.common.StandardResponse); + + // 审批某个阶段的结果 + rpc ApproveStageResult(ApproveStageResultRequest) + returns (.agv.calibration.common.StandardResponse); + + // 对某阶段参数进行回滚 + rpc RollbackStageParameters(RollbackStageParametersRequest) + returns (.agv.calibration.common.StandardResponse); + + // 获取最终车间报告 + rpc GetWorkshopReport(WorkshopSessionQuery) + returns (WorkshopReportResponse); +} + +// ========================================================= +// 会话状态 +// 作用：描述整场整车标定会话当前所处阶段 +// ========================================================= +enum WorkshopSessionState { + WORKSHOP_SESSION_STATE_UNSPECIFIED = 0; // 未指定 + DRAFT = 1; // 草稿，尚未规划 + PLANNING = 2; // 正在规划 + READY = 3; // 已规划完成，可启动 + PRECHECK_RUNNING = 4; // 预检执行中 + RUNNING = 5; // 运行中 + PAUSED = 6; // 已暂停 + WAITING_MANUAL_ACTION = 7; // 等待人工动作 + WAITING_APPROVAL = 8; // 等待审批 + SUCCEEDED = 9; // 整场成功 + FAILED = 10; // 整场失败 + CANCELED = 11; // 已取消 + ROLLED_BACK = 12; // 已回滚 +} + +// ========================================================= +// 阶段执行策略 +// 作用：控制某个阶段失败时系统该如何处理 +// ========================================================= +enum StageExecutionPolicy { + STAGE_EXECUTION_POLICY_UNSPECIFIED = 0; // 未指定 + REQUIRED = 1; // 必须执行，失败即整场失败 + OPTIONAL = 2; // 可选执行，失败后可继续 + SKIP_IF_UNSUPPORTED = 3; // 若车辆不支持则自动跳过 +} + +// ========================================================= +// 审批状态 +// 作用：描述某一阶段结果是否需要人工审核 +// ========================================================= +enum ApprovalState { + APPROVAL_STATE_UNSPECIFIED = 0; // 未指定 + APPROVAL_NOT_REQUIRED = 1; // 不需要审批 + APPROVAL_PENDING = 2; // 待审批 + APPROVED = 3; // 已通过 + REJECTED = 4; // 已拒绝 +} + +// ========================================================= +// 人工动作类型 +// 作用：表达需要人工介入的节点类型 +// ========================================================= +enum ManualActionType { + MANUAL_ACTION_TYPE_UNSPECIFIED = 0; // 未指定 + PLACE_TARGET_BOARD = 1; // 放置标靶 / 标定板 + CONFIRM_WORKCELL_CLEAR = 2; // 确认工位区域安全 + CONFIRM_VEHICLE_POSE = 3; // 确认车辆姿态已就位 + CONFIRM_ARM_HOME = 4; // 确认机械臂已回零 + CONFIRM_SENSOR_INSTALLATION = 5; // 确认传感器安装状态 + CONFIRM_READY_TO_CONTINUE = 6; // 确认可继续执行 + OTHER_MANUAL_ACTION = 7; // 其他人工动作 +} + +// ========================================================= +// 车间事件类型 +// 作用：用于异步通知界面 / 上层系统 +// ========================================================= +enum WorkshopEventType { + WORKSHOP_EVENT_TYPE_UNSPECIFIED = 0; // 未指定 + SESSION_STATE_CHANGED = 1; // 会话状态变化 + EXECUTION_PLAN_BUILT = 2; // 执行计划已生成 + PRECHECK_STARTED = 3; // 预检开始 + PRECHECK_COMPLETED = 4; // 预检完成 + STAGE_STARTED = 5; // 阶段开始 + STAGE_COMPLETED = 6; // 阶段完成 + STAGE_FAILED = 7; // 阶段失败 + MANUAL_ACTION_REQUIRED = 8; // 需要人工操作 + APPROVAL_REQUIRED = 9; // 需要审批 + SAFETY_TRIGGERED = 10; // 安全保护触发 + ROLLBACK_EXECUTED = 11; // 已执行回滚 + REPORT_READY = 12; // 报告已生成 +} + +// ========================================================= +// 操作员信息 +// 作用：记录本次会话的人工责任归属 +// ========================================================= +message WorkshopOperatorInfo { + string operator_id = 1; // 操作员工号 + string operator_name = 2; // 操作员姓名 + string workstation_id = 3; // 工位 ID + string shift_id = 4; // 班次 ID +} + +// ========================================================= +// 单项标定任务配置 +// 作用：表达本次会话中某类流程阶段是否启用 +// ========================================================= +message RequestedCalibrationTask { + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 1; // 阶段类型 + bool enabled = 2; // 是否启用 + bool require_manual_approval = 3; // 是否要求人工审批 + StageExecutionPolicy execution_policy = 4; // 执行策略 + string reason = 5; // 启用 / 禁用原因 +} + +// ========================================================= +// 会话配置 +// 作用：描述整场自动化标定运行策略 +// ========================================================= +message WorkshopSessionConfig { + repeated RequestedCalibrationTask requested_tasks = 1; // 请求执行的阶段列表 + bool auto_commit_parameters = 2; // 是否自动写入参数 + bool require_manual_approval_before_commit = 3; // 写入前是否要求审批 + bool run_validation_after_each_stage = 4; // 每阶段后是否自动复测 + bool stop_on_first_failure = 5; // 是否首错即停 + bool allow_optional_stage_skip = 6; // 是否允许可选阶段跳过 + bool enable_auto_rollback_on_validation_failure = 7; // 验证失败是否自动回滚 + bool allow_rebuild_execution_plan = 8; // 是否允许运行前重建计划 +} + +// ========================================================= +// 阶段计划 +// 作用：描述车间编排器生成的一步执行计划 +// ========================================================= +message StagePlan { + string stage_id = 1; // 阶段 ID + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 2; // 流程阶段类型 + .agv.calibration.vehicle.profile.CalibrationModuleType module_type = 3; // 所属模块类型 + string display_name = 4; // 阶段显示名 + uint32 order_index = 5; // 阶段顺序号 + + StageExecutionPolicy execution_policy = 6; // 执行策略 + repeated string depends_on_stage_ids = 7; // 依赖的前置阶段 ID + bool requires_manual_confirmation_before_start = 8; // 开始前是否要求人工确认 + bool requires_approval_before_commit = 9; // 参数提交前是否要求审批 + ManualActionType manual_action_type = 10; // 若为人工阶段，对应的人工动作类型 + string executor_endpoint_name = 11; // 实际执行的端点名（可对应 action / service） + string description = 12; // 阶段说明 + uint32 retry_limit = 13; // 最大重试次数 + bool auto_generated = 14; // 是否由系统自动生成 + repeated string target_resource_ids = 15; // 阶段关联的资源 ID 列表 + repeated .agv.calibration.common.KeyValuePair metadata = 16; // 扩展元数据 +} + +// ========================================================= +// 执行计划校验问题 +// 作用：描述当前阶段计划中的冲突、缺项或风险 +// ========================================================= +message ExecutionPlanValidationIssue { + bool blocking = 1; // 是否为阻塞问题 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string issue_code = 3; // 问题代码 + string message = 4; // 问题说明 + string related_stage_id = 5; // 关联阶段 ID +} + +// ========================================================= +// 车间会话 +// 作用：统一表达整场标定会话的核心状态 +// ========================================================= +message WorkshopSession { + string session_id = 1; // 会话 ID + WorkshopSessionState state = 2; // 会话状态 + + .agv.calibration.vehicle.profile.VehicleProfile vehicle_profile_snapshot = 3; // 车辆画像快照 + WorkshopOperatorInfo operator_info = 4; // 操作员信息 + WorkshopSessionConfig config = 5; // 会话配置 + repeated StagePlan stage_plan = 6; // 阶段计划 + + int64 created_timestamp_us = 7; // 创建时间 + int64 updated_timestamp_us = 8; // 最近更新时间 + string active_stage_id = 9; // 当前活跃阶段 ID + + string workshop_line_id = 10; // 所属工位 / 产线 ID + string active_job_id = 11; // 当前整场执行的 job_id + .agv.calibration.common.FileDigest vehicle_profile_digest = 12; // 车辆画像摘要 +} + +// ========================================================= +// 创建会话请求 +// 作用：创建一场新的整车标定会话 +// ========================================================= +message CreateWorkshopSessionRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + .agv.calibration.vehicle.profile.VehicleProfile vehicle_profile_snapshot = 2; // 车辆画像快照 + WorkshopOperatorInfo operator_info = 3; // 操作员信息 + WorkshopSessionConfig config = 4; // 会话配置 + string workshop_line_id = 5; // 产线 / 工位线 ID + bool auto_build_execution_plan = 6; // 创建后是否自动生成计划 +} + +// ========================================================= +// 创建会话响应 +// 作用：返回新建会话结果 +// ========================================================= +message CreateWorkshopSessionResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + string session_id = 4; // 新建会话 ID + WorkshopSessionState state = 5; // 初始状态 + WorkshopSession session = 6; // 完整会话内容 +} + +// ========================================================= +// 会话查询请求 +// 作用：按 session_id 查询会话 +// ========================================================= +message WorkshopSessionQuery { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID +} + +// ========================================================= +// 会话查询响应 +// 作用：返回整场会话信息 +// ========================================================= +message WorkshopSessionResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + WorkshopSession session = 4; // 会话详情 +} + +// ========================================================= +// 生成执行计划请求 +// 作用：根据车辆画像与配置生成阶段计划 +// ========================================================= +message BuildExecutionPlanRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + bool validate_profile_before_build = 3; // 生成前是否先校验车辆画像 +} + +// ========================================================= +// 重建执行计划请求 +// 作用：在修改配置 / 修正依赖关系后重建计划 +// ========================================================= +message RebuildExecutionPlanRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string rebuild_reason = 3; // 重建原因 + bool keep_finished_stage_results = 4; // 是否保留已完成阶段结果 +} + +// ========================================================= +// 生成执行计划响应 +// 作用：返回生成后的阶段计划 +// ========================================================= +message BuildExecutionPlanResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + repeated StagePlan stage_plan = 4; // 阶段计划 + repeated string warnings = 5; // 警告信息 +} + +// ========================================================= +// 校验执行计划请求 +// 作用：检查当前阶段计划是否闭合、依赖是否正确、资源是否冲突 +// ========================================================= +message ValidateExecutionPlanRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID +} + +// ========================================================= +// 校验执行计划响应 +// 作用：返回当前执行计划的校验结果 +// ========================================================= +message ValidateExecutionPlanResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + bool valid = 4; // 计划是否有效 + repeated ExecutionPlanValidationIssue issues = 5; // 校验问题列表 +} + +// ========================================================= +// 车间预检请求 +// 作用：在正式启动前检查车辆、工位、服务、传感器、资源锁等是否就绪 +// ========================================================= +message RunWorkshopPrecheckRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID +} + +// ========================================================= +// 单个预检项 +// 作用：表达一条 readiness / 安全 / 资源检查结果 +// ========================================================= +message PrecheckItem { + string item_code = 1; // 检查项编码 + string display_name = 2; // 检查项名称 + bool passed = 3; // 是否通过 + bool blocking = 4; // 是否为阻塞项 + .agv.calibration.common.ErrorCode error_code = 5; // 错误码 + string message = 6; // 结果说明 + .agv.calibration.vehicle.profile.WorkflowStageType related_stage_type = 7; // 关联阶段 + .agv.calibration.vehicle.profile.CalibrationModuleType related_module_type = 8; // 关联模块 +} + +// ========================================================= +// 车间预检结果 +// 作用：返回最近一次预检详细结果 +// ========================================================= +message WorkshopPrecheckResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + bool all_passed = 4; // 是否全部通过 + repeated PrecheckItem items = 5; // 检查项明细 + int64 checked_timestamp_us = 6; // 检查时间 + uint32 blocking_issue_count = 7; // 阻塞问题数量 + bool ready_for_start = 8; // 是否可进入正式执行 +} + +// ========================================================= +// 启动会话请求 +// 作用：正式启动整场自动化标定 +// ========================================================= +message StartWorkshopSessionRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID +} + +// ========================================================= +// 暂停会话请求 +// 作用：暂停整场标定 +// ========================================================= +message PauseWorkshopSessionRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string reason = 3; // 暂停原因 +} + +// ========================================================= +// 恢复会话请求 +// 作用：从暂停态恢复整场标定 +// ========================================================= +message ResumeWorkshopSessionRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string reason = 3; // 恢复原因 +} + +// ========================================================= +// 取消会话请求 +// 作用：终止整场标定 +// ========================================================= +message CancelWorkshopSessionRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string reason = 3; // 取消原因 +} + +// ========================================================= +// 车间事件 +// 作用：向界面 / 上层系统异步推送状态变化 +// ========================================================= +message WorkshopEvent { + int64 server_timestamp_us = 1; // 服务端时间戳 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + + WorkshopEventType event_type = 4; // 事件类型 + WorkshopSessionState session_state = 5; // 当前会话状态 + .agv.calibration.common.JobState stage_job_state = 6; // 当前阶段任务状态 + + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 7; // 流程阶段类型 + .agv.calibration.vehicle.profile.CalibrationModuleType module_type = 8; // 模块类型 + + bool requires_manual_ack = 9; // 是否需要人工确认 + ApprovalState approval_state = 10; // 审批状态 + string message = 11; // 事件说明 +} + +// ========================================================= +// 人工步骤确认请求 +// 作用：当系统停在人工步骤时，由操作员确认完成 +// ========================================================= +message ManualStepAckRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + bool confirmed = 4; // 是否确认完成 + string note = 5; // 备注 +} + +// ========================================================= +// 阶段结果审批请求 +// 作用：对某阶段结果进行通过 / 驳回 +// ========================================================= +message ApproveStageResultRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + ApprovalState decision = 4; // 审批结论 + string reviewer_id = 5; // 审批人 ID + string comment = 6; // 审批说明 +} + +// ========================================================= +// 参数回滚请求 +// 作用：对某阶段参数回滚到指定版本 +// ========================================================= +message RollbackStageParametersRequest { + .agv.calibration.common.RequestHeader header = 1; // 请求头 + string session_id = 2; // 会话 ID + string stage_id = 3; // 阶段 ID + string target_parameter_version = 4; // 回滚目标版本 + string reason = 5; // 回滚原因 +} + +// ========================================================= +// 阶段结果摘要 +// 作用：用于最终报告中概述每个阶段的结果 +// ========================================================= +message StageResultSummary { + string stage_id = 1; // 阶段 ID + .agv.calibration.vehicle.profile.WorkflowStageType stage_type = 2; // 阶段类型 + .agv.calibration.vehicle.profile.CalibrationModuleType module_type = 3; // 模块类型 + + bool success = 4; // 是否成功 + bool auto_acceptance_passed = 5; // 自动验收是否通过 + .agv.calibration.common.JobState final_job_state = 6; // 最终任务状态 + ApprovalState approval_state = 7; // 审批状态 + string parameter_version = 8; // 该阶段产出的参数版本 + + repeated .agv.calibration.common.FileReference artifacts = 9; // 关联产物 + string summary = 10; // 摘要说明 + + bool rolled_back = 11; // 是否发生回滚 + string failure_root_cause = 12; // 失败根因说明 + repeated .agv.calibration.common.KeyValuePair metadata = 13; // 阶段结果扩展信息 +} + +// ========================================================= +// 车间最终报告 +// 作用：归档整场会话的总体结果与所有阶段结果 +// ========================================================= +message WorkshopReport { + string session_id = 1; // 会话 ID + bool overall_success = 2; // 是否总体成功 + int64 started_timestamp_us = 3; // 开始时间 + int64 finished_timestamp_us = 4; // 结束时间 + repeated StageResultSummary stage_results = 5; // 各阶段结果 + repeated .agv.calibration.common.FileReference report_files = 6; // 报告文件 + string summary = 7; // 总结说明 + string active_parameter_bundle_version = 8; // 最终生效参数包版本 + repeated .agv.calibration.common.KeyValuePair metadata = 9; // 报告扩展元数据 +} + +// ========================================================= +// 车间报告响应 +// 作用：返回最终归档报告 +// ========================================================= +message WorkshopReportResponse { + bool success = 1; // 是否成功 + .agv.calibration.common.ErrorCode error_code = 2; // 错误码 + string message = 3; // 说明 + WorkshopReport report = 4; // 报告内容 +} \ No newline at end of file