feat: 自动化标定车间v1.0.....
This commit is contained in:
li-shihao-code
@@ -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`。
|
||||
@@ -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
|
||||
@@ -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** 用来解耦流程和算法实现。
|
||||
@@ -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 始终只负责编排，不直接承载算法。
|
||||
@@ -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 编译
|
||||
@@ -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 接口一起核对。
|
||||
@@ -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()
|
||||
@@ -0,0 +1 @@
|
||||
PySide6>=6.6.0
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||
)
|
||||
|
||||
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()
|
||||
@@ -1,23 +0,0 @@
|
||||
<root main_tree_to_execute="MainCalibrationFlow">
|
||||
<BehaviorTree ID="MainCalibrationFlow">
|
||||
<Sequence name="全自动标定主干">
|
||||
|
||||
<Sequence name="Phase_1_Chassis">
|
||||
<MockConnectAGV />
|
||||
<MockCallChassisAlgo />
|
||||
</Sequence>
|
||||
|
||||
<Sequence name="Phase_2_Control">
|
||||
<MockTuneControl />
|
||||
<MockCallControlAlgo />
|
||||
</Sequence>
|
||||
|
||||
<Sequence name="Phase_3_Sensor">
|
||||
<MockMoveAndCapture />
|
||||
<MockCallSensorAlgo />
|
||||
<MockCommitAllParams />
|
||||
</Sequence>
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -1,9 +0,0 @@
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence name="全自动标定总流程">
|
||||
<SetChassisMode target_mode="1" />
|
||||
<TriggerCapture sensor_id="cam_front" capture_code_out="{shared_code}" />
|
||||
<DownloadData sensor_id="cam_front" capture_code_in="{shared_code}" save_dir="/tmp/calib_data" saved_path_out="{saved_image_path}" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -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()
|
||||
+37
@@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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
|
||||
+160
@@ -0,0 +1,160 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<calibration_chassis_interfaces::msg::ChassisTelemetry> 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<calibration_control_interfaces::msg::ControlTelemetry> 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<calibration_sensor_interfaces::msg::SensorCalibrationTelemetry> sensor_telemetry_history;
|
||||
|
||||
// ===== 外部定位 / 真值源反馈 =====
|
||||
// 外部定位就绪状态：是否适合作为参考源、时间同步是否达标、质量摘要等。
|
||||
calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness;
|
||||
|
||||
// 最新一帧外部定位遥测：真值位姿、标准差、时间同步偏差、质量分数等。
|
||||
calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry;
|
||||
|
||||
// 历史外部定位遥测窗口：用于和车端里程计、控制误差、标定结果做对齐分析。
|
||||
std::vector<calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry> 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
|
||||
+30
@@ -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
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
|
||||
// readiness 服务：让编排器确认当前底盘是否具备执行条件。
|
||||
rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
|
||||
// 车辆画像客户端：从 vehicle_profile_manager 取当前车辆的底盘类型。
|
||||
rclcpp::Client<GetVehicleProfileSrv>::SharedPtr get_vehicle_profile_client_;
|
||||
// action server：接收底盘动作原语任务，并在后台线程里执行。
|
||||
rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
|
||||
|
||||
// readiness 回调：把底盘运行状态翻译成统一的 readiness 响应。
|
||||
void handle_readiness(
|
||||
const std::shared_ptr<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> response);
|
||||
// goal 回调：在真正执行之前先做一次最小合法性检查。
|
||||
rclcpp_action::GoalResponse handle_goal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const ExecuteTask::Goal> goal);
|
||||
// cancel 回调：当前最小版本直接接受取消，真正的中止逻辑后面再接。
|
||||
rclcpp_action::CancelResponse handle_cancel(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
// goal 接收后回调：把长任务放到后台线程，避免卡住 ROS 回调线程。
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleExecuteTask> 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<GoalHandleExecuteTask> goal_handle);
|
||||
|
||||
// 底盘标定算法模板：负责承接输入校验、底盘类型分发与结果填充。
|
||||
ChassisCalibrationAlgorithmTemplate algorithm_;
|
||||
};
|
||||
|
||||
} // namespace chassis_calibration_service
|
||||
+33
@@ -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,
|
||||
])
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>chassis_calibration_service</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Minimal chassis calibration service stub.</description>
|
||||
<maintainer email="you@example.com">you</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>calibration_common_interfaces</depend>
|
||||
<depend>calibration_chassis_interfaces</depend>
|
||||
<depend>calibration_control_interfaces</depend>
|
||||
<depend>calibration_external_localization_interfaces</depend>
|
||||
<depend>calibration_sensor_interfaces</depend>
|
||||
<depend>calibration_vehicle_profile_interfaces</depend>
|
||||
<depend>calibration_workshop_orchestration_interfaces</depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+56
@@ -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
|
||||
+81
@@ -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
|
||||
+80
@@ -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
|
||||
+185
@@ -0,0 +1,185 @@
|
||||
#include "chassis_calibration_service/chassis_calibration_service_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#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::microseconds>(
|
||||
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<ReadinessSrv>(
|
||||
"/chassis/get_readiness",
|
||||
std::bind(&ChassisCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// 车辆画像客户端：底盘类型、车辆静态配置等都从这里查询。
|
||||
get_vehicle_profile_client_ = create_client<GetVehicleProfileSrv>("/vehicle_profile_manager/get_vehicle_profile");
|
||||
|
||||
// action：底盘动作原语执行入口，后续真实算法也从这里接入。
|
||||
execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
|
||||
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<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> 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<const ExecuteTask::Goal> 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<GoalHandleExecuteTask> /*goal_handle*/)
|
||||
{
|
||||
// 当前最小版本接受取消请求，但不在这里实现复杂中止逻辑。
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void ChassisCalibrationServiceNode::handle_accepted(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> 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<GetVehicleProfileSrv::Request>();
|
||||
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<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
// 先发一个进度反馈，让前端和 orchestrator 知道底盘任务已经开始运行。
|
||||
auto feedback = std::make_shared<ExecuteTask::Feedback>();
|
||||
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<ExecuteTask::Result>();
|
||||
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<ExecuteTask::Result>();
|
||||
result->result = output.response.result;
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
|
||||
auto result = std::make_shared<ExecuteTask::Result>();
|
||||
result->result = output.response.result;
|
||||
goal_handle->succeed(result);
|
||||
}
|
||||
|
||||
} // namespace chassis_calibration_service
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(chassis_calibration_service::ChassisCalibrationServiceNode)
|
||||
+24
@@ -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
|
||||
@@ -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<chassis_calibration_service::ChassisCalibrationServiceNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+24
@@ -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
|
||||
+24
@@ -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
|
||||
@@ -1,6 +0,0 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
# 动态指定要加载的 XML 剧本文件名
|
||||
tree_xml_filename: "main_tree.xml"
|
||||
# 行为树的 Tick 循环频率 (毫秒)
|
||||
tick_rate_ms: 50
|
||||
@@ -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()
|
||||
+42
@@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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
|
||||
+224
@@ -0,0 +1,224 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<calibration_control_interfaces::msg::TrajectoryPoint> 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<calibration_control_interfaces::msg::ControlTelemetry> control_telemetry_history;
|
||||
|
||||
// 控制内部诊断扩展：为后续接入真实控制器内部状态预留。
|
||||
// 例如：积分项、观测器状态、预测窗口代价、约束激活信息、状态机状态等。
|
||||
std::vector<std::string> control_internal_diagnostics;
|
||||
|
||||
// 执行器饱和原因扩展：用于区分是转向打满、油门受限、制动限幅还是安全逻辑介入。
|
||||
std::vector<std::string> 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<calibration_chassis_interfaces::msg::ChassisTelemetry> 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<calibration_sensor_interfaces::msg::SensorCalibrationTelemetry> sensor_telemetry_history;
|
||||
|
||||
// 传感器质量诊断扩展：例如丢帧、视觉目标丢失、IMU 饱和、时间戳抖动等。
|
||||
std::vector<std::string> sensor_quality_diagnostics;
|
||||
|
||||
// ===== 外部定位 / 真值源反馈 =====
|
||||
// 外部定位就绪状态：用于判断真值源是否可以参与控制评估与自动验收。
|
||||
calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness;
|
||||
|
||||
// 最新一帧外部定位遥测：真值位姿、同步误差、质量评分、观测协方差等。
|
||||
calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry;
|
||||
|
||||
// 历史外部定位遥测窗口：用于和控制误差、轨迹点、底盘反馈做时序对齐。
|
||||
std::vector<calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry> external_localization_telemetry_history;
|
||||
|
||||
// 真值源质量诊断扩展：例如时间同步超标、观测缺失、协方差异常、切源事件等。
|
||||
std::vector<std::string> 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
|
||||
+37
@@ -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
|
||||
+52
@@ -0,0 +1,52 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
using ControllerAlgorithmType = calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType;
|
||||
|
||||
rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
|
||||
rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
|
||||
ControlCalibrationAlgorithmTemplate algorithm_;
|
||||
|
||||
void handle_readiness(
|
||||
const std::shared_ptr<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> response);
|
||||
rclcpp_action::GoalResponse handle_goal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const ExecuteTask::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_cancel(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
void execute_goal(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
|
||||
ControllerAlgorithmType resolve_controller_algorithm(const ExecuteTask::Goal & goal) const;
|
||||
};
|
||||
|
||||
} // namespace control_calibration_service
|
||||
+31
@@ -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,
|
||||
])
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>control_calibration_service</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Minimal control calibration service stub.</description>
|
||||
<maintainer email="you@example.com">you</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>calibration_chassis_interfaces</depend>
|
||||
<depend>calibration_common_interfaces</depend>
|
||||
<depend>calibration_control_interfaces</depend>
|
||||
<depend>calibration_external_localization_interfaces</depend>
|
||||
<depend>calibration_sensor_interfaces</depend>
|
||||
<depend>calibration_vehicle_profile_interfaces</depend>
|
||||
<depend>calibration_workshop_orchestration_interfaces</depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+83
@@ -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
|
||||
+173
@@ -0,0 +1,173 @@
|
||||
#include "control_calibration_service/control_calibration_service_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#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::microseconds>(
|
||||
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<ReadinessSrv>(
|
||||
"/control/get_readiness",
|
||||
std::bind(&ControlCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
|
||||
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<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> 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<const ExecuteTask::Goal> 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<GoalHandleExecuteTask> /*goal_handle*/)
|
||||
{
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void ControlCalibrationServiceNode::handle_accepted(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
std::thread(std::bind(&ControlCalibrationServiceNode::execute_goal, this, goal_handle)).detach();
|
||||
}
|
||||
|
||||
void ControlCalibrationServiceNode::execute_goal(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
// 先回一帧 RUNNING feedback，告诉 orchestrator 当前任务已经进入执行阶段。
|
||||
auto feedback = std::make_shared<ExecuteTask::Feedback>();
|
||||
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<ExecuteTask::Result>();
|
||||
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<ExecuteTask::Result>(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)
|
||||
+33
@@ -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
|
||||
@@ -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<control_calibration_service::ControlCalibrationServiceNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+37
@@ -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
|
||||
+56
@@ -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
|
||||
+33
@@ -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
|
||||
@@ -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()
|
||||
+36
@@ -0,0 +1,36 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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
|
||||
+192
@@ -0,0 +1,192 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry>
|
||||
external_localization_telemetry_history;
|
||||
|
||||
// 真值源质量诊断扩展：例如时间同步超标、观测缺失、切源事件、协方差异常、目标丢失等。
|
||||
std::vector<std::string> truth_source_diagnostics;
|
||||
|
||||
// 标靶检测与对齐诊断扩展：例如标靶检测失败、有效角点不足、位姿解算不稳定等。
|
||||
std::vector<std::string> marker_alignment_diagnostics;
|
||||
|
||||
// 数据采集与落盘诊断扩展：例如原始位姿采集窗口不足、采样频率不满足、落盘失败等。
|
||||
std::vector<std::string> 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<calibration_chassis_interfaces::msg::ChassisTelemetry> 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<calibration_control_interfaces::msg::ControlTelemetry> 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<calibration_sensor_interfaces::msg::SensorCalibrationTelemetry> sensor_telemetry_history;
|
||||
|
||||
// 传感器质量诊断扩展：例如相机丢帧、IMU 饱和、LiDAR 数据稀疏、时间戳抖动等。
|
||||
std::vector<std::string> 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
|
||||
+57
@@ -0,0 +1,57 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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
|
||||
+48
@@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
|
||||
rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
|
||||
rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
|
||||
ExternalReferenceExecutor executor_;
|
||||
|
||||
void handle_readiness(
|
||||
const std::shared_ptr<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> response);
|
||||
rclcpp_action::GoalResponse handle_goal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const ExecuteTask::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_cancel(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
void execute_goal(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
};
|
||||
|
||||
} // namespace external_localization_service
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>external_localization_service</name>
|
||||
<version>0.0.1</version>
|
||||
<description>External localization service skeleton for reference checking and truth-source validation.</description>
|
||||
<maintainer email="you@example.com">you</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>calibration_common_interfaces</depend>
|
||||
<depend>calibration_chassis_interfaces</depend>
|
||||
<depend>calibration_control_interfaces</depend>
|
||||
<depend>calibration_sensor_interfaces</depend>
|
||||
<depend>calibration_external_localization_interfaces</depend>
|
||||
<depend>calibration_vehicle_profile_interfaces</depend>
|
||||
<depend>calibration_workshop_orchestration_interfaces</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+66
@@ -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
|
||||
+120
@@ -0,0 +1,120 @@
|
||||
#include "external_localization_service/external_localization_service_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#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::microseconds>(
|
||||
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<ReadinessSrv>(
|
||||
"/external_localization/get_readiness",
|
||||
std::bind(&ExternalLocalizationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
|
||||
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<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> 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<const ExecuteTask::Goal> 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<GoalHandleExecuteTask> /*goal_handle*/)
|
||||
{
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void ExternalLocalizationServiceNode::handle_accepted(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
std::thread(std::bind(&ExternalLocalizationServiceNode::execute_goal, this, goal_handle)).detach();
|
||||
}
|
||||
|
||||
void ExternalLocalizationServiceNode::execute_goal(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
auto feedback = std::make_shared<ExecuteTask::Feedback>();
|
||||
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<ExecuteTask::Result>();
|
||||
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
|
||||
+137
@@ -0,0 +1,137 @@
|
||||
#include "external_localization_service/external_reference_executor.hpp"
|
||||
|
||||
#include <string>
|
||||
|
||||
#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
|
||||
@@ -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<external_localization_service::ExternalLocalizationServiceNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+66
@@ -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
|
||||
+65
@@ -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
|
||||
+77
@@ -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
|
||||
@@ -1,25 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <behaviortree_cpp_v3/bt_factory.h>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <string>
|
||||
|
||||
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<bool> is_running_;
|
||||
};
|
||||
|
||||
} // namespace agv_calib_core
|
||||
@@ -1,109 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <behaviortree_cpp_v3/action_node.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
// 引入底层 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<win_ubuntu_bridge::srv::SetDiagnosticMode>("/chassis_gateway/set_diagnostic_mode");
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() { return { BT::InputPort<int>("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<win_ubuntu_bridge::srv::SetDiagnosticMode::Request>(); 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<win_ubuntu_bridge::srv::SetDiagnosticMode>::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<win_ubuntu_bridge::srv::TriggerSyncCapture>("/sensor_gateway/trigger_sync_capture");
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return { BT::InputPort<std::string>("sensor_id"), BT::OutputPort<int64_t>("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<win_ubuntu_bridge::srv::TriggerSyncCapture::Request>(); 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<win_ubuntu_bridge::srv::TriggerSyncCapture>::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<win_ubuntu_bridge::action::DownloadSensorData>(node_, "/sensor_gateway/download_sensor_data");
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return { BT::InputPort<std::string>("sensor_id"), BT::InputPort<int64_t>("capture_code_in"),
|
||||
BT::InputPort<std::string>("save_dir"), BT::OutputPort<std::string>("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<win_ubuntu_bridge::action::DownloadSensorData>::SendGoalOptions();
|
||||
send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle<win_ubuntu_bridge::action::DownloadSensorData>::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<win_ubuntu_bridge::action::DownloadSensorData>::SharedPtr action_client_;
|
||||
bool done_ = false; bool success_ = false;
|
||||
};
|
||||
|
||||
} // namespace agv_calib_core
|
||||
@@ -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])
|
||||
@@ -1,22 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>agv_calib_core</name>
|
||||
<version>1.0.0</version>
|
||||
<description>行为树总控大脑</description>
|
||||
<maintainer email="2469171725@qq.com">nvidia</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>behaviortree_cpp_v3</depend>
|
||||
<depend>ament_index_cpp</depend>
|
||||
<depend>win_ubuntu_bridge</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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()
|
||||
+45
@@ -0,0 +1,45 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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
|
||||
+246
@@ -0,0 +1,246 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<calibration_sensor_interfaces::msg::SensorCalibrationTelemetry> sensor_telemetry_history;
|
||||
|
||||
// 采集质量诊断扩展：例如棋盘格检测失败、LiDAR 反光不足、IMU 饱和、图像模糊等。
|
||||
std::vector<std::string> capture_quality_diagnostics;
|
||||
|
||||
// 存储与产物诊断扩展：例如落盘失败、目录权限、磁盘空间不足、文件损坏等。
|
||||
std::vector<std::string> 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<calibration_chassis_interfaces::msg::ChassisTelemetry> 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<calibration_control_interfaces::msg::ControlTelemetry> control_telemetry_history;
|
||||
|
||||
// ===== 外部定位 / 真值源反馈 =====
|
||||
calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse external_localization_readiness;
|
||||
calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry latest_external_localization_telemetry;
|
||||
std::vector<calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry> external_localization_telemetry_history;
|
||||
std::vector<std::string> 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
|
||||
+34
@@ -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
|
||||
+53
@@ -0,0 +1,53 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
using TaskType = calibration_sensor_interfaces::msg::SensorCalibrationTaskType;
|
||||
|
||||
rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
|
||||
rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
|
||||
SensorCalibrationAlgorithmTemplate algorithm_;
|
||||
|
||||
void handle_readiness(
|
||||
const std::shared_ptr<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> response);
|
||||
rclcpp_action::GoalResponse handle_goal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const ExecuteTask::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_cancel(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
void execute_goal(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
|
||||
|
||||
std::string resolve_target_sensor_id(const ExecuteTask::Goal & goal) const;
|
||||
};
|
||||
|
||||
} // namespace sensor_calibration_service
|
||||
@@ -0,0 +1,23 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sensor_calibration_service</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Sensor calibration service template for algorithm integration.</description>
|
||||
<maintainer email="you@example.com">you</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>calibration_chassis_interfaces</depend>
|
||||
<depend>calibration_common_interfaces</depend>
|
||||
<depend>calibration_control_interfaces</depend>
|
||||
<depend>calibration_external_localization_interfaces</depend>
|
||||
<depend>calibration_sensor_interfaces</depend>
|
||||
<depend>calibration_vehicle_profile_interfaces</depend>
|
||||
<depend>calibration_workshop_orchestration_interfaces</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+50
@@ -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
|
||||
+34
@@ -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
|
||||
+24
@@ -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
|
||||
@@ -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<sensor_calibration_service::SensorCalibrationServiceNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+112
@@ -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
|
||||
+167
@@ -0,0 +1,167 @@
|
||||
#include "sensor_calibration_service/sensor_calibration_service_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#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::microseconds>(
|
||||
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<ReadinessSrv>(
|
||||
"/sensor_calibration/get_readiness",
|
||||
std::bind(&SensorCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
|
||||
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<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> 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<const ExecuteTask::Goal> 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<GoalHandleExecuteTask> /*goal_handle*/)
|
||||
{
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void SensorCalibrationServiceNode::handle_accepted(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
std::thread(std::bind(&SensorCalibrationServiceNode::execute_goal, this, goal_handle)).detach();
|
||||
}
|
||||
|
||||
void SensorCalibrationServiceNode::execute_goal(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
// 先回一帧 RUNNING feedback，告诉 orchestrator 当前任务已经进入执行阶段。
|
||||
auto feedback = std::make_shared<ExecuteTask::Feedback>();
|
||||
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<ExecuteTask::Result>();
|
||||
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<ExecuteTask::Result>(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
|
||||
+76
@@ -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
|
||||
@@ -1,87 +0,0 @@
|
||||
#include "agv_calib_core/brain_node.hpp"
|
||||
#include "agv_calib_core/bt_ros2_nodes.hpp"
|
||||
|
||||
#include <behaviortree_cpp_v3/bt_factory.h>
|
||||
#include <behaviortree_cpp_v3/loggers/bt_cout_logger.h>
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
|
||||
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<std::string>("tree_xml_filename", "main_tree.xml");
|
||||
this->declare_parameter<int>("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<SetChassisModeNode>("SetChassisMode",
|
||||
[this](const std::string& name, const BT::NodeConfiguration& config) {
|
||||
return std::make_unique<SetChassisModeNode>(name, config, this);
|
||||
});
|
||||
|
||||
factory.registerBuilder<TriggerCaptureNode>("TriggerCapture",
|
||||
[this](const std::string& name, const BT::NodeConfiguration& config) {
|
||||
return std::make_unique<TriggerCaptureNode>(name, config, this);
|
||||
});
|
||||
|
||||
factory.registerBuilder<DownloadDataNode>("DownloadData",
|
||||
[this](const std::string& name, const BT::NodeConfiguration& config) {
|
||||
return std::make_unique<DownloadDataNode>(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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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('<II', header)
|
||||
payload = recv_exactly(conn, payload_len) if payload_len > 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('<II', msg_type, len(data)) + data)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. 消息类型枚举（MsgType）
|
||||
|
||||
### 3.1 底盘域（端口 9001）
|
||||
|
||||
| 值 | 名称 | 方向 | 说明 |
|
||||
|----|------|------|------|
|
||||
| 1 | `CHASSIS_GET_READINESS_REQ` | Ubuntu → Windows | 查询底盘是否就绪 |
|
||||
| 2 | `CHASSIS_GET_READINESS_RSP` | Windows → Ubuntu | 底盘就绪响应 |
|
||||
| 3 | `CHASSIS_MOTION_PRIMITIVE_REQ` | Ubuntu → Windows | 执行动作原语 |
|
||||
| 4 | `CHASSIS_MOTION_PRIMITIVE_RSP` | Windows → Ubuntu | 动作原语执行结果 |
|
||||
| 5 | `CHASSIS_EMERGENCY_BRAKE_REQ` | Ubuntu → Windows | 紧急制动 |
|
||||
| 6 | `CHASSIS_EMERGENCY_BRAKE_RSP` | Windows → Ubuntu | 紧急制动响应 |
|
||||
|
||||
### 3.2 运控域（端口 9002）
|
||||
|
||||
| 值 | 名称 | 方向 | 说明 |
|
||||
|----|------|------|------|
|
||||
| 11 | `CONTROL_GET_READINESS_REQ` | Ubuntu → Windows | 查询运控是否就绪 |
|
||||
| 12 | `CONTROL_GET_READINESS_RSP` | Windows → Ubuntu | 运控就绪响应 |
|
||||
| 13 | `CONTROL_EVALUATION_REQ` | Ubuntu → Windows | 执行控制评估任务 |
|
||||
| 14 | `CONTROL_EVALUATION_RSP` | Windows → Ubuntu | 控制评估结果 |
|
||||
|
||||
---
|
||||
|
||||
## 4. 底盘域 payload 格式（端口 9001）
|
||||
|
||||
### 4.1 CHASSIS_GET_READINESS_REQ（type=1）
|
||||
|
||||
Ubuntu 发送，询问底盘当前是否具备执行标定动作的条件。
|
||||
|
||||
```json
|
||||
{
|
||||
"agent_name": "chassis",
|
||||
"include_details": true,
|
||||
"target_resource_ids": []
|
||||
}
|
||||
```
|
||||
|
||||
| 字段 | 类型 | 说明 |
|
||||
|------|------|------|
|
||||
| `agent_name` | string | 固定为 `"chassis"` |
|
||||
| `include_details` | bool | 是否返回详细问题列表 |
|
||||
| `target_resource_ids` | string[] | 指定资源 ID，通常为空 |
|
||||
|
||||
### 4.2 CHASSIS_GET_READINESS_RSP（type=2）
|
||||
|
||||
Windows 返回，描述底盘当前状态。**所有布尔字段均须填写**。
|
||||
|
||||
```json
|
||||
{
|
||||
"success": true,
|
||||
"error_code": {"code": 0},
|
||||
"message": "底盘就绪。",
|
||||
"agent_ready": true,
|
||||
"chassis_driver_online": true,
|
||||
"motion_control_ready": true,
|
||||
"estop_released": true,
|
||||
"vehicle_safe_to_move": true,
|
||||
"telemetry_available": true,
|
||||
"checked_timestamp_us": 1711234567000000
|
||||
}
|
||||
```
|
||||
|
||||
| 字段 | 类型 | 说明 |
|
||||
|------|------|------|
|
||||
| `success` | bool | 查询本身是否成功 |
|
||||
| `error_code.code` | uint32 | 错误码（0=OK，见附录 A）|
|
||||
| `message` | string | 人读说明 |
|
||||
| `agent_ready` | bool | 底盘代理服务是否就绪 |
|
||||
| `chassis_driver_online` | bool | 底盘驱动是否在线 |
|
||||
| `motion_control_ready` | bool | 运控链路是否可执行动作 |
|
||||
| `estop_released` | bool | 急停是否已释放（**必须为 true 才会执行标定动作**）|
|
||||
| `vehicle_safe_to_move` | bool | 当前车辆是否允许移动（**必须为 true**）|
|
||||
| `telemetry_available` | bool | 是否可回传遥测 |
|
||||
| `checked_timestamp_us` | int64 | 检查时刻（Unix 微秒）|
|
||||
|
||||
### 4.3 CHASSIS_MOTION_PRIMITIVE_REQ（type=3）
|
||||
|
||||
Ubuntu 发送，让车辆执行一个标定动作原语。`selected_primitive` 决定哪个子对象有效。
|
||||
|
||||
```json
|
||||
{
|
||||
"header": {
|
||||
"session_id": "workshop_v2_1711234567000000_1",
|
||||
"task_id": "stage_chassis_straight_001",
|
||||
"vehicle_id": "AGV-001",
|
||||
"request_id": "chassis_req_1711234567100000",
|
||||
"client_send_timestamp_us": 1711234567100000,
|
||||
"operator_id": "op_001",
|
||||
"workshop_host": "ubuntu-workshop-pc"
|
||||
},
|
||||
"test_case_id": "straight_line_v1",
|
||||
"task_purpose": 1,
|
||||
"selected_primitive": 1,
|
||||
"brake_when_finished": true,
|
||||
"timeout_sec": 30.0,
|
||||
"source_iteration_id": "iter_001",
|
||||
"straight_line": {
|
||||
"target_speed_ms": 0.3,
|
||||
"target_distance_m": 5.0,
|
||||
"reverse": false
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### `selected_primitive` 枚举值
|
||||
|
||||
| 值 | 名称 | 有效子对象 |
|
||||
|----|------|----------|
|
||||
| 0 | UNSPECIFIED | — |
|
||||
| 1 | STRAIGHT_LINE | `straight_line` |
|
||||
| 2 | ARC | `arc` |
|
||||
| 3 | IN_PLACE_ROTATION | `in_place_rotation` |
|
||||
| 4 | STEERING_SWEEP | `steering_sweep` |
|
||||
| 5 | LATERAL_TRANSLATION | `lateral_translation` |
|
||||
| 6 | DIAGONAL_MOTION | `diagonal_motion` |
|
||||
| 7 | MODULE_ALIGNMENT | `module_alignment` |
|
||||
| 8 | COORDINATED_STEERING | `coordinated_steering` |
|
||||
|
||||
#### 各动作原语子对象格式
|
||||
|
||||
**straight_line**（直线行驶）
|
||||
```json
|
||||
{
|
||||
"target_speed_ms": 0.3,
|
||||
"target_distance_m": 5.0,
|
||||
"reverse": false
|
||||
}
|
||||
```
|
||||
|
||||
**arc**（圆弧行驶）
|
||||
```json
|
||||
{
|
||||
"target_speed_ms": 0.2,
|
||||
"radius_m": 2.0,
|
||||
"sweep_angle_deg": 90.0,
|
||||
"clockwise": true
|
||||
}
|
||||
```
|
||||
|
||||
**in_place_rotation**（原地旋转）
|
||||
```json
|
||||
{
|
||||
"target_yaw_deg": 90.0,
|
||||
"target_angular_vel_deg_s": 10.0
|
||||
}
|
||||
```
|
||||
|
||||
**steering_sweep**（舵角扫动）
|
||||
```json
|
||||
{
|
||||
"target_angle_deg": 0.0,
|
||||
"sweep_amplitude_deg": 15.0,
|
||||
"sweep_frequency_hz": 0.5,
|
||||
"duration_sec": 10.0
|
||||
}
|
||||
```
|
||||
|
||||
**lateral_translation**（横移，多舵轮专用）
|
||||
```json
|
||||
{
|
||||
"target_speed_ms": 0.2,
|
||||
"target_distance_m": 1.0,
|
||||
"move_left": true
|
||||
}
|
||||
```
|
||||
|
||||
**diagonal_motion**（斜移，多舵轮专用）
|
||||
```json
|
||||
{
|
||||
"target_speed_ms": 0.2,
|
||||
"target_distance_m": 2.0,
|
||||
"heading_deg": 45.0
|
||||
}
|
||||
```
|
||||
|
||||
**module_alignment**（模块零位检查）
|
||||
```json
|
||||
{
|
||||
"module_ids": ["module_0", "module_1", "module_2", "module_3"],
|
||||
"target_zero_deg": 0.0,
|
||||
"tolerance_deg": 0.5
|
||||
}
|
||||
```
|
||||
|
||||
**coordinated_steering**（模块协同转向）
|
||||
```json
|
||||
{
|
||||
"module_ids": ["module_0", "module_1"],
|
||||
"target_angle_deg": 30.0,
|
||||
"hold_time_sec": 3.0
|
||||
}
|
||||
```
|
||||
|
||||
### 4.4 CHASSIS_MOTION_PRIMITIVE_RSP（type=4）
|
||||
|
||||
Windows 在动作完成后返回，包含质量评估结果供编排器做自动验收。
|
||||
|
||||
```json
|
||||
{
|
||||
"success": true,
|
||||
"error_code": {"code": 0},
|
||||
"message": "直线行驶完成。",
|
||||
"job_id": "chassis_req_1711234567100000",
|
||||
"data_quality_passed": true,
|
||||
"suitable_for_commit": true,
|
||||
"recommended_parameter_version": "chassis_v1_iter001",
|
||||
"estimated_straight_line_bias": 0.003,
|
||||
"validation_summary": {
|
||||
"max_lateral_error_m": 0.012,
|
||||
"max_yaw_error_rad": 0.008,
|
||||
"rms_lateral_error_m": 0.006,
|
||||
"rms_yaw_error_rad": 0.004,
|
||||
"repeatability_error_m": 0.002,
|
||||
"curvature_error": 0.001,
|
||||
"module_consistency_error": 0.0,
|
||||
"auto_acceptance_passed": true
|
||||
},
|
||||
"artifacts": [
|
||||
{
|
||||
"file_name": "chassis_run_001.bag",
|
||||
"file_uri": "C:/calib_data/chassis_run_001.bag",
|
||||
"size_bytes": 1048576,
|
||||
"description": "底盘标定采集数据包"
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
| 字段 | 类型 | 说明 |
|
||||
|------|------|------|
|
||||
| `success` | bool | 动作是否成功执行 |
|
||||
| `data_quality_passed` | bool | 采集数据质量是否达标（影响自动验收）|
|
||||
| `suitable_for_commit` | bool | 估计参数是否适合写入（影响自动验收）|
|
||||
| `recommended_parameter_version` | string | 本次估计的参数版本号 |
|
||||
| `estimated_straight_line_bias` | float64 | 估计的直线跑偏量（m）|
|
||||
| `validation_summary` | object | 验收关键指标（见上）|
|
||||
| `artifacts` | array | 关联数据文件列表 |
|
||||
|
||||
**自动验收规则**：编排器要求 `data_quality_passed == true && suitable_for_commit == true`
|
||||
@@ -0,0 +1,60 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(vehicle_profile_manager)
|
||||
|
||||
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_components REQUIRED)
|
||||
find_package(calibration_common_interfaces REQUIRED)
|
||||
find_package(calibration_vehicle_profile_interfaces REQUIRED)
|
||||
|
||||
# 共享库（组件形式）：节点可以被 component_container 动态加载
|
||||
add_library(vehicle_profile_manager_component SHARED
|
||||
src/vehicle_profile_manager_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(vehicle_profile_manager_component PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
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()
|
||||
+68
@@ -0,0 +1,68 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
|
||||
#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<GetProfileSrv>::SharedPtr get_profile_service_;
|
||||
rclcpp::Service<RegisterSrv>::SharedPtr register_service_;
|
||||
rclcpp::Service<ApplicabilitySrv>::SharedPtr applicability_service_;
|
||||
rclcpp::Service<HeartbeatSrv>::SharedPtr heartbeat_service_;
|
||||
|
||||
// 内存存储：vehicle_id → 完整车辆画像
|
||||
std::unordered_map<std::string, VehicleProfile> profiles_;
|
||||
|
||||
void handle_get_profile(
|
||||
const std::shared_ptr<GetProfileSrv::Request> request,
|
||||
std::shared_ptr<GetProfileSrv::Response> response);
|
||||
|
||||
void handle_register(
|
||||
const std::shared_ptr<RegisterSrv::Request> request,
|
||||
std::shared_ptr<RegisterSrv::Response> response);
|
||||
|
||||
void handle_applicability(
|
||||
const std::shared_ptr<ApplicabilitySrv::Request> request,
|
||||
std::shared_ptr<ApplicabilitySrv::Response> response);
|
||||
|
||||
void handle_heartbeat(
|
||||
const std::shared_ptr<HeartbeatSrv::Request> request,
|
||||
std::shared_ptr<HeartbeatSrv::Response> response);
|
||||
|
||||
// 根据车辆画像内容推断支持哪些工作流阶段。
|
||||
std::vector<WorkflowStageType> evaluate_supported_stages(
|
||||
const VehicleProfile & profile) const;
|
||||
|
||||
// 预载 demo 车辆画像，vehicle_id = "demo_agv_001"。
|
||||
void load_demo_profile();
|
||||
};
|
||||
|
||||
} // namespace vehicle_profile_manager
|
||||
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>vehicle_profile_manager</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Vehicle profile manager service for automated calibration workshop.</description>
|
||||
<maintainer email="you@example.com">you</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>calibration_common_interfaces</depend>
|
||||
<depend>calibration_vehicle_profile_interfaces</depend>
|
||||
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,12 @@
|
||||
#include <memory>
|
||||
|
||||
#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<vehicle_profile_manager::VehicleProfileManagerNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+180
@@ -0,0 +1,180 @@
|
||||
#include "vehicle_profile_manager/vehicle_profile_manager_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
|
||||
#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<GetProfileSrv>(
|
||||
"/vehicle_profile_manager/get_vehicle_profile",
|
||||
std::bind(
|
||||
&VehicleProfileManagerNode::handle_get_profile, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
register_service_ = create_service<RegisterSrv>(
|
||||
"/vehicle_profile_manager/register_or_update_vehicle_profile",
|
||||
std::bind(
|
||||
&VehicleProfileManagerNode::handle_register, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
applicability_service_ = create_service<ApplicabilitySrv>(
|
||||
"/vehicle_profile_manager/evaluate_vehicle_calibration_applicability",
|
||||
std::bind(
|
||||
&VehicleProfileManagerNode::handle_applicability, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
heartbeat_service_ = create_service<HeartbeatSrv>(
|
||||
"/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<GetProfileSrv::Request> request,
|
||||
std::shared_ptr<GetProfileSrv::Response> 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<RegisterSrv::Request> request,
|
||||
std::shared_ptr<RegisterSrv::Response> 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<ApplicabilitySrv::Request> request,
|
||||
std::shared_ptr<ApplicabilitySrv::Response> 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<HeartbeatSrv::Request> /*request*/,
|
||||
std::shared_ptr<HeartbeatSrv::Response> response)
|
||||
{
|
||||
const auto now_us = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
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<WorkflowStageType>
|
||||
VehicleProfileManagerNode::evaluate_supported_stages(const VehicleProfile & profile) const
|
||||
{
|
||||
std::vector<WorkflowStageType> 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)
|
||||
@@ -0,0 +1,2 @@
|
||||
这是一份能直接下载的中文具体作用版。
|
||||
重点已重写 types.hpp / workshop_orchestrator_v2_node.hpp / workshop_orchestrator_v2_node.cpp 的注释。
|
||||
@@ -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()
|
||||
@@ -0,0 +1,11 @@
|
||||
把这个包接入你现在的仓库时，建议放到：
|
||||
|
||||
`src/win_ubuntu_bridge/service/workshop_orchestrator_v2`
|
||||
|
||||
如果你已经创建了同名包，可以直接把 `include/`、`src/`、`launch/` 里的对应文件合并进去。
|
||||
|
||||
这一版的核心变化不是新功能，而是：
|
||||
|
||||
- `execute_session()` 只保留薄流程
|
||||
- 预检失败、取消、成功、单步执行都拆成独立函数
|
||||
- 下一步就可以把 `run_single_stage()` 改成真实 gateway 调用
|
||||
@@ -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 按同样原则继续归位
|
||||
@@ -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`<br>`src/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp`<br>`src/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp`<br>`src/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp` / `include/workshop_orchestrator_v2/plan_builder.hpp`<br>`src/agv_calib_core/workshop_orchestrator/src/precheck_runner.cpp` / `include/workshop_orchestrator_v2/precheck_runner.hpp`<br>`src/agv_calib_core/workshop_orchestrator/src/report_builder.cpp` / `include/workshop_orchestrator_v2/report_builder.hpp`<br>`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`<br>`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`<br>`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`<br>`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`<br>`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`<br>`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 与实际实现保持同步更新
|
||||
+61
@@ -0,0 +1,61 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
|
||||
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<ReadinessSrv>::SharedPtr readiness_client_;
|
||||
rclcpp_action::Client<ExecuteTask>::SharedPtr execute_task_client_;
|
||||
};
|
||||
|
||||
} // namespace workshop_orchestrator_v2
|
||||
+68
@@ -0,0 +1,68 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
|
||||
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<calibration_control_interfaces::msg::TrajectoryPoint>
|
||||
build_trajectory_from_metadata(const StagePlan & stage, std::string & failure_reason) const;
|
||||
|
||||
rclcpp::Node * node_{nullptr};
|
||||
rclcpp::Client<ReadinessSrv>::SharedPtr readiness_client_;
|
||||
rclcpp_action::Client<ExecuteTask>::SharedPtr execute_task_client_;
|
||||
};
|
||||
|
||||
} // namespace workshop_orchestrator_v2
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
|
||||
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<ReadinessSrv>::SharedPtr readiness_client_;
|
||||
rclcpp_action::Client<ExecuteTask>::SharedPtr execute_task_client_;
|
||||
};
|
||||
|
||||
} // namespace workshop_orchestrator_v2
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#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<StagePlan> 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
|
||||
+18
@@ -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
|
||||
+23
@@ -0,0 +1,23 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "workshop_orchestrator_v2/types.hpp"
|
||||
|
||||
namespace workshop_orchestrator_v2
|
||||
{
|
||||
|
||||
class ReportBuilder
|
||||
{
|
||||
public:
|
||||
WorkshopReport build(
|
||||
const WorkshopSession & session,
|
||||
const std::vector<StageResultSummary> & stage_results,
|
||||
bool overall_success,
|
||||
int64_t started_timestamp_us,
|
||||
int64_t finished_timestamp_us,
|
||||
const std::string & summary) const;
|
||||
};
|
||||
|
||||
} // namespace workshop_orchestrator_v2
|
||||
+62
@@ -0,0 +1,62 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#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<ExecuteTask>;
|
||||
|
||||
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<ReadinessSrv>::SharedPtr readiness_client_;
|
||||
rclcpp_action::Client<ExecuteTask>::SharedPtr execute_task_client_;
|
||||
};
|
||||
|
||||
} // namespace workshop_orchestrator_v2
|
||||
+145
@@ -0,0 +1,145 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono> // 用来取当前时间，给创建时间、更新时间、报告时间戳赋值。
|
||||
#include <string> // 保存字符串字段，比如 session_id、summary。
|
||||
#include <vector> // 保存步骤列表、步骤执行结果列表。
|
||||
|
||||
#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::microseconds>(
|
||||
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<StageResultSummary> 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
|
||||
+224
@@ -0,0 +1,224 @@
|
||||
#pragma once
|
||||
|
||||
#include <condition_variable> // 暂停/恢复、人工确认、审批等待都需要条件变量。
|
||||
#include <memory>
|
||||
#include <mutex> // 保护 sessions_，避免 service 和 action 线程同时改同一条任务记录。
|
||||
#include <string> // 保存 topic 名、ID、说明文本。
|
||||
#include <unordered_map> // 按 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<ExecuteWorkshopSession>;
|
||||
|
||||
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<calibration_workshop_orchestration_interfaces::srv::CreateWorkshopSession::Request> request,
|
||||
std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::CreateWorkshopSession::Response> response);
|
||||
void handle_get_session(
|
||||
const std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::GetWorkshopSession::Request> request,
|
||||
std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::GetWorkshopSession::Response> response);
|
||||
void handle_get_report(
|
||||
const std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::GetWorkshopReport::Request> request,
|
||||
std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::GetWorkshopReport::Response> response);
|
||||
|
||||
// ─── 新增 service handler ───
|
||||
void handle_pause_session(
|
||||
const std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::PauseWorkshopSession::Request> request,
|
||||
std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::PauseWorkshopSession::Response> response);
|
||||
void handle_resume_session(
|
||||
const std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::ResumeWorkshopSession::Request> request,
|
||||
std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::ResumeWorkshopSession::Response> response);
|
||||
void handle_acknowledge_manual_step(
|
||||
const std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::AcknowledgeManualStep::Request> request,
|
||||
std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::AcknowledgeManualStep::Response> response);
|
||||
void handle_approve_stage_result(
|
||||
const std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::ApproveStageResult::Request> request,
|
||||
std::shared_ptr<calibration_workshop_orchestration_interfaces::srv::ApproveStageResult::Response> response);
|
||||
|
||||
// ─── Action handler ───
|
||||
rclcpp_action::GoalResponse handle_execute_goal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const ExecuteWorkshopSession::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_execute_cancel(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle);
|
||||
void handle_execute_accepted(const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle);
|
||||
|
||||
// ─── 执行主流程 ───
|
||||
void execute_session(const std::shared_ptr<GoalHandleExecuteWorkshopSession> 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<GoalHandleExecuteWorkshopSession> goal_handle,
|
||||
const SessionExecutionContext & context);
|
||||
bool run_all_stages(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle,
|
||||
SessionExecutionContext & context,
|
||||
std::string & failure_reason);
|
||||
bool run_single_stage(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle,
|
||||
const std::string & session_id,
|
||||
const StagePlan & stage,
|
||||
std::string & failure_reason);
|
||||
bool wait_for_manual_confirmation(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle,
|
||||
const std::string & session_id,
|
||||
const StagePlan & stage,
|
||||
std::string & failure_reason);
|
||||
bool wait_for_stage_approval(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> 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<GoalHandleExecuteWorkshopSession> goal_handle,
|
||||
const SessionExecutionContext & context,
|
||||
const WorkshopPrecheckResponse & precheck);
|
||||
void finish_session_failed(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle,
|
||||
const SessionExecutionContext & context,
|
||||
const std::string & failure_reason);
|
||||
void finish_session_succeeded(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle,
|
||||
const SessionExecutionContext & context);
|
||||
void finish_session_canceled(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> 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<std::string, SessionRecord> 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<GoalHandleExecuteWorkshopSession> active_goal_handle_;
|
||||
|
||||
PlanBuilder plan_builder_;
|
||||
PrecheckRunner precheck_runner_;
|
||||
ReportBuilder report_builder_;
|
||||
std::unique_ptr<ExternalLocalizationClient> external_localization_client_;
|
||||
std::unique_ptr<ChassisGatewayClient> chassis_gateway_client_;
|
||||
std::unique_ptr<ControlGatewayClient> control_gateway_client_;
|
||||
std::unique_ptr<SensorGatewayClient> sensor_gateway_client_;
|
||||
|
||||
// ─── ROS 接口 ───
|
||||
rclcpp::Service<calibration_workshop_orchestration_interfaces::srv::CreateWorkshopSession>::SharedPtr create_session_service_;
|
||||
rclcpp::Service<calibration_workshop_orchestration_interfaces::srv::GetWorkshopSession>::SharedPtr get_session_service_;
|
||||
rclcpp::Service<calibration_workshop_orchestration_interfaces::srv::GetWorkshopReport>::SharedPtr get_report_service_;
|
||||
rclcpp::Service<calibration_workshop_orchestration_interfaces::srv::PauseWorkshopSession>::SharedPtr pause_session_service_;
|
||||
rclcpp::Service<calibration_workshop_orchestration_interfaces::srv::ResumeWorkshopSession>::SharedPtr resume_session_service_;
|
||||
rclcpp::Service<calibration_workshop_orchestration_interfaces::srv::AcknowledgeManualStep>::SharedPtr acknowledge_manual_step_service_;
|
||||
rclcpp::Service<calibration_workshop_orchestration_interfaces::srv::ApproveStageResult>::SharedPtr approve_stage_result_service_;
|
||||
rclcpp_action::Server<ExecuteWorkshopSession>::SharedPtr execute_session_action_server_;
|
||||
rclcpp::Publisher<calibration_workshop_orchestration_interfaces::msg::WorkshopEvent>::SharedPtr event_publisher_;
|
||||
};
|
||||
|
||||
} // namespace workshop_orchestrator_v2
|
||||
+13
@@ -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",
|
||||
)
|
||||
])
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>workshop_orchestrator_v2</name>
|
||||
<version>0.0.3</version>
|
||||
<description>Workshop orchestrator v2 with external localization thin client.</description>
|
||||
<maintainer email="you@example.com">you</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>calibration_common_interfaces</depend>
|
||||
<depend>calibration_vehicle_profile_interfaces</depend>
|
||||
<depend>calibration_workshop_orchestration_interfaces</depend>
|
||||
<depend>calibration_external_localization_interfaces</depend>
|
||||
<depend>calibration_chassis_interfaces</depend>
|
||||
<depend>calibration_control_interfaces</depend>
|
||||
<depend>calibration_sensor_interfaces</depend>
|
||||
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user