whut/AutoCalib-Workshop
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: whut/AutoCalib-Workshop:efc644a77d9a04197104958c21aa8dba70cf99d8
Branches Tags
whut/AutoCalib-Workshop:main
..
compare: whut/AutoCalib-Workshop:main
Branches Tags
whut/AutoCalib-Workshop:main
10 Commits
efc644a77d .. main
Author SHA1 Message Date
li-shihao-code f2e5a4bb7d 完善.... 2026-06-05 12:53:48 +08:00
li-shihao-code a713b5d9ab 更新完善 2026-05-23 21:43:45 +08:00
li-shihao-code d708e1bbea 完善.... 2026-05-17 19:48:25 +08:00
li-shihao-code 93fa1b64b3 更新 2026-05-15 21:55:42 +08:00
li-shihao-code fd04f869b1 feat:修复了车载Camera和Lidar问题.... 2026-05-06 17:25:05 +08:00
li-shihao-code 6593ab0e67 feat:完善仿真环境 2026-05-04 11:13:59 +08:00
li-shihao-code df884252ff feat:完善仿真环境 2026-05-04 10:48:06 +08:00
li-shihao-code 1cc1ed1544 feat:完善结构与仿真环境 2026-05-04 10:42:25 +08:00
li-shihao-code f82c3ad12a feat: 自动化标定车间v1.0..... 2026-03-30 12:41:13 +08:00
li-shihao-code 672c77726d feat: 自动化标定车间v1.0..... 2026-03-29 17:04:15 +08:00
643 changed files with 63087 additions and 4671 deletions
Expand all files Collapse all files
README.md
+14
View File
@@ -8,6 +8,7 @@
3. [输出的 ROS 2 话题](#3-输出的-ros-2-话题)
4. [团队协作与 Git 配置指南 (必读)](#4-团队协作与-git-配置指南-必读)
5. [开发与分支规范](#5-开发与分支规范)
6. [相关文档](#6-相关文档)
---
@@ -214,3 +215,16 @@ git lfs track "*.usd"
git add .gitattributes
git commit -m "Add Git LFS tracking for large files"
⚠️ 注意：如果已经提交了大文件到历史记录，需要使用 git lfs migrate 来重写历史，否则只是追踪新文件。
---
## 6. 相关文档
本项目包含多个子模块，相关文档位置如下：
| 文档 | 路径 | 说明 |
|------|------|------|
| **AGV 标定系统总述** | [`agv_calib_brain/README.md`](./agv_calib_brain/README.md) | 自动化标定车间整体架构、核心能力、快速开始 |
| **仿真启动指南** | [`agv_calib_brain/SIMULATION_GUIDE.md`](./agv_calib_brain/SIMULATION_GUIDE.md) | 如何单独启动 Isaac 仿真各组件 |
| **源码目录说明** | [`agv_calib_brain/src/README.md`](./agv_calib_brain/src/README.md) | 源码组织结构、边界规则 |
| **Isaac 仿真车间** | 本文档 | Isaac Sim 环境配置、基础仿真运行 |
agv_calib_brain/.gitignore
+2
View File
@@ -0,0 +1,2 @@
__pycache__/
*.py[cod]
agv_calib_brain/.isaac_cache/ack_m_isaac_fixed.urdf
+301
View File
@@ -0,0 +1,301 @@
<?xml version='1.0' encoding='utf-8'?>
<robot name="qykj_ackm">
<link name="base_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.0174221592416108 0 0.0010674027636553" />
<mass value="2.0946205" />
<inertia ixx="0.00725227532564324" ixy="1.826574180869E-05" ixz="1.74522061015835E-05" iyy="0.0112392911749803" iyz="-1.16120716894918E-06" izz="0.0572118967643173" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/base_link.stl" />
</geometry>
<material name="base_material">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/base_link.stl" />
</geometry>
</collision>
</link>
<link name="base_footprint">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
</inertial>
<visual>
<geometry>
<sphere radius="0.001" />
</geometry>
</visual>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin rpy="0 0 0" xyz="0 0 0.055" />
</joint>
<link name="left_steering_hinge">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0" />
<mass value="0.085506993927856" />
<inertia ixx="0.00342027975711424" ixy="-1.72810689862337E-03" ixz="2.68361824483564E-03" iyy="0.00342027975711424" iyz="1.65092622583439E-04" izz="0.00342027975711424" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_steer_link.stl" />
</geometry>
<material name="steering_hinge_material">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_steer_link.stl" />
</geometry>
</collision>
</link>
<joint name="left_steering_hinge_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.17831 0.10435 0.0070129" />
<parent link="base_link" />
<child link="left_steering_hinge" />
<axis xyz="0 0 1" />
<limit effort="8" lower="-0.5" upper="0.5" velocity="800" />
<dynamics damping="0.5" />
</joint>
<link name="right_steering_hinge">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0" />
<mass value="0.085506993927856" />
<inertia ixx="0.00342027975711424" ixy="-1.72810689862337E-03" ixz="2.68361824483564E-03" iyy="0.00342027975711424" iyz="1.65092622583439E-04" izz="0.00342027975711424" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_steer_link.stl" />
</geometry>
<material name="steering_hinge_material">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_steer_link.stl" />
</geometry>
</collision>
</link>
<joint name="right_steering_hinge_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.17831 -0.10435 0.0070129" />
<parent link="base_link" />
<child link="right_steering_hinge" />
<axis xyz="0 0 1" />
<limit effort="8" lower="-0.5" upper="0.5" velocity="800" />
<dynamics damping="0.5" />
</joint>
<link name="left_wheel">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.01 0" />
<mass value="0.083615304385799" />
<inertia ixx="0.00010451913048224875" ixy="-1.02836030755788E-5" ixz="1.03858733509779E-10" iyy="0.000261297820954496875" iyz="-3.52038618447715E-09" izz="0.00010451913048224875" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_wheel_link.stl" />
</geometry>
<material name="wheel_material">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_wheel_link.stl" />
</geometry>
</collision>
</link>
<joint name="left_wheel_joint" type="continuous">
<origin rpy="0 0 0" xyz="0 0.074 0" />
<parent link="left_steering_hinge" />
<child link="left_wheel" />
<axis xyz="0 1 0" />
<limit effort="6" velocity="800" />
</joint>
<link name="right_wheel">
<inertial>
<origin rpy="0 0 0" xyz="0 0.01 0" />
<mass value="0.083615304385799" />
<inertia ixx="0.00010451913048224875" ixy="-1.02836030755788E-5" ixz="1.03858733509779E-10" iyy="0.000261297820954496875" iyz="-3.52038618447715E-09" izz="0.00010451913048224875" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_wheel_link.stl" />
</geometry>
<material name="wheel_material">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_wheel_link.stl" />
</geometry>
</collision>
</link>
<joint name="right_wheel_joint" type="continuous">
<origin rpy="0 0 0" xyz="0 -0.074 0" />
<parent link="right_steering_hinge" />
<child link="right_wheel" />
<axis xyz="0 1 0" />
<limit effort="6" velocity="800" />
</joint>
<link name="left_rear_wheel">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.01 0" />
<mass value="0.083615304385799" />
<inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="-1.03858733509779E-10" iyy="0.00617238444547154" iyz="3.52038618447715E-09" izz="0.00064504895313491" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_back_wheel_link.stl" />
</geometry>
<material name="wheel_material">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_back_wheel_link.stl" />
</geometry>
</collision>
</link>
<joint name="left_rear_wheel_joint" type="continuous">
<origin rpy="0 0 0" xyz="-0.11072 0.18046 0.0099082" />
<parent link="base_link" />
<child link="left_rear_wheel" />
<axis xyz="0 1 0" />
<limit effort="10" lower="-3.14159" upper="3.14159" velocity="800" />
</joint>
<link name="right_rear_wheel">
<inertial>
<origin rpy="0 0 0" xyz="0 0.01 0" />
<mass value="0.083615304385799" />
<inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="-1.03858733509779E-10" iyy="0.00617238444547154" iyz="3.52038618447715E-09" izz="0.00064504895313491" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_back_wheel_link.stl" />
</geometry>
<material name="wheel_material">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_back_wheel_link.stl" />
</geometry>
</collision>
</link>
<joint name="right_rear_wheel_joint" type="continuous">
<origin rpy="0 0 0" xyz="-0.11072 -0.18046 0.0099082" />
<parent link="base_link" />
<child link="right_rear_wheel" />
<axis xyz="0 1 0" />
<limit effort="10" lower="-3.14159" upper="3.14159" velocity="800" />
</joint>
<link name="camera">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0" />
<mass value="0.128875262431144" />
<inertia ixx="0.000226098241375409" ixy="-7.63151331329159E-09" ixz="-6.02732800767945E-07" iyy="1.50549325275966E-05" iyz="-7.62743002721805E-10" izz="0.000225285047549157" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/camera.stl" />
</geometry>
<material name="camera_material">
<color rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/camera.stl" />
</geometry>
</collision>
</link>
<joint name="camera_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.21669 -0.00063349 0.142" />
<parent link="base_link" />
<child link="camera" />
<axis xyz="0 0 0" />
</joint>
<link name="laser">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0" />
<mass value="0.136175408631878" />
<inertia ixx="5.05126026373552E-05" ixy="1.31150574294136E-08" ixz="1.65013289478E-06" iyy="7.08926003407945E-05" iyz="2.49898507713873E-09" izz="0.00010333009695655" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/laser_link.stl" />
</geometry>
<material name="sensor_material">
<color rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/laser_link.stl" />
</geometry>
</collision>
</link>
<joint name="laser_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.092303 5.3876E-05 0.2031" />
<parent link="base_link" />
<child link="laser" />
<axis xyz="0 0 0" />
</joint>
<link name="imu">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.005" />
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
</inertial>
<visual>
<geometry>
<box size="0.005 0.005 0.007" />
</geometry>
</visual>
</link>
<joint name="imu_joints" type="fixed">
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0035" />
<parent link="base_link" />
<child link="imu" />
</joint>
</robot>
agv_calib_brain/.isaac_cache/ack_m_isaac_fixed.usd
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:ac7fb8e48883155d1feffe880c55a972c5562b34bc340f0002ff80965d20cfac
size 75606548
agv_calib_brain/.isaac_cache/checkerboard.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 73 KiB

agv_calib_brain/.isaac_cache/down_camera_charuco.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 14 KiB

agv_calib_brain/.isaac_cache/workshop_scene_manifest.json
+852
View File
@@ -0,0 +1,852 @@
{
"vehicle": {
"vehicle_id": "demo_agv_001",
"vehicle_source": "urdf_direct",
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"usd_path": null,
"stage_prim_path": "/ackermann_front_steer_rear_drive",
"drive_wheels": [
"rear_left_wheel_link",
"rear_right_wheel_link"
],
"steer_wheels": [
"front_left_wheel_link",
"front_right_wheel_link"
],
"initial_pose": {
"x_m": 0.0,
"y_m": 0.0,
"z_m": 0.0,
"yaw_rad": 0.0
}
},
"workshop": {
"workcell_zone_id": "isaac_workcell_zone_a",
"room_length_m": 10.0,
"room_width_m": 6.0,
"room_height_m": 3.5,
"checkerboard": {
"rows": 6,
"cols": 9,
"square_size_m": 0.12,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
"reserved_wall_zones": {
"front": [
{
"axis_min": -2.82,
"axis_max": -0.9199999999999999,
"z_min": 0.0,
"z_max": 3.5,
"reason": "2d_lidar_extrinsic_corner_bay"
}
],
"right": [
{
"axis_min": 2.9200000000000004,
"axis_max": 4.82,
"z_min": 0.0,
"z_max": 3.5,
"reason": "2d_lidar_extrinsic_corner_bay"
}
]
},
"layout": [
{
"id": "front_wall_r00_c01",
"wall": "front",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
4.985,
0.0,
1.2100000000000002
],
"rotation_deg": [
90,
0,
90
]
},
{
"id": "front_wall_r00_c02",
"wall": "front",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
4.985,
1.4999999999999998,
1.2100000000000002
],
"rotation_deg": [
90,
0,
90
]
},
{
"id": "front_wall_r01_c01",
"wall": "front",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
4.985,
0.0,
2.33
],
"rotation_deg": [
90,
0,
90
]
},
{
"id": "front_wall_r01_c02",
"wall": "front",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
4.985,
1.4999999999999998,
2.33
],
"rotation_deg": [
90,
0,
90
]
},
{
"id": "back_wall_r00_c00",
"wall": "back",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-4.985,
-1.4999999999999998,
1.2100000000000002
],
"rotation_deg": [
90,
0,
-90
]
},
{
"id": "back_wall_r00_c01",
"wall": "back",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-4.985,
0.0,
1.2100000000000002
],
"rotation_deg": [
90,
0,
-90
]
},
{
"id": "back_wall_r00_c02",
"wall": "back",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-4.985,
1.4999999999999998,
1.2100000000000002
],
"rotation_deg": [
90,
0,
-90
]
},
{
"id": "back_wall_r01_c00",
"wall": "back",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-4.985,
-1.4999999999999998,
2.33
],
"rotation_deg": [
90,
0,
-90
]
},
{
"id": "back_wall_r01_c01",
"wall": "back",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-4.985,
0.0,
2.33
],
"rotation_deg": [
90,
0,
-90
]
},
{
"id": "back_wall_r01_c02",
"wall": "back",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-4.985,
1.4999999999999998,
2.33
],
"rotation_deg": [
90,
0,
-90
]
},
{
"id": "left_wall_r00_c00",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-3.749999999999999,
2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r00_c01",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-2.249999999999999,
2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r00_c02",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-0.7499999999999996,
2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r00_c03",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
0.75,
2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r00_c04",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
2.25,
2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r00_c05",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
3.75,
2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r01_c00",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-3.749999999999999,
2.985,
2.33
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r01_c01",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-2.249999999999999,
2.985,
2.33
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r01_c02",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-0.7499999999999996,
2.985,
2.33
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r01_c03",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
0.75,
2.985,
2.33
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r01_c04",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
2.25,
2.985,
2.33
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "left_wall_r01_c05",
"wall": "left",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
3.75,
2.985,
2.33
],
"rotation_deg": [
90,
0,
0
]
},
{
"id": "right_wall_r00_c00",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-3.749999999999999,
-2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r00_c01",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-2.249999999999999,
-2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r00_c02",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-0.7499999999999996,
-2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r00_c03",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
0.75,
-2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r00_c04",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
2.25,
-2.985,
1.2100000000000002
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r01_c00",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-3.749999999999999,
-2.985,
2.33
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r01_c01",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-2.249999999999999,
-2.985,
2.33
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r01_c02",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
-0.7499999999999996,
-2.985,
2.33
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r01_c03",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
0.75,
-2.985,
2.33
],
"rotation_deg": [
90,
0,
180
]
},
{
"id": "right_wall_r01_c04",
"wall": "right",
"mount": "flush",
"purpose": "wall_checkerboard_array",
"position": [
2.25,
-2.985,
2.33
],
"rotation_deg": [
90,
0,
180
]
}
]
},
"down_camera_intrinsic_target": {
"enabled": true,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
"pattern": "charuco",
"squares_x": 30,
"squares_y": 10,
"square_size_x_m": 0.09000000000000001,
"square_size_y_m": 0.09,
"layout": [
{
"id": "down_camera_3d_charuco_ramp",
"target_type": "down_camera_intrinsic",
"pattern": "charuco",
"purpose": "intrinsic_calibration_with_depth_and_pose_gradient",
"center": [
0.0,
-2.15,
0.028499999999999998
],
"length_m": 2.7,
"width_m": 0.9,
"squares_x": 30,
"squares_y": 10,
"square_size_m": 0.09,
"min_height_m": 0.012,
"max_height_m": 0.045,
"recommended_drive_axis": "+x",
"recommended_speed_mps": 0.1,
"surfaces": [
{
"id": "low_flat_charuco",
"type": "flat",
"x_range_m": [
-1.35,
-0.756
],
"z_range_m": [
0.012,
0.012
]
},
{
"id": "continuous_slope_charuco",
"type": "continuous_slope",
"x_range_m": [
-0.756,
0.6479999999999999
],
"z_range_m": [
0.012,
0.045
]
},
{
"id": "high_flat_charuco",
"type": "flat",
"x_range_m": [
0.6479999999999999,
1.35
],
"z_range_m": [
0.045,
0.045
]
}
]
}
]
}
},
"ros_topics": {
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
"lidar_prefix": "/AutoCalib_Workshop/lidar",
"front_camera_image": "/sensor/front_camera/image_raw",
"down_camera_image": "/sensor/down_camera/image_raw",
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
"lidar_2d_scan": "/sensor/lidar_2d/scan",
"imu": "/sensor/imu/data",
"external_telemetry": "/isaac/external_localization/vehicle/pose",
"chassis_telemetry": "/chassis/telemetry",
"control_telemetry": "/control/telemetry",
"sensor_telemetry": "/sensor_calibration/telemetry"
},
"chassis_calibration": {
"chassis_type": "ackermann",
"wheel_radius_m": 0.1,
"wheel_track_m": 0.52,
"wheel_base_m": 0.8
},
"control_calibration": {
"parameter_version": "isaac_control_baseline_v1"
},
"external_truth": {
"localization_source_id": "isaac_sim_truth_source",
"reference_source_name": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"expected_position_stddev_m": 0.01,
"expected_yaw_stddev_rad": 0.01,
"expected_time_sync_offset_ms": 2.0,
"visualization_enabled": false,
"visualization_root_prim": "/World/ExternalTruthVisualization",
"target_ball_radius_m": 0.075,
"target_balls": [
{
"ball_id": "front_left",
"center_in_base_link": {
"x_m": 0.58,
"y_m": 0.32,
"z_m": 1.28
}
},
{
"ball_id": "front_right",
"center_in_base_link": {
"x_m": 0.58,
"y_m": -0.32,
"z_m": 1.28
}
},
{
"ball_id": "rear_center",
"center_in_base_link": {
"x_m": -0.5,
"y_m": 0.0,
"z_m": 1.22
}
}
]
},
"sensors": [
{
"sensor_id": "demo_front_camera",
"sensor_type": "front_camera",
"frame_id": "front_camera_link",
"image_topic": "/sensor/front_camera/image_raw",
"telemetry_topic": "/sensor_calibration/telemetry",
"mount_pose": {
"x_m": 1.12,
"y_m": 0.0,
"z_m": 1.18,
"roll_rad": 1.5707963267948966,
"pitch_rad": 0.0,
"yaw_rad": -1.5707963267948966
}
},
{
"sensor_id": "demo_down_camera",
"sensor_type": "down_camera",
"frame_id": "down_camera_link",
"image_topic": "/sensor/down_camera/image_raw",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_3d",
"sensor_type": "lidar_3d",
"frame_id": "lidar_3d_link",
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
"mount_pose": {
"x_m": 0.55,
"y_m": 0.0,
"z_m": 1.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_2d",
"sensor_type": "lidar_2d",
"frame_id": "lidar_2d_link",
"scan_topic": "/sensor/lidar_2d/scan",
"mount_pose": {
"x_m": 0.7,
"y_m": 0.0,
"z_m": 1.0,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_imu",
"sensor_type": "imu",
"frame_id": "imu_link",
"imu_topic": "/sensor/imu/data",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.26,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
}
],
"lidar_2d_calibration_targets": {
"enabled": true,
"target_width_m": 0.82,
"target_height_m": 1.6,
"target_thickness_m": 0.04,
"target_bottom_z_m": 0.08,
"slope_angle_deg": 45.0,
"targets": [
{
"id": "front_vertical_reference_panel",
"wall": "front",
"type": "vertical_reference",
"position": [
4.920000000000001,
-2.34,
0.88
],
"scale": [
0.04,
0.82,
1.6
],
"rotation_deg": [
0.0,
0.0,
0.0
],
"nominal_plane": "x = room_length/2 - standoff - thickness"
},
{
"id": "front_45deg_height_encoding_panel",
"wall": "front",
"type": "height_encoding_slope",
"position": [
4.1000000000000005,
-1.4,
0.88
],
"scale": [
0.04,
0.82,
2.262741699796952
],
"rotation_deg": [
0.0,
-45.0,
0.0
],
"slope_angle_deg": 45.0,
"height_to_range_sign": "higher_scan_plane_farther_from_front_wall"
},
{
"id": "right_vertical_reference_panel",
"wall": "right",
"type": "vertical_reference",
"position": [
3.87,
-2.92,
0.88
],
"scale": [
0.82,
0.04,
1.6
],
"rotation_deg": [
0.0,
0.0,
0.0
],
"nominal_plane": "y = -room_width/2 + standoff + thickness"
}
]
},
"orchestrator_session_config_hint": {
"vehicle_id": "demo_agv_001",
"localization_source_id": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"reference_target_id": "isaac_external_truth"
}
}
agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json
+168
View File
@@ -0,0 +1,168 @@
{
"vehicle": {
"vehicle_id": "demo_agv_001",
"vehicle_source": "urdf_direct",
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"usd_path": null,
"stage_prim_path": "/ackermann_front_steer_rear_drive",
"drive_wheels": [
"rear_left_wheel_link",
"rear_right_wheel_link"
],
"steer_wheels": [
"front_left_wheel_link",
"front_right_wheel_link"
],
"initial_pose": {
"x_m": 0.0,
"y_m": 0.0,
"z_m": 0.0,
"yaw_rad": 0.0
}
},
"workshop": {
"workcell_zone_id": "isaac_workcell_zone_a",
"room_length_m": 10.0,
"room_width_m": 6.0,
"room_height_m": 3.5,
"checkerboard": {
"rows": 6,
"cols": 9,
"square_size_m": 0.12,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
"reserved_wall_zones": {},
"layout": []
},
"down_camera_intrinsic_target": {
"enabled": false,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
"pattern": "charuco",
"squares_x": 30,
"squares_y": 10,
"square_size_x_m": 0.09000000000000001,
"square_size_y_m": 0.09,
"layout": []
}
},
"ros_topics": {
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
"lidar_prefix": "/AutoCalib_Workshop/lidar",
"front_camera_image": "/sensor/front_camera/image_raw",
"down_camera_image": "/sensor/down_camera/image_raw",
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
"lidar_2d_scan": "/sensor/lidar_2d/scan",
"imu": "/sensor/imu/data",
"external_telemetry": "/isaac/external_localization/vehicle/pose",
"chassis_telemetry": "/chassis/telemetry",
"control_telemetry": "/control/telemetry",
"sensor_telemetry": "/sensor_calibration/telemetry"
},
"chassis_calibration": {
"chassis_type": "ackermann",
"wheel_radius_m": 0.1,
"wheel_track_m": 0.52,
"wheel_base_m": 0.8
},
"control_calibration": {
"parameter_version": "isaac_control_baseline_v1"
},
"external_truth": {
"localization_source_id": "isaac_sim_truth_source",
"reference_source_name": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"expected_position_stddev_m": 0.01,
"expected_yaw_stddev_rad": 0.01,
"expected_time_sync_offset_ms": 2.0,
"visualization_enabled": false
},
"sensors": [
{
"sensor_id": "demo_front_camera",
"sensor_type": "front_camera",
"frame_id": "front_camera_link",
"image_topic": "/sensor/front_camera/image_raw",
"telemetry_topic": "/sensor_calibration/telemetry",
"mount_pose": {
"x_m": 1.12,
"y_m": 0.0,
"z_m": 1.18,
"roll_rad": 1.5707963267948966,
"pitch_rad": 0.0,
"yaw_rad": -1.5707963267948966
}
},
{
"sensor_id": "demo_down_camera",
"sensor_type": "down_camera",
"frame_id": "down_camera_link",
"image_topic": "/sensor/down_camera/image_raw",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_3d",
"sensor_type": "lidar_3d",
"frame_id": "lidar_3d_link",
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
"mount_pose": {
"x_m": 0.55,
"y_m": 0.0,
"z_m": 1.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_2d",
"sensor_type": "lidar_2d",
"frame_id": "lidar_2d_link",
"scan_topic": "/sensor/lidar_2d/scan",
"mount_pose": {
"x_m": 0.7,
"y_m": 0.0,
"z_m": 1.0,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_imu",
"sensor_type": "imu",
"frame_id": "imu_link",
"imu_topic": "/sensor/imu/data",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.26,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
}
],
"lidar_2d_calibration_targets": {
"enabled": false,
"target_width_m": 0.82,
"target_height_m": 1.6,
"target_thickness_m": 0.04,
"target_bottom_z_m": 0.08,
"slope_angle_deg": 45.0,
"targets": []
},
"orchestrator_session_config_hint": {
"vehicle_id": "demo_agv_001",
"localization_source_id": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"reference_target_id": "isaac_external_truth"
}
}
agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json
+168
View File
@@ -0,0 +1,168 @@
{
"vehicle": {
"vehicle_id": "demo_agv_001",
"vehicle_source": "urdf_direct",
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"usd_path": null,
"stage_prim_path": "/ackermann_front_steer_rear_drive",
"drive_wheels": [
"rear_left_wheel_link",
"rear_right_wheel_link"
],
"steer_wheels": [
"front_left_wheel_link",
"front_right_wheel_link"
],
"initial_pose": {
"x_m": 0.0,
"y_m": 0.0,
"z_m": 0.0,
"yaw_rad": 0.0
}
},
"workshop": {
"workcell_zone_id": "isaac_workcell_zone_a",
"room_length_m": 10.0,
"room_width_m": 6.0,
"room_height_m": 3.5,
"checkerboard": {
"rows": 6,
"cols": 9,
"square_size_m": 0.12,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
"reserved_wall_zones": {},
"layout": []
},
"down_camera_intrinsic_target": {
"enabled": false,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
"pattern": "charuco",
"squares_x": 30,
"squares_y": 10,
"square_size_x_m": 0.09000000000000001,
"square_size_y_m": 0.09,
"layout": []
}
},
"ros_topics": {
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
"lidar_prefix": "/AutoCalib_Workshop/lidar",
"front_camera_image": "/sensor/front_camera/image_raw",
"down_camera_image": "/sensor/down_camera/image_raw",
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
"lidar_2d_scan": "/sensor/lidar_2d/scan",
"imu": "/sensor/imu/data",
"external_telemetry": "/isaac/external_localization/vehicle/pose",
"chassis_telemetry": "/chassis/telemetry",
"control_telemetry": "/control/telemetry",
"sensor_telemetry": "/sensor_calibration/telemetry"
},
"chassis_calibration": {
"chassis_type": "ackermann",
"wheel_radius_m": 0.1,
"wheel_track_m": 0.52,
"wheel_base_m": 0.8
},
"control_calibration": {
"parameter_version": "isaac_control_baseline_v1"
},
"external_truth": {
"localization_source_id": "isaac_sim_truth_source",
"reference_source_name": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"expected_position_stddev_m": 0.01,
"expected_yaw_stddev_rad": 0.01,
"expected_time_sync_offset_ms": 2.0,
"visualization_enabled": false
},
"sensors": [
{
"sensor_id": "demo_front_camera",
"sensor_type": "front_camera",
"frame_id": "front_camera_link",
"image_topic": "/sensor/front_camera/image_raw",
"telemetry_topic": "/sensor_calibration/telemetry",
"mount_pose": {
"x_m": 1.12,
"y_m": 0.0,
"z_m": 1.18,
"roll_rad": 1.5707963267948966,
"pitch_rad": 0.0,
"yaw_rad": -1.5707963267948966
}
},
{
"sensor_id": "demo_down_camera",
"sensor_type": "down_camera",
"frame_id": "down_camera_link",
"image_topic": "/sensor/down_camera/image_raw",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_3d",
"sensor_type": "lidar_3d",
"frame_id": "lidar_3d_link",
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
"mount_pose": {
"x_m": 0.55,
"y_m": 0.0,
"z_m": 1.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_2d",
"sensor_type": "lidar_2d",
"frame_id": "lidar_2d_link",
"scan_topic": "/sensor/lidar_2d/scan",
"mount_pose": {
"x_m": 0.7,
"y_m": 0.0,
"z_m": 1.0,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_imu",
"sensor_type": "imu",
"frame_id": "imu_link",
"imu_topic": "/sensor/imu/data",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.26,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
}
],
"lidar_2d_calibration_targets": {
"enabled": false,
"target_width_m": 0.82,
"target_height_m": 1.6,
"target_thickness_m": 0.04,
"target_bottom_z_m": 0.08,
"slope_angle_deg": 45.0,
"targets": []
},
"orchestrator_session_config_hint": {
"vehicle_id": "demo_agv_001",
"localization_source_id": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"reference_target_id": "isaac_external_truth"
}
}
agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json
+168
View File
@@ -0,0 +1,168 @@
{
"vehicle": {
"vehicle_id": "demo_agv_001",
"vehicle_source": "urdf_direct",
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"usd_path": null,
"stage_prim_path": "/ackermann_front_steer_rear_drive",
"drive_wheels": [
"rear_left_wheel_link",
"rear_right_wheel_link"
],
"steer_wheels": [
"front_left_wheel_link",
"front_right_wheel_link"
],
"initial_pose": {
"x_m": 0.0,
"y_m": 0.0,
"z_m": 0.0,
"yaw_rad": 0.0
}
},
"workshop": {
"workcell_zone_id": "isaac_workcell_zone_a",
"room_length_m": 10.0,
"room_width_m": 6.0,
"room_height_m": 3.5,
"checkerboard": {
"rows": 6,
"cols": 9,
"square_size_m": 0.12,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
"reserved_wall_zones": {},
"layout": []
},
"down_camera_intrinsic_target": {
"enabled": false,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
"pattern": "charuco",
"squares_x": 30,
"squares_y": 10,
"square_size_x_m": 0.09000000000000001,
"square_size_y_m": 0.09,
"layout": []
}
},
"ros_topics": {
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
"lidar_prefix": "/AutoCalib_Workshop/lidar",
"front_camera_image": "/sensor/front_camera/image_raw",
"down_camera_image": "/sensor/down_camera/image_raw",
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
"lidar_2d_scan": "/sensor/lidar_2d/scan",
"imu": "/sensor/imu/data",
"external_telemetry": "/isaac/external_localization/vehicle/pose",
"chassis_telemetry": "/chassis/telemetry",
"control_telemetry": "/control/telemetry",
"sensor_telemetry": "/sensor_calibration/telemetry"
},
"chassis_calibration": {
"chassis_type": "ackermann",
"wheel_radius_m": 0.1,
"wheel_track_m": 0.52,
"wheel_base_m": 0.8
},
"control_calibration": {
"parameter_version": "isaac_control_baseline_v1"
},
"external_truth": {
"localization_source_id": "isaac_sim_truth_source",
"reference_source_name": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"expected_position_stddev_m": 0.01,
"expected_yaw_stddev_rad": 0.01,
"expected_time_sync_offset_ms": 2.0,
"visualization_enabled": false
},
"sensors": [
{
"sensor_id": "demo_front_camera",
"sensor_type": "front_camera",
"frame_id": "front_camera_link",
"image_topic": "/sensor/front_camera/image_raw",
"telemetry_topic": "/sensor_calibration/telemetry",
"mount_pose": {
"x_m": 1.12,
"y_m": 0.0,
"z_m": 1.18,
"roll_rad": 1.5707963267948966,
"pitch_rad": 0.0,
"yaw_rad": -1.5707963267948966
}
},
{
"sensor_id": "demo_down_camera",
"sensor_type": "down_camera",
"frame_id": "down_camera_link",
"image_topic": "/sensor/down_camera/image_raw",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_3d",
"sensor_type": "lidar_3d",
"frame_id": "lidar_3d_link",
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
"mount_pose": {
"x_m": 0.55,
"y_m": 0.0,
"z_m": 1.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_2d",
"sensor_type": "lidar_2d",
"frame_id": "lidar_2d_link",
"scan_topic": "/sensor/lidar_2d/scan",
"mount_pose": {
"x_m": 0.7,
"y_m": 0.0,
"z_m": 1.0,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_imu",
"sensor_type": "imu",
"frame_id": "imu_link",
"imu_topic": "/sensor/imu/data",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.26,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
}
],
"lidar_2d_calibration_targets": {
"enabled": false,
"target_width_m": 0.82,
"target_height_m": 1.6,
"target_thickness_m": 0.04,
"target_bottom_z_m": 0.08,
"slope_angle_deg": 45.0,
"targets": []
},
"orchestrator_session_config_hint": {
"vehicle_id": "demo_agv_001",
"localization_source_id": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"reference_target_id": "isaac_external_truth"
}
}
agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json
+168
View File
@@ -0,0 +1,168 @@
{
"vehicle": {
"vehicle_id": "demo_agv_001",
"vehicle_source": "urdf_direct",
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"usd_path": null,
"stage_prim_path": "/ackermann_front_steer_rear_drive",
"drive_wheels": [
"rear_left_wheel_link",
"rear_right_wheel_link"
],
"steer_wheels": [
"front_left_wheel_link",
"front_right_wheel_link"
],
"initial_pose": {
"x_m": 0.0,
"y_m": 0.0,
"z_m": 0.0,
"yaw_rad": 0.0
}
},
"workshop": {
"workcell_zone_id": "isaac_workcell_zone_a",
"room_length_m": 10.0,
"room_width_m": 6.0,
"room_height_m": 3.5,
"checkerboard": {
"rows": 6,
"cols": 9,
"square_size_m": 0.12,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
"reserved_wall_zones": {},
"layout": []
},
"down_camera_intrinsic_target": {
"enabled": false,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
"pattern": "charuco",
"squares_x": 30,
"squares_y": 10,
"square_size_x_m": 0.09000000000000001,
"square_size_y_m": 0.09,
"layout": []
}
},
"ros_topics": {
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
"lidar_prefix": "/AutoCalib_Workshop/lidar",
"front_camera_image": "/sensor/front_camera/image_raw",
"down_camera_image": "/sensor/down_camera/image_raw",
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
"lidar_2d_scan": "/sensor/lidar_2d/scan",
"imu": "/sensor/imu/data",
"external_telemetry": "/isaac/external_localization/vehicle/pose",
"chassis_telemetry": "/chassis/telemetry",
"control_telemetry": "/control/telemetry",
"sensor_telemetry": "/sensor_calibration/telemetry"
},
"chassis_calibration": {
"chassis_type": "ackermann",
"wheel_radius_m": 0.1,
"wheel_track_m": 0.52,
"wheel_base_m": 0.8
},
"control_calibration": {
"parameter_version": "isaac_control_baseline_v1"
},
"external_truth": {
"localization_source_id": "isaac_sim_truth_source",
"reference_source_name": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"expected_position_stddev_m": 0.01,
"expected_yaw_stddev_rad": 0.01,
"expected_time_sync_offset_ms": 2.0,
"visualization_enabled": false
},
"sensors": [
{
"sensor_id": "demo_front_camera",
"sensor_type": "front_camera",
"frame_id": "front_camera_link",
"image_topic": "/sensor/front_camera/image_raw",
"telemetry_topic": "/sensor_calibration/telemetry",
"mount_pose": {
"x_m": 1.12,
"y_m": 0.0,
"z_m": 1.18,
"roll_rad": 1.5707963267948966,
"pitch_rad": 0.0,
"yaw_rad": -1.5707963267948966
}
},
{
"sensor_id": "demo_down_camera",
"sensor_type": "down_camera",
"frame_id": "down_camera_link",
"image_topic": "/sensor/down_camera/image_raw",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_3d",
"sensor_type": "lidar_3d",
"frame_id": "lidar_3d_link",
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
"mount_pose": {
"x_m": 0.55,
"y_m": 0.0,
"z_m": 1.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_2d",
"sensor_type": "lidar_2d",
"frame_id": "lidar_2d_link",
"scan_topic": "/sensor/lidar_2d/scan",
"mount_pose": {
"x_m": 0.7,
"y_m": 0.0,
"z_m": 1.0,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_imu",
"sensor_type": "imu",
"frame_id": "imu_link",
"imu_topic": "/sensor/imu/data",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.26,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
}
],
"lidar_2d_calibration_targets": {
"enabled": false,
"target_width_m": 0.82,
"target_height_m": 1.6,
"target_thickness_m": 0.04,
"target_bottom_z_m": 0.08,
"slope_angle_deg": 45.0,
"targets": []
},
"orchestrator_session_config_hint": {
"vehicle_id": "demo_agv_001",
"localization_source_id": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"reference_target_id": "isaac_external_truth"
}
}
agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json
+168
View File
@@ -0,0 +1,168 @@
{
"vehicle": {
"vehicle_id": "demo_agv_001",
"vehicle_source": "urdf_direct",
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
"usd_path": null,
"stage_prim_path": "/ackermann_front_steer_rear_drive",
"drive_wheels": [
"rear_left_wheel_link",
"rear_right_wheel_link"
],
"steer_wheels": [
"front_left_wheel_link",
"front_right_wheel_link"
],
"initial_pose": {
"x_m": 0.0,
"y_m": 0.0,
"z_m": 0.0,
"yaw_rad": 0.0
}
},
"workshop": {
"workcell_zone_id": "isaac_workcell_zone_a",
"room_length_m": 10.0,
"room_width_m": 6.0,
"room_height_m": 3.5,
"checkerboard": {
"rows": 6,
"cols": 9,
"square_size_m": 0.12,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
"reserved_wall_zones": {},
"layout": []
},
"down_camera_intrinsic_target": {
"enabled": false,
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
"pattern": "charuco",
"squares_x": 30,
"squares_y": 10,
"square_size_x_m": 0.09000000000000001,
"square_size_y_m": 0.09,
"layout": []
}
},
"ros_topics": {
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
"lidar_prefix": "/AutoCalib_Workshop/lidar",
"front_camera_image": "/sensor/front_camera/image_raw",
"down_camera_image": "/sensor/down_camera/image_raw",
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
"lidar_2d_scan": "/sensor/lidar_2d/scan",
"imu": "/sensor/imu/data",
"external_telemetry": "/isaac/external_localization/vehicle/pose",
"chassis_telemetry": "/chassis/telemetry",
"control_telemetry": "/control/telemetry",
"sensor_telemetry": "/sensor_calibration/telemetry"
},
"chassis_calibration": {
"chassis_type": "ackermann",
"wheel_radius_m": 0.1,
"wheel_track_m": 0.52,
"wheel_base_m": 0.8
},
"control_calibration": {
"parameter_version": "isaac_control_baseline_v1"
},
"external_truth": {
"localization_source_id": "isaac_sim_truth_source",
"reference_source_name": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"expected_position_stddev_m": 0.01,
"expected_yaw_stddev_rad": 0.01,
"expected_time_sync_offset_ms": 2.0,
"visualization_enabled": false
},
"sensors": [
{
"sensor_id": "demo_front_camera",
"sensor_type": "front_camera",
"frame_id": "front_camera_link",
"image_topic": "/sensor/front_camera/image_raw",
"telemetry_topic": "/sensor_calibration/telemetry",
"mount_pose": {
"x_m": 1.12,
"y_m": 0.0,
"z_m": 1.18,
"roll_rad": 1.5707963267948966,
"pitch_rad": 0.0,
"yaw_rad": -1.5707963267948966
}
},
{
"sensor_id": "demo_down_camera",
"sensor_type": "down_camera",
"frame_id": "down_camera_link",
"image_topic": "/sensor/down_camera/image_raw",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_3d",
"sensor_type": "lidar_3d",
"frame_id": "lidar_3d_link",
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
"mount_pose": {
"x_m": 0.55,
"y_m": 0.0,
"z_m": 1.2,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_lidar_2d",
"sensor_type": "lidar_2d",
"frame_id": "lidar_2d_link",
"scan_topic": "/sensor/lidar_2d/scan",
"mount_pose": {
"x_m": 0.7,
"y_m": 0.0,
"z_m": 1.0,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
},
{
"sensor_id": "demo_imu",
"sensor_type": "imu",
"frame_id": "imu_link",
"imu_topic": "/sensor/imu/data",
"mount_pose": {
"x_m": 0.4,
"y_m": 0.0,
"z_m": 0.26,
"roll_rad": 0.0,
"pitch_rad": 0.0,
"yaw_rad": 0.0
}
}
],
"lidar_2d_calibration_targets": {
"enabled": false,
"target_width_m": 0.82,
"target_height_m": 1.6,
"target_thickness_m": 0.04,
"target_bottom_z_m": 0.08,
"slope_angle_deg": 45.0,
"targets": []
},
"orchestrator_session_config_hint": {
"vehicle_id": "demo_agv_001",
"localization_source_id": "isaac_sim_truth_source",
"workcell_zone_id": "isaac_workcell_zone_a",
"reference_target_id": "isaac_external_truth"
}
}
agv_calib_brain/README.md
+134
View File
@@ -0,0 +1,134 @@
# AGV 自动标定车间 - 总述
本项目是 AGV（自动导引车）自动化标定车间的软件系统，用于在车间环境中对 AGV 进行底盘参数标定、运控参数标定、传感器内参/外参标定，以及手眼标定。
## 项目概述
### 核心能力
| 标定类型 | 说明 | 状态 |
|---------|------|------|
| 外部真值接入 | 通过外部定位系统（如 OptiTrack、激光雷达定位）获取车辆位姿真值 | ✅ 已验证 |
| 底盘参数标定 | 标定轮距、轴距、转向角等底盘几何参数 | ✅ 已验证（模板） |
| 运控参数标定 | 标定横向/纵向控制器的 PID/MPC/LQR 参数 | ✅ 已验证（模板） |
| 传感器内参标定 | 相机（前视/下视）、IMU 的内参标定 | ✅ 已验证（模板） |
| 传感器外参标定 | 相机、雷达等传感器相对于车体坐标系的外参标定 | 🚧 待完善 |
| 手眼标定 | 机械臂与相机之间的变换关系标定 | 🚧 待开发 |
### 技术架构
```
┌─────────────────────────────────────────────────────────────────┐
│ 车间工控机（Ubuntu） │
│ ┌─────────────────┐ ┌──────────────┐ ┌─────────────────────┐ │
│ │ workshop_ │ │ 底盘标定服务 │ │ 传感器标定服务 │ │
│ │ orchestrator_v2 │ │ chassis_ │ │ sensor_calibration_ │ │
│ │ (总控) │ │ calibration │ │ service │ │
│ └────────┬────────┘ └──────────────┘ └─────────────────────┘ │
│ │ │
│ ┌────────▼────────┐ ┌──────────────┐ ┌─────────────────────┐ │
│ │ vehicle_agent_ │ │ 运控标定服务 │ │ 外部位姿服务 │ │
│ │ gateway │ │ control_ │ │ external_ │ │
│ │ (WiFi6/TCP网关) │ │ calibration │ │ localization_ │ │
│ └─────────────────┘ └──────────────┘ └─────────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
│ WiFi6/TCP
▼
┌─────────────────────────────────────────────────────────────────┐
│ 车端电脑（Windows/Linux） │
│ ┌─────────────────┐ ┌──────────────┐ ┌─────────────────────┐ │
│ │ 车端 Agent │ │ 底盘控制器 │ │ 传感器驱动 │ │
│ │ (TCP<->CAN/PLC) │ │ (CAN/PLC) │ │ (相机/雷达/IMU) │ │
│ └─────────────────┘ └──────────────┘ └─────────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
```
## 目录结构
```
agv_calib_brain/
├── src/
│ ├── core/ # 核心标定逻辑
│ │ ├── workshop_orchestrator/ # 车间总控编排器
│ │ ├── chassis_calibration_service/ # 底盘标定服务
│ │ ├── control_calibration_service/ # 运控标定服务
│ │ ├── sensor_calibration_service/ # 传感器标定服务
│ │ ├── external_localization_service/ # 外部位姿服务
│ │ └── vehicle_profile_manager/ # 车辆画像管理
│ │
│ ├── communication/ # 通信层
│ │ ├── win_ubuntu_bridge/ # 车间<->车端通信桥
│ │ └── interfaces/ # ROS2 接口定义
│ │
│ ├── simulation/ # 仿真验证
│ │ ├── isaac_workshop_sim/ # Isaac Sim 标定车间
│ │ ├── vehicle_agent_sim/ # 仿真车端 Agent
│ │ ├── vehicle_sensor_agent_sim/ # 仿真传感器 Agent
│ │ └── tools/ # 仿真工具脚本
│ │
│ ├── deployment/ # 部署配置
│ │ └── profiles/ # 仿真/部署配置文件
│ │
│ ├── site_deployment/ # 真实现场部署
│ │ └── (真实车辆 SDK、PLC/CAN 适配)
│ │
│ └── apps/ # 操作员工具
│
├── run_isaac_real_sim_test.sh # 一键启动 Isaac 仿真测试
├── stop_isaac_real_sim_stack.sh # 停止仿真栈
├── SIMULATION_GUIDE.md # 仿真环境启动指南
└── README.md # 本文件
```
## 快速开始
### 1. 编译
```bash
cd /home/nvidia/study/AutoCalib-Workshop/agv_calib_brain
colcon build
source install/setup.bash
```
### 2. 一键启动完整仿真
```bash
# 启动 Isaac + 车端 Agent + 车间总控 + 执行验收
./run_isaac_real_sim_test.sh --headless
```
### 3. 查看详细指南
- **仿真启动指南**: [SIMULATION_GUIDE.md](./SIMULATION_GUIDE.md) - 如何单独启动各组件
- **源码目录说明**: [src/README.md](./src/README.md) - 源码组织结构
## 主要验证场景
| 场景 | 启动方式 | 验证内容 |
|------|---------|---------|
| 快速闭环验证 | `launch_sim_stack.py --component vehicle-agent` + `minimal_workshop_demo.launch.py` | 车间总控能创建 session、生成阶段计划、调用车端接口 |
| Isaac topic 检查 | `run_isaac_real_sim_test.sh --headless`（前半段） | Isaac 能发布传感器和真值 topic |
| 完整 Isaac 仿真闭环 | `run_isaac_real_sim_test.sh --headless` | 场景、agent、桥接、总控一起运行 |
| 传感器转发链路 | `--component vehicle-agent,sensor-ingest` | 车端能订阅 Isaac topic，车间端能拉取数据 |
## 四条核心通信链路
| 链路 | 方向 | 功能 | 组件 |
|------|------|------|------|
| 外部真值链路 | 外部系统 → 车端 | 位姿、速度、时间戳 | `external-pose-bridge` |
| 底盘链路 | 车间 → 车端 | 下发标定动作、读取遥测 | `vehicle-agent-gateway` |
| 运控链路 | 车间 → 车端 | 下发控制任务、读取误差 | `vehicle-agent-gateway` |
| 传感器链路 | 车端 → 车间 | 相机、雷达、IMU 数据 | `sensor-ingest` |
## 开发状态
- ✅ **已完成**: 车间总控编排器、四类标定服务框架、车辆画像管理、Isaac 仿真环境、WiFi6/TCP 通信链路、端到端验收脚本
- 🚧 **进行中**: 传感器外参标定采样链路、真实算法参数收敛
- ⏳ **待开发**: 手眼标定、真实现场部署验证
## 关键设计原则
1. **主控只编排流程** - `workshop_orchestrator_v2` 负责阶段调度，具体标定由独立 ROS2 包实现
2. **仿真与现场分离** - Isaac 代码只在 `simulation/`，真实车辆代码只在 `site_deployment/`
3. **统一通信协议** - 车间与车端通过 WiFi6/TCP 通信，协议由 `communication/` 定义
4. **车辆画像驱动** - 车辆能力、传感器配置通过 `vehicle_profile` 描述，支持多车型
agv_calib_brain/SIMULATION_GUIDE.md
+150
View File
@@ -0,0 +1,150 @@
# 仿真环境启动指南
本文档说明如何单独启动 Isaac 仿真环境的各个组件。
## 前置条件
```bash
# 先 source ROS2 环境
source install/setup.bash
```
## 单独启动组件
### 1. Isaac 仿真环境
```bash
# 带可视化界面
python3 src/simulation/tools/launch_sim_stack.py --component isaac
# headless 模式（无界面）
python3 src/simulation/tools/launch_sim_stack.py --component isaac --headless
```
### 2. 仿真车端统一 Agent
包含底盘、运控、传感器数据转发：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component vehicle-agent
```
### 3. 外部位姿桥
将 Isaac 真值转发给车端：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component external-pose-bridge
```
### 4. 车间侧传感器 Ingest
从车端轮询传感器数据：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component sensor-ingest
```
### 5. 车间 Gateway
ROS2 节点，对接底盘/运控：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component gateway
```
## 启动完整仿真链路
### 方式一：一键启动所有仿真组件（不含车间总控）
```bash
python3 src/simulation/tools/launch_sim_stack.py --component all
```
### 方式二：分步启动（推荐用于调试）
终端 1 - 启动 Isaac：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component isaac
```
终端 2 - 启动车端 Agent：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component vehicle-agent
```
终端 3 - 启动外部位姿桥：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component external-pose-bridge
```
终端 4 - 启动传感器 Ingest：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component sensor-ingest
```
终端 5 - 启动车间 Gateway：
```bash
python3 src/simulation/tools/launch_sim_stack.py --component gateway
```
终端 6 - 启动车间总控：
```bash
ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \
use_gateway:=true \
chassis_host:=127.0.0.1 \
control_host:=127.0.0.1 \
sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu
```
## 验证仿真环境
启动后检查 topic：
```bash
# 查看 Isaac 发布的传感器数据
ros2 topic list | grep isaac
# 检查各传感器数据频率
ros2 topic hz /sensor/front_camera/image_raw
ros2 topic hz /sensor/down_camera/image_raw
ros2 topic hz /sensor/lidar_3d/pointcloud
ros2 topic hz /sensor/lidar_2d/scan
ros2 topic hz /sensor/imu/data
# 检查外部位姿
ros2 topic hz /isaac/external_localization/vehicle/pose
```
## 执行端到端验收测试
仿真环境启动后，执行验收：
```bash
python3 src/simulation/tools/smoke_test_workshop_orchestrator.py \
--verbose-feedback \
--no-publish-fake-external-telemetry
```
## 停止仿真环境
按 `Ctrl+C` 停止各组件，或使用：
```bash
./stop_isaac_real_sim_stack.sh
```
## 快捷脚本
也可以使用一键测试脚本（启动所有组件并执行验收）：
```bash
# 完整测试（启动 + 验收 + 停止）
./run_isaac_real_sim_test.sh
# 保持运行（不执行验收）
./run_isaac_real_sim_test.sh --no-smoke --keep-running
# headless 模式
./run_isaac_real_sim_test.sh --headless
```
agv_calib_brain/auto_calib_flowchart.md
+50
View File
@@ -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`。
agv_calib_brain/build.sh
+16 -3
View File
@@ -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
@@ -33,5 +48,3 @@ if AUTOWARE_COMPILE_WITH_CUDA=1 "${BUILD_ARGS[@]}"; then
else
warn "Build failed, skip sourcing."
fi
agv_calib_brain/calibration_architecture.md
+63
View File
@@ -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** 用来解耦流程和算法实现。
agv_calib_brain/calibration_architecture_compare.md
+49
View File
@@ -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 始终只负责编排，不直接承载算法。
agv_calib_brain/run_isaac_real_sim_test.sh
Executable
+661
View File
@@ -0,0 +1,661 @@
#!/usr/bin/env bash
# 一键启动 Isaac 真实仿真闭环测试。
#
# 默认流程：
# 1. 使用 conda 环境 AutoCalib_Workshop 启动 Isaac 标定车间；
# 2. 启动仿真车端统一 agent、外部位姿桥和车间侧传感器 ingest；
# 3. 启动车间总控 demo；
# 4. 使用 Isaac 真实发布的真值/传感器数据执行 orchestrator 端到端验收；
# 5. 验收结束后自动停止后台进程。
set -Eeuo pipefail
WORKSPACE_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
CONDA_ENV_NAME="${CONDA_ENV_NAME:-AutoCalib_Workshop}"
ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}"
RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/isaac_real_sim_$(date +%Y%m%d_%H%M%S)}"
HEADLESS=0
DO_BUILD=0
RUN_SMOKE=1
KEEP_RUNNING=0
WITH_SENSOR_INGEST=1
WAIT_FOR_ISAAC_TOPICS=1
WORKSHOP_USE_GATEWAY=1
ISAAC_WAIT_SEC="${ISAAC_WAIT_SEC:-180}"
WORKSHOP_WARMUP_SEC="${WORKSHOP_WARMUP_SEC:-5}"
START_GRACE_SEC="${START_GRACE_SEC:-2}"
STOP_GRACE_SEC="${STOP_GRACE_SEC:-8}"
TERM_GRACE_SEC="${TERM_GRACE_SEC:-3}"
SMOKE_TASKS=""
DATA_INPUT_PARAMS_FILE=""
DATASET_INDEX_FILE=""
PIDS=()
NAMES=()
LOGS=()
CLEANED=0
usage() {
cat <<'EOF'
用法:
./run_isaac_real_sim_test.sh [选项]
默认会验证:
1. Isaac 发布外部真值、前视相机、下视相机、3D 雷达、2D 雷达、IMU topic。
2. 仿真车端统一 agent、外部位姿桥、车间侧传感器 ingest 都能启动并通信。
3. 车间总控能创建 session，并默认执行 external,chassis,control,sensor_intrinsic 阶段。
4. 传感器数据能从 Isaac -> 车端 -> WiFi6/TCP -> 车间侧链路流动。
当前不验证:
1. 真实底盘参数是否已经收敛。
2. 真实相机内参、传感器外参是否已经被算法正确求解。
3. 棋盘格角点数量、点云几何质量、时间同步精度等采样质量。
选项:
--headless 以 headless 模式启动 Isaac。
--build 启动前先执行 colcon build。
--no-smoke 只启动仿真链路，不执行 orchestrator 验收。
--keep-running 验收结束后保持仿真链路运行，按 Ctrl+C 停止。
--no-sensor-ingest 不启动车间侧传感器 ingest 轮询进程。
--skip-topic-check 不等待 Isaac 真值/传感器 topic，直接继续启动后续链路。
--local-calibration-services
底盘和运控阶段使用本地标定服务节点，便于验证四个算法服务的数据输入注入。
--data-input-params-file PATH
把真实采集/ingest 生成的 site_data_input.yaml 注入算法服务。
--dataset-index-file PATH
把真实采集/ingest 生成的 dataset_index.yaml 写入最终报告。
--isaac-wait-sec SEC 等待 Isaac topic 的最长时间，默认 180 秒。
--warmup-sec SEC 总控 demo 启动后等待采集真值历史的时间，默认 5 秒。
--tasks LIST 覆盖 smoke_test_workshop_orchestrator.py 的任务列表。
例: external,chassis,control,sensor_intrinsic
--conda-env NAME Isaac 进程使用的 conda 环境，默认 AutoCalib_Workshop。
-h, --help 显示帮助。
环境变量:
CONDA_ENV_NAME 同 --conda-env。
ROS_LOG_DIR ROS 日志目录，默认 /tmp/roslog。
RUN_LOG_ROOT 本脚本各后台进程日志目录。
STOP_GRACE_SEC SIGINT 后等待后台进程优雅退出的秒数，默认 8。
TERM_GRACE_SEC SIGTERM 后等待后台进程退出的秒数，默认 3。
示例:
./run_isaac_real_sim_test.sh --headless
./run_isaac_real_sim_test.sh --headless --keep-running
./run_isaac_real_sim_test.sh --headless --local-calibration-services --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml
./run_isaac_real_sim_test.sh --no-smoke
EOF
}
log() {
echo "[INFO] $*"
}
warn() {
echo "[WARN] $*" >&2
}
die() {
echo "[ERROR] $*" >&2
exit 1
}
resolve_workspace_path() {
local path="$1"
case "${path}" in
/*)
echo "${path}"
;;
*)
echo "${WORKSPACE_DIR}/${path}"
;;
esac
}
while [[ $# -gt 0 ]]; do
case "$1" in
--headless)
HEADLESS=1
shift
;;
--build)
DO_BUILD=1
shift
;;
--no-smoke)
RUN_SMOKE=0
KEEP_RUNNING=1
shift
;;
--keep-running)
KEEP_RUNNING=1
shift
;;
--no-sensor-ingest)
WITH_SENSOR_INGEST=0
shift
;;
--skip-topic-check)
WAIT_FOR_ISAAC_TOPICS=0
shift
;;
--local-calibration-services)
WORKSHOP_USE_GATEWAY=0
shift
;;
--data-input-params-file)
[[ $# -ge 2 ]] || die "--data-input-params-file 需要参数"
DATA_INPUT_PARAMS_FILE="$2"
shift 2
;;
--dataset-index-file)
[[ $# -ge 2 ]] || die "--dataset-index-file 需要参数"
DATASET_INDEX_FILE="$2"
shift 2
;;
--isaac-wait-sec)
[[ $# -ge 2 ]] || die "--isaac-wait-sec 需要参数"
ISAAC_WAIT_SEC="$2"
shift 2
;;
--warmup-sec)
[[ $# -ge 2 ]] || die "--warmup-sec 需要参数"
WORKSHOP_WARMUP_SEC="$2"
shift 2
;;
--tasks)
[[ $# -ge 2 ]] || die "--tasks 需要参数"
SMOKE_TASKS="$2"
shift 2
;;
--conda-env)
[[ $# -ge 2 ]] || die "--conda-env 需要参数"
CONDA_ENV_NAME="$2"
shift 2
;;
-h|--help)
usage
exit 0
;;
*)
die "未知参数: $1"
;;
esac
done
if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then
DATA_INPUT_PARAMS_FILE="$(resolve_workspace_path "${DATA_INPUT_PARAMS_FILE}")"
[[ -f "${DATA_INPUT_PARAMS_FILE}" ]] || die "找不到数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}"
[[ "${DATA_INPUT_PARAMS_FILE}" != *"'"* ]] || die "data_input_params_file 路径不能包含单引号"
if [[ -z "${DATASET_INDEX_FILE}" ]]; then
DATASET_INDEX_FILE="$(dirname "${DATA_INPUT_PARAMS_FILE}")/dataset_index.yaml"
fi
fi
if [[ -n "${DATASET_INDEX_FILE}" ]]; then
DATASET_INDEX_FILE="$(resolve_workspace_path "${DATASET_INDEX_FILE}")"
[[ "${DATASET_INDEX_FILE}" != *"'"* ]] || die "dataset_index_file 路径不能包含单引号"
fi
if [[ "${WORKSHOP_USE_GATEWAY}" -eq 1 ]]; then
WORKSHOP_USE_GATEWAY_VALUE="true"
else
WORKSHOP_USE_GATEWAY_VALUE="false"
fi
resolve_conda_sh() {
if [[ -n "${CONDA_SH:-}" && -f "${CONDA_SH}" ]]; then
echo "${CONDA_SH}"
return 0
fi
local candidate
for candidate in \
"${HOME}/anaconda3/etc/profile.d/conda.sh" \
"${HOME}/miniconda3/etc/profile.d/conda.sh" \
"/opt/conda/etc/profile.d/conda.sh"
do
if [[ -f "${candidate}" ]]; then
echo "${candidate}"
return 0
fi
done
if command -v conda >/dev/null 2>&1; then
local base
base="$(conda info --base 2>/dev/null || true)"
if [[ -n "${base}" && -f "${base}/etc/profile.d/conda.sh" ]]; then
echo "${base}/etc/profile.d/conda.sh"
return 0
fi
fi
return 1
}
source_ros_env() {
set +u
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
source "${WORKSPACE_DIR}/install/setup.bash"
set -u
}
wait_for_background_exit() {
local timeout_sec="$1"
local deadline=$((SECONDS + timeout_sec))
local pid
while (( SECONDS < deadline )); do
local any_alive=0
for pid in "${PIDS[@]}"; do
if kill -0 "${pid}" >/dev/null 2>&1; then
any_alive=1
break
fi
done
if [[ "${any_alive}" -eq 0 ]]; then
return 0
fi
sleep 1
done
return 1
}
cleanup() {
if [[ "${CLEANED}" -eq 1 ]]; then
return
fi
CLEANED=1
if [[ "${#PIDS[@]}" -eq 0 ]]; then
return
fi
log "正在停止后台进程..."
local i pid name
for ((i=${#PIDS[@]}-1; i>=0; i--)); do
pid="${PIDS[$i]}"
name="${NAMES[$i]}"
if kill -0 "${pid}" >/dev/null 2>&1; then
log "发送 SIGINT: ${name} pid=${pid}"
kill -INT "${pid}" >/dev/null 2>&1 || true
fi
done
wait_for_background_exit "${STOP_GRACE_SEC}" || true
for ((i=${#PIDS[@]}-1; i>=0; i--)); do
pid="${PIDS[$i]}"
name="${NAMES[$i]}"
if kill -0 "${pid}" >/dev/null 2>&1; then
log "${name} 未在 ${STOP_GRACE_SEC}s 内退出，发送 SIGTERM: pid=${pid}"
kill -TERM "${pid}" >/dev/null 2>&1 || true
fi
done
wait_for_background_exit "${TERM_GRACE_SEC}" || true
for ((i=${#PIDS[@]}-1; i>=0; i--)); do
pid="${PIDS[$i]}"
name="${NAMES[$i]}"
if kill -0 "${pid}" >/dev/null 2>&1; then
warn "${name} 仍未退出，发送 SIGKILL: pid=${pid}"
kill -KILL "${pid}" >/dev/null 2>&1 || true
fi
done
}
on_signal() {
trap - INT TERM
cleanup
exit 130
}
trap cleanup EXIT
trap on_signal INT TERM
start_bg() {
local name="$1"
shift
local logfile="${RUN_LOG_ROOT}/${name}.log"
log "启动 ${name}，日志: ${logfile}"
"$@" >"${logfile}" 2>&1 &
local pid=$!
PIDS+=("${pid}")
NAMES+=("${name}")
LOGS+=("${logfile}")
sleep "${START_GRACE_SEC}"
if ! kill -0 "${pid}" >/dev/null 2>&1; then
warn "${name} 启动后立即退出，最近日志如下:"
tail -n 120 "${logfile}" >&2 || true
exit 1
fi
}
wait_for_tcp_port() {
local host="$1"
local port="$2"
local timeout_sec="$3"
local name="$4"
local deadline=$((SECONDS + timeout_sec))
log "等待 ${name} TCP ${host}:${port}"
while (( SECONDS < deadline )); do
if timeout 1 bash -c "</dev/tcp/${host}/${port}" >/dev/null 2>&1; then
log "${name} 已就绪"
return 0
fi
sleep 1
done
warn "等待 ${name} 超时"
return 1
}
wait_for_ros_topic_once() {
local topic="$1"
local timeout_sec="$2"
local deadline=$((SECONDS + timeout_sec))
local echo_args=(--qos-reliability best_effort --once "${topic}")
case "${topic}" in
/sensor/front_camera/*|/sensor/down_camera/*|/sensor/lidar_3d/*)
echo_args=(--qos-reliability best_effort --once "${topic}" --field header)
;;
esac
log "等待 Isaac topic: ${topic}"
while (( SECONDS < deadline )); do
if timeout 8s ros2 topic echo "${echo_args[@]}" >/dev/null 2>&1; then
log "已收到 topic: ${topic}"
return 0
fi
sleep 2
done
warn "等待 topic 超时: ${topic}"
warn "可检查 Isaac 日志: ${RUN_LOG_ROOT}/isaac.log"
if [[ -f "${RUN_LOG_ROOT}/isaac.log" ]]; then
warn "Isaac 最近日志:"
tail -n 120 "${RUN_LOG_ROOT}/isaac.log" >&2 || true
fi
return 1
}
wait_for_ros_service() {
local service="$1"
local timeout_sec="$2"
local deadline=$((SECONDS + timeout_sec))
log "等待 ROS service: ${service}"
while (( SECONDS < deadline )); do
if ros2 service list 2>/dev/null | grep -qx "${service}"; then
log "service 已就绪: ${service}"
return 0
fi
sleep 1
done
warn "等待 service 超时: ${service}"
return 1
}
wait_for_ros_param() {
local node="$1"
local param="$2"
local timeout_sec="$3"
local deadline=$((SECONDS + timeout_sec))
log "等待 ROS 参数: ${node} ${param}"
while (( SECONDS < deadline )); do
if ros2 param get "${node}" "${param}" >/dev/null 2>&1; then
log "参数已就绪: ${node} ${param}"
return 0
fi
sleep 1
done
warn "等待参数超时: ${node} ${param}"
return 1
}
require_ros_param_non_empty() {
local node="$1"
local param="$2"
local output
output="$(ros2 param get "${node}" "${param}" 2>/dev/null || true)"
if [[ -z "${output}" || "${output}" == *"[]"* || "${output}" == *"not set"* ]]; then
warn "数据输入参数为空: ${node} ${param}; 输出: ${output}"
return 1
fi
log "数据输入参数已注入: ${node} ${param}"
return 0
}
verify_data_input_params() {
if [[ -z "${DATA_INPUT_PARAMS_FILE}" ]]; then
return 0
fi
log "验证 data_input 参数注入: ${DATA_INPUT_PARAMS_FILE}"
wait_for_ros_param "/external_localization_service" "data_input.data_window_start_timestamp_us" 30 || return 1
wait_for_ros_param "/sensor_calibration_service" "data_input.data_window_start_timestamp_us" 30 || return 1
require_ros_param_non_empty "/external_localization_service" "data_input.external_observation_files" || return 1
require_ros_param_non_empty "/sensor_calibration_service" "data_input.synchronized_dataset_files" || return 1
wait_for_ros_param "/workshop_orchestrator_v2" "data_input_params_file" 30 || return 1
require_ros_param_non_empty "/workshop_orchestrator_v2" "data_input_params_file" || return 1
if [[ "${WORKSHOP_USE_GATEWAY}" -eq 0 ]]; then
wait_for_ros_param "/chassis_calibration_service" "data_input.data_window_start_timestamp_us" 30 || return 1
wait_for_ros_param "/control_calibration_service" "data_input.data_window_start_timestamp_us" 30 || return 1
require_ros_param_non_empty "/chassis_calibration_service" "data_input.chassis_motion_data_files" || return 1
require_ros_param_non_empty "/control_calibration_service" "data_input.control_evaluation_data_files" || return 1
else
warn "当前使用 vehicle_agent_gateway，底盘/运控 data_input 由车端 agent 侧负责，本脚本只验证 external/sensor 注入。"
fi
}
check_no_existing_workshop_orchestrator() {
local nodes
nodes="$(timeout 5s ros2 node list 2>/dev/null || true)"
if [[ -z "${nodes}" ]]; then
return
fi
local count
count="$(printf '%s\n' "${nodes}" | grep -xc "/workshop_orchestrator_v2" || true)"
if [[ "${count}" -gt 0 ]]; then
die "检测到已有 /workshop_orchestrator_v2 节点。请先关闭旧的 workshop-demo/仿真脚本，或执行 ./stop_isaac_real_sim_stack.sh --kill 清理遗留进程，否则 create_session 和 execute_session 可能连到不同实例。"
fi
}
mkdir -p "${ROS_LOG_DIR}" "${RUN_LOG_ROOT}"
if [[ "${DO_BUILD}" -eq 1 ]]; then
log "执行 colcon build"
(
cd "${WORKSPACE_DIR}"
set +u
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
set -u
colcon build
)
fi
[[ -f "${WORKSPACE_DIR}/install/setup.bash" ]] || die "找不到 install/setup.bash，请先执行 colcon build，或使用 --build"
CONDA_SH_PATH="$(resolve_conda_sh || true)"
[[ -n "${CONDA_SH_PATH}" ]] || die "找不到 conda.sh，无法激活 conda 环境 ${CONDA_ENV_NAME}"
source_ros_env
log "工作空间: ${WORKSPACE_DIR}"
log "ROS_LOG_DIR: ${ROS_LOG_DIR}"
log "进程日志目录: ${RUN_LOG_ROOT}"
log "Isaac conda 环境: ${CONDA_ENV_NAME}"
log "Isaac headless: ${HEADLESS}"
log "总控底盘/运控模式 use_gateway=${WORKSHOP_USE_GATEWAY_VALUE}"
if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then
log "数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}"
fi
export PYTHONUNBUFFERED=1
check_no_existing_workshop_orchestrator
ISAAC_HEADLESS_ARG=""
if [[ "${HEADLESS}" -eq 1 ]]; then
ISAAC_HEADLESS_ARG="--headless"
fi
start_bg "isaac" bash -lc "
set -Eeuo pipefail
cd '${WORKSPACE_DIR}'
set +u
source '${CONDA_SH_PATH}'
conda activate '${CONDA_ENV_NAME}'
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
source '${WORKSPACE_DIR}/install/setup.bash'
set -u
export ROS_LOG_DIR='${ROS_LOG_DIR}'
export PYTHONNOUSERSITE=1
exec python3 src/simulation/tools/launch_sim_stack.py --component isaac ${ISAAC_HEADLESS_ARG}
"
if [[ "${WAIT_FOR_ISAAC_TOPICS}" -eq 1 ]]; then
wait_for_ros_topic_once "/isaac/external_localization/vehicle/pose" "${ISAAC_WAIT_SEC}" || exit 1
wait_for_ros_topic_once "/sensor/front_camera/image_raw" "${ISAAC_WAIT_SEC}" || exit 1
wait_for_ros_topic_once "/sensor/down_camera/image_raw" "${ISAAC_WAIT_SEC}" || exit 1
wait_for_ros_topic_once "/sensor/lidar_3d/pointcloud" "${ISAAC_WAIT_SEC}" || exit 1
wait_for_ros_topic_once "/sensor/imu/data" "${ISAAC_WAIT_SEC}" || exit 1
wait_for_ros_topic_once "/sensor/lidar_2d/scan" "${ISAAC_WAIT_SEC}" || exit 1
fi
start_bg "vehicle-agent" bash -lc "
set -Eeuo pipefail
cd '${WORKSPACE_DIR}'
set +u
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
source '${WORKSPACE_DIR}/install/setup.bash'
set -u
export ROS_LOG_DIR='${ROS_LOG_DIR}'
exec python3 src/simulation/tools/launch_sim_stack.py --component vehicle-agent
"
wait_for_tcp_port "127.0.0.1" 9000 30 "vehicle-agent unified WiFi6" || exit 1
start_bg "external-pose-bridge" bash -lc "
set -Eeuo pipefail
cd '${WORKSPACE_DIR}'
set +u
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
source '${WORKSPACE_DIR}/install/setup.bash'
set -u
export ROS_LOG_DIR='${ROS_LOG_DIR}'
exec python3 src/simulation/tools/launch_sim_stack.py --component external-pose-bridge
"
if [[ "${WITH_SENSOR_INGEST}" -eq 1 ]]; then
start_bg "sensor-ingest" bash -lc "
set -Eeuo pipefail
cd '${WORKSPACE_DIR}'
set +u
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
source '${WORKSPACE_DIR}/install/setup.bash'
set -u
export ROS_LOG_DIR='${ROS_LOG_DIR}'
exec python3 src/simulation/tools/launch_sim_stack.py --component sensor-ingest
"
fi
start_bg "workshop-demo" bash -lc "
set -Eeuo pipefail
cd '${WORKSPACE_DIR}'
set +u
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
source '${WORKSPACE_DIR}/install/setup.bash'
set -u
export ROS_LOG_DIR='${ROS_LOG_DIR}'
launch_args=(
'use_gateway:=${WORKSHOP_USE_GATEWAY_VALUE}'
'chassis_host:=127.0.0.1'
'control_host:=127.0.0.1'
'sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu'
)
if [[ -n '${DATA_INPUT_PARAMS_FILE}' ]]; then
launch_args+=('data_input_params_file:=${DATA_INPUT_PARAMS_FILE}')
fi
if [[ -n '${DATASET_INDEX_FILE}' ]]; then
launch_args+=('dataset_index_file:=${DATASET_INDEX_FILE}')
fi
exec ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \"\${launch_args[@]}\"
"
wait_for_ros_service "/workshop_v2/create_session" 60 || exit 1
verify_data_input_params || exit 1
log "等待 ${WORKSHOP_WARMUP_SEC}s，让 external_localization_service 累积 Isaac 真值历史"
sleep "${WORKSHOP_WARMUP_SEC}"
SMOKE_STATUS=0
if [[ "${RUN_SMOKE}" -eq 1 ]]; then
SMOKE_ARGS=(
--verbose-feedback
--no-publish-fake-external-telemetry
)
if [[ "${WORKSHOP_USE_GATEWAY}" -eq 0 ]]; then
SMOKE_ARGS+=(--disable-wifi6-precheck)
fi
if [[ -n "${SMOKE_TASKS}" ]]; then
SMOKE_ARGS+=(--tasks "${SMOKE_TASKS}")
fi
SMOKE_LOG="${RUN_LOG_ROOT}/smoke.log"
log "执行 orchestrator 端到端验收，日志: ${SMOKE_LOG}"
set +e
(
cd "${WORKSPACE_DIR}"
source_ros_env
export ROS_LOG_DIR="${ROS_LOG_DIR}"
python3 src/simulation/tools/smoke_test_workshop_orchestrator.py "${SMOKE_ARGS[@]}"
) 2>&1 | tee "${SMOKE_LOG}"
SMOKE_STATUS=${PIPESTATUS[0]}
set -e
if [[ "${SMOKE_STATUS}" -eq 0 ]]; then
if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then
grep -q "site_data_input.yaml" "${SMOKE_LOG}" || die "报告中没有记录 site_data_input.yaml"
if [[ -f "${DATASET_INDEX_FILE}" ]]; then
grep -q "dataset_index.yaml" "${SMOKE_LOG}" || die "报告中没有记录 dataset_index.yaml"
fi
fi
log "仿真闭环验收通过"
else
warn "仿真闭环验收失败，退出码: ${SMOKE_STATUS}"
fi
else
log "已按 --no-smoke 跳过 orchestrator 验收"
fi
if [[ "${KEEP_RUNNING}" -eq 1 ]]; then
log "仿真链路保持运行中。按 Ctrl+C 停止。"
while true; do
sleep 3600
done
fi
exit "${SMOKE_STATUS}"
agv_calib_brain/run_local_data_input_smoke.sh
Executable
+280
View File
@@ -0,0 +1,280 @@
#!/usr/bin/env bash
# 本地数据输入回归测试。
#
# 验证链路：
# dataset_index.yaml -> site_data_input.yaml -> external -> chassis -> control -> sensor_intrinsic -> report
#
# 这个脚本不启动 Isaac，不依赖 GPU；用于快速检查算法模板和数据输入注入合同。
set -Eeuo pipefail
WORKSPACE_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}"
RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/local_data_input_smoke_$(date +%Y%m%d_%H%M%S)}"
export ROS_LOG_DIR
SITE_PROFILE="src/deployment/profiles/site_template.yaml"
SESSION_ID="session_001"
SESSION_DIR="/tmp/agv_calib_local_data_input_smoke/session_001"
DATASET_INDEX_FILE=""
DATA_INPUT_PARAMS_FILE=""
TASKS="external,chassis,control,sensor_intrinsic"
KEEP_RUNNING=0
SKIP_GENERATE=0
STRICT_INDEX=1
PIDS=()
NAMES=()
CLEANED=0
usage() {
cat <<'EOF'
用法:
./run_local_data_input_smoke.sh [选项]
用途:
不启动 Isaac，只启动本地 ROS 标定服务和总控，验证数据输入参数注入与 report 链路。
选项:
--site-profile PATH 现场 profile，默认 src/deployment/profiles/site_template.yaml。
--session-id ID 自动生成样例数据集时使用的 session_id，默认 session_001。
--session-dir PATH 自动生成样例数据集时使用的会话目录。
--data-input-params-file PATH
使用已有 site_data_input.yaml，跳过自动生成。
--dataset-index PATH 使用已有 site_data_input.yaml 时，对应的 dataset_index.yaml。
--tasks LIST 逗号分隔任务列表，默认 external,chassis,control,sensor_intrinsic。
--skip-generate 不生成 dataset_index/site_data_input，只使用 --data-input-params-file。
--no-strict-index 关闭 dataset_index 严格任务输入校验。
--keep-running smoke 结束后保持 launch 进程运行。
-h, --help 显示帮助。
示例:
./run_local_data_input_smoke.sh
./run_local_data_input_smoke.sh --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml
EOF
}
log() {
echo "[INFO] $*"
}
warn() {
echo "[WARN] $*" >&2
}
die() {
echo "[ERROR] $*" >&2
exit 1
}
resolve_workspace_path() {
local path="$1"
case "${path}" in
/*)
echo "${path}"
;;
*)
echo "${WORKSPACE_DIR}/${path}"
;;
esac
}
source_ros_env() {
set +u
if [[ -f /opt/ros/humble/setup.bash ]]; then
source /opt/ros/humble/setup.bash
fi
source "${WORKSPACE_DIR}/install/setup.bash"
set -u
}
cleanup() {
if [[ "${CLEANED}" -eq 1 ]]; then
return
fi
CLEANED=1
local i pid name
for ((i=${#PIDS[@]}-1; i>=0; i--)); do
pid="${PIDS[$i]}"
name="${NAMES[$i]}"
if kill -0 "${pid}" >/dev/null 2>&1; then
log "停止 ${name}: pid=${pid}"
kill -INT "${pid}" >/dev/null 2>&1 || true
fi
done
sleep 2
for ((i=${#PIDS[@]}-1; i>=0; i--)); do
pid="${PIDS[$i]}"
name="${NAMES[$i]}"
if kill -0 "${pid}" >/dev/null 2>&1; then
warn "${name} 未退出，发送 SIGTERM: pid=${pid}"
kill -TERM "${pid}" >/dev/null 2>&1 || true
fi
done
}
trap cleanup EXIT
while [[ $# -gt 0 ]]; do
case "$1" in
--site-profile)
[[ $# -ge 2 ]] || die "--site-profile 需要参数"
SITE_PROFILE="$2"
shift 2
;;
--session-id)
[[ $# -ge 2 ]] || die "--session-id 需要参数"
SESSION_ID="$2"
shift 2
;;
--session-dir)
[[ $# -ge 2 ]] || die "--session-dir 需要参数"
SESSION_DIR="$2"
shift 2
;;
--data-input-params-file)
[[ $# -ge 2 ]] || die "--data-input-params-file 需要参数"
DATA_INPUT_PARAMS_FILE="$2"
SKIP_GENERATE=1
shift 2
;;
--dataset-index)
[[ $# -ge 2 ]] || die "--dataset-index 需要参数"
DATASET_INDEX_FILE="$2"
shift 2
;;
--tasks)
[[ $# -ge 2 ]] || die "--tasks 需要参数"
TASKS="$2"
shift 2
;;
--skip-generate)
SKIP_GENERATE=1
shift
;;
--no-strict-index)
STRICT_INDEX=0
shift
;;
--keep-running)
KEEP_RUNNING=1
shift
;;
-h|--help)
usage
exit 0
;;
*)
die "未知参数: $1"
;;
esac
done
SITE_PROFILE="$(resolve_workspace_path "${SITE_PROFILE}")"
SESSION_DIR="$(resolve_workspace_path "${SESSION_DIR}")"
if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then
DATA_INPUT_PARAMS_FILE="$(resolve_workspace_path "${DATA_INPUT_PARAMS_FILE}")"
fi
if [[ -n "${DATASET_INDEX_FILE}" ]]; then
DATASET_INDEX_FILE="$(resolve_workspace_path "${DATASET_INDEX_FILE}")"
fi
[[ -f "${SITE_PROFILE}" ]] || die "找不到 site profile: ${SITE_PROFILE}"
[[ -f "${WORKSPACE_DIR}/install/setup.bash" ]] || die "找不到 install/setup.bash，请先执行 colcon build"
mkdir -p "${ROS_LOG_DIR}" "${RUN_LOG_ROOT}"
source_ros_env
if [[ "${SKIP_GENERATE}" -eq 0 ]]; then
log "生成本地样例数据集索引和数据输入参数文件"
python3 "${WORKSPACE_DIR}/src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py" \
--site-profile "${SITE_PROFILE}" \
--session-dir "${SESSION_DIR}" \
--session-id "${SESSION_ID}" \
--site-id local_smoke \
--vehicle-id demo_agv_001 \
--start-us 1770000000000000 \
--end-us 1770000120000000 \
--overwrite \
--file external.external_observation_files=external/external_observations.csv \
--file chassis.chassis_motion_data_files=chassis/chassis_motion.csv \
--file control.control_evaluation_data_files=control/control_eval.csv \
--file control.reference_signal_files=control/reference_signal.csv \
--file control.chassis_response_files=control/chassis_response.csv \
--file control.truth_trajectory_files=control/truth_trajectory.csv \
--file sensor.image_sample_files=sensor/front_camera/images.yaml \
--file sensor.target_detection_files=sensor/front_camera/charuco_detections.json \
--finalize
DATA_INPUT_PARAMS_FILE="${SESSION_DIR}/site_data_input.yaml"
DATASET_INDEX_FILE="${SESSION_DIR}/dataset_index.yaml"
elif [[ -z "${DATASET_INDEX_FILE}" ]]; then
DATASET_INDEX_FILE="$(dirname "${DATA_INPUT_PARAMS_FILE}")/dataset_index.yaml"
fi
[[ -f "${DATA_INPUT_PARAMS_FILE}" ]] || die "找不到数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}"
if [[ "${STRICT_INDEX}" -eq 1 ]]; then
if [[ -f "${DATASET_INDEX_FILE}" ]]; then
log "校验数据集索引: ${DATASET_INDEX_FILE}"
python3 "${WORKSPACE_DIR}/src/deployment/tools/validate_dataset_index.py" \
"${DATASET_INDEX_FILE}" \
--tasks "${TASKS}"
else
warn "未找到 dataset_index.yaml，跳过索引校验: ${DATASET_INDEX_FILE}"
fi
fi
LAUNCH_LOG="${RUN_LOG_ROOT}/launch.log"
log "启动本地标定服务和总控，日志: ${LAUNCH_LOG}"
ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \
use_gateway:=false \
data_input_params_file:="${DATA_INPUT_PARAMS_FILE}" \
dataset_index_file:="${DATASET_INDEX_FILE}" \
sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu \
>"${LAUNCH_LOG}" 2>&1 &
PIDS+=("$!")
NAMES+=("local-workshop-demo")
log "等待 /workshop_v2/create_session"
for _ in $(seq 1 60); do
if ros2 service list 2>/dev/null | grep -qx /workshop_v2/create_session; then
break
fi
sleep 1
done
ros2 service list 2>/dev/null | grep -qx /workshop_v2/create_session || {
tail -n 120 "${LAUNCH_LOG}" >&2 || true
die "等待 /workshop_v2/create_session 超时"
}
log "确认四个算法服务已读取 data_input 参数"
ros2 param get /external_localization_service data_input.external_observation_files
ros2 param get /chassis_calibration_service data_input.chassis_motion_data_files
ros2 param get /control_calibration_service data_input.control_evaluation_data_files
ros2 param get /sensor_calibration_service data_input.synchronized_dataset_files
ros2 param get /workshop_orchestrator_v2 data_input_params_file
ros2 param get /workshop_orchestrator_v2 dataset_index_file
SMOKE_LOG="${RUN_LOG_ROOT}/smoke.log"
log "执行本地四阶段 smoke，日志: ${SMOKE_LOG}"
python3 "${WORKSPACE_DIR}/src/simulation/tools/smoke_test_workshop_orchestrator.py" \
--tasks "${TASKS}" \
--verbose-feedback \
--disable-wifi6-precheck \
2>&1 | tee "${SMOKE_LOG}"
grep -q "site_data_input.yaml" "${SMOKE_LOG}" || die "报告中没有记录 site_data_input.yaml"
if [[ -f "${DATASET_INDEX_FILE}" ]]; then
grep -q "dataset_index.yaml" "${SMOKE_LOG}" || die "报告中没有记录 dataset_index.yaml"
fi
if [[ "${KEEP_RUNNING}" -eq 1 ]]; then
log "本地服务保持运行中。按 Ctrl+C 停止。"
while true; do
sleep 3600
done
fi
log "本地数据输入回归测试通过"
agv_calib_brain/src/README.md
+73
View File
@@ -0,0 +1,73 @@
# 源码目录结构
`src` 按“仿真、核心逻辑、通信、现场部署”的边界组织，而不是按临时实验文件组织。
## 目录说明
- `apps/`
面向操作人员的工具和界面原型。
- `communication/`
ROS 2 接口包、TCP 帧协议、车间工控机到车端电脑的 gateway。这个层应该同时服务于仿真和现场部署。
- `core/`
标定流程和标定算法，包括 `workshop_orchestrator`、底盘标定、运控标定、传感器标定、车辆参数管理等可复用核心逻辑。
- `simulation/`
部署前仿真验证代码。Isaac 车间、仿真车辆、仿真车端 agent、仿真传感器、仿真标定靶和旧版仿真包都放在这里。
- `deployment/`
部署 profile、数据集索引转换工具和从仿真迁移到现场前的检查清单。这里放配置基准，不放算法实现。
- `docs/`
源码树内的设计说明、边界说明和迁移规则。
- `site_deployment/`
真实现场部署代码，例如真实车端电脑适配器、真实车辆 SDK、PLC/CAN 或厂商控制器对接代码。
## 边界规则
- Isaac API 只放在 `simulation/`。
- 真实车辆 SDK、PLC、CAN、厂商控制器相关代码只放在 `site_deployment/`。
- ROS 2 接口、TCP 协议和 gateway 放在 `communication/`。
- 编排流程和标定算法放在 `core/`。
- 部署 profile 放在 `deployment/`。
- 设计说明和迁移边界说明放在 `docs/`。
## 文档入口
- `docs/sim_to_site_code_boundary.md`
仿真代码、可复用核心代码和真实现场部署代码的边界。
- `docs/algorithm_template_contract.md`
标定算法模板合同，定义模板维护者和算法实现者的责任边界、可修改文件、输入输出字段和 smoke 验证要求。
## 主要入口
本地数据输入回归测试：
```bash
./run_local_data_input_smoke.sh
```
Isaac 车间仿真：
```bash
python3 src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py
```
仿真车端 agent：
```bash
python3 src/simulation/vehicle_agent_sim/scripts/isaac_vehicle_agent_sim.py
```
按 profile 启动完整仿真链路：
```bash
python3 src/simulation/tools/launch_sim_stack.py
```
这条链路中，车间工控机与车端电脑之间的底盘、运控、外部真值位姿和传感器数据都通过 TCP/WiFi6 仿真边界传输；Isaac topic 只留在仿真内部。
车间 gateway 和 `workshop_orchestrator` 仍然按 ROS 2 包名启动；源码分别在 `communication/` 和 `core/` 下。
agv_calib_brain/src/agv_calib_control_pb2.py
-56
View File
@@ -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)
agv_calib_brain/src/agv_calib_control_pb2_grpc.py
-444
View File
@@ -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)
agv_calib_brain/src/agv_calib_core/CMakeLists.txt
-51
View File
@@ -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()
agv_calib_brain/src/agv_calib_core/behavior_trees/main_pipeline.xml
-23
View File
@@ -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>
agv_calib_brain/src/agv_calib_core/behavior_trees/main_tree.xml
-9
View File
@@ -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>
agv_calib_brain/src/agv_calib_core/config/brain_params.yaml
-6
View File
@@ -1,6 +0,0 @@
/**:
ros__parameters:
# 动态指定要加载的 XML 剧本文件名
tree_xml_filename: "main_tree.xml"
# 行为树的 Tick 循环频率 (毫秒)
tick_rate_ms: 50
agv_calib_brain/src/agv_calib_core/include/agv_calib_core/brain_node.hpp
-25
View File
@@ -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
agv_calib_brain/src/agv_calib_core/include/agv_calib_core/bt_ros2_nodes.hpp
-109
View File
@@ -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
agv_calib_brain/src/agv_calib_core/launch/brain_bringup.launch.py
-31
View File
@@ -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])
agv_calib_brain/src/agv_calib_core/package.xml
-22
View File
@@ -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>
agv_calib_brain/src/agv_calib_core/src/brain_node.cpp
-87
View File
@@ -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)
agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2.py
-56
View File
@@ -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)
agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2_grpc.py
-444
View File
@@ -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)
agv_calib_brain/src/agv_calib_core/test/mock_agv_server.py
-26
View File
@@ -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()
agv_calib_brain/src/apps/operator_ui/README.md
+101
View File
@@ -0,0 +1,101 @@
# 标定车间现场操作台
这个 PySide6 界面面向真实部署使用，不再只是静态原型。它围绕车间电脑侧的实际流程组织：
- 后台加载固定车间部署配置
- 选择、加载或保存可复用的车辆画像文件
- 检查未替换的占位配置
- 生成本轮任务文件
- 在界面中选择底盘类型、底盘标定参数、运控算法和传感器标定项目
- 启动或停止 ROS 现场服务
- 执行本轮标定流程
- 在“车间定位”窗口显示 3D 车间图、车辆实时位置、本轮计划轨迹和定位质量信息
- 解析并展示最终 report、阶段结果和 metadata
## 运行方法
```bash
source install/setup.bash
python3 src/apps/operator_ui/main.py
```
仿真联调时可以让界面直接加载仿真 profile：
```bash
AGV_OPERATOR_SITE_PROFILE=src/deployment/profiles/sim_workshop.yaml python3 src/apps/operator_ui/main.py
```
如果环境里还没有界面依赖：
```bash
pip install -r src/apps/operator_ui/requirements.txt
```
## 真实部署前需要先确认
1. 把 `src/deployment/profiles/site_template.yaml` 复制成唯一车间的部署配置文件，并在程序默认配置里固定使用。
2. 替换所有 `replace_with_*`、`measured_on_site`、`session_xxx` 占位项。
3. 为首次标定的车型新建并保存车辆画像，至少填写车辆长宽高、轴距、轮距、轮半径、底盘类型、传感器 ID 和运控默认限制；同车型新车可直接选择已有车辆画像。
4. 按现场测量并填写车间长、宽、高，单位为米。
5. 确认车间定位输出话题是 `/workshop/external_localization/vehicle/pose`。
6. 确认车上 Windows 程序的 IP 和端口。
7. 真实采集完成后，把采集数据索引文件转成算法数据参数文件，再在界面里填入。
## 界面操作顺序
1. 查看“现场检查”页，确认固定车间配置和关键项没有失败。
2. 在右上角查看车间尺寸是否已从固定部署配置加载。
3. 在“车辆画像”里选择已有画像；需要新车型时点击“新建画像”，在弹出的画像窗口里填写车辆尺寸、底盘几何、运控限制，并勾选车上安装的传感器后填写编号；手眼相机需要选择眼在手上或眼在手外。
4. 在“本轮要标定的内容”里选择：
- 底盘类型和要标定的底盘参数
- 运控轴向、控制算法和评估项目
- 传感器内参、外参或手眼任务；传感器 ID 来自车辆画像
5. 点击“生成本轮任务文件”，检查“本轮任务预览”里的任务列表。
6. 查看“车间定位”窗口，确认 3D 车间图中的本轮计划轨迹、车辆位置和质量信息在更新。
7. 点击“启动现场服务”，等待日志中各节点启动。
8. 点击“开始执行本轮标定”，在“报告”页查看最终结果。
9. 验证结束后点击“停止现场服务”。
## Isaac 中测试底盘选路采集
如果只是验证“底盘标定流程里能不能按选定路径让车动起来、采集底盘遥测和外部真值”，先启动 Isaac 仿真链路：
```bash
./run_isaac_real_sim_test.sh --no-smoke --keep-running
```
再启动 UI：
```bash
AGV_OPERATOR_SITE_PROFILE=src/deployment/profiles/sim_workshop.yaml python3 src/apps/operator_ui/main.py
```
在“底盘标定”页选择底盘类型和“参考路径”，然后点击“开始底盘路径采集测试”。该按钮会调用
`run_chassis_profile_capture.py`，自动传入当前选中的 `reference_path_id`，并把采集结果写到 `/tmp/agv_calib_chassis_sim/ui_*`。
## 在 UI 中录制参考路径
底盘标定、运控参数和传感器标定页的“参考路径”下拉框旁都有“录制”和“停止录制”按钮。点击“录制”后填写中文名称、路径 ID、录制来源、话题、路径类型和目标速度；录制来源可选外部真值定位或底盘遥测。录制时长填 `0` 表示一直录，点击“停止录制”后会收尾并把单条路径 YAML 写入对应模块的 `reference_paths/` 目录。
底盘路径会按当前底盘类型写入 `allowed_chassis_types`，因此录制完成后只会出现在对应底盘类型下。需要注意的是，“开始底盘路径采集测试”仍然走 `chassis_action_profile.yaml` 的动作原语执行链路；如果一条新录制底盘路径还没有绑定到动作 profile，UI 会阻止直接执行，只把它作为参考路径文件用于路径管理和任务预览。
## 注意事项
- “跳过网络质量检查（仅调试）”只适合本地联调，真实部署默认不要勾选。
- 车上 Windows 程序链路是真实部署的必需链路，界面固定启用，不再让操作人员选择。
- 车辆 ID 表示当前这辆车；车辆画像表示同一车型的可复用静态配置，两者不要混用。
- 底盘标定和运控参数标定都依赖车辆画像；画像会写入本轮任务文件，smoke 执行时会按当前车辆 ID 注册这份画像快照。
- 主界面只保留车辆画像选择；画像明细在独立画像窗口里新建、编辑和保存。
- 画像文件固定保存在 `src/deployment/profiles/vehicle_profiles/`，保存时按“画像名称.ymal”生成文件名。
- 车间长、宽、高是固定现场配置，只能从现场配置文件读取，不允许操作员在界面手动输入。
- 现场只有一个车间，所以界面不提供现场配置文件选择入口；部署配置由程序后台固定加载。
- 车间长、宽、高会写入本轮任务文件，并通过 `workshop_geometry.*` 进入总控报告 metadata。
- 车间定位输出话题、车间定位系统名称、当前标定工位从现场配置文件读取，不作为普通操作项显示。
- “车间定位”窗口只显示 3D 位置、本轮计划轨迹和定位数据，不允许修改固定 topic 或定位系统名称。
- 3D 车间图支持鼠标左键拖动旋转视角，左键双击恢复默认视角。
- 本轮计划轨迹用绿色点显示，来自当前勾选的底盘动作和运控轨迹参数，不要求操作员额外输入。
- 底盘标定、运控参数标定和传感器标定页都有“参考路径”下拉框，选项来自对应模块的 `reference_paths/*.yaml`；运控轨迹跟踪会把选中的路径展开为实际下发的轨迹点。
- 标定流程运行时，界面订阅 `/workshop_v2/events`，上一项任务完成后会自动切到下一项需要动车采集的轨迹。
- 界面会把明细选择写入本轮任务文件，并让标定流程读取这些任务。
- 界面通过现有 CLI 和 ROS launch 运行流程，没有绕过总控。
- 如果 report 显示失败，优先查看“阶段”表里的 `summary` 和日志中的 `[STAGE]` 行。
agv_calib_brain/src/apps/operator_ui/constants.py
+388
View File
@@ -0,0 +1,388 @@
"""操作台固定配置和任务选项。"""
from __future__ import annotations
import os
from pathlib import Path
REPO_ROOT = Path(__file__).resolve().parents[3]
_DEFAULT_SITE_PROFILE_RAW = Path(
os.environ.get("AGV_OPERATOR_SITE_PROFILE", "src/deployment/profiles/site_template.yaml")
).expanduser()
DEFAULT_SITE_PROFILE = (
_DEFAULT_SITE_PROFILE_RAW.resolve(strict=False)
if _DEFAULT_SITE_PROFILE_RAW.is_absolute()
else (REPO_ROOT / _DEFAULT_SITE_PROFILE_RAW).resolve(strict=False)
)
DEFAULT_VEHICLE_PROFILE_DIR = REPO_ROOT / "src/deployment/profiles/vehicle_profiles"
DEFAULT_VEHICLE_PROFILE = DEFAULT_VEHICLE_PROFILE_DIR / "ackermann_default.yaml"
DEFAULT_REFERENCE_PATH_DIRS = {
"chassis": REPO_ROOT / "src/site_deployment/workshop_chassis_calibration_real/reference_paths",
"control": REPO_ROOT / "src/site_deployment/workshop_control_calibration_real/reference_paths",
"sensor": REPO_ROOT / "src/site_deployment/workshop_sensor_calibration_real/reference_paths",
}
VEHICLE_PROFILE_SAVE_EXTENSION = ".ymal"
DEFAULT_SESSION_CONFIG = Path("/tmp/agv_calib_operator_ui/workshop_session_config.yaml")
PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx")
TASKS = [
("external", "检查车间定位"),
("chassis", "底盘标定"),
("control", "运控参数标定"),
("sensor_intrinsic", "传感器内参标定"),
("sensor_extrinsic", "传感器外参标定"),
("hand_eye", "手眼标定"),
]
CHASSIS_TYPES = [
("ackermann", "阿克曼"),
("differential", "差速轮"),
("single_steer_wheel", "单舵轮"),
("multi_steer_wheel", "多舵轮"),
]
STAGE_EXTERNAL = "EXTERNAL_REFERENCE_READY_CHECK_STAGE"
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"
STAGE_DISPLAY_NAMES = {
STAGE_EXTERNAL: "车间定位检查",
STAGE_CHASSIS: "底盘标定",
STAGE_CONTROL: "运控参数标定",
STAGE_SENSOR_INTRINSIC: "传感器内参标定",
STAGE_SENSOR_EXTRINSIC: "传感器外参标定",
STAGE_HAND_EYE: "手眼标定",
}
STAGE_ID_PREFIX_BY_STAGE_TYPE = {
STAGE_EXTERNAL: "stage_external_reference",
STAGE_CHASSIS: "stage_chassis_calibration",
STAGE_CONTROL: "stage_control_calibration",
STAGE_SENSOR_INTRINSIC: "stage_sensor_intrinsic_calibration",
STAGE_SENSOR_EXTRINSIC: "stage_sensor_extrinsic_calibration",
STAGE_HAND_EYE: "stage_hand_eye_calibration",
}
WORKSHOP_EVENT_STAGE_STARTED = 5
WORKSHOP_EVENT_STAGE_COMPLETED = 6
WORKSHOP_EVENT_STAGE_FAILED = 7
WORKSHOP_EVENT_REPORT_READY = 12
POLICY_DISPLAY_NAMES = {
"REQUIRED": "必做",
"OPTIONAL": "可选",
"SKIP_IF_UNSUPPORTED": "不支持则跳过",
}
LOCALIZATION_DISPLAY_ROWS = [
"状态",
"更新时间",
"X(m)",
"Y(m)",
"Z(m)",
"Yaw(deg)",
"质量分数",
"位置标准差(m)",
"航向标准差(deg)",
"跟踪丢失率",
"时间同步偏差(ms)",
"目标数量",
"定位系统",
"任务 ID",
]
CHASSIS_PARAMETER_OPTIONS = {
"ackermann": [
{
"code": "chassis.ackermann.rear_wheel_radius",
"label": "后轮有效半径",
"target": "rear_drive_wheels",
"params": {
"primitive_type": "straight_line",
"straight_line.target_distance_m": "0.5",
"straight_line.target_speed_ms": "0.1",
"straight_line.reverse": "false",
},
},
{
"code": "chassis.ackermann.steering_zero",
"label": "前轮舵角零偏",
"target": "front_steering",
"params": {
"primitive_type": "steering_sweep",
"steering_sweep.target_angle_deg": "0.0",
"steering_sweep.sweep_amplitude_deg": "12.0",
"steering_sweep.sweep_frequency_hz": "0.2",
"steering_sweep.duration_sec": "8.0",
},
},
{
"code": "chassis.ackermann.steering_ratio",
"label": "转向传动比/等效轴距",
"target": "front_steering",
"params": {
"primitive_type": "arc",
"arc.target_speed_ms": "0.1",
"arc.radius_m": "1.2",
"arc.sweep_angle_deg": "90.0",
"arc.clockwise": "false",
},
},
],
"differential": [
{
"code": "chassis.differential.wheel_radius",
"label": "左右轮有效半径",
"target": "drive_wheels",
"params": {
"primitive_type": "straight_line",
"straight_line.target_distance_m": "0.5",
"straight_line.target_speed_ms": "0.1",
"straight_line.reverse": "false",
},
},
{
"code": "chassis.differential.track_width",
"label": "驱动轮轮距",
"target": "drive_wheels",
"params": {
"primitive_type": "in_place_rotation",
"in_place_rotation.target_yaw_deg": "360.0",
"in_place_rotation.target_angular_vel_deg_s": "20.0",
},
},
{
"code": "chassis.differential.encoder_scale",
"label": "左右编码器比例",
"target": "wheel_encoders",
"params": {
"primitive_type": "straight_line",
"straight_line.target_distance_m": "0.5",
"straight_line.target_speed_ms": "0.08",
"straight_line.reverse": "false",
},
},
],
"single_steer_wheel": [
{
"code": "chassis.single_steer.drive_wheel_radius",
"label": "驱动轮有效半径",
"target": "steer_drive_module",
"params": {
"primitive_type": "straight_line",
"straight_line.target_distance_m": "0.5",
"straight_line.target_speed_ms": "0.1",
"straight_line.reverse": "false",
},
},
{
"code": "chassis.single_steer.steer_zero",
"label": "舵轮零位",
"target": "steer_drive_module",
"params": {
"primitive_type": "steering_sweep",
"steering_sweep.target_angle_deg": "0.0",
"steering_sweep.sweep_amplitude_deg": "15.0",
"steering_sweep.sweep_frequency_hz": "0.2",
"steering_sweep.duration_sec": "8.0",
},
},
{
"code": "chassis.single_steer.steering_ratio",
"label": "转向比例",
"target": "steer_drive_module",
"params": {
"primitive_type": "arc",
"arc.target_speed_ms": "0.1",
"arc.radius_m": "1.0",
"arc.sweep_angle_deg": "90.0",
"arc.clockwise": "false",
},
},
],
"multi_steer_wheel": [
{
"code": "chassis.multi_steer.module_wheel_radius",
"label": "各模块轮半径",
"target": "steering_modules",
"params": {
"primitive_type": "straight_line",
"straight_line.target_distance_m": "0.5",
"straight_line.target_speed_ms": "0.1",
"straight_line.reverse": "false",
},
},
{
"code": "chassis.multi_steer.module_zero",
"label": "各模块舵角零位",
"target": "steering_modules",
"params": {
"primitive_type": "module_alignment",
"module_alignment.module_ids": "module_1,module_2,module_3,module_4",
"module_alignment.target_zero_deg": "0.0",
"module_alignment.tolerance_deg": "0.5",
},
},
{
"code": "chassis.multi_steer.coordinated_steering",
"label": "多模块协同转向一致性",
"target": "steering_modules",
"params": {
"primitive_type": "coordinated_steering",
"coordinated_steering.module_ids": "module_1,module_2,module_3,module_4",
"coordinated_steering.target_angle_deg": "30.0",
"coordinated_steering.hold_time_sec": "3.0",
},
},
],
}
CONTROL_PARAMETER_OPTIONS = [
{
"code": "control.lateral.mpc",
"label": "横向 MPC",
"target": "lateral_controller",
"axis": "lateral",
"algorithm": "mpc",
"params": {
"control.task_type": "trajectory_tracking",
"control.stop_at_end": "true",
"control.timeout_sec": "60.0",
"trajectory_tracking.trajectory_id": "ui_lateral_mpc_eval",
"trajectory_tracking.segment_index": "0",
"trajectory_tracking.total_segments": "1",
"trajectory_tracking.is_final_segment": "true",
"trajectory_tracking.max_external_pose_age_ms": "200.0",
"trajectory_tracking.min_external_pose_quality_score": "0.5",
"traj_pt_0_x_m": "0.0",
"traj_pt_0_y_m": "0.0",
"traj_pt_0_yaw_rad": "0.0",
"traj_pt_0_speed_ms": "0.1",
"traj_pt_1_x_m": "1.0",
"traj_pt_1_y_m": "0.0",
"traj_pt_1_yaw_rad": "0.0",
"traj_pt_1_speed_ms": "0.1",
},
},
{
"code": "control.lateral.lqr",
"label": "横向 LQR",
"target": "lateral_controller",
"axis": "lateral",
"algorithm": "lqr",
"params": {
"control.task_type": "trajectory_tracking",
"control.stop_at_end": "true",
"control.timeout_sec": "60.0",
"trajectory_tracking.trajectory_id": "ui_lateral_lqr_eval",
"traj_pt_0_x_m": "0.0",
"traj_pt_0_y_m": "0.0",
"traj_pt_0_yaw_rad": "0.0",
"traj_pt_0_speed_ms": "0.1",
"traj_pt_1_x_m": "1.0",
"traj_pt_1_y_m": "0.0",
"traj_pt_1_yaw_rad": "0.0",
"traj_pt_1_speed_ms": "0.1",
},
},
{
"code": "control.lateral.pure_pursuit",
"label": "横向 Pure Pursuit",
"target": "lateral_controller",
"axis": "lateral",
"algorithm": "pure_pursuit",
"params": {
"control.task_type": "trajectory_tracking",
"control.stop_at_end": "true",
"control.timeout_sec": "60.0",
"trajectory_tracking.trajectory_id": "ui_lateral_pp_eval",
"traj_pt_0_x_m": "0.0",
"traj_pt_0_y_m": "0.0",
"traj_pt_0_yaw_rad": "0.0",
"traj_pt_0_speed_ms": "0.1",
"traj_pt_1_x_m": "1.0",
"traj_pt_1_y_m": "0.0",
"traj_pt_1_yaw_rad": "0.0",
"traj_pt_1_speed_ms": "0.1",
},
},
{
"code": "control.longitudinal.pid",
"label": "纵向 PID",
"target": "longitudinal_controller",
"axis": "longitudinal",
"algorithm": "pid",
"params": {
"control.task_type": "velocity_step",
"velocity_step.target_velocity_ms": "0.1",
"velocity_step.hold_time_sec": "0.5",
"velocity_step.settle_before_step_sec": "0.1",
"control.timeout_sec": "10.0",
},
},
{
"code": "control.longitudinal.mpc",
"label": "纵向 MPC",
"target": "longitudinal_controller",
"axis": "longitudinal",
"algorithm": "mpc",
"params": {
"control.task_type": "accel_decel",
"accel_decel.start_velocity_ms": "0.0",
"accel_decel.target_velocity_ms": "0.15",
"accel_decel.target_accel_ms2": "0.1",
"accel_decel.hold_time_sec": "0.5",
"control.timeout_sec": "10.0",
},
},
{
"code": "control.stop_accuracy",
"label": "停车精度",
"target": "stop_controller",
"axis": "combined",
"algorithm": "stop_accuracy",
"params": {
"control.task_type": "stop_accuracy",
"stop_accuracy.target_stop_x_m": "0.0",
"stop_accuracy.target_stop_y_m": "0.0",
"stop_accuracy.target_stop_yaw_rad": "0.0",
"control.timeout_sec": "10.0",
},
},
]
SENSOR_TASK_OPTIONS = [
("front_camera", "前视相机", "front_camera_intrinsic", "前视相机内参", STAGE_SENSOR_INTRINSIC),
("front_camera", "前视相机", "front_camera_extrinsic", "前视相机到车体外参", STAGE_SENSOR_EXTRINSIC),
("down_camera", "下视相机", "downward_camera_intrinsic", "下视相机内参", STAGE_SENSOR_INTRINSIC),
("down_camera", "下视相机", "downward_camera_extrinsic", "下视相机到车体外参", STAGE_SENSOR_EXTRINSIC),
("lidar_2d", "2D 激光雷达", "lidar_2d_extrinsic", "2D 激光雷达到车体外参", STAGE_SENSOR_EXTRINSIC),
("lidar_3d", "3D 激光雷达", "lidar_3d_extrinsic", "3D 激光雷达到车体外参", STAGE_SENSOR_EXTRINSIC),
("imu", "IMU", "imu_intrinsic", "IMU 内参", STAGE_SENSOR_INTRINSIC),
("imu", "IMU", "imu_extrinsic", "IMU 到车体外参", STAGE_SENSOR_EXTRINSIC),
("arm_camera", "手眼相机", "eye_in_hand", "眼在手上手眼标定", STAGE_HAND_EYE),
("arm_camera", "手眼相机", "eye_to_hand", "眼在手外手眼标定", STAGE_HAND_EYE),
]
TASK_DISPLAY_NAMES = {"external": "车间定位可用性检查"}
for options in CHASSIS_PARAMETER_OPTIONS.values():
for option in options:
TASK_DISPLAY_NAMES[str(option["code"])] = str(option["label"])
for option in CONTROL_PARAMETER_OPTIONS:
TASK_DISPLAY_NAMES[str(option["code"])] = str(option["label"])
for sensor_key, sensor_label, subtype, label, _stage_type in SENSOR_TASK_OPTIONS:
TASK_DISPLAY_NAMES[f"sensor.{sensor_key}.{subtype}"] = label
SENSOR_STAGE_DISPLAY_NAMES: dict[str, str] = {}
for sensor_key, sensor_label, subtype, _label, stage_type in SENSOR_TASK_OPTIONS:
task_code = f"sensor.{sensor_key}.{subtype}"
if stage_type == STAGE_SENSOR_INTRINSIC:
SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}内参标定"
elif stage_type == STAGE_SENSOR_EXTRINSIC:
SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}外参标定"
elif stage_type == STAGE_HAND_EYE:
SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}手眼标定"
agv_calib_brain/src/apps/operator_ui/localization_view.py
+272
View File
@@ -0,0 +1,272 @@
"""车间 3D 定位显示控件。"""
from __future__ import annotations
import math
from PySide6.QtCore import QPointF, Qt
from PySide6.QtGui import QBrush, QColor, QFont, QPainter, QPen, QPolygonF
from PySide6.QtWidgets import QWidget
class WorkshopLocalization3DView(QWidget):
def __init__(self) -> None:
super().__init__()
self.setMinimumHeight(360)
self.room_length_m = 0.0
self.room_width_m = 0.0
self.room_height_m = 0.0
self.pose: dict[str, float] | None = None
self.pose_valid = False
self.quality_score: float | None = None
self.status_text = "等待定位数据"
self.trajectory_segments: list[list[tuple[float, float, float]]] = []
self.trajectory_label = "本轮计划轨迹"
self.view_yaw_rad = math.radians(45.0)
self.view_pitch_rad = math.radians(35.0)
self.view_drag_last_pos: QPointF | None = None
self.setCursor(Qt.OpenHandCursor)
def set_workshop_geometry(self, geometry: dict[str, str]) -> None:
try:
self.room_length_m = float(geometry.get("length_m", "0"))
self.room_width_m = float(geometry.get("width_m", "0"))
self.room_height_m = float(geometry.get("height_m", "0"))
except ValueError:
self.room_length_m = 0.0
self.room_width_m = 0.0
self.room_height_m = 0.0
self.update()
def set_status(self, text: str) -> None:
self.status_text = text
self.update()
def set_pose(self, x_m: float, y_m: float, z_m: float, yaw_rad: float, valid: bool, quality_score: float | None) -> None:
self.pose = {"x": x_m, "y": y_m, "z": z_m, "yaw": yaw_rad}
self.pose_valid = valid
self.quality_score = quality_score
self.update()
def set_trajectory(self, segments: list[list[tuple[float, float, float]]], label: str = "本轮计划轨迹") -> None:
self.trajectory_segments = segments
self.trajectory_label = label
self.update()
def _geometry_ready(self) -> bool:
return self.room_length_m > 0.0 and self.room_width_m > 0.0 and self.room_height_m > 0.0
def paintEvent(self, event) -> None:
painter = QPainter(self)
painter.setRenderHint(QPainter.Antialiasing, True)
painter.fillRect(self.rect(), QColor("#f8fafc"))
if not self._geometry_ready():
painter.setPen(QColor("#991b1b"))
painter.setFont(QFont("", 12, QFont.Bold))
painter.drawText(self.rect(), Qt.AlignCenter, "车间尺寸未配置，无法显示 3D 定位图")
return
length = self.room_length_m
width = self.room_width_m
height = self.room_height_m
margin = 32.0
raw_points = [
self._raw_project(x, y, z)
for x in (-length / 2.0, length / 2.0)
for y in (-width / 2.0, width / 2.0)
for z in (0.0, height)
]
min_x = min(point.x() for point in raw_points)
max_x = max(point.x() for point in raw_points)
min_y = min(point.y() for point in raw_points)
max_y = max(point.y() for point in raw_points)
span_x = max(max_x - min_x, 1e-6)
span_y = max(max_y - min_y, 1e-6)
scale = min((self.width() - margin * 2.0) / span_x, (self.height() - margin * 2.0) / span_y)
raw_center = QPointF((min_x + max_x) / 2.0, (min_y + max_y) / 2.0)
screen_center = QPointF(self.width() / 2.0, self.height() / 2.0 + 18.0)
def project(x: float, y: float, z: float) -> QPointF:
raw = self._raw_project(x, y, z)
return QPointF(
(raw.x() - raw_center.x()) * scale + screen_center.x(),
(raw.y() - raw_center.y()) * scale + screen_center.y(),
)
floor = [
project(-length / 2.0, -width / 2.0, 0.0),
project(length / 2.0, -width / 2.0, 0.0),
project(length / 2.0, width / 2.0, 0.0),
project(-length / 2.0, width / 2.0, 0.0),
]
top = [
project(-length / 2.0, -width / 2.0, height),
project(length / 2.0, -width / 2.0, height),
project(length / 2.0, width / 2.0, height),
project(-length / 2.0, width / 2.0, height),
]
painter.setPen(QPen(QColor("#94a3b8"), 1))
painter.setBrush(QBrush(QColor("#e0f2fe")))
painter.drawPolygon(QPolygonF(floor))
painter.setBrush(Qt.NoBrush)
grid_pen = QPen(QColor("#cbd5e1"), 1)
grid_pen.setStyle(Qt.DotLine)
painter.setPen(grid_pen)
grid_count = 8
for i in range(1, grid_count):
x = -length / 2.0 + length * i / grid_count
painter.drawLine(project(x, -width / 2.0, 0.0), project(x, width / 2.0, 0.0))
y = -width / 2.0 + width * i / grid_count
painter.drawLine(project(-length / 2.0, y, 0.0), project(length / 2.0, y, 0.0))
painter.setPen(QPen(QColor("#475569"), 2))
painter.drawPolygon(QPolygonF(floor))
top_pen = QPen(QColor("#64748b"), 1)
top_pen.setStyle(Qt.DashLine)
painter.setPen(top_pen)
painter.drawPolygon(QPolygonF(top))
painter.setPen(QPen(QColor("#64748b"), 1))
for bottom, upper in zip(floor, top):
painter.drawLine(bottom, upper)
self._draw_axes(painter, project, min(length, width, height) * 0.22)
self._draw_trajectory(painter, project)
if self.pose is not None:
self._draw_vehicle(painter, project)
painter.setPen(QColor("#0f172a"))
painter.setFont(QFont("", 10, QFont.Bold))
painter.drawText(16, 24, f"车间 {length:g} x {width:g} x {height:g} m")
painter.setFont(QFont("", 9))
if self.pose is None:
painter.drawText(16, 44, self.status_text)
else:
quality = "-" if self.quality_score is None else f"{self.quality_score:.3f}"
painter.drawText(
16,
44,
f"x={self.pose['x']:.3f} m, y={self.pose['y']:.3f} m, yaw={math.degrees(self.pose['yaw']):.2f} deg, 质量={quality}",
)
def _raw_project(self, x: float, y: float, z: float) -> QPointF:
yaw_cos = math.cos(self.view_yaw_rad)
yaw_sin = math.sin(self.view_yaw_rad)
pitch_sin = math.sin(self.view_pitch_rad)
pitch_cos = math.cos(self.view_pitch_rad)
rotated_x = x * yaw_cos - y * yaw_sin
rotated_y = x * yaw_sin + y * yaw_cos
return QPointF(rotated_x, rotated_y * pitch_sin - z * pitch_cos)
def mousePressEvent(self, event) -> None:
if event.button() == Qt.LeftButton:
self.view_drag_last_pos = event.position()
self.setCursor(Qt.ClosedHandCursor)
def mouseMoveEvent(self, event) -> None:
if self.view_drag_last_pos is None or not (event.buttons() & Qt.LeftButton):
return
current_pos = event.position()
delta = current_pos - self.view_drag_last_pos
self.view_drag_last_pos = current_pos
self.view_yaw_rad += delta.x() * 0.01
min_pitch = math.radians(8.0)
max_pitch = math.radians(78.0)
self.view_pitch_rad = min(max_pitch, max(min_pitch, self.view_pitch_rad - delta.y() * 0.008))
self.update()
def mouseReleaseEvent(self, event) -> None:
if event.button() == Qt.LeftButton:
self.view_drag_last_pos = None
self.setCursor(Qt.OpenHandCursor)
def mouseDoubleClickEvent(self, event) -> None:
if event.button() == Qt.LeftButton:
self.view_yaw_rad = math.radians(45.0)
self.view_pitch_rad = math.radians(35.0)
self.update()
def _draw_axes(self, painter: QPainter, project, axis_len: float) -> None:
origin = project(0.0, 0.0, 0.0)
axes = [
(project(axis_len, 0.0, 0.0), QColor("#dc2626"), "X"),
(project(0.0, axis_len, 0.0), QColor("#16a34a"), "Y"),
(project(0.0, 0.0, axis_len), QColor("#2563eb"), "Z"),
]
for end, color, label in axes:
painter.setPen(QPen(color, 2))
painter.drawLine(origin, end)
painter.drawText(end + QPointF(4.0, -4.0), label)
def _draw_trajectory(self, painter: QPainter, project) -> None:
if not self.trajectory_segments:
return
path_pen = QPen(QColor("#22c55e"), 3)
marker_pen = QPen(QColor("#166534"), 1)
marker_brush = QBrush(QColor("#22c55e"))
label_drawn = False
for segment in self.trajectory_segments:
if len(segment) < 2:
continue
points = [project(x, y, z) for x, y, z in segment]
painter.setPen(path_pen)
for index in range(len(points) - 1):
painter.drawLine(points[index], points[index + 1])
painter.setPen(marker_pen)
painter.setBrush(marker_brush)
for point in points:
painter.drawEllipse(point, 3.5, 3.5)
painter.setBrush(Qt.NoBrush)
if not label_drawn:
painter.setPen(QColor("#166534"))
painter.setFont(QFont("", 9, QFont.Bold))
painter.drawText(points[0] + QPointF(6.0, -6.0), self.trajectory_label)
label_drawn = True
def _draw_vehicle(self, painter: QPainter, project) -> None:
if self.pose is None:
return
length = min(max(self.room_length_m * 0.08, 0.55), 1.20)
width = length * 0.58
height = min(max(self.room_height_m * 0.08, 0.25), 0.55)
x = self.pose["x"]
y = self.pose["y"]
yaw = self.pose["yaw"]
cos_yaw = math.cos(yaw)
sin_yaw = math.sin(yaw)
def corner(dx: float, dy: float, z: float) -> QPointF:
return project(
x + dx * cos_yaw - dy * sin_yaw,
y + dx * sin_yaw + dy * cos_yaw,
z,
)
bottom = [
corner(length / 2.0, width / 2.0, 0.0),
corner(length / 2.0, -width / 2.0, 0.0),
corner(-length / 2.0, -width / 2.0, 0.0),
corner(-length / 2.0, width / 2.0, 0.0),
]
upper = [
corner(length / 2.0, width / 2.0, height),
corner(length / 2.0, -width / 2.0, height),
corner(-length / 2.0, -width / 2.0, height),
corner(-length / 2.0, width / 2.0, height),
]
color = QColor("#22c55e") if self.pose_valid else QColor("#ef4444")
color.setAlpha(190)
painter.setPen(QPen(QColor("#14532d") if self.pose_valid else QColor("#7f1d1d"), 2))
painter.setBrush(QBrush(color))
painter.drawPolygon(QPolygonF(upper))
painter.setBrush(Qt.NoBrush)
painter.drawPolygon(QPolygonF(bottom))
painter.drawPolygon(QPolygonF(upper))
for p0, p1 in zip(bottom, upper):
painter.drawLine(p0, p1)
center = project(x, y, height + 0.03)
front = project(x + math.cos(yaw) * length * 0.75, y + math.sin(yaw) * length * 0.75, height + 0.03)
painter.setPen(QPen(QColor("#0f172a"), 3))
painter.drawLine(center, front)
agv_calib_brain/src/apps/operator_ui/main.py
+566
View File
@@ -0,0 +1,566 @@
#!/usr/bin/env python3
"""真实部署用标定车间操作台。"""
from __future__ import annotations
import signal
import sys
from typing import Any
try:
import rclpy
except ImportError:
rclpy = None
from PySide6.QtCore import QProcess, QTimer, Qt
from PySide6.QtGui import QFont
from PySide6.QtWidgets import (
QApplication,
QCheckBox,
QComboBox,
QFrame,
QGridLayout,
QGroupBox,
QHBoxLayout,
QHeaderView,
QLabel,
QLineEdit,
QMainWindow,
QPlainTextEdit,
QPushButton,
QSplitter,
QTableWidget,
QTabWidget,
QVBoxLayout,
QWidget,
)
try:
from .constants import *
from .localization_view import WorkshopLocalization3DView
from .process_report import ProcessReportMixin
from .profile_handlers import ProfileHandlersMixin
from .profile_io import *
from .runtime_monitor import RuntimeMonitorMixin
from .session_builder import SessionBuilderMixin
from .ui_helpers import *
except ImportError:
from constants import *
from localization_view import WorkshopLocalization3DView
from process_report import ProcessReportMixin
from profile_handlers import ProfileHandlersMixin
from profile_io import *
from runtime_monitor import RuntimeMonitorMixin
from session_builder import SessionBuilderMixin
from ui_helpers import *
class OperatorMainWindow(
RuntimeMonitorMixin,
ProfileHandlersMixin,
SessionBuilderMixin,
ProcessReportMixin,
QMainWindow,
):
def __init__(self) -> None:
super().__init__()
self.setWindowTitle("自动化标定车间现场操作台")
self.setWindowFlags(
self.windowFlags()
| Qt.Window
| Qt.WindowMinimizeButtonHint
| Qt.WindowMaximizeButtonHint
| Qt.WindowCloseButtonHint
)
self.resize(1520, 920)
self.site_profile: dict[str, Any] = {}
self.vehicle_profile_data: dict[str, Any] = {}
self.generated_session_config: dict[str, Any] = {}
self.launch_process = QProcess(self)
self.smoke_process = QProcess(self)
self.chassis_capture_process = QProcess(self)
self.reference_path_record_process = QProcess(self)
self.reference_path_record_module = ""
self.report_metadata: dict[str, str] = {}
self.report_stages: list[dict[str, str]] = []
self.last_unresolved_signature = ""
self.localization_node = None
self.localization_subscription = None
self.workshop_event_subscription = None
self.localization_rclpy_initialized = False
self.localization_last_sample_wall_time = 0.0
self.localization_spin_timer = QTimer(self)
self.localization_spin_timer.timeout.connect(self.poll_localization_messages)
self.trajectory_task_records: list[dict[str, Any]] = []
self.trajectory_stage_order: dict[str, int] = {}
self.trajectory_runtime_active = False
self.trajectory_last_completed_stage_id = ""
self._setup_process(self.launch_process, "现场服务")
self._setup_process(self.smoke_process, "标定流程")
self._setup_process(self.chassis_capture_process, "底盘路径采集")
self._setup_process(self.reference_path_record_process, "参考路径录制")
self._build_ui()
self._connect_live_refresh()
self.load_profile()
def _setup_process(self, process: QProcess, name: str) -> None:
process.setWorkingDirectory(str(REPO_ROOT))
process.readyReadStandardOutput.connect(lambda p=process, n=name: self._read_process_output(p, n))
process.readyReadStandardError.connect(lambda p=process, n=name: self._read_process_output(p, n))
process.finished.connect(lambda code, status, n=name: self._process_finished(n, code, status))
def _build_ui(self) -> None:
self.setStyleSheet(
"""
QMainWindow { background: #f6f7f9; color: #111827; }
QGroupBox { border: 1px solid #d1d5db; border-radius: 6px; margin-top: 10px; padding-top: 12px; font-weight: 600; }
QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 4px; color: #1f2937; }
QLineEdit { border: 1px solid #cbd5e1; border-radius: 4px; padding: 6px; background: #ffffff; }
QPushButton { border: 1px solid #cbd5e1; border-radius: 4px; padding: 8px 10px; background: #ffffff; }
QPushButton:hover { background: #f1f5f9; }
QPushButton:disabled { color: #9ca3af; background: #f3f4f6; }
QPlainTextEdit { border: 1px solid #cbd5e1; border-radius: 4px; background: #0f172a; color: #e5e7eb; }
QTableWidget { border: 1px solid #cbd5e1; background: #ffffff; gridline-color: #e5e7eb; }
QHeaderView::section { background: #f3f4f6; padding: 5px; border: 0; border-right: 1px solid #d1d5db; font-weight: 600; }
"""
)
central = QWidget()
self.setCentralWidget(central)
root = QVBoxLayout(central)
root.setContentsMargins(12, 12, 12, 12)
root.setSpacing(10)
header = QFrame()
header.setStyleSheet("QFrame { background: #ffffff; border: 1px solid #d1d5db; border-radius: 6px; }")
header_layout = QHBoxLayout(header)
title = QLabel("自动化标定车间现场操作台")
title.setFont(QFont("", 18, QFont.Bold))
self.workshop_dimension_state = QLabel("车间尺寸：未加载")
self.workshop_dimension_state.setMinimumWidth(220)
self.workshop_dimension_state.setAlignment(Qt.AlignRight | Qt.AlignVCenter)
self.localization_state = QLabel("车间定位：未加载")
self.localization_state.setMinimumWidth(260)
self.localization_state.setAlignment(Qt.AlignRight | Qt.AlignVCenter)
self.stack_state = QLabel("现场服务：未启动")
self.smoke_state = QLabel("标定流程：空闲")
header_layout.addWidget(title)
header_layout.addStretch(1)
header_layout.addWidget(self.workshop_dimension_state)
header_layout.addWidget(self.localization_state)
header_layout.addWidget(self.stack_state)
header_layout.addWidget(self.smoke_state)
root.addWidget(header)
splitter = QSplitter(Qt.Horizontal)
root.addWidget(splitter, 1)
splitter.addWidget(self._build_left_panel())
splitter.addWidget(self._build_right_panel())
splitter.setSizes([470, 1050])
def _connect_live_refresh(self) -> None:
watched_edits = [
self.vehicle_id_edit,
self.vehicle_profile_path_edit,
self.vehicle_profile_name_edit,
self.vehicle_model_name_edit,
self.vehicle_manufacturer_edit,
self.vehicle_length_edit,
self.vehicle_width_edit,
self.vehicle_height_edit,
self.vehicle_ground_clearance_edit,
self.vehicle_wheel_base_edit,
self.vehicle_track_width_edit,
self.vehicle_wheel_radius_edit,
self.vehicle_max_steering_angle_edit,
self.vehicle_min_turning_radius_edit,
self.vehicle_max_speed_edit,
self.vehicle_max_accel_edit,
self.vehicle_control_frequency_edit,
self.vehicle_host_edit,
self.vehicle_port_edit,
self.external_topic_edit,
self.reference_source_edit,
self.workcell_zone_edit,
self.reference_target_edit,
self.data_input_edit,
self.dataset_index_edit,
self.sensor_storage_edit,
self.sensor_registry_edit,
self.session_config_output_edit,
]
for edit in watched_edits:
edit.textChanged.connect(self.refresh_command_preview)
self.skip_wifi6_check.stateChanged.connect(self.refresh_command_preview)
def _build_left_panel(self) -> QWidget:
panel = QWidget()
layout = QVBoxLayout(panel)
layout.setContentsMargins(0, 0, 8, 0)
self.profile_box = QGroupBox("固定车间配置")
profile_grid = QGridLayout(self.profile_box)
self.profile_path_edit = QLineEdit(str(DEFAULT_SITE_PROFILE))
browse_profile = QPushButton("选择")
browse_profile.clicked.connect(self.browse_profile)
reload_profile = QPushButton("加载")
reload_profile.clicked.connect(self.load_profile)
profile_grid.addWidget(QLabel("文件"), 0, 0)
profile_grid.addWidget(self.profile_path_edit, 0, 1)
profile_grid.addWidget(browse_profile, 0, 2)
profile_grid.addWidget(reload_profile, 0, 3)
self.profile_state_label = QLabel("未加载")
profile_grid.addWidget(QLabel("状态"), 1, 0)
profile_grid.addWidget(self.profile_state_label, 1, 1, 1, 3)
self.profile_box.hide()
layout.addWidget(self.profile_box)
vehicle_profile_box = QGroupBox("车辆画像")
vehicle_profile_grid = QGridLayout(vehicle_profile_box)
self.vehicle_profile_combo = QComboBox()
self.vehicle_profile_combo.activated.connect(self.load_selected_vehicle_profile)
self.vehicle_profile_path_edit = QLineEdit(str(DEFAULT_VEHICLE_PROFILE))
self.vehicle_profile_name_edit = QLineEdit()
self.vehicle_model_name_edit = QLineEdit()
self.vehicle_manufacturer_edit = QLineEdit()
self.vehicle_length_edit = QLineEdit()
self.vehicle_width_edit = QLineEdit()
self.vehicle_height_edit = QLineEdit()
self.vehicle_ground_clearance_edit = QLineEdit()
self.vehicle_wheel_base_edit = QLineEdit()
self.vehicle_track_width_edit = QLineEdit()
self.vehicle_wheel_radius_edit = QLineEdit()
self.vehicle_max_steering_angle_edit = QLineEdit()
self.vehicle_min_turning_radius_edit = QLineEdit()
self.vehicle_max_speed_edit = QLineEdit()
self.vehicle_max_accel_edit = QLineEdit()
self.vehicle_control_frequency_edit = QLineEdit()
new_vehicle_profile = QPushButton("新建画像")
new_vehicle_profile.clicked.connect(self.open_new_vehicle_profile_dialog)
edit_vehicle_profile = QPushButton("编辑画像")
edit_vehicle_profile.clicked.connect(self.open_edit_vehicle_profile_dialog)
vehicle_profile_grid.addWidget(QLabel("选择画像"), 0, 0)
vehicle_profile_grid.addWidget(self.vehicle_profile_combo, 0, 1)
vehicle_profile_grid.addWidget(new_vehicle_profile, 0, 2)
vehicle_profile_grid.addWidget(edit_vehicle_profile, 0, 3)
layout.addWidget(vehicle_profile_box)
self.refresh_vehicle_profile_combo()
site_box = QGroupBox("车辆连接")
site_grid = QGridLayout(site_box)
self.vehicle_id_edit = QLineEdit()
self.vehicle_host_edit = QLineEdit()
self.vehicle_port_edit = QLineEdit("9000")
self.skip_wifi6_check = QCheckBox("跳过网络质量检查（仅调试）")
self.external_topic_edit = QLineEdit()
self.reference_source_edit = QLineEdit()
self.workcell_zone_edit = QLineEdit()
self.reference_target_edit = QLineEdit("site_reference_target")
rows = [
("车辆 ID", self.vehicle_id_edit),
("车上 Windows 程序 IP", self.vehicle_host_edit),
("车上 Windows 程序端口", self.vehicle_port_edit),
]
for row, (label, widget) in enumerate(rows):
site_grid.addWidget(QLabel(label), row, 0)
site_grid.addWidget(widget, row, 1, 1, 2)
site_grid.addWidget(self.skip_wifi6_check, len(rows), 0, 1, 3)
layout.addWidget(site_box)
data_box = QGroupBox("采集数据和配置")
data_grid = QGridLayout(data_box)
self.data_input_edit = QLineEdit()
self.dataset_index_edit = QLineEdit()
self.sensor_storage_edit = QLineEdit("/tmp/agv_sensor_calibration")
self.sensor_registry_edit = QLineEdit("demo_front_camera")
self.session_config_output_edit = QLineEdit(str(DEFAULT_SESSION_CONFIG))
self._add_path_row(data_grid, 0, "算法数据参数文件", self.data_input_edit, self.browse_data_input)
self._add_path_row(data_grid, 1, "采集数据索引文件", self.dataset_index_edit, self.browse_dataset_index)
self._add_path_row(data_grid, 2, "本轮任务文件", self.session_config_output_edit, self.browse_session_config_output)
data_grid.addWidget(QLabel("传感器数据保存目录"), 3, 0)
data_grid.addWidget(self.sensor_storage_edit, 3, 1, 1, 2)
data_grid.addWidget(QLabel("车辆传感器 ID 列表"), 4, 0)
data_grid.addWidget(self.sensor_registry_edit, 4, 1, 1, 2)
layout.addWidget(data_box)
task_box = QGroupBox("本轮要标定的内容")
task_layout = QVBoxLayout(task_box)
self.external_check = QCheckBox("先检查车间定位是否可用")
self.external_check.setChecked(True)
self.external_check.stateChanged.connect(self.refresh_command_preview)
task_layout.addWidget(self.external_check)
task_tabs = QTabWidget()
task_tabs.addTab(self._build_chassis_task_tab(), "底盘标定")
task_tabs.addTab(self._build_control_task_tab(), "运控参数")
task_tabs.addTab(self._build_sensor_task_tab(), "传感器标定")
task_layout.addWidget(task_tabs)
layout.addWidget(task_box)
action_box = QGroupBox("操作")
action_layout = QGridLayout(action_box)
self.build_config_btn = QPushButton("生成本轮任务文件")
self.launch_btn = QPushButton("启动现场服务")
self.stop_launch_btn = QPushButton("停止现场服务")
self.smoke_btn = QPushButton("开始执行本轮标定")
self.stop_smoke_btn = QPushButton("停止本轮标定")
self.chassis_capture_btn = QPushButton("开始底盘路径采集测试")
self.stop_chassis_capture_btn = QPushButton("停止底盘路径采集")
self.build_config_btn.clicked.connect(self.build_session_config)
self.launch_btn.clicked.connect(self.start_launch_stack)
self.stop_launch_btn.clicked.connect(lambda: self.stop_process(self.launch_process, "现场服务"))
self.smoke_btn.clicked.connect(self.start_smoke_test)
self.stop_smoke_btn.clicked.connect(lambda: self.stop_process(self.smoke_process, "标定流程"))
self.chassis_capture_btn.clicked.connect(self.start_chassis_path_capture)
self.stop_chassis_capture_btn.clicked.connect(
lambda: self.stop_process(self.chassis_capture_process, "底盘路径采集")
)
action_layout.addWidget(self.build_config_btn, 0, 0)
action_layout.addWidget(self.launch_btn, 0, 1)
action_layout.addWidget(self.stop_launch_btn, 1, 1)
action_layout.addWidget(self.smoke_btn, 1, 0)
action_layout.addWidget(self.stop_smoke_btn, 2, 0, 1, 2)
action_layout.addWidget(self.chassis_capture_btn, 3, 0)
action_layout.addWidget(self.stop_chassis_capture_btn, 3, 1)
layout.addWidget(action_box)
layout.addStretch(1)
return panel
def _build_chassis_task_tab(self) -> QWidget:
tab = QWidget()
layout = QVBoxLayout(tab)
type_row = QHBoxLayout()
type_row.addWidget(QLabel("底盘类型"))
self.chassis_type_combo = QComboBox()
for value, label in CHASSIS_TYPES:
self.chassis_type_combo.addItem(label, value)
self.chassis_type_combo.currentIndexChanged.connect(self.rebuild_chassis_parameter_checks)
self.chassis_type_combo.currentIndexChanged.connect(self.refresh_reference_path_combos)
type_row.addWidget(self.chassis_type_combo)
type_row.addStretch(1)
layout.addLayout(type_row)
path_row = QHBoxLayout()
path_row.addWidget(QLabel("参考路径"))
self.chassis_reference_path_combo = QComboBox()
self.chassis_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview)
path_row.addWidget(self.chassis_reference_path_combo, 1)
self.record_chassis_path_btn = QPushButton("录制")
self.stop_chassis_path_record_btn = QPushButton("停止录制")
self.record_chassis_path_btn.clicked.connect(lambda: self.start_reference_path_recording("chassis"))
self.stop_chassis_path_record_btn.clicked.connect(
lambda: self.stop_process(self.reference_path_record_process, "参考路径录制")
)
path_row.addWidget(self.record_chassis_path_btn)
path_row.addWidget(self.stop_chassis_path_record_btn)
layout.addLayout(path_row)
self.chassis_param_area = QWidget()
self.chassis_param_layout = QVBoxLayout(self.chassis_param_area)
self.chassis_param_layout.setContentsMargins(0, 0, 0, 0)
layout.addWidget(self.chassis_param_area)
layout.addStretch(1)
self.chassis_param_checks: dict[str, QCheckBox] = {}
self.rebuild_chassis_parameter_checks()
return tab
def _build_control_task_tab(self) -> QWidget:
tab = QWidget()
layout = QVBoxLayout(tab)
path_row = QHBoxLayout()
path_row.addWidget(QLabel("参考路径"))
self.control_reference_path_combo = QComboBox()
self.control_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview)
path_row.addWidget(self.control_reference_path_combo, 1)
self.record_control_path_btn = QPushButton("录制")
self.stop_control_path_record_btn = QPushButton("停止录制")
self.record_control_path_btn.clicked.connect(lambda: self.start_reference_path_recording("control"))
self.stop_control_path_record_btn.clicked.connect(
lambda: self.stop_process(self.reference_path_record_process, "参考路径录制")
)
path_row.addWidget(self.record_control_path_btn)
path_row.addWidget(self.stop_control_path_record_btn)
layout.addLayout(path_row)
self.control_param_checks: dict[str, QCheckBox] = {}
for option in CONTROL_PARAMETER_OPTIONS:
check = QCheckBox(f"{option['label']} · {option['axis']} · {option['algorithm']}")
check.setChecked(option["code"] in {"control.lateral.mpc", "control.longitudinal.pid"})
check.stateChanged.connect(self.refresh_command_preview)
self.control_param_checks[str(option["code"])] = check
layout.addWidget(check)
layout.addStretch(1)
return tab
def _build_sensor_task_tab(self) -> QWidget:
tab = QWidget()
layout = QVBoxLayout(tab)
path_row = QHBoxLayout()
path_row.addWidget(QLabel("参考路径"))
self.sensor_reference_path_combo = QComboBox()
self.sensor_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview)
path_row.addWidget(self.sensor_reference_path_combo, 1)
self.record_sensor_path_btn = QPushButton("录制")
self.stop_sensor_path_record_btn = QPushButton("停止录制")
self.record_sensor_path_btn.clicked.connect(lambda: self.start_reference_path_recording("sensor"))
self.stop_sensor_path_record_btn.clicked.connect(
lambda: self.stop_process(self.reference_path_record_process, "参考路径录制")
)
path_row.addWidget(self.record_sensor_path_btn)
path_row.addWidget(self.stop_sensor_path_record_btn)
layout.addLayout(path_row)
self.sensor_id_edits: dict[str, QLineEdit] = {
"front_camera": QLineEdit("demo_front_camera"),
"down_camera": QLineEdit("demo_down_camera"),
"lidar_2d": QLineEdit("demo_lidar_2d"),
"lidar_3d": QLineEdit("demo_lidar_3d"),
"imu": QLineEdit("demo_imu"),
"arm_camera": QLineEdit("demo_arm_camera"),
}
for edit in self.sensor_id_edits.values():
edit.hide()
edit.textChanged.connect(self.refresh_command_preview)
self.sensor_profile_summary_label = QLabel("当前画像传感器：未加载")
self.sensor_profile_summary_label.setWordWrap(True)
layout.addWidget(self.sensor_profile_summary_label)
self.sensor_task_checks: dict[tuple[str, str], QCheckBox] = {}
for sensor_key, sensor_label, subtype, label, stage_type in SENSOR_TASK_OPTIONS:
check = QCheckBox(f"{sensor_label} · {label}")
check.setChecked(subtype == "front_camera_intrinsic")
check.stateChanged.connect(self.refresh_command_preview)
self.sensor_task_checks[(sensor_key, subtype)] = check
layout.addWidget(check)
layout.addStretch(1)
return tab
def rebuild_chassis_parameter_checks(self) -> None:
if not hasattr(self, "chassis_param_layout"):
return
while self.chassis_param_layout.count():
item = self.chassis_param_layout.takeAt(0)
widget = item.widget()
if widget is not None:
widget.deleteLater()
self.chassis_param_checks = {}
chassis_type = self.chassis_type_combo.currentData() or "ackermann"
for option in CHASSIS_PARAMETER_OPTIONS[str(chassis_type)]:
check = QCheckBox(f"{option['label']} · {option['target']}")
check.setChecked(False)
check.stateChanged.connect(self.refresh_command_preview)
self.chassis_param_checks[str(option["code"])] = check
self.chassis_param_layout.addWidget(check)
self.chassis_param_layout.addStretch(1)
if hasattr(self, "launch_command_preview"):
self.refresh_command_preview()
def _add_path_row(self, layout: QGridLayout, row: int, label: str, edit: QLineEdit, slot) -> None:
button = QPushButton("选择")
button.clicked.connect(slot)
layout.addWidget(QLabel(label), row, 0)
layout.addWidget(edit, row, 1)
layout.addWidget(button, row, 2)
def _build_right_panel(self) -> QWidget:
panel = QWidget()
layout = QVBoxLayout(panel)
layout.setContentsMargins(0, 0, 0, 0)
tabs = QTabWidget()
layout.addWidget(tabs, 3)
self.check_table = QTableWidget(0, 3)
self.check_table.setHorizontalHeaderLabels(["检查项", "状态", "说明"])
self.check_table.horizontalHeader().setSectionResizeMode(0, QHeaderView.ResizeToContents)
self.check_table.horizontalHeader().setSectionResizeMode(1, QHeaderView.ResizeToContents)
self.check_table.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch)
tabs.addTab(self.check_table, "现场检查")
task_tab = QWidget()
task_layout = QVBoxLayout(task_tab)
self.task_table = QTableWidget(0, 4)
self.task_table.setHorizontalHeaderLabels(["顺序", "阶段", "标定项", "策略"])
self.task_table.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch)
self.session_config_preview = QPlainTextEdit()
self.session_config_preview.setReadOnly(True)
task_layout.addWidget(self.task_table, 2)
tabs.addTab(task_tab, "本轮任务预览")
localization_tab = QWidget()
localization_layout = QVBoxLayout(localization_tab)
self.localization_3d_view = WorkshopLocalization3DView()
self.localization_summary_label = QLabel("等待定位数据")
self.localization_table = QTableWidget(len(LOCALIZATION_DISPLAY_ROWS), 2)
self.localization_table.setHorizontalHeaderLabels(["字段", "值"])
self.localization_table.horizontalHeader().setSectionResizeMode(0, QHeaderView.ResizeToContents)
self.localization_table.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch)
for row, name in enumerate(LOCALIZATION_DISPLAY_ROWS):
self.localization_table.setItem(row, 0, table_item(name))
self.localization_table.setItem(row, 1, table_item("-"))
localization_layout.addWidget(self.localization_3d_view, 3)
localization_layout.addWidget(self.localization_summary_label)
localization_layout.addWidget(self.localization_table, 1)
tabs.addTab(localization_tab, "车间定位")
self.launch_command_preview = QPlainTextEdit()
self.launch_command_preview.setReadOnly(True)
self.smoke_command_preview = QPlainTextEdit()
self.smoke_command_preview.setReadOnly(True)
report_tab = QWidget()
report_layout = QVBoxLayout(report_tab)
self.report_summary_label = QLabel("暂无报告")
self.stage_table = QTableWidget(0, 5)
self.stage_table.setHorizontalHeaderLabels(["阶段", "成功", "自动验收", "状态", "摘要"])
self.stage_table.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch)
self.metadata_table = QTableWidget(0, 2)
self.metadata_table.setHorizontalHeaderLabels(["字段", "值"])
self.metadata_table.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch)
report_layout.addWidget(self.report_summary_label)
report_layout.addWidget(self.stage_table, 2)
report_layout.addWidget(QLabel("报告附加信息"))
report_layout.addWidget(self.metadata_table, 1)
tabs.addTab(report_tab, "报告")
self.log_view = QPlainTextEdit()
self.log_view.setReadOnly(True)
self.log_view.setFont(QFont("Consolas", 10))
self.log_view.setMinimumHeight(230)
layout.addWidget(QLabel("运行日志"))
layout.addWidget(self.log_view, 1)
return panel
def closeEvent(self, event) -> None:
self.stop_process(self.reference_path_record_process, "参考路径录制")
self.stop_process(self.chassis_capture_process, "底盘路径采集")
self.stop_process(self.smoke_process, "标定流程")
self.stop_process(self.launch_process, "现场服务")
if self.localization_spin_timer.isActive():
self.localization_spin_timer.stop()
if self.localization_node is not None:
try:
self.localization_node.destroy_node()
except Exception:
pass
if rclpy is not None and self.localization_rclpy_initialized and rclpy.ok():
rclpy.shutdown()
event.accept()
def main() -> int:
app = QApplication(sys.argv)
signal.signal(signal.SIGINT, lambda *_: app.quit())
timer = QTimer()
timer.timeout.connect(lambda: None)
timer.start(200)
window = OperatorMainWindow()
window.show()
return app.exec()
if __name__ == "__main__":
raise SystemExit(main())
agv_calib_brain/src/apps/operator_ui/process_report.py
+619
View File
@@ -0,0 +1,619 @@
"""操作台进程启动、日志和报告展示。"""
from __future__ import annotations
import re
import sys
from datetime import datetime
from pathlib import Path
from PySide6.QtCore import QProcess
from PySide6.QtWidgets import (
QCheckBox,
QComboBox,
QDialog,
QDialogButtonBox,
QFormLayout,
QLineEdit,
QMessageBox,
QVBoxLayout,
)
try:
from .constants import REPO_ROOT
from .profile_io import launch_arg, read_nested, shell_join
from .ui_helpers import table_item
except ImportError:
from constants import REPO_ROOT
from profile_io import launch_arg, read_nested, shell_join
from ui_helpers import table_item
REFERENCE_PATH_RECORDER = "src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py"
RECORD_MODULE_LABELS = {
"chassis": "底盘标定",
"control": "运控参数",
"sensor": "传感器标定",
}
RECORD_DEFAULT_PATH_TYPES = {
"chassis": "recorded",
"control": "recorded",
"sensor": "recorded",
}
RECORD_DEFAULT_RECOMMENDED_TASKS = {
"chassis": "recorded",
"control": "trajectory_tracking",
"sensor": "sensor_extrinsic",
}
class ReferencePathRecordDialog(QDialog):
def __init__(self, module: str, defaults: dict[str, str], parent=None) -> None:
super().__init__(parent)
self.setWindowTitle(f"录制{RECORD_MODULE_LABELS.get(module, module)}参考路径")
self.module = module
self.default_topics = {
"external_pose": defaults.get("external_pose_topic", ""),
"chassis_telemetry": defaults.get("chassis_telemetry_topic", ""),
}
layout = QVBoxLayout(self)
form = QFormLayout()
layout.addLayout(form)
self.display_name_edit = QLineEdit(defaults.get("display_name", ""))
self.path_id_edit = QLineEdit(defaults.get("path_id", ""))
self.source_combo = QComboBox()
self.source_combo.addItem("外部真值定位", "external_pose")
self.source_combo.addItem("底盘遥测里程计", "chassis_telemetry")
self.topic_edit = QLineEdit(defaults.get("topic", ""))
self.path_type_combo = QComboBox()
for item in defaults.get("path_type_options", "").split(","):
item = item.strip()
if item:
self.path_type_combo.addItem(item)
self.path_type_combo.setEditable(True)
current_path_type = defaults.get("path_type", "")
if current_path_type:
index = self.path_type_combo.findText(current_path_type)
if index >= 0:
self.path_type_combo.setCurrentIndex(index)
else:
self.path_type_combo.setEditText(current_path_type)
self.recommended_task_types_edit = QLineEdit(defaults.get("recommended_task_types", ""))
self.target_speed_edit = QLineEdit(defaults.get("target_speed_ms", "0.05"))
self.duration_edit = QLineEdit(defaults.get("duration_sec", "0"))
self.min_distance_step_edit = QLineEdit(defaults.get("min_distance_step_m", "0.03"))
self.min_yaw_step_edit = QLineEdit(defaults.get("min_yaw_step_rad", "0.03"))
self.description_edit = QLineEdit(defaults.get("description", ""))
self.overwrite_check = QCheckBox("允许覆盖同名路径文件")
source = defaults.get("source", "external_pose")
source_index = self.source_combo.findData(source)
if source_index >= 0:
self.source_combo.setCurrentIndex(source_index)
self.source_combo.currentIndexChanged.connect(self._apply_default_topic_for_source)
form.addRow("中文名称", self.display_name_edit)
form.addRow("路径 ID", self.path_id_edit)
form.addRow("录制来源", self.source_combo)
form.addRow("话题", self.topic_edit)
form.addRow("路径类型", self.path_type_combo)
form.addRow("推荐任务类型", self.recommended_task_types_edit)
form.addRow("目标速度 m/s", self.target_speed_edit)
form.addRow("录制时长 s", self.duration_edit)
form.addRow("最小点间距 m", self.min_distance_step_edit)
form.addRow("最小 yaw 间隔 rad", self.min_yaw_step_edit)
form.addRow("说明", self.description_edit)
form.addRow("", self.overwrite_check)
buttons = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel)
buttons.accepted.connect(self.accept)
buttons.rejected.connect(self.reject)
layout.addWidget(buttons)
def _apply_default_topic_for_source(self, *_args) -> None:
source = str(self.source_combo.currentData() or "external_pose")
self.topic_edit.setText(self.default_topics.get(source, ""))
def values(self) -> dict[str, object]:
return {
"module": self.module,
"display_name": self.display_name_edit.text().strip(),
"path_id": self.path_id_edit.text().strip(),
"source": str(self.source_combo.currentData() or "external_pose"),
"topic": self.topic_edit.text().strip(),
"path_type": self.path_type_combo.currentText().strip() or "recorded",
"recommended_task_types": [
item.strip()
for item in re.split(r"[,;，；\s]+", self.recommended_task_types_edit.text().strip())
if item.strip()
],
"target_speed_ms": self.target_speed_edit.text().strip() or "0.0",
"duration_sec": self.duration_edit.text().strip() or "0",
"min_distance_step_m": self.min_distance_step_edit.text().strip() or "0.03",
"min_yaw_step_rad": self.min_yaw_step_edit.text().strip() or "0.03",
"description": self.description_edit.text().strip(),
"overwrite": self.overwrite_check.isChecked(),
}
class ProcessReportMixin:
def _resolved_repo_path(self, raw_path: str) -> str:
path = Path(raw_path).expanduser()
if path.is_absolute():
return str(path.resolve(strict=False))
return str((REPO_ROOT / path).resolve(strict=False))
def _resolved_bridge_script(self, raw_path: str) -> str:
return self._resolved_repo_path(raw_path)
def _configured_path_or_default(self, keys: tuple[str, ...], default_path: str) -> str:
raw_path = read_nested(getattr(self, "site_profile", {}), keys, "")
return self._resolved_repo_path(raw_path or default_path)
def default_record_topic(self, source: str) -> str:
if source == "chassis_telemetry":
return (
read_nested(self.site_profile, ("vehicle_agent", "chassis_telemetry_topic"))
or read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic"))
or "/chassis/telemetry"
)
return self.external_topic_edit.text().strip() or "/workshop/external_localization/vehicle/pose"
def reference_path_record_defaults(self, module: str) -> dict[str, str]:
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
selected = self.selected_reference_path(module) if hasattr(self, "selected_reference_path") else None
recommended = selected.get("recommended_task_types", []) if isinstance(selected, dict) else []
recommended_text = ",".join(str(item) for item in recommended) if recommended else RECORD_DEFAULT_RECOMMENDED_TASKS[module]
path_type = str(selected.get("path_type", "")) if isinstance(selected, dict) else ""
path_type_options = {
"chassis": "recorded,straight_line,arc,s_curve,in_place_rotation,lateral_translation,diagonal_motion",
"control": "recorded,straight_line,arc,s_curve,lateral_offset,stop_accuracy",
"sensor": "recorded,static_station,sampling_line,imu_motion,pose_sweep",
}[module]
return {
"display_name": f"{RECORD_MODULE_LABELS[module]}录制路径 {timestamp}",
"path_id": f"{module}_recorded_{timestamp}",
"source": "external_pose",
"topic": self.default_record_topic("external_pose"),
"external_pose_topic": self.default_record_topic("external_pose"),
"chassis_telemetry_topic": self.default_record_topic("chassis_telemetry"),
"path_type": path_type or RECORD_DEFAULT_PATH_TYPES[module],
"path_type_options": path_type_options,
"recommended_task_types": recommended_text,
"target_speed_ms": "0.05" if module != "chassis" else "0.08",
"duration_sec": "0",
"min_distance_step_m": "0.03",
"min_yaw_step_rad": "0.03",
"description": "操作台录制参考路径",
}
def validate_reference_path_record_values(self, values: dict[str, object]) -> bool:
path_id = str(values.get("path_id", "")).strip()
display_name = str(values.get("display_name", "")).strip()
if not display_name:
QMessageBox.warning(self, "路径名称缺失", "请填写中文名称。")
return False
if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_-]*", path_id):
QMessageBox.warning(self, "路径 ID 不合法", "路径 ID 只能使用英文、数字、下划线和短横线，且不能以符号开头。")
return False
for key, label in [
("target_speed_ms", "目标速度"),
("duration_sec", "录制时长"),
("min_distance_step_m", "最小点间距"),
("min_yaw_step_rad", "最小 yaw 间隔"),
]:
try:
value = float(str(values.get(key, "0")))
except ValueError:
QMessageBox.warning(self, "数值格式错误", f"{label} 必须是数字。")
return False
if key != "target_speed_ms" and value < 0.0:
QMessageBox.warning(self, "数值格式错误", f"{label} 不能小于 0。")
return False
if key == "target_speed_ms" and value < 0.0:
QMessageBox.warning(self, "数值格式错误", f"{label} 不能小于 0。")
return False
return True
def build_reference_path_record_command(self, values: dict[str, object]) -> str:
module = str(values["module"])
recorder = self._configured_path_or_default(
("reference_paths", "recorder"),
REFERENCE_PATH_RECORDER,
)
args = [
sys.executable,
recorder,
"--output-dir",
str(self.reference_path_dir(module)),
"--path-id",
str(values["path_id"]),
"--display-name",
str(values["display_name"]),
"--module-type",
module,
"--path-type",
str(values["path_type"]),
"--frame-id",
"workshop",
"--target-speed-ms",
str(values["target_speed_ms"]),
"--duration-sec",
str(values["duration_sec"]),
"--min-distance-step-m",
str(values["min_distance_step_m"]),
"--min-yaw-step-rad",
str(values["min_yaw_step_rad"]),
"--source",
str(values["source"]),
]
topic = str(values.get("topic", "")).strip()
if topic:
args.extend(["--topic", topic])
description = str(values.get("description", "")).strip()
if description:
args.extend(["--description", description])
for task_type in values.get("recommended_task_types", []) or []:
args.extend(["--recommended-task-type", str(task_type)])
if module == "chassis":
args.extend(["--allowed-chassis-type", str(self.chassis_type_combo.currentData() or "ackermann")])
if bool(values.get("overwrite", False)):
args.append("--overwrite")
return "source install/setup.bash && exec " + shell_join(args)
def build_chassis_path_capture_command(self) -> str:
reference_path = self.selected_reference_path("chassis")
reference_path_id = str(reference_path.get("path_id", "")).strip() if reference_path else ""
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session_dir = Path("/tmp/agv_calib_chassis_sim") / f"ui_{timestamp}"
vehicle_id = self.vehicle_id_edit.text().strip() or "demo_agv_001"
chassis_type = str(self.chassis_type_combo.currentData() or "ackermann")
chassis_topic = (
read_nested(self.site_profile, ("vehicle_agent", "chassis_telemetry_topic"))
or read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic"))
or "/chassis/telemetry"
)
ackermann_topic = read_nested(
self.site_profile,
("vehicle_agent", "internal_ackermann_command_topic"),
f"/vehicle/{vehicle_id}/internal/ackermann_cmd",
)
runner = self._configured_path_or_default(
("chassis_calibration", "action_profile_capture_runner"),
"src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py",
)
config_file = self._configured_path_or_default(
("chassis_calibration", "data_config_file"),
"src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_sim.yaml",
)
action_profile = self._configured_path_or_default(
("chassis_calibration", "action_profile_file"),
"src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml",
)
args = [
sys.executable,
runner,
"--config",
config_file,
"--action-profile",
action_profile,
"--chassis-type",
chassis_type,
"--session-id",
f"ui_chassis_path_{timestamp}",
"--site-id",
self.workcell_zone_edit.text().strip() or "isaac_workcell_zone_a",
"--vehicle-id",
vehicle_id,
"--session-dir",
str(session_dir),
"--dataset-index-path",
str(session_dir / "dataset_index.yaml"),
"--chassis-telemetry-topic",
chassis_topic,
"--external-pose-topic",
self.external_topic_edit.text().strip() or "/isaac/external_localization/vehicle/pose",
"--ackermann-command-topic",
ackermann_topic,
"--command-source",
"auto",
"--action-server",
"/chassis/execute_motion_primitive",
]
if reference_path_id:
args.extend(["--reference-path-id", reference_path_id])
return "source install/setup.bash && " + shell_join(args)
def build_launch_command(self) -> str:
args = [
"ros2",
"launch",
"win_ubuntu_bridge",
"minimal_workshop_demo.launch.py",
launch_arg("external_telemetry_topic", self.external_topic_edit.text().strip()),
launch_arg("expected_reference_source_name", self.reference_source_edit.text().strip()),
launch_arg("expected_workcell_zone_id", self.workcell_zone_edit.text().strip()),
launch_arg("use_gateway", "true"),
launch_arg("chassis_host", self.vehicle_host_edit.text().strip()),
launch_arg("control_host", self.vehicle_host_edit.text().strip()),
launch_arg("sensor_storage_root", self.sensor_storage_edit.text().strip()),
launch_arg("sensor_registry", self.sensor_registry_edit.text().strip()),
]
if self.data_input_edit.text().strip():
args.append(launch_arg("data_input_params_file", self.data_input_edit.text().strip()))
if self.dataset_index_edit.text().strip():
args.append(launch_arg("dataset_index_file", self.dataset_index_edit.text().strip()))
bridge_config = getattr(self, "site_profile", {}).get("chassis_telemetry_bridge", {})
if isinstance(bridge_config, dict) and bridge_config:
bridge_script = read_nested(
self.site_profile,
("chassis_telemetry_bridge", "bridge_tool"),
"src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py",
)
args.extend([
launch_arg("enable_chassis_telemetry_bridge", "true"),
launch_arg("chassis_telemetry_bridge_script", self._resolved_bridge_script(bridge_script)),
launch_arg(
"chassis_telemetry_bind_host",
read_nested(self.site_profile, ("chassis_telemetry_bridge", "bind_host"), "0.0.0.0"),
),
launch_arg(
"chassis_telemetry_bind_port",
read_nested(self.site_profile, ("chassis_telemetry_bridge", "bind_port"), "9010"),
),
launch_arg(
"chassis_telemetry_protocol",
read_nested(self.site_profile, ("chassis_telemetry_bridge", "protocol"), "frame"),
),
launch_arg(
"chassis_telemetry_expected_msg_type",
read_nested(self.site_profile, ("chassis_telemetry_bridge", "expected_msg_type"), "0"),
),
launch_arg(
"chassis_telemetry_topic",
read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic"), "/chassis/telemetry"),
),
launch_arg(
"chassis_telemetry_default_chassis_type",
read_nested(self.site_profile, ("chassis_telemetry_bridge", "default_chassis_type"), "ackermann"),
),
launch_arg(
"chassis_telemetry_max_payload_bytes",
read_nested(self.site_profile, ("chassis_telemetry_bridge", "max_payload_bytes"), "262144"),
),
])
return "source install/setup.bash && " + shell_join(args)
def build_smoke_command(self) -> str:
args = [
sys.executable,
"src/simulation/tools/smoke_test_workshop_orchestrator.py",
"--site-profile",
self.profile_path_edit.text().strip(),
"--tasks",
self.selected_tasks_csv(),
"--session-config-file",
self.session_config_output_edit.text().strip(),
"--no-publish-fake-external-telemetry",
"--external-telemetry-topic",
self.external_topic_edit.text().strip(),
"--reference-source-name",
self.reference_source_edit.text().strip(),
"--workcell-zone-id",
self.workcell_zone_edit.text().strip(),
"--vehicle-id",
self.vehicle_id_edit.text().strip(),
"--action-timeout-sec",
"240",
"--service-timeout-sec",
"10",
"--verbose-feedback",
]
if self.vehicle_profile_path_edit.text().strip():
args.extend(["--vehicle-profile-file", self.vehicle_profile_path_edit.text().strip()])
if self.skip_wifi6_check.isChecked():
args.append("--disable-wifi6-precheck")
return "source install/setup.bash && " + shell_join(args)
def refresh_command_preview(self) -> None:
if not hasattr(self, "launch_command_preview"):
return
self.launch_command_preview.setPlainText(self.build_launch_command())
self.smoke_command_preview.setPlainText(self.build_smoke_command())
self.refresh_deployment_checks()
self.refresh_task_preview_from_config()
def start_launch_stack(self) -> None:
if self.launch_process.state() != QProcess.NotRunning:
QMessageBox.warning(self, "现场服务已运行", "现场服务进程已经在运行。")
return
command = self.build_launch_command()
self.append_log("[现场服务] 启动：" + command)
self.stack_state.setText("现场服务：运行中")
self.launch_process.start("bash", ["-lc", command])
def start_smoke_test(self) -> None:
if self.smoke_process.state() != QProcess.NotRunning:
QMessageBox.warning(self, "标定流程已运行", "标定流程已经在运行。")
return
if not self.selected_tasks():
QMessageBox.warning(self, "任务为空", "请至少选择一个任务。")
return
if not self.build_session_config():
return
if self.launch_process.state() == QProcess.NotRunning:
reply = QMessageBox.question(
self,
"现场服务未运行",
"当前界面没有检测到由本界面启动的现场服务，仍然继续执行本轮标定吗？",
)
if reply != QMessageBox.Yes:
return
self.reset_report()
if self.external_check.isChecked():
self.update_localization_state("检查中", "running")
else:
self.update_localization_state("本轮未检查", "neutral")
self.trajectory_runtime_active = True
self.trajectory_last_completed_stage_id = ""
self.refresh_planned_trajectory()
command = self.build_smoke_command()
self.append_log("[标定流程] 启动：" + command)
self.smoke_state.setText("标定流程：运行中")
self.smoke_process.start("bash", ["-lc", command])
def start_chassis_path_capture(self) -> None:
if self.chassis_capture_process.state() != QProcess.NotRunning:
QMessageBox.warning(self, "底盘路径采集已运行", "底盘路径采集进程已经在运行。")
return
if not self.selected_reference_path("chassis"):
QMessageBox.warning(self, "未选择路径", "请先在底盘标定页选择参考路径。")
return
reference_path = self.selected_reference_path("chassis")
reference_path_id = str(reference_path.get("path_id", "")).strip() if reference_path else ""
chassis_type = str(self.chassis_type_combo.currentData() or "ackermann")
if reference_path_id not in self.allowed_chassis_reference_path_ids(chassis_type):
QMessageBox.warning(
self,
"路径未绑定动作",
"这条路径是录制参考路径，但还没有绑定到底盘动作 profile，不能直接用于“底盘路径采集测试”。"
"请先在 chassis_action_profile.yaml 中为当前底盘类型添加对应动作，或仅用于任务预览/路径管理。",
)
return
if self.launch_process.state() == QProcess.NotRunning:
reply = QMessageBox.question(
self,
"现场服务未运行",
"当前界面没有检测到由本界面启动的现场服务。若你已经用脚本启动 Isaac 仿真链路，可以继续执行底盘路径采集测试。",
)
if reply != QMessageBox.Yes:
return
command = self.build_chassis_path_capture_command()
self.append_log("[底盘路径采集] 启动：" + command)
self.smoke_state.setText("底盘路径采集：运行中")
self.chassis_capture_process.start("bash", ["-lc", command])
def start_reference_path_recording(self, module: str) -> None:
if self.reference_path_record_process.state() != QProcess.NotRunning:
QMessageBox.warning(self, "参考路径录制已运行", "参考路径录制进程已经在运行。")
return
if self.launch_process.state() == QProcess.NotRunning:
reply = QMessageBox.question(
self,
"现场服务未运行",
"当前界面没有检测到由本界面启动的现场服务。若你已经用脚本启动 Isaac 或 ROS 现场链路，可以继续录制参考路径。",
)
if reply != QMessageBox.Yes:
return
dialog = ReferencePathRecordDialog(module, self.reference_path_record_defaults(module), self)
if dialog.exec() != QDialog.Accepted:
return
values = dialog.values()
if not self.validate_reference_path_record_values(values):
return
command = self.build_reference_path_record_command(values)
self.reference_path_record_module = module
self.append_log("[参考路径录制] 启动：" + command)
self.smoke_state.setText("参考路径录制：运行中")
self.reference_path_record_process.start("bash", ["-lc", command])
def stop_process(self, process: QProcess, name: str) -> None:
if process.state() == QProcess.NotRunning:
self.append_log(f"[{name}] 当前没有运行中的进程。")
return
self.append_log(f"[{name}] 正在停止。")
process.terminate()
if not process.waitForFinished(3000):
process.kill()
process.waitForFinished(2000)
def _read_process_output(self, process: QProcess, name: str) -> None:
chunks = [
bytes(process.readAllStandardOutput()).decode(errors="replace"),
bytes(process.readAllStandardError()).decode(errors="replace"),
]
for chunk in chunks:
if not chunk:
continue
for line in chunk.splitlines():
self.append_log(f"[{name}] {line}")
self.parse_report_line(line)
def _process_finished(self, name: str, code: int, status: QProcess.ExitStatus) -> None:
status_text = "正常退出" if status == QProcess.NormalExit and code == 0 else f"退出码 {code}"
self.append_log(f"[{name}] {status_text}")
if name == "现场服务":
self.stack_state.setText("现场服务：未启动")
elif name == "标定流程":
self.trajectory_runtime_active = False
self.trajectory_last_completed_stage_id = ""
self.refresh_planned_trajectory()
self.smoke_state.setText("标定流程：空闲")
elif name == "底盘路径采集":
self.smoke_state.setText("底盘路径采集：空闲")
elif name == "参考路径录制":
self.smoke_state.setText("参考路径录制：空闲")
self.refresh_reference_path_combos()
def append_log(self, text: str) -> None:
self.log_view.appendPlainText(text)
scrollbar = self.log_view.verticalScrollBar()
scrollbar.setValue(scrollbar.maximum())
def reset_report(self) -> None:
self.report_metadata.clear()
self.report_stages.clear()
self.report_summary_label.setText("等待本轮标定报告")
self.stage_table.setRowCount(0)
self.metadata_table.setRowCount(0)
def parse_report_line(self, line: str) -> None:
if line.startswith("[REPORT] "):
self.report_summary_label.setText(line.replace("[REPORT] ", "", 1))
return
metadata_match = re.match(r"^\[REPORT_METADATA\]\s+([^=]+)=(.*)$", line)
if metadata_match:
self.report_metadata[metadata_match.group(1)] = metadata_match.group(2)
self.refresh_metadata_table()
return
stage_match = re.match(
r"^\[STAGE\]\s+id=(.*?)\s+success=(.*?)\s+auto_acceptance=(.*?)\s+state=(.*?)\s+summary=(.*)$",
line,
)
if stage_match:
stage_id = stage_match.group(1)
success_text = stage_match.group(2)
summary = stage_match.group(5)
self.report_stages.append({
"stage_id": stage_id,
"success": success_text,
"auto_acceptance": stage_match.group(3),
"state": stage_match.group(4),
"summary": summary,
})
if "external_reference" in stage_id or stage_id.endswith("_external"):
if success_text == "True":
self.update_localization_state("检查通过", "ok", summary)
else:
self.update_localization_state("检查失败", "fail", summary)
self.refresh_stage_table()
def refresh_metadata_table(self) -> None:
items = list(self.report_metadata.items())
self.metadata_table.setRowCount(len(items))
for row, (key, value) in enumerate(items):
self.metadata_table.setItem(row, 0, table_item(key))
self.metadata_table.setItem(row, 1, table_item(value))
def refresh_stage_table(self) -> None:
self.stage_table.setRowCount(len(self.report_stages))
for row, stage in enumerate(self.report_stages):
success = stage["success"] == "True"
accepted = stage["auto_acceptance"] == "True"
self.stage_table.setItem(row, 0, table_item(stage["stage_id"]))
self.stage_table.setItem(row, 1, table_item(stage["success"], "ok" if success else "fail"))
self.stage_table.setItem(row, 2, table_item(stage["auto_acceptance"], "ok" if accepted else "fail"))
self.stage_table.setItem(row, 3, table_item(stage["state"]))
self.stage_table.setItem(row, 4, table_item(stage["summary"]))
agv_calib_brain/src/apps/operator_ui/profile_handlers.py
+384
View File
@@ -0,0 +1,384 @@
"""操作台现场配置和车辆画像处理。"""
from __future__ import annotations
from pathlib import Path
from typing import Any
from PySide6.QtWidgets import QFileDialog, QMessageBox
try:
from .constants import DEFAULT_SESSION_CONFIG, DEFAULT_SITE_PROFILE, DEFAULT_VEHICLE_PROFILE, DEFAULT_VEHICLE_PROFILE_DIR, REPO_ROOT
from .profile_io import (
bool_from_text,
collect_placeholders,
dump_yaml,
load_yaml,
read_nested,
resolve_profile_path,
resolve_vehicle_profile_path,
vehicle_profile_path_from_name,
)
from .vehicle_profile_dialog import open_vehicle_profile_dialog as show_vehicle_profile_dialog
except ImportError:
from constants import DEFAULT_SESSION_CONFIG, DEFAULT_SITE_PROFILE, DEFAULT_VEHICLE_PROFILE, DEFAULT_VEHICLE_PROFILE_DIR, REPO_ROOT
from profile_io import (
bool_from_text,
collect_placeholders,
dump_yaml,
load_yaml,
read_nested,
resolve_profile_path,
resolve_vehicle_profile_path,
vehicle_profile_path_from_name,
)
from vehicle_profile_dialog import open_vehicle_profile_dialog as show_vehicle_profile_dialog
class ProfileHandlersMixin:
def browse_profile(self) -> None:
path, _ = QFileDialog.getOpenFileName(self, "选择现场配置文件", str(REPO_ROOT), "YAML Files (*.yaml *.yml)")
if path:
self.profile_path_edit.setText(path)
self.load_profile()
def refresh_vehicle_profile_combo(self) -> None:
if not hasattr(self, "vehicle_profile_combo"):
return
current_path = self.vehicle_profile_path_edit.text().strip() if hasattr(self, "vehicle_profile_path_edit") else ""
self.vehicle_profile_combo.blockSignals(True)
self.vehicle_profile_combo.clear()
DEFAULT_VEHICLE_PROFILE_DIR.mkdir(parents=True, exist_ok=True)
profile_paths: list[Path] = []
for pattern in ("*.yaml", "*.yml", "*.ymal"):
profile_paths.extend(DEFAULT_VEHICLE_PROFILE_DIR.glob(pattern))
for profile_path in sorted(set(profile_paths)):
display_name = profile_path.stem
try:
profile = load_yaml(profile_path)
display_name = str(profile.get("display_name") or profile.get("profile_name") or profile_path.stem)
except Exception:
pass
self.vehicle_profile_combo.addItem(display_name, str(profile_path))
self.vehicle_profile_combo.blockSignals(False)
if current_path:
resolved = str(resolve_vehicle_profile_path(current_path))
for index in range(self.vehicle_profile_combo.count()):
if str(resolve_vehicle_profile_path(str(self.vehicle_profile_combo.itemData(index)))) == resolved:
self.vehicle_profile_combo.setCurrentIndex(index)
break
def browse_vehicle_profile(self) -> None:
path, _ = QFileDialog.getOpenFileName(
self,
"选择车辆画像文件",
str(DEFAULT_VEHICLE_PROFILE_DIR),
"YAML Files (*.yaml *.yml)",
)
if path:
self.vehicle_profile_path_edit.setText(path)
self.load_vehicle_profile_from_current_path()
def open_new_vehicle_profile_dialog(self) -> None:
self.open_vehicle_profile_dialog(new_profile=True)
def open_edit_vehicle_profile_dialog(self) -> None:
self.open_vehicle_profile_dialog(new_profile=False)
def open_vehicle_profile_dialog(self, new_profile: bool) -> None:
show_vehicle_profile_dialog(self, new_profile)
def load_selected_vehicle_profile(self, _index: int | None = None) -> None:
path = self.vehicle_profile_combo.currentData()
if path:
self.vehicle_profile_path_edit.setText(str(path))
self.load_vehicle_profile_from_current_path()
def load_vehicle_profile_from_current_path(self) -> None:
path = resolve_vehicle_profile_path(self.vehicle_profile_path_edit.text().strip() or str(DEFAULT_VEHICLE_PROFILE))
try:
profile = load_yaml(path)
except Exception as exc:
QMessageBox.warning(self, "车辆画像加载失败", f"无法加载车辆画像文件：{exc}")
return
self.vehicle_profile_data = profile
self.vehicle_profile_path_edit.setText(str(path))
self.vehicle_profile_name_edit.setText(str(profile.get("profile_name") or profile.get("display_name") or path.stem))
self.vehicle_model_name_edit.setText(str(profile.get("model_name") or ""))
self.vehicle_manufacturer_edit.setText(str(profile.get("manufacturer") or ""))
self.apply_vehicle_profile_to_ui(profile)
self.refresh_vehicle_profile_combo()
self.append_log(f"[车辆画像] 已加载：{path}")
self.refresh_all()
def apply_vehicle_profile_to_ui(self, profile: dict[str, Any]) -> None:
geometry = profile.get("geometry", {})
if not isinstance(geometry, dict):
geometry = {}
self.vehicle_length_edit.setText(str(geometry.get("length_m", "")))
self.vehicle_width_edit.setText(str(geometry.get("width_m", "")))
self.vehicle_height_edit.setText(str(geometry.get("height_m", "")))
self.vehicle_ground_clearance_edit.setText(str(geometry.get("ground_clearance_m", "")))
chassis = profile.get("chassis", {})
if not isinstance(chassis, dict):
chassis = {}
self.vehicle_wheel_base_edit.setText(str(chassis.get("wheel_base_m", "")))
self.vehicle_track_width_edit.setText(str(chassis.get("track_width_m", "")))
self.vehicle_wheel_radius_edit.setText(str(chassis.get("wheel_radius_m", "")))
self.vehicle_max_steering_angle_edit.setText(str(chassis.get("max_steering_angle_rad", "")))
self.vehicle_min_turning_radius_edit.setText(str(chassis.get("min_turning_radius_m", "")))
chassis_type = str(chassis.get("chassis_type") or profile.get("vehicle_class") or "")
if chassis_type and hasattr(self, "chassis_type_combo"):
index = self.chassis_type_combo.findData(chassis_type)
if index >= 0:
self.chassis_type_combo.setCurrentIndex(index)
self.rebuild_chassis_parameter_checks()
controllers = profile.get("controllers", {})
if not isinstance(controllers, dict):
controllers = {}
self.vehicle_max_speed_edit.setText(str(controllers.get("max_speed_mps", "")))
self.vehicle_max_accel_edit.setText(str(controllers.get("max_accel_mps2", "")))
self.vehicle_control_frequency_edit.setText(str(controllers.get("control_frequency_hz", "")))
sensors = profile.get("sensors", {})
if isinstance(sensors, dict) and hasattr(self, "sensor_id_edits"):
for edit in self.sensor_id_edits.values():
edit.clear()
for sensor_key, edit in self.sensor_id_edits.items():
sensor_profile = sensors.get(sensor_key, {})
if (
isinstance(sensor_profile, dict)
and bool_from_text(sensor_profile.get("enabled", True))
and sensor_profile.get("sensor_id")
):
edit.setText(str(sensor_profile["sensor_id"]))
sensor_ids = [
edit.text().strip()
for edit in self.sensor_id_edits.values()
if edit.text().strip()
]
self.sensor_registry_edit.setText(",".join(sensor_ids))
sensor_display_names = {
"front_camera": "前视相机",
"down_camera": "下视相机",
"lidar_2d": "2D 雷达",
"lidar_3d": "3D 雷达",
"imu": "IMU",
"arm_camera": "手眼相机",
}
summary_parts = [
f"{sensor_display_names.get(sensor_key, sensor_key)}：{edit.text().strip()}"
for sensor_key, edit in self.sensor_id_edits.items()
if edit.text().strip()
]
if hasattr(self, "sensor_profile_summary_label"):
summary = "；".join(summary_parts) if summary_parts else "当前车辆画像没有启用传感器"
self.sensor_profile_summary_label.setText(f"当前画像传感器：{summary}")
if hasattr(self, "sensor_task_checks"):
for (sensor_key, _subtype), check in self.sensor_task_checks.items():
available = bool(self.sensor_id_edits.get(sensor_key) and self.sensor_id_edits[sensor_key].text().strip())
check.setEnabled(available)
if not available:
check.setChecked(False)
def current_vehicle_profile_payload(self) -> dict[str, Any]:
chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") if hasattr(self, "chassis_type_combo") else "ackermann"
loaded_sensors = self.vehicle_profile_data.get("sensors", {}) if isinstance(self.vehicle_profile_data, dict) else {}
if not isinstance(loaded_sensors, dict):
loaded_sensors = {}
sensor_payload: dict[str, dict[str, Any]] = {}
if hasattr(self, "sensor_id_edits"):
for sensor_key, edit in self.sensor_id_edits.items():
existing = loaded_sensors.get(sensor_key, {})
if not isinstance(existing, dict):
existing = {}
sensor_id = edit.text().strip()
if not sensor_id:
continue
mount_type = existing.get("camera_mount_type", "eye_in_hand" if sensor_key == "arm_camera" else "unspecified")
sensor_payload[sensor_key] = {
"sensor_id": sensor_id,
"sensor_name": existing.get("sensor_name", sensor_key),
"frame_id": existing.get("frame_id", f"{sensor_key}_link"),
"sensor_type": existing.get("sensor_type", sensor_key),
"camera_mount_type": mount_type,
"enabled": existing.get("enabled", True),
"needs_intrinsic_calibration": existing.get("needs_intrinsic_calibration", sensor_key in {"front_camera", "down_camera", "imu"}),
"needs_extrinsic_calibration": existing.get("needs_extrinsic_calibration", True),
}
loaded_chassis = self.vehicle_profile_data.get("chassis", {}) if isinstance(self.vehicle_profile_data, dict) else {}
if not isinstance(loaded_chassis, dict):
loaded_chassis = {}
loaded_controllers = self.vehicle_profile_data.get("controllers", {}) if isinstance(self.vehicle_profile_data, dict) else {}
if not isinstance(loaded_controllers, dict):
loaded_controllers = {}
return {
"schema_version": 1,
"profile_name": self.vehicle_profile_name_edit.text().strip() or "unnamed_vehicle_profile",
"display_name": self.vehicle_profile_name_edit.text().strip() or "未命名车辆画像",
"model_name": self.vehicle_model_name_edit.text().strip(),
"manufacturer": self.vehicle_manufacturer_edit.text().strip(),
"profile_version": self.vehicle_profile_data.get("profile_version", "v1") if isinstance(self.vehicle_profile_data, dict) else "v1",
"description": self.vehicle_profile_data.get("description", "") if isinstance(self.vehicle_profile_data, dict) else "",
"base_link_frame": read_nested(self.site_profile, ("frames", "base_link"), "base_link"),
"geometry": {
"length_m": self.vehicle_length_edit.text().strip(),
"width_m": self.vehicle_width_edit.text().strip(),
"height_m": self.vehicle_height_edit.text().strip(),
"ground_clearance_m": self.vehicle_ground_clearance_edit.text().strip(),
},
"chassis": {
"chassis_type": chassis_type,
"wheel_base_m": self.vehicle_wheel_base_edit.text().strip() or read_nested(self.site_profile, ("vehicle_agent", "wheel_base_m"), ""),
"track_width_m": self.vehicle_track_width_edit.text().strip() or loaded_chassis.get("track_width_m", ""),
"wheel_radius_m": self.vehicle_wheel_radius_edit.text().strip() or loaded_chassis.get("wheel_radius_m", ""),
"max_steering_angle_rad": self.vehicle_max_steering_angle_edit.text().strip() or read_nested(self.site_profile, ("vehicle_agent", "max_steering_angle_rad"), ""),
"min_turning_radius_m": self.vehicle_min_turning_radius_edit.text().strip() or loaded_chassis.get("min_turning_radius_m", ""),
},
"sensors": sensor_payload,
"controllers": {
"lateral_default": loaded_controllers.get("lateral_default", "mpc"),
"longitudinal_default": loaded_controllers.get("longitudinal_default", "pid"),
"max_speed_mps": self.vehicle_max_speed_edit.text().strip(),
"max_accel_mps2": self.vehicle_max_accel_edit.text().strip(),
"control_frequency_hz": self.vehicle_control_frequency_edit.text().strip(),
},
}
def save_vehicle_profile_to_path(self, path: Path) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
payload = self.current_vehicle_profile_payload()
path.write_text(dump_yaml(payload), encoding="utf-8")
self.vehicle_profile_data = payload
self.vehicle_profile_path_edit.setText(str(path))
self.refresh_vehicle_profile_combo()
self.append_log(f"[车辆画像] 已保存：{path}")
self.refresh_all()
def save_vehicle_profile(self) -> None:
self.save_vehicle_profile_to_path(vehicle_profile_path_from_name(self.vehicle_profile_name_edit.text()))
def save_vehicle_profile_as(self) -> None:
self.save_vehicle_profile()
def browse_data_input(self) -> None:
path, _ = QFileDialog.getOpenFileName(self, "选择算法数据参数文件", str(REPO_ROOT), "YAML Files (*.yaml *.yml)")
if path:
self.data_input_edit.setText(path)
self.refresh_all()
def browse_dataset_index(self) -> None:
path, _ = QFileDialog.getOpenFileName(self, "选择采集数据索引文件", str(REPO_ROOT), "YAML Files (*.yaml *.yml)")
if path:
self.dataset_index_edit.setText(path)
self.refresh_all()
def browse_session_config_output(self) -> None:
path, _ = QFileDialog.getSaveFileName(self, "选择本轮任务文件保存位置", str(DEFAULT_SESSION_CONFIG), "YAML Files (*.yaml *.yml)")
if path:
self.session_config_output_edit.setText(path)
self.refresh_command_preview()
def load_profile(self) -> None:
path = resolve_profile_path(self.profile_path_edit.text().strip())
try:
self.site_profile = load_yaml(path)
except Exception as exc:
self.site_profile = {}
self.profile_state_label.setText(f"加载失败：{exc}")
self.append_log(f"[界面] 现场配置文件加载失败: {exc}")
self.refresh_all()
return
self.profile_path_edit.setText(str(path))
self._apply_profile_defaults(path)
unresolved = collect_placeholders(self.site_profile)
self.profile_state_label.setText(f"已加载，占位项 {len(unresolved)} 个")
self.append_log(f"[界面] 已加载现场配置文件: {path}")
self.refresh_all()
self.restart_localization_monitor()
self.restart_workshop_event_monitor()
def _apply_profile_defaults(self, profile_path: Path) -> None:
profile = self.site_profile
self.vehicle_id_edit.setText(read_nested(profile, ("vehicle_id",), "demo_agv_001"))
self.update_workshop_dimension_state()
self.vehicle_host_edit.setText(read_nested(profile, ("workshop_pc", "gateway", "vehicle_host"), "127.0.0.1"))
self.vehicle_port_edit.setText(read_nested(profile, ("workshop_pc", "gateway", "vehicle_port"), "9000"))
self.external_topic_edit.setText(
read_nested(profile, ("external_pose_bridge", "source_topic"))
or read_nested(profile, ("external_localization", "output_topic"), "/workshop/external_localization/vehicle/pose")
)
self.reference_source_edit.setText(
read_nested(profile, ("external_pose_bridge", "reference_source_name"))
or read_nested(profile, ("external_localization", "reference_source_name"), "workshop_four_lidar_ball_truth")
)
self.workcell_zone_edit.setText(read_nested(profile, ("external_localization", "workcell_zone_id"), "workcell_zone_a"))
self.update_localization_state("待检查", "warn")
self.data_input_edit.setText(read_nested(profile, ("algorithm_data_inputs", "ros_params_file"), ""))
self.dataset_index_edit.setText(
read_nested(profile, ("external_localization", "recording_dataset_index_path"))
or read_nested(profile, ("chassis_calibration", "recording_dataset_index_path"))
or read_nested(profile, ("sensor_calibration", "recording_dataset_index_path"), "")
)
self.sensor_storage_edit.setText(
read_nested(profile, ("sensor_calibration", "recording_session_dir"), "/tmp/agv_sensor_calibration")
)
sensor_ids = [
read_nested(profile, ("vehicle_sensor_agent", "front_camera_sensor_id")),
read_nested(profile, ("vehicle_sensor_agent", "down_camera_sensor_id")),
read_nested(profile, ("vehicle_sensor_agent", "lidar_3d_sensor_id")),
read_nested(profile, ("vehicle_sensor_agent", "lidar_2d_sensor_id")),
read_nested(profile, ("vehicle_sensor_agent", "imu_sensor_id")),
]
sensor_ids = [value for value in sensor_ids if value]
self.sensor_registry_edit.setText(",".join(sensor_ids) if sensor_ids else "demo_front_camera")
if hasattr(self, "sensor_id_edits"):
sensor_defaults = {
"front_camera": read_nested(profile, ("vehicle_sensor_agent", "front_camera_sensor_id"), "demo_front_camera"),
"down_camera": read_nested(profile, ("vehicle_sensor_agent", "down_camera_sensor_id"), "demo_down_camera"),
"lidar_2d": read_nested(profile, ("vehicle_sensor_agent", "lidar_2d_sensor_id"), "demo_lidar_2d"),
"lidar_3d": read_nested(profile, ("vehicle_sensor_agent", "lidar_3d_sensor_id"), "demo_lidar_3d"),
"imu": read_nested(profile, ("vehicle_sensor_agent", "imu_sensor_id"), "demo_imu"),
"arm_camera": "demo_arm_camera",
}
for sensor_key, sensor_id in sensor_defaults.items():
self.sensor_id_edits[sensor_key].setText(sensor_id)
sensor_display_names = {
"front_camera": "前视相机",
"down_camera": "下视相机",
"lidar_2d": "2D 雷达",
"lidar_3d": "3D 雷达",
"imu": "IMU",
"arm_camera": "手眼相机",
}
summary_parts = [
f"{sensor_display_names.get(sensor_key, sensor_key)}：{edit.text().strip()}"
for sensor_key, edit in self.sensor_id_edits.items()
if edit.text().strip()
]
if hasattr(self, "sensor_profile_summary_label"):
summary = "；".join(summary_parts) if summary_parts else "当前车辆画像没有启用传感器"
self.sensor_profile_summary_label.setText(f"当前画像传感器：{summary}")
chassis_type = (
read_nested(profile, ("chassis_calibration", "chassis_type"))
or read_nested(profile, ("control_calibration", "chassis_type"))
or "ackermann"
)
if hasattr(self, "chassis_type_combo"):
index = self.chassis_type_combo.findData(chassis_type)
if index >= 0:
self.chassis_type_combo.setCurrentIndex(index)
self.rebuild_chassis_parameter_checks()
output_path = read_nested(profile, ("orchestrator_session", "generated_session_config_file"), "")
self.session_config_output_edit.setText(output_path if output_path else str(DEFAULT_SESSION_CONFIG))
if not Path(self.session_config_output_edit.text()).is_absolute():
self.session_config_output_edit.setText(str((profile_path.parent / self.session_config_output_edit.text()).resolve(strict=False)))
vehicle_profile_file = read_nested(profile, ("vehicle_profile", "profile_file"), str(DEFAULT_VEHICLE_PROFILE))
self.vehicle_profile_path_edit.setText(str(resolve_vehicle_profile_path(vehicle_profile_file)))
if Path(self.vehicle_profile_path_edit.text()).exists():
self.load_vehicle_profile_from_current_path()
if hasattr(self, "refresh_reference_path_combos"):
self.refresh_reference_path_combos()
agv_calib_brain/src/apps/operator_ui/profile_io.py
+134
View File
@@ -0,0 +1,134 @@
"""操作台配置文件读写工具。"""
from __future__ import annotations
import json
import re
import shlex
from pathlib import Path
from typing import Any
try:
import yaml
except ImportError:
yaml = None
try:
from .constants import (
DEFAULT_VEHICLE_PROFILE_DIR,
PLACEHOLDER_MARKERS,
REPO_ROOT,
VEHICLE_PROFILE_SAVE_EXTENSION,
)
except ImportError:
from constants import (
DEFAULT_VEHICLE_PROFILE_DIR,
PLACEHOLDER_MARKERS,
REPO_ROOT,
VEHICLE_PROFILE_SAVE_EXTENSION,
)
def has_placeholder(value: Any) -> bool:
return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS)
def read_nested(data: dict[str, Any], path: tuple[str, ...], default: str = "") -> str:
cursor: Any = data
for key in path:
if not isinstance(cursor, dict):
return default
cursor = cursor.get(key)
if cursor in (None, "") or has_placeholder(cursor):
return default
return str(cursor)
def collect_placeholders(value: Any, prefix: str = "") -> list[str]:
unresolved: list[str] = []
if isinstance(value, dict):
for key, child in value.items():
child_prefix = f"{prefix}.{key}" if prefix else str(key)
unresolved.extend(collect_placeholders(child, child_prefix))
return unresolved
if isinstance(value, list):
for index, child in enumerate(value):
child_prefix = f"{prefix}[{index}]"
unresolved.extend(collect_placeholders(child, child_prefix))
return unresolved
if has_placeholder(value):
unresolved.append(f"{prefix}: {value}")
return unresolved
def load_yaml(path: Path) -> dict[str, Any]:
if yaml is None:
raise RuntimeError("缺少 PyYAML，无法加载现场配置文件。")
with path.open("r", encoding="utf-8") as stream:
data = yaml.safe_load(stream) or {}
if not isinstance(data, dict):
raise ValueError(f"{path} 顶层必须是 YAML map。")
return data
class NoAliasDumper(yaml.SafeDumper if yaml is not None else object):
def ignore_aliases(self, data: Any) -> bool:
return True
def dump_yaml(data: Any) -> str:
if yaml is None:
return json.dumps(data, ensure_ascii=False, indent=2)
return yaml.dump(data, Dumper=NoAliasDumper, sort_keys=False, allow_unicode=True)
def resolve_profile_path(raw_path: str) -> Path:
path = Path(raw_path).expanduser()
if path.is_absolute():
return path.resolve(strict=False)
return (REPO_ROOT / path).resolve(strict=False)
def resolve_vehicle_profile_path(raw_path: str) -> Path:
path = Path(raw_path).expanduser()
if path.is_absolute():
return path.resolve(strict=False)
return (REPO_ROOT / path).resolve(strict=False)
def relative_to_repo(path: Path) -> str:
try:
return str(path.resolve(strict=False).relative_to(REPO_ROOT))
except ValueError:
return str(path.resolve(strict=False))
def vehicle_profile_filename(profile_name: str) -> str:
name = re.sub(r"[^\w\-]+", "_", profile_name.strip() or "vehicle_profile")
name = name.strip("_") or "vehicle_profile"
return f"{name}{VEHICLE_PROFILE_SAVE_EXTENSION}"
def vehicle_profile_path_from_name(profile_name: str) -> Path:
return DEFAULT_VEHICLE_PROFILE_DIR / vehicle_profile_filename(profile_name)
def shell_join(args: list[str]) -> str:
return " ".join(shlex.quote(str(arg)) for arg in args if str(arg) != "")
def launch_arg(name: str, value: str) -> str:
return f"{name}:={value}"
def is_positive_number(raw_value: str) -> bool:
try:
return float(raw_value) > 0.0
except ValueError:
return False
def float_or_none(raw_value: Any) -> float | None:
try:
return float(raw_value)
except (TypeError, ValueError):
return None
def bool_from_text(raw_value: Any) -> bool:
return str(raw_value).strip().lower() in {"1", "true", "yes", "y", "on"}
def task_params_map(task: dict[str, Any]) -> dict[str, str]:
params: dict[str, str] = {}
for item in task.get("task_params", []):
if not isinstance(item, dict):
continue
key = str(item.get("key", "")).strip()
if key:
params[key] = str(item.get("value", ""))
return params
agv_calib_brain/src/apps/operator_ui/requirements.txt
+1
View File
@@ -0,0 +1 @@
PySide6>=6.6.0
agv_calib_brain/src/apps/operator_ui/runtime_monitor.py
+255
View File
@@ -0,0 +1,255 @@
"""操作台 ROS 监听和实时定位状态。"""
from __future__ import annotations
import math
import time
try:
import rclpy
except ImportError:
rclpy = None
try:
if rclpy is None:
raise ImportError
from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry
except ImportError:
ExternalLocalizationTelemetry = None
try:
if rclpy is None:
raise ImportError
from calibration_workshop_orchestration_interfaces.msg import WorkshopEvent
except ImportError:
WorkshopEvent = None
try:
from .constants import (
LOCALIZATION_DISPLAY_ROWS,
WORKSHOP_EVENT_REPORT_READY,
WORKSHOP_EVENT_STAGE_COMPLETED,
WORKSHOP_EVENT_STAGE_FAILED,
WORKSHOP_EVENT_STAGE_STARTED,
)
from .profile_io import is_positive_number
from .ui_helpers import table_item
except ImportError:
from constants import (
LOCALIZATION_DISPLAY_ROWS,
WORKSHOP_EVENT_REPORT_READY,
WORKSHOP_EVENT_STAGE_COMPLETED,
WORKSHOP_EVENT_STAGE_FAILED,
WORKSHOP_EVENT_STAGE_STARTED,
)
from profile_io import is_positive_number
from ui_helpers import table_item
class RuntimeMonitorMixin:
def update_localization_state(self, state_text: str, status: str = "neutral", detail: str = "") -> None:
if not hasattr(self, "localization_state"):
return
zone = self.workcell_zone_edit.text().strip() or "未配置工位"
topic = self.external_topic_edit.text().strip() or "未配置"
source = self.reference_source_edit.text().strip() or "未配置"
self.localization_state.setText(f"车间定位：{state_text} · {zone}")
tooltip = f"定位话题：{topic}\n定位系统：{source}\n标定工位：{zone}"
if detail:
tooltip += f"\n最近结果：{detail}"
self.localization_state.setToolTip(tooltip)
style = {
"ok": ("#166534", "#dcfce7", "#86efac"),
"warn": ("#92400e", "#fef3c7", "#fbbf24"),
"fail": ("#991b1b", "#fee2e2", "#fca5a5"),
"running": ("#1d4ed8", "#dbeafe", "#93c5fd"),
"neutral": ("#374151", "#f3f4f6", "#d1d5db"),
}.get(status, ("#374151", "#f3f4f6", "#d1d5db"))
self.localization_state.setStyleSheet(
"QLabel {"
f"color: {style[0]}; background: {style[1]}; border: 1px solid {style[2]}; "
"border-radius: 4px; padding: 4px 8px;"
"}"
)
def update_workshop_dimension_state(self) -> None:
if not hasattr(self, "workshop_dimension_state"):
return
geometry = self.workshop_geometry_payload()
geometry_ok = all(is_positive_number(geometry[key]) for key in ("length_m", "width_m", "height_m"))
if geometry_ok:
text = f"车间尺寸：{geometry['length_m']} x {geometry['width_m']} x {geometry['height_m']} m"
color, background, border = "#166534", "#dcfce7", "#86efac"
else:
text = "车间尺寸：配置缺失"
color, background, border = "#991b1b", "#fee2e2", "#fca5a5"
self.workshop_dimension_state.setText(text)
self.workshop_dimension_state.setToolTip(
"车间尺寸来自现场配置文件 workshop_geometry，操作员界面不允许手动修改。"
)
if hasattr(self, "localization_3d_view"):
self.localization_3d_view.set_workshop_geometry(geometry)
self.workshop_dimension_state.setStyleSheet(
"QLabel {"
f"color: {color}; background: {background}; border: 1px solid {border}; "
"border-radius: 4px; padding: 4px 8px;"
"}"
)
def set_localization_display(self, values: dict[str, str], status: str | None = None) -> None:
if not hasattr(self, "localization_table"):
return
for row, name in enumerate(LOCALIZATION_DISPLAY_ROWS):
self.localization_table.setItem(row, 0, table_item(name))
self.localization_table.setItem(row, 1, table_item(values.get(name, "-"), status if name == "状态" else None))
def set_localization_message(self, summary: str, status_text: str = "-", status: str = "warn") -> None:
if hasattr(self, "localization_summary_label"):
self.localization_summary_label.setText(summary)
if hasattr(self, "localization_3d_view"):
self.localization_3d_view.set_status(summary)
self.set_localization_display({
"状态": status_text,
"定位系统": self.reference_source_edit.text().strip() or "-",
}, status)
def ensure_ros_monitor_node(self):
if rclpy is None:
return None
if not rclpy.ok():
rclpy.init(args=None)
self.localization_rclpy_initialized = True
if self.localization_node is None:
self.localization_node = rclpy.create_node("operator_ui_runtime_monitor")
if not self.localization_spin_timer.isActive():
self.localization_spin_timer.start(50)
return self.localization_node
def restart_localization_monitor(self) -> None:
topic = self.external_topic_edit.text().strip()
source = self.reference_source_edit.text().strip()
if not topic:
self.set_localization_message("现场配置文件没有定位话题。", "配置缺失", "fail")
self.update_localization_state("配置缺失", "fail")
return
self.localization_last_sample_wall_time = 0.0
if rclpy is None or ExternalLocalizationTelemetry is None:
self.set_localization_message("未连接 ROS 环境，无法订阅车间定位数据。", "未连接", "warn")
self.update_localization_state("未连接", "warn")
return
try:
node = self.ensure_ros_monitor_node()
if node is None:
return
if self.localization_subscription is not None:
node.destroy_subscription(self.localization_subscription)
self.localization_subscription = None
self.localization_subscription = node.create_subscription(
ExternalLocalizationTelemetry,
topic,
self.handle_localization_message,
10,
)
self.set_localization_message(f"正在监听车间定位数据：{source or topic}", "等待数据", "warn")
self.update_localization_state("等待数据", "warn")
except Exception as exc:
self.set_localization_message(f"车间定位订阅失败：{exc}", "订阅失败", "fail")
self.update_localization_state("订阅失败", "fail", str(exc))
def restart_workshop_event_monitor(self) -> None:
if rclpy is None or WorkshopEvent is None:
return
try:
node = self.ensure_ros_monitor_node()
if node is None:
return
if self.workshop_event_subscription is not None:
node.destroy_subscription(self.workshop_event_subscription)
self.workshop_event_subscription = None
self.workshop_event_subscription = node.create_subscription(
WorkshopEvent,
"/workshop_v2/events",
self.handle_workshop_event,
50,
)
except Exception as exc:
self.append_log(f"[界面] 订阅总控事件失败: {exc}")
def poll_localization_messages(self) -> None:
if rclpy is None or self.localization_node is None:
return
try:
rclpy.spin_once(self.localization_node, timeout_sec=0.0)
except Exception as exc:
self.localization_spin_timer.stop()
self.set_localization_message(f"车间定位读取失败：{exc}", "读取失败", "fail")
self.update_localization_state("读取失败", "fail", str(exc))
return
if self.localization_last_sample_wall_time > 0.0:
age_sec = time.time() - self.localization_last_sample_wall_time
if age_sec > 2.0:
self.update_localization_state("数据超时", "warn", f"{age_sec:.1f}s 未更新")
def handle_localization_message(self, msg) -> None:
self.localization_last_sample_wall_time = time.time()
pose = msg.workshop_pose
yaw_deg = math.degrees(float(pose.yaw_rad))
yaw_stddev_deg = math.degrees(float(msg.yaw_stddev_rad))
valid = bool(msg.pose_valid)
status_text = "正常" if valid else "位姿无效"
status = "ok" if valid else "fail"
source = msg.reference_source_name or self.reference_source_edit.text().strip()
values = {
"状态": status_text,
"更新时间": time.strftime("%H:%M:%S"),
"X(m)": f"{float(pose.x_m):.3f}",
"Y(m)": f"{float(pose.y_m):.3f}",
"Z(m)": f"{float(pose.z_m):.3f}",
"Yaw(deg)": f"{yaw_deg:.2f}",
"质量分数": f"{float(msg.quality_score):.3f}",
"位置标准差(m)": f"{float(msg.position_stddev_m):.4f}",
"航向标准差(deg)": f"{yaw_stddev_deg:.3f}",
"跟踪丢失率": f"{float(msg.tracking_loss_ratio):.3f}",
"时间同步偏差(ms)": f"{float(msg.time_sync_offset_ms):.2f}",
"目标数量": str(int(msg.observed_target_count)),
"定位系统": source or "-",
"任务 ID": msg.active_job_id or "-",
}
self.set_localization_display(values, status)
if hasattr(self, "localization_summary_label"):
self.localization_summary_label.setText(
f"x={values['X(m)']} m, y={values['Y(m)']} m, yaw={values['Yaw(deg)']} deg, 质量={values['质量分数']}"
)
if hasattr(self, "localization_3d_view"):
self.localization_3d_view.set_pose(
float(pose.x_m),
float(pose.y_m),
float(pose.z_m),
float(pose.yaw_rad),
valid,
float(msg.quality_score),
)
self.update_localization_state("实时正常" if valid else "位姿无效", status)
def handle_workshop_event(self, msg) -> None:
event_type = int(msg.event_type.value)
stage_id = str(msg.stage_id)
if event_type == WORKSHOP_EVENT_STAGE_STARTED:
self.trajectory_runtime_active = True
if not self.show_stage_trajectory(stage_id, "正在执行轨迹"):
self.show_next_trajectory_after_stage(stage_id)
return
if event_type == WORKSHOP_EVENT_STAGE_COMPLETED:
self.trajectory_runtime_active = True
self.trajectory_last_completed_stage_id = stage_id
self.show_next_trajectory_after_stage(stage_id)
return
if event_type == WORKSHOP_EVENT_STAGE_FAILED:
self.trajectory_runtime_active = True
if not self.show_stage_trajectory(stage_id, "失败阶段轨迹"):
self.show_next_trajectory_after_stage(stage_id)
return
if event_type == WORKSHOP_EVENT_REPORT_READY:
self.trajectory_runtime_active = False
self.trajectory_last_completed_stage_id = ""
self.show_all_planned_trajectories()
agv_calib_brain/src/apps/operator_ui/session_builder.py
+757
View File
@@ -0,0 +1,757 @@
"""操作台本轮任务和会话文件生成。"""
from __future__ import annotations
import math
import re
from pathlib import Path
from typing import Any
from PySide6.QtWidgets import QMessageBox
try:
from .constants import (
CHASSIS_PARAMETER_OPTIONS,
CONTROL_PARAMETER_OPTIONS,
DEFAULT_REFERENCE_PATH_DIRS,
DEFAULT_VEHICLE_PROFILE,
POLICY_DISPLAY_NAMES,
REPO_ROOT,
SENSOR_TASK_OPTIONS,
STAGE_CHASSIS,
STAGE_CONTROL,
STAGE_EXTERNAL,
STAGE_HAND_EYE,
STAGE_SENSOR_EXTRINSIC,
STAGE_SENSOR_INTRINSIC,
)
from .profile_io import (
bool_from_text,
collect_placeholders,
dump_yaml,
float_or_none,
is_positive_number,
load_yaml,
read_nested,
resolve_profile_path,
resolve_vehicle_profile_path,
task_params_map,
)
from .ui_helpers import table_item, task_item_display_name, task_stage_display_name, task_stage_id
except ImportError:
from constants import (
CHASSIS_PARAMETER_OPTIONS,
CONTROL_PARAMETER_OPTIONS,
DEFAULT_REFERENCE_PATH_DIRS,
DEFAULT_VEHICLE_PROFILE,
POLICY_DISPLAY_NAMES,
REPO_ROOT,
SENSOR_TASK_OPTIONS,
STAGE_CHASSIS,
STAGE_CONTROL,
STAGE_EXTERNAL,
STAGE_HAND_EYE,
STAGE_SENSOR_EXTRINSIC,
STAGE_SENSOR_INTRINSIC,
)
from profile_io import (
bool_from_text,
collect_placeholders,
dump_yaml,
float_or_none,
is_positive_number,
load_yaml,
read_nested,
resolve_profile_path,
resolve_vehicle_profile_path,
task_params_map,
)
from ui_helpers import table_item, task_item_display_name, task_stage_display_name, task_stage_id
class SessionBuilderMixin:
def selected_tasks(self) -> list[str]:
tasks: list[str] = []
if self.external_check.isChecked():
tasks.append("external")
if self.selected_chassis_tasks():
tasks.append("chassis")
if self.selected_control_tasks():
tasks.append("control")
sensor_stages = {task["stage_type"] for task in self.selected_sensor_tasks()}
if STAGE_SENSOR_INTRINSIC in sensor_stages:
tasks.append("sensor_intrinsic")
if STAGE_SENSOR_EXTRINSIC in sensor_stages:
tasks.append("sensor_extrinsic")
if STAGE_HAND_EYE in sensor_stages:
tasks.append("hand_eye")
return tasks
def selected_tasks_csv(self) -> str:
tasks = self.selected_tasks()
return ",".join(tasks) if tasks else "external"
def reference_path_dir(self, module: str) -> Path:
config_keys = {
"chassis": ("chassis_calibration", "reference_path_dir"),
"control": ("control_calibration", "reference_path_dir"),
"sensor": ("sensor_calibration", "reference_path_dir"),
}
raw_path = read_nested(self.site_profile, config_keys[module])
if raw_path:
path = Path(raw_path).expanduser()
if path.is_absolute():
return path.resolve(strict=False)
profile_path = resolve_profile_path(self.profile_path_edit.text().strip())
return (profile_path.parent / path).resolve(strict=False)
return DEFAULT_REFERENCE_PATH_DIRS[module]
def resolve_site_or_repo_path(self, raw_path: str) -> Path:
path = Path(raw_path).expanduser()
if path.is_absolute():
return path.resolve(strict=False)
profile_path = resolve_profile_path(self.profile_path_edit.text().strip())
profile_relative = (profile_path.parent / path).resolve(strict=False)
if profile_relative.exists():
return profile_relative
return (REPO_ROOT / path).resolve(strict=False)
def chassis_action_profile_path(self) -> Path:
raw_path = read_nested(
self.site_profile,
("chassis_calibration", "action_profile_file"),
"src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml",
)
return self.resolve_site_or_repo_path(raw_path)
def allowed_chassis_reference_path_ids(self, chassis_type: str) -> set[str]:
profile_path = self.chassis_action_profile_path()
if not profile_path.exists():
return set()
try:
profile = load_yaml(profile_path)
except Exception as exc:
self.append_log(f"[参考路径] 底盘动作 profile 加载失败 {profile_path}: {exc}")
return set()
section = profile.get("chassis_profiles", {}).get(chassis_type, {})
actions = section.get("actions", []) if isinstance(section, dict) else []
if not isinstance(actions, list):
return set()
return {
str(action.get("reference_path_id", "")).strip()
for action in actions
if isinstance(action, dict) and str(action.get("reference_path_id", "")).strip()
}
def load_reference_path_options(self, module: str) -> tuple[dict[str, Any], list[dict[str, Any]]]:
path_dir = self.reference_path_dir(module)
index_path = path_dir / "index.yaml"
index = load_yaml(index_path) if index_path.exists() else {}
paths: list[dict[str, Any]] = []
if not path_dir.exists():
return index, paths
for path_file in sorted(path_dir.glob("*.yaml")):
if path_file.name in {"index.yaml", "_index.yaml"}:
continue
try:
payload = load_yaml(path_file)
except Exception as exc:
self.append_log(f"[参考路径] 跳过 {path_file}: {exc}")
continue
path_id = str(payload.get("path_id", "")).strip()
display_name = str(payload.get("display_name", "")).strip()
points = payload.get("points", [])
if not path_id or not display_name or not isinstance(points, list) or not points:
self.append_log(f"[参考路径] 跳过无效路径文件: {path_file}")
continue
payload["_source_file"] = str(path_file.resolve(strict=False))
payload["_path_dir"] = str(path_dir.resolve(strict=False))
paths.append(payload)
if module == "chassis":
chassis_type = (
str(self.chassis_type_combo.currentData() or "ackermann")
if hasattr(self, "chassis_type_combo")
else "ackermann"
)
allowed_ids = self.allowed_chassis_reference_path_ids(chassis_type)
if allowed_ids:
def recorded_path_chassis_types(path: dict[str, Any]) -> set[str]:
raw_types = path.get("allowed_chassis_types", []) or []
if isinstance(raw_types, str):
return {raw_types}
if isinstance(raw_types, list):
return {str(item) for item in raw_types}
return set()
paths = [
path
for path in paths
if str(path.get("path_id", "")).strip() in allowed_ids
or chassis_type in recorded_path_chassis_types(path)
]
return index, paths
def default_reference_path_id(self, module: str, index: dict[str, Any], paths: list[dict[str, Any]]) -> str:
chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") if hasattr(self, "chassis_type_combo") else "ackermann"
by_chassis = index.get("default_path_id_by_chassis", {}) if isinstance(index, dict) else {}
by_task = index.get("default_path_id_by_task_type", {}) if isinstance(index, dict) else {}
if module in {"chassis", "control"} and isinstance(by_chassis, dict):
default_id = str(by_chassis.get(chassis_type, "")).strip()
if default_id:
return default_id
if isinstance(by_task, dict):
task_key = {
"chassis": "straight_line",
"control": "trajectory_tracking",
"sensor": "camera_intrinsic",
}[module]
default_id = str(by_task.get(task_key, "")).strip()
if default_id:
return default_id
return str(paths[0].get("path_id", "")) if paths else ""
def refresh_reference_path_combos(self) -> None:
combo_specs = {
"chassis": "chassis_reference_path_combo",
"control": "control_reference_path_combo",
"sensor": "sensor_reference_path_combo",
}
for module, attr_name in combo_specs.items():
if not hasattr(self, attr_name):
continue
combo = getattr(self, attr_name)
previous = combo.currentData()
previous_id = str(previous.get("path_id", "")) if isinstance(previous, dict) else ""
index, paths = self.load_reference_path_options(module)
ids = {str(path.get("path_id", "")) for path in paths}
selected_id = previous_id if previous_id in ids else self.default_reference_path_id(module, index, paths)
if selected_id not in ids and paths:
selected_id = str(paths[0].get("path_id", ""))
combo.blockSignals(True)
combo.clear()
if not paths:
combo.addItem("未找到参考路径", None)
combo.setEnabled(False)
else:
combo.setEnabled(True)
for path in paths:
label = f"{path.get('display_name', path.get('path_id'))} ({path.get('path_id')})"
combo.addItem(label, path)
if str(path.get("path_id", "")) == selected_id:
combo.setCurrentIndex(combo.count() - 1)
combo.blockSignals(False)
if hasattr(self, "launch_command_preview"):
self.refresh_command_preview()
def selected_reference_path(self, module: str) -> dict[str, Any] | None:
combo = getattr(self, f"{module}_reference_path_combo", None)
if combo is None:
return None
path = combo.currentData()
return path if isinstance(path, dict) else None
@staticmethod
def stringify_reference_value(value: Any) -> str:
if isinstance(value, bool):
return "true" if value else "false"
if isinstance(value, float):
return f"{value:.9g}"
return str(value)
def add_reference_path_metadata(self, params: dict[str, Any], path: dict[str, Any] | None) -> None:
if not path:
return
points = path.get("points", [])
if not isinstance(points, list):
return
params["reference_path.id"] = path.get("path_id", "")
params["reference_path.display_name"] = path.get("display_name", "")
params["reference_path.frame_id"] = path.get("frame_id", "workshop")
params["reference_path.path_type"] = path.get("path_type", "polyline")
params["reference_path.point_count"] = len(points)
if path.get("_path_dir"):
params["reference_path.path_dir"] = path["_path_dir"]
if path.get("_source_file"):
params["reference_path.file"] = path["_source_file"]
if path.get("module_type"):
params["reference_path.module_type"] = path["module_type"]
if path.get("description"):
params["reference_path.description"] = path["description"]
for index, point in enumerate(points):
if not isinstance(point, dict):
continue
prefix = f"reference_path.pt_{index}"
params[f"{prefix}_x_m"] = self.stringify_reference_value(point.get("x_m", 0.0))
params[f"{prefix}_y_m"] = self.stringify_reference_value(point.get("y_m", 0.0))
params[f"{prefix}_z_m"] = self.stringify_reference_value(point.get("z_m", 0.0))
params[f"{prefix}_yaw_rad"] = self.stringify_reference_value(point.get("yaw_rad", 0.0))
params[f"{prefix}_speed_ms"] = self.stringify_reference_value(point.get("target_speed_ms", 0.0))
def apply_reference_path_as_trajectory(self, params: dict[str, Any], path: dict[str, Any] | None) -> None:
if not path:
return
points = path.get("points", [])
if not isinstance(points, list) or len(points) < 2:
return
for key in list(params):
if key.startswith("traj_pt_"):
del params[key]
for index, point in enumerate(points):
if not isinstance(point, dict):
continue
params[f"traj_pt_{index}_x_m"] = self.stringify_reference_value(point.get("x_m", 0.0))
params[f"traj_pt_{index}_y_m"] = self.stringify_reference_value(point.get("y_m", 0.0))
params[f"traj_pt_{index}_yaw_rad"] = self.stringify_reference_value(point.get("yaw_rad", 0.0))
params[f"traj_pt_{index}_speed_ms"] = self.stringify_reference_value(point.get("target_speed_ms", 0.0))
def refresh_all(self) -> None:
self.refresh_command_preview()
self.refresh_task_preview_from_config()
def make_task(
self,
stage_type: str,
task_code: str,
target_id: str,
params: dict[str, Any],
reason: str,
) -> dict[str, Any]:
return {
"stage_type": stage_type,
"enabled": True,
"require_manual_approval": False,
"execution_policy": "REQUIRED",
"reason": reason,
"task_code": task_code,
"target_id": target_id,
"task_params": [
{"key": str(key), "value": str(value)}
for key, value in params.items()
],
}
def workshop_geometry_payload(self) -> dict[str, str]:
return {
"length_m": read_nested(self.site_profile, ("workshop_geometry", "length_m"), ""),
"width_m": read_nested(self.site_profile, ("workshop_geometry", "width_m"), ""),
"height_m": read_nested(self.site_profile, ("workshop_geometry", "height_m"), ""),
}
def refresh_planned_trajectory(self) -> None:
self.trajectory_task_records = self.planned_trajectory_task_records()
if self.trajectory_runtime_active:
self.show_next_trajectory_after_stage(self.trajectory_last_completed_stage_id)
else:
self.show_all_planned_trajectories()
def planned_trajectory_segments(self) -> list[list[tuple[float, float, float]]]:
return [
segment
for record in self.trajectory_task_records
for segment in record["segments"]
]
def planned_trajectory_task_records(self) -> list[dict[str, Any]]:
tasks = self.build_requested_tasks()
self.trajectory_stage_order = {
task_stage_id(task): order
for order, task in enumerate(tasks)
}
records: list[dict[str, Any]] = []
for order, task in enumerate(tasks):
params = task_params_map(task)
segments = self.trajectory_segments_from_task_params(params)
if not segments:
continue
records.append({
"order": order,
"stage_id": task_stage_id(task),
"display_name": f"{task_stage_display_name(task)}：{task_item_display_name(task)}",
"segments": segments,
})
return records
def trajectory_segments_from_task_params(self, params: dict[str, str]) -> list[list[tuple[float, float, float]]]:
segments: list[list[tuple[float, float, float]]] = []
segment = self.trajectory_points_from_reference_path(params)
if segment:
segments.append(segment)
return segments
segment = self.trajectory_points_from_explicit_params(params)
if segment:
segments.append(segment)
return segments
segment = self.trajectory_points_from_motion_primitive(params)
if segment:
segments.append(segment)
return segments
def show_all_planned_trajectories(self) -> None:
if not hasattr(self, "localization_3d_view"):
return
self.localization_3d_view.set_trajectory(self.planned_trajectory_segments(), "本轮计划轨迹")
def show_stage_trajectory(self, stage_id: str, label_prefix: str) -> bool:
if not hasattr(self, "localization_3d_view"):
return False
for record in self.trajectory_task_records:
if record["stage_id"] == stage_id:
self.localization_3d_view.set_trajectory(
record["segments"],
f"{label_prefix}：{record['display_name']}",
)
return True
return False
def show_next_trajectory_after_stage(self, stage_id: str) -> bool:
if not hasattr(self, "localization_3d_view"):
return False
completed_order = self.trajectory_stage_order.get(stage_id, -1) if stage_id else -1
for record in self.trajectory_task_records:
if int(record["order"]) > completed_order:
self.localization_3d_view.set_trajectory(
record["segments"],
f"下一项轨迹：{record['display_name']}",
)
return True
self.localization_3d_view.set_trajectory([], "后续没有计划轨迹")
return False
def trajectory_points_from_explicit_params(self, params: dict[str, str]) -> list[tuple[float, float, float]]:
indexed_points: dict[int, tuple[float, float]] = {}
indexes = sorted({
int(match.group(1))
for key in params
if (match := re.fullmatch(r"traj_pt_(\d+)_x_m", key))
})
for index in indexes:
x = float_or_none(params.get(f"traj_pt_{index}_x_m"))
y = float_or_none(params.get(f"traj_pt_{index}_y_m"))
if x is not None and y is not None:
indexed_points[index] = (x, y)
return [(x, y, 0.04) for _, (x, y) in sorted(indexed_points.items())]
def trajectory_points_from_reference_path(self, params: dict[str, str]) -> list[tuple[float, float, float]]:
indexes = sorted({
int(match.group(1))
for key in params
if (match := re.fullmatch(r"reference_path\.pt_(\d+)_x_m", key))
})
points: list[tuple[float, float, float]] = []
for index in indexes:
x = float_or_none(params.get(f"reference_path.pt_{index}_x_m"))
y = float_or_none(params.get(f"reference_path.pt_{index}_y_m"))
z = float_or_none(params.get(f"reference_path.pt_{index}_z_m"))
if x is not None and y is not None:
points.append((x, y, z if z is not None else 0.04))
return points
def trajectory_points_from_motion_primitive(self, params: dict[str, str]) -> list[tuple[float, float, float]]:
primitive_type = params.get("primitive_type", "")
if primitive_type == "straight_line":
distance = float_or_none(params.get("straight_line.target_distance_m"))
if distance is None:
return []
direction = -1.0 if bool_from_text(params.get("straight_line.reverse")) else 1.0
return [(0.0, 0.0, 0.04), (direction * abs(distance), 0.0, 0.04)]
if primitive_type == "arc":
radius = float_or_none(params.get("arc.radius_m"))
sweep_angle_deg = float_or_none(params.get("arc.sweep_angle_deg"))
if radius is None or sweep_angle_deg is None or radius <= 0.0:
return []
clockwise = bool_from_text(params.get("arc.clockwise"))
turn_sign = -1.0 if clockwise else 1.0
sweep_rad = math.radians(abs(sweep_angle_deg))
sample_count = max(8, min(64, int(abs(sweep_angle_deg) / 5.0) + 1))
points: list[tuple[float, float, float]] = []
for index in range(sample_count):
ratio = index / float(sample_count - 1)
theta = sweep_rad * ratio
x = radius * math.sin(theta)
y = turn_sign * radius * (1.0 - math.cos(theta))
points.append((x, y, 0.04))
return points
if primitive_type == "in_place_rotation":
radius = 0.25
points = []
for index in range(25):
theta = math.tau * index / 24.0
points.append((radius * math.cos(theta), radius * math.sin(theta), 0.04))
return points
return []
def external_task(self) -> dict[str, Any]:
return self.make_task(
STAGE_EXTERNAL,
"external",
self.reference_target_edit.text().strip() or "site_reference_target",
{
"external.static_sample_count": "1",
"external.dynamic_sample_count": "1",
"external.require_short_motion_segment": "false",
"external.max_position_stddev_m": "0.05",
"external.max_yaw_stddev_rad": "0.05",
"external.max_tracking_loss_ratio": "0.05",
"external.max_time_sync_offset_ms": "50.0",
"external.timeout_sec": "5.0",
},
"现场界面选择的车间定位可用性检查",
)
def selected_chassis_tasks(self) -> list[dict[str, Any]]:
if not hasattr(self, "chassis_type_combo"):
return []
chassis_type = str(self.chassis_type_combo.currentData() or "ackermann")
reference_path = self.selected_reference_path("chassis")
tasks: list[dict[str, Any]] = []
for option in CHASSIS_PARAMETER_OPTIONS[chassis_type]:
check = self.chassis_param_checks.get(str(option["code"]))
if check is None or not check.isChecked():
continue
params = {
"chassis_type": chassis_type,
"calibration_parameter": option["code"],
**option["params"],
}
params.setdefault("brake_when_finished", "true")
params.setdefault("timeout_sec", "20.0")
self.add_reference_path_metadata(params, reference_path)
tasks.append(self.make_task(
STAGE_CHASSIS,
str(option["code"]),
str(option["target"]),
params,
"现场 UI 选择的底盘标定参数",
))
return tasks
def selected_control_tasks(self) -> list[dict[str, Any]]:
if not hasattr(self, "control_param_checks"):
return []
reference_path = self.selected_reference_path("control")
tasks: list[dict[str, Any]] = []
for option in CONTROL_PARAMETER_OPTIONS:
check = self.control_param_checks.get(str(option["code"]))
if check is None or not check.isChecked():
continue
params = {
"control.axis": option["axis"],
"control.algorithm": option["algorithm"],
"calibration_parameter": option["code"],
**option["params"],
}
if params.get("control.task_type") == "trajectory_tracking":
params.setdefault(
"trajectory_tracking.required_external_pose_source_id",
self.reference_source_edit.text().strip(),
)
self.apply_reference_path_as_trajectory(params, reference_path)
self.add_reference_path_metadata(params, reference_path)
tasks.append(self.make_task(
STAGE_CONTROL,
str(option["code"]),
str(option["target"]),
params,
"现场 UI 选择的运控参数标定",
))
return tasks
def selected_sensor_tasks(self) -> list[dict[str, Any]]:
if not hasattr(self, "sensor_task_checks"):
return []
reference_path = self.selected_reference_path("sensor")
tasks: list[dict[str, Any]] = []
for sensor_key, sensor_label, subtype, label, stage_type in SENSOR_TASK_OPTIONS:
check = self.sensor_task_checks.get((sensor_key, subtype))
if check is None or not check.isChecked():
continue
sensor_edit = self.sensor_id_edits.get(sensor_key)
sensor_id = sensor_edit.text().strip() if sensor_edit is not None else ""
if not sensor_id:
continue
params: dict[str, Any] = {
"sensor.sensor_id": sensor_id,
"sensor.task_subtype": subtype,
"calibration_parameter": subtype,
}
if stage_type == STAGE_SENSOR_INTRINSIC and subtype.endswith("camera_intrinsic"):
params.update({
"camera_intrinsic.required_image_count": "1",
"camera_intrinsic.target_board_id": self.reference_target_edit.text().strip() or "site_reference_target",
"camera_intrinsic.timeout_sec": "5.0",
})
elif stage_type == STAGE_SENSOR_INTRINSIC and subtype == "imu_intrinsic":
params.update({
"imu_intrinsic.required_static_segment_count": "1",
"imu_intrinsic.required_motion_segment_count": "1",
"imu_intrinsic.timeout_sec": "5.0",
})
elif stage_type == STAGE_SENSOR_EXTRINSIC:
params.update({
"sensor_extrinsic.base_frame_id": read_nested(self.site_profile, ("frames", "base_link"), "base_link"),
"sensor_extrinsic.required_sample_count": "1",
"sensor_extrinsic.timeout_sec": "5.0",
})
elif stage_type == STAGE_HAND_EYE:
params.update({
"hand_eye.arm_id": "demo_arm",
"hand_eye.required_pose_count": "1",
"hand_eye.timeout_sec": "5.0",
})
self.add_reference_path_metadata(params, reference_path)
tasks.append(self.make_task(
stage_type,
f"sensor.{sensor_key}.{subtype}",
sensor_id,
params,
f"现场 UI 选择的传感器标定：{label}",
))
return tasks
def build_requested_tasks(self) -> list[dict[str, Any]]:
tasks: list[dict[str, Any]] = []
if self.external_check.isChecked():
tasks.append(self.external_task())
tasks.extend(self.selected_chassis_tasks())
tasks.extend(self.selected_control_tasks())
tasks.extend(self.selected_sensor_tasks())
return tasks
def build_session_payload(self) -> dict[str, Any]:
requested_tasks = self.build_requested_tasks()
workshop_geometry = self.workshop_geometry_payload()
vehicle_profile_file = self.vehicle_profile_path_edit.text().strip()
vehicle_profile = self.current_vehicle_profile_payload()
vehicle_profile["profile_file"] = vehicle_profile_file
return {
"schema_version": 1,
"source_site_profile": self.profile_path_edit.text().strip(),
"vehicle_profile_file": vehicle_profile_file,
"vehicle_profile": vehicle_profile,
"generated_by": "operator_ui",
"workshop_geometry": workshop_geometry,
"session_config": {
"auto_commit_parameters": False,
"require_manual_approval_before_commit": False,
"run_validation_after_each_stage": False,
"stop_on_first_failure": True,
"allow_optional_stage_skip": False,
"enable_auto_rollback_on_validation_failure": False,
"allow_rebuild_execution_plan": False,
"localization_source_id": self.reference_source_edit.text().strip(),
"workcell_zone_id": self.workcell_zone_edit.text().strip(),
"reference_target_id": self.reference_target_edit.text().strip() or "site_reference_target",
"workshop_geometry": workshop_geometry,
"vehicle_profile_file": vehicle_profile_file,
"vehicle_profile_name": vehicle_profile.get("profile_name", ""),
"requested_tasks": requested_tasks,
},
"requested_tasks": requested_tasks,
}
def refresh_deployment_checks(self) -> None:
rows: list[tuple[str, str, str, str]] = []
profile_path = resolve_profile_path(self.profile_path_edit.text().strip())
unresolved = collect_placeholders(self.site_profile)
rows.append(("固定车间配置", "正常" if profile_path.exists() else "失败", "已按部署配置加载" if profile_path.exists() else "部署配置文件不存在", "ok" if profile_path.exists() else "fail"))
rows.append(("未替换占位项", "正常" if not unresolved else "警告", f"{len(unresolved)} 个", "ok" if not unresolved else "warn"))
rows.append(("车辆 ID", "正常" if self.vehicle_id_edit.text().strip() else "失败", self.vehicle_id_edit.text().strip(), "ok" if self.vehicle_id_edit.text().strip() else "fail"))
vehicle_profile_path = resolve_vehicle_profile_path(self.vehicle_profile_path_edit.text().strip() or str(DEFAULT_VEHICLE_PROFILE))
rows.append(("车辆画像", "正常" if vehicle_profile_path.exists() else "失败", f"{self.vehicle_profile_name_edit.text().strip() or '-'} · {vehicle_profile_path}", "ok" if vehicle_profile_path.exists() else "fail"))
vehicle_geometry_values = [
self.vehicle_length_edit.text().strip(),
self.vehicle_width_edit.text().strip(),
self.vehicle_height_edit.text().strip(),
]
vehicle_geometry_ok = all(is_positive_number(value) for value in vehicle_geometry_values)
rows.append(("车辆外形尺寸", "正常" if vehicle_geometry_ok else "失败", f"长 {vehicle_geometry_values[0] or '-'} m，宽 {vehicle_geometry_values[1] or '-'} m，高 {vehicle_geometry_values[2] or '-'} m", "ok" if vehicle_geometry_ok else "fail"))
chassis_geometry_values = [
self.vehicle_wheel_base_edit.text().strip(),
self.vehicle_track_width_edit.text().strip(),
self.vehicle_wheel_radius_edit.text().strip(),
]
chassis_geometry_ok = all(is_positive_number(value) for value in chassis_geometry_values)
rows.append(("底盘几何画像", "正常" if chassis_geometry_ok else "失败", f"轴距 {chassis_geometry_values[0] or '-'} m，轮距 {chassis_geometry_values[1] or '-'} m，轮半径 {chassis_geometry_values[2] or '-'} m", "ok" if chassis_geometry_ok else "fail"))
geometry = self.workshop_geometry_payload()
geometry_ok = all(is_positive_number(geometry[key]) for key in ("length_m", "width_m", "height_m"))
geometry_detail = f"长 {geometry['length_m'] or '-'} m，宽 {geometry['width_m'] or '-'} m，高 {geometry['height_m'] or '-'} m"
rows.append(("车间尺寸", "正常" if geometry_ok else "失败", geometry_detail, "ok" if geometry_ok else "fail"))
rows.append(("车上 Windows 程序", "正常" if self.vehicle_host_edit.text().strip() else "失败", f"{self.vehicle_host_edit.text()}:{self.vehicle_port_edit.text()}", "ok" if self.vehicle_host_edit.text().strip() else "fail"))
data_input = self.data_input_edit.text().strip()
rows.append(("算法数据参数文件", "正常" if data_input and Path(data_input).exists() else "警告", data_input or "未填写", "ok" if data_input and Path(data_input).exists() else "warn"))
dataset_index = self.dataset_index_edit.text().strip()
rows.append(("采集数据索引文件", "正常" if dataset_index and Path(dataset_index).exists() else "警告", dataset_index or "未填写", "ok" if dataset_index and Path(dataset_index).exists() else "warn"))
rows.append(("任务选择", "正常" if self.selected_tasks() else "失败", self.selected_tasks_csv(), "ok" if self.selected_tasks() else "fail"))
self.check_table.setRowCount(len(rows))
for row, (name, status, detail, color) in enumerate(rows):
self.check_table.setItem(row, 0, table_item(name))
self.check_table.setItem(row, 1, table_item(status, color))
self.check_table.setItem(row, 2, table_item(detail))
unresolved_signature = "\n".join(unresolved)
if unresolved and unresolved_signature != self.last_unresolved_signature:
self.append_log("[界面] 现场配置文件仍有占位项，真实部署前需要替换。前几项：")
for item in unresolved[:8]:
self.append_log(f" - {item}")
self.last_unresolved_signature = unresolved_signature
def build_session_config(self) -> bool:
output_path = Path(self.session_config_output_edit.text().strip()).expanduser()
if not output_path.is_absolute():
output_path = (REPO_ROOT / output_path).resolve(strict=False)
output_path.parent.mkdir(parents=True, exist_ok=True)
payload = self.build_session_payload()
if not payload["requested_tasks"]:
QMessageBox.warning(self, "任务为空", "请至少选择一个标定任务。")
return False
geometry = payload.get("workshop_geometry", {})
if not all(is_positive_number(str(geometry.get(key, ""))) for key in ("length_m", "width_m", "height_m")):
QMessageBox.warning(self, "车间尺寸缺失", "现场配置文件缺少 workshop_geometry.length_m / width_m / height_m。")
return False
vehicle_profile_file = payload.get("vehicle_profile_file", "")
if vehicle_profile_file and not resolve_vehicle_profile_path(str(vehicle_profile_file)).exists():
QMessageBox.warning(self, "车辆画像缺失", "当前选择的车辆画像文件不存在。")
return False
vehicle_profile = payload.get("vehicle_profile", {})
vehicle_geometry = vehicle_profile.get("geometry", {}) if isinstance(vehicle_profile, dict) else {}
if not all(is_positive_number(str(vehicle_geometry.get(key, ""))) for key in ("length_m", "width_m", "height_m")):
QMessageBox.warning(self, "车辆尺寸缺失", "车辆画像缺少 geometry.length_m / width_m / height_m。")
return False
chassis_geometry = vehicle_profile.get("chassis", {}) if isinstance(vehicle_profile, dict) else {}
if not all(is_positive_number(str(chassis_geometry.get(key, ""))) for key in ("wheel_base_m", "track_width_m", "wheel_radius_m")):
QMessageBox.warning(self, "底盘画像缺失", "车辆画像缺少 chassis.wheel_base_m / track_width_m / wheel_radius_m。")
return False
output_path.write_text(dump_yaml(payload), encoding="utf-8")
self.session_config_output_edit.setText(str(output_path))
try:
self.generated_session_config = load_yaml(output_path)
except Exception as exc:
QMessageBox.critical(self, "读取失败", f"本轮任务文件已生成，但读取失败：{exc}")
return False
self.append_log(f"[本轮任务文件] 已生成：{output_path}")
self.refresh_task_preview_from_config()
self.refresh_command_preview()
return True
def default_sensor_id(self) -> str:
first = self.sensor_registry_edit.text().split(",")[0].strip()
return first or "demo_front_camera"
def refresh_task_preview_from_config(self) -> None:
tasks: list[dict[str, Any]] = []
preview_config = self.build_session_payload()
if preview_config:
session_config = preview_config.get("session_config", {})
tasks = list(session_config.get("requested_tasks", preview_config.get("requested_tasks", [])))
self.task_table.setRowCount(len(tasks))
for index, task in enumerate(tasks):
execution_policy = str(task.get("execution_policy", ""))
self.task_table.setItem(index, 0, table_item(str(index + 1)))
self.task_table.setItem(index, 1, table_item(task_stage_display_name(task)))
self.task_table.setItem(index, 2, table_item(task_item_display_name(task)))
self.task_table.setItem(index, 3, table_item(POLICY_DISPLAY_NAMES.get(execution_policy, execution_policy)))
self.session_config_preview.setPlainText(dump_yaml(preview_config) if preview_config else "尚未生成本轮任务文件。")
self.refresh_planned_trajectory()
agv_calib_brain/src/UI/standalone_dashboard.py → agv_calib_brain/src/apps/operator_ui/standalone_dashboard.py
View File
agv_calib_brain/src/apps/operator_ui/ui_helpers.py
+61
View File
@@ -0,0 +1,61 @@
"""操作台通用显示工具。"""
from __future__ import annotations
from typing import Any
from PySide6.QtCore import Qt
from PySide6.QtGui import QColor
from PySide6.QtWidgets import QTableWidgetItem
try:
from .constants import (
SENSOR_STAGE_DISPLAY_NAMES,
STAGE_DISPLAY_NAMES,
STAGE_ID_PREFIX_BY_STAGE_TYPE,
TASK_DISPLAY_NAMES,
)
except ImportError:
from constants import (
SENSOR_STAGE_DISPLAY_NAMES,
STAGE_DISPLAY_NAMES,
STAGE_ID_PREFIX_BY_STAGE_TYPE,
TASK_DISPLAY_NAMES,
)
def table_item(text: str, status: str | None = None) -> QTableWidgetItem:
item = QTableWidgetItem(text)
item.setFlags(item.flags() ^ Qt.ItemIsEditable)
if status == "ok":
item.setBackground(QColor("#dcfce7"))
elif status == "warn":
item.setBackground(QColor("#fef3c7"))
elif status == "fail":
item.setBackground(QColor("#fee2e2"))
return item
def task_stage_display_name(task: dict[str, Any]) -> str:
task_code = str(task.get("task_code", ""))
stage_type = str(task.get("stage_type", ""))
return SENSOR_STAGE_DISPLAY_NAMES.get(task_code, STAGE_DISPLAY_NAMES.get(stage_type, stage_type))
def task_item_display_name(task: dict[str, Any]) -> str:
task_code = str(task.get("task_code", ""))
name = TASK_DISPLAY_NAMES.get(task_code, task_code)
if task_code.startswith("sensor."):
target_id = str(task.get("target_id", "")).strip()
if target_id:
return f"{name}（{target_id}）"
return name
def make_task_suffix(task_code: str) -> str:
if not task_code:
return ""
return "_" + "".join(ch if ch.isalnum() else "_" for ch in task_code)
def task_stage_id(task: dict[str, Any]) -> str:
stage_type = str(task.get("stage_type", ""))
task_code = str(task.get("task_code", ""))
prefix = STAGE_ID_PREFIX_BY_STAGE_TYPE.get(stage_type, "stage_unknown")
return prefix + make_task_suffix(task_code)
agv_calib_brain/src/apps/operator_ui/vehicle_profile_dialog.py
+278
View File
@@ -0,0 +1,278 @@
"""车辆画像新建和编辑窗口。"""
from __future__ import annotations
from pathlib import Path
from typing import Any
from PySide6.QtWidgets import (
QCheckBox,
QComboBox,
QDialog,
QDialogButtonBox,
QGridLayout,
QLabel,
QLineEdit,
QMessageBox,
QVBoxLayout,
)
try:
from .constants import CHASSIS_TYPES
from .profile_io import (
bool_from_text,
dump_yaml,
is_positive_number,
read_nested,
vehicle_profile_filename,
vehicle_profile_path_from_name,
)
except ImportError:
from constants import CHASSIS_TYPES
from profile_io import (
bool_from_text,
dump_yaml,
is_positive_number,
read_nested,
vehicle_profile_filename,
vehicle_profile_path_from_name,
)
def open_vehicle_profile_dialog(window: Any, new_profile: bool) -> None:
self = window
source_payload = self.current_vehicle_profile_payload()
if new_profile:
source_payload["profile_name"] = ""
source_payload["display_name"] = ""
source_payload["manufacturer"] = ""
title = "新建车辆画像"
else:
title = "编辑车辆画像"
dialog = QDialog(self)
dialog.setWindowTitle(title)
dialog.resize(760, 560)
layout = QVBoxLayout(dialog)
grid = QGridLayout()
layout.addLayout(grid)
name_edit = QLineEdit(str(source_payload.get("profile_name", "")))
file_name_label = QLabel(vehicle_profile_filename(name_edit.text()))
model_edit = QLineEdit(str(source_payload.get("model_name", "")))
chassis_type_combo = QComboBox()
for value, label in CHASSIS_TYPES:
chassis_type_combo.addItem(label, value)
chassis_type = str(source_payload.get("chassis", {}).get("chassis_type", "ackermann"))
chassis_index = chassis_type_combo.findData(chassis_type)
if chassis_index >= 0:
chassis_type_combo.setCurrentIndex(chassis_index)
geometry = source_payload.get("geometry", {})
chassis = source_payload.get("chassis", {})
controllers = source_payload.get("controllers", {})
if not isinstance(geometry, dict):
geometry = {}
if not isinstance(chassis, dict):
chassis = {}
if not isinstance(controllers, dict):
controllers = {}
length_edit = QLineEdit(str(geometry.get("length_m", "")))
width_edit = QLineEdit(str(geometry.get("width_m", "")))
height_edit = QLineEdit(str(geometry.get("height_m", "")))
ground_clearance_edit = QLineEdit(str(geometry.get("ground_clearance_m", "")))
wheel_base_edit = QLineEdit(str(chassis.get("wheel_base_m", "")))
track_width_edit = QLineEdit(str(chassis.get("track_width_m", "")))
wheel_radius_edit = QLineEdit(str(chassis.get("wheel_radius_m", "")))
max_steering_angle_edit = QLineEdit(str(chassis.get("max_steering_angle_rad", "")))
min_turning_radius_edit = QLineEdit(str(chassis.get("min_turning_radius_m", "")))
max_speed_edit = QLineEdit(str(controllers.get("max_speed_mps", "")))
max_accel_edit = QLineEdit(str(controllers.get("max_accel_mps2", "")))
control_frequency_edit = QLineEdit(str(controllers.get("control_frequency_hz", "")))
name_edit.textChanged.connect(lambda text: file_name_label.setText(vehicle_profile_filename(text)))
grid.addWidget(QLabel("保存文件"), 0, 0)
grid.addWidget(file_name_label, 0, 1, 1, 2)
grid.addWidget(QLabel("画像名称"), 1, 0)
grid.addWidget(name_edit, 1, 1)
grid.addWidget(QLabel("车型名称"), 1, 2)
grid.addWidget(model_edit, 1, 3, 1, 2)
grid.addWidget(QLabel("底盘类型"), 2, 0)
grid.addWidget(chassis_type_combo, 2, 1, 1, 2)
grid.addWidget(QLabel("车长 m"), 3, 0)
grid.addWidget(length_edit, 3, 1)
grid.addWidget(QLabel("车宽 m"), 3, 2)
grid.addWidget(width_edit, 3, 3)
grid.addWidget(QLabel("车高 m"), 3, 4)
grid.addWidget(height_edit, 3, 5)
grid.addWidget(QLabel("离地间隙 m"), 4, 0)
grid.addWidget(ground_clearance_edit, 4, 1)
grid.addWidget(QLabel("轴距 m"), 4, 2)
grid.addWidget(wheel_base_edit, 4, 3)
grid.addWidget(QLabel("轮距 m"), 4, 4)
grid.addWidget(track_width_edit, 4, 5)
grid.addWidget(QLabel("轮半径 m"), 5, 0)
grid.addWidget(wheel_radius_edit, 5, 1)
grid.addWidget(QLabel("最大转角 rad"), 5, 2)
grid.addWidget(max_steering_angle_edit, 5, 3)
grid.addWidget(QLabel("最小转弯半径 m"), 5, 4)
grid.addWidget(min_turning_radius_edit, 5, 5)
grid.addWidget(QLabel("最大速度 m/s"), 6, 0)
grid.addWidget(max_speed_edit, 6, 1)
grid.addWidget(QLabel("最大加速度 m/s²"), 6, 2)
grid.addWidget(max_accel_edit, 6, 3)
grid.addWidget(QLabel("控制频率 Hz"), 6, 4)
grid.addWidget(control_frequency_edit, 6, 5)
sensor_fields: dict[str, tuple[QCheckBox, QLineEdit, QComboBox | None]] = {}
sensors = source_payload.get("sensors", {})
if not isinstance(sensors, dict):
sensors = {}
sensor_labels = [
("front_camera", "前视相机 ID"),
("down_camera", "下视相机 ID"),
("lidar_3d", "3D 雷达 ID"),
("lidar_2d", "2D 雷达 ID"),
("imu", "IMU ID"),
("arm_camera", "手眼相机 ID"),
]
start_row = 7
for index, (sensor_key, label) in enumerate(sensor_labels):
row = start_row + index // 2
column = (index % 2) * 3
sensor_profile = sensors.get(sensor_key, {})
sensor_id = sensor_profile.get("sensor_id", "") if isinstance(sensor_profile, dict) else ""
enabled = bool_from_text(sensor_profile.get("enabled", True)) if isinstance(sensor_profile, dict) else False
installed_check = QCheckBox(label.replace(" ID", ""))
installed_check.setChecked(enabled)
edit = QLineEdit(str(sensor_id))
edit.setEnabled(enabled)
mount_combo: QComboBox | None = None
if sensor_key == "arm_camera":
mount_combo = QComboBox()
mount_combo.addItem("眼在手上", "eye_in_hand")
mount_combo.addItem("眼在手外", "eye_to_hand")
mount_type = sensor_profile.get("camera_mount_type", "eye_in_hand") if isinstance(sensor_profile, dict) else "eye_in_hand"
combo_index = mount_combo.findData(mount_type)
mount_combo.setCurrentIndex(combo_index if combo_index >= 0 else 0)
mount_combo.setEnabled(enabled)
installed_check.stateChanged.connect(
lambda _state, line_edit=edit, combo=mount_combo, check=installed_check: (
line_edit.setEnabled(check.isChecked()),
combo.setEnabled(check.isChecked()),
)
)
else:
installed_check.stateChanged.connect(
lambda _state, line_edit=edit, check=installed_check: line_edit.setEnabled(check.isChecked())
)
sensor_fields[sensor_key] = (installed_check, edit, mount_combo)
grid.addWidget(installed_check, row, column)
if mount_combo is None:
grid.addWidget(edit, row, column + 1, 1, 2)
else:
grid.addWidget(edit, row, column + 1)
grid.addWidget(mount_combo, row, column + 2)
buttons = QDialogButtonBox()
save_btn = buttons.addButton("保存", QDialogButtonBox.AcceptRole)
close_btn = buttons.addButton("关闭", QDialogButtonBox.RejectRole)
layout.addWidget(buttons)
def build_payload() -> dict[str, Any] | None:
required_values = [
length_edit.text().strip(),
width_edit.text().strip(),
height_edit.text().strip(),
wheel_base_edit.text().strip(),
track_width_edit.text().strip(),
wheel_radius_edit.text().strip(),
]
if not all(is_positive_number(value) for value in required_values):
QMessageBox.warning(dialog, "画像不完整", "车辆长宽高、轴距、轮距、轮半径必须填写正数。")
return None
loaded_sensors = source_payload.get("sensors", {})
if not isinstance(loaded_sensors, dict):
loaded_sensors = {}
sensor_payload: dict[str, dict[str, Any]] = {}
for sensor_key, (installed_check, edit, mount_combo) in sensor_fields.items():
if not installed_check.isChecked():
continue
existing = loaded_sensors.get(sensor_key, {})
if not isinstance(existing, dict):
existing = {}
if not edit.text().strip():
QMessageBox.warning(dialog, "传感器编号缺失", f"{installed_check.text()}已选择安装，请填写编号。")
return None
mount_type = existing.get("camera_mount_type", "eye_in_hand" if sensor_key == "arm_camera" else "unspecified")
if mount_combo is not None:
mount_type = str(mount_combo.currentData() or "eye_in_hand")
sensor_payload[sensor_key] = {
"sensor_id": edit.text().strip(),
"sensor_name": existing.get("sensor_name", sensor_key),
"frame_id": existing.get("frame_id", f"{sensor_key}_link"),
"sensor_type": existing.get("sensor_type", sensor_key),
"camera_mount_type": mount_type,
"enabled": True,
"needs_intrinsic_calibration": existing.get("needs_intrinsic_calibration", sensor_key in {"front_camera", "down_camera", "imu"}),
"needs_extrinsic_calibration": existing.get("needs_extrinsic_calibration", True),
}
return {
"schema_version": 1,
"profile_name": name_edit.text().strip() or "unnamed_vehicle_profile",
"display_name": name_edit.text().strip() or "未命名车辆画像",
"model_name": model_edit.text().strip(),
"manufacturer": source_payload.get("manufacturer", ""),
"profile_version": source_payload.get("profile_version", "v1"),
"description": source_payload.get("description", ""),
"base_link_frame": read_nested(self.site_profile, ("frames", "base_link"), "base_link"),
"geometry": {
"length_m": length_edit.text().strip(),
"width_m": width_edit.text().strip(),
"height_m": height_edit.text().strip(),
"ground_clearance_m": ground_clearance_edit.text().strip(),
},
"chassis": {
"chassis_type": str(chassis_type_combo.currentData() or "ackermann"),
"wheel_base_m": wheel_base_edit.text().strip(),
"track_width_m": track_width_edit.text().strip(),
"wheel_radius_m": wheel_radius_edit.text().strip(),
"max_steering_angle_rad": max_steering_angle_edit.text().strip(),
"min_turning_radius_m": min_turning_radius_edit.text().strip(),
},
"sensors": sensor_payload,
"controllers": {
"lateral_default": controllers.get("lateral_default", "mpc"),
"longitudinal_default": controllers.get("longitudinal_default", "pid"),
"max_speed_mps": max_speed_edit.text().strip(),
"max_accel_mps2": max_accel_edit.text().strip(),
"control_frequency_hz": control_frequency_edit.text().strip(),
},
}
def save_to_path(target_path: Path) -> bool:
payload = build_payload()
if payload is None:
return False
target_path.parent.mkdir(parents=True, exist_ok=True)
target_path.write_text(dump_yaml(payload), encoding="utf-8")
self.vehicle_profile_data = payload
self.vehicle_profile_path_edit.setText(str(target_path))
self.vehicle_profile_name_edit.setText(str(payload["profile_name"]))
self.vehicle_model_name_edit.setText(str(payload["model_name"]))
self.vehicle_manufacturer_edit.setText(str(payload.get("manufacturer", "")))
self.apply_vehicle_profile_to_ui(payload)
self.refresh_vehicle_profile_combo()
self.append_log(f"[车辆画像] 已保存：{target_path}")
self.refresh_all()
return True
def save_current() -> None:
target_path = vehicle_profile_path_from_name(name_edit.text())
if save_to_path(target_path):
dialog.accept()
save_btn.clicked.connect(save_current)
close_btn.clicked.connect(dialog.reject)
dialog.exec()
agv_calib_brain/src/communication/vehicle_internal_interfaces/CMakeLists.txt
+25
View File
@@ -0,0 +1,25 @@
cmake_minimum_required(VERSION 3.8)
project(vehicle_internal_interfaces)
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(calibration_vehicle_profile_interfaces REQUIRED)
set(msg_files
"msg/VehicleControlMode.msg"
"msg/AckermannDriveCommand.msg"
"msg/VehicleSafetyCommand.msg"
"msg/AckermannActuatorState.msg"
"msg/VehicleInternalState.msg"
"msg/VehicleHealthStatus.msg"
"msg/VehicleTimeSyncStatus.msg"
"msg/SensorLinkStatus.msg"
)
rosidl_generate_interfaces(${PROJECT_NAME}
${msg_files}
DEPENDENCIES calibration_vehicle_profile_interfaces
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
agv_calib_brain/src/communication/vehicle_internal_interfaces/README.md
+17
View File
@@ -0,0 +1,17 @@
# 车辆内部接口
这个包定义车端电脑和车辆本体之间的内部 ROS2 消息。
它不直接作为车间电脑和车端电脑之间的 WiFi6 协议。WiFi6 对外协议仍由 `calibration_*_interfaces/proto` 定义。这个包用于把真实 Windows 小车的 CAN、串口、厂商 SDK 或仿真 Isaac topic 统一映射成车端内部语义。
当前消息包括：
- `AckermannDriveCommand`：阿克曼底盘速度、转角、制动、超时命令。
- `VehicleSafetyCommand`：上使能、下使能、急停、清故障、标定低速模式。
- `AckermannActuatorState`：速度、转角、轮速、电机电流、制动/油门反馈。
- `VehicleInternalState`：车辆模式、安全状态、底盘执行器状态、电源与运动状态。
- `VehicleHealthStatus`：控制器在线、总线状态、通信质量和车端资源状态。
- `VehicleTimeSyncStatus`：车端、传感器、外部真值之间的时间同步状态。
- `SensorLinkStatus`：车载传感器在线状态、帧率、丢帧和延迟。
仿真中，`vehicle_agent_sim` 会把车间电脑下发的控制请求转换为 `AckermannDriveCommand`，同时继续发布 Isaac 当前需要的 `cmd_vel`。真实部署时，Windows 车端应把这些内部消息映射到实际车辆接口。
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/AckermannActuatorState.msg
+35
View File
@@ -0,0 +1,35 @@
# =========================================================
# 阿克曼底盘内部执行器状态
# 发送方：车辆控制器 / Isaac 底盘执行器适配层
# 接收方：车端电脑
# =========================================================
# 状态时间戳
int64 hardware_timestamp_us
# 实际纵向速度
float64 actual_speed_ms
# 实际纵向加速度
float64 actual_accel_ms2
# 实际前轮等效转角
float64 actual_steering_angle_rad
# 实际转向角速度
float64 actual_steering_rate_rads
# 左后驱动轮速度
float64 rear_left_wheel_speed_ms
# 右后驱动轮速度
float64 rear_right_wheel_speed_ms
# 左前轮等效转角
float64 front_left_steering_angle_rad
# 右前轮等效转角
float64 front_right_steering_angle_rad
# 驱动电机电流
float64 drive_motor_current_amp
# 转向电机电流
float64 steering_motor_current_amp
# 制动压力或制动比例，范围 [0, 1]
float64 brake_pressure
# 驱动控制输出，范围 [0, 1]
float64 throttle_output
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/AckermannDriveCommand.msg
+33
View File
@@ -0,0 +1,33 @@
# =========================================================
# 阿克曼底盘内部控制命令
# 发送方：车端电脑
# 接收方：车辆运动控制器 / Isaac 底盘执行器适配层
# =========================================================
# 命令时间戳
int64 command_timestamp_us
# 命令 ID，用于追踪和去重
string command_id
# 命令来源，例如 vehicle_agent_sim / real_vehicle_agent
string source
# 控制模式
vehicle_internal_interfaces/VehicleControlMode control_mode
# 目标纵向速度
float64 target_speed_ms
# 目标纵向加速度；0 表示由车辆控制器默认限幅
float64 target_accel_ms2
# 目标前轮等效转角，左正右负
float64 target_steering_angle_rad
# 目标转向角速度；0 表示由车辆控制器默认限幅
float64 target_steering_rate_rads
# 制动命令，范围 [0, 1]
float64 brake_command
# 油门 / 驱动命令，范围 [0, 1]；仿真可选
float64 throttle_command
# 命令超时时间
float64 command_timeout_sec
# 是否要求控制器在超时或任务结束后停车
bool stop_when_timeout
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/SensorLinkStatus.msg
+33
View File
@@ -0,0 +1,33 @@
# =========================================================
# 车端传感器链路状态
# 发送方：车端传感器代理
# 接收方：车端电脑 / 车间电脑桥接层
# =========================================================
# 状态时间戳
int64 hardware_timestamp_us
# 传感器 ID
string sensor_id
# 传感器类型
calibration_vehicle_profile_interfaces/SensorType sensor_type
# 传感器 frame
string frame_id
# 车端订阅或驱动 topic / 通道名
string source_channel
# 是否在线
bool online
# 当前帧率
float64 frame_rate_hz
# 最近一帧年龄
float64 latest_frame_age_ms
# 累计帧数
uint64 frame_count
# 丢帧比例
float64 dropped_frame_ratio
# 最近一帧传输延迟
float64 latest_transport_latency_ms
# 当前链路状态说明
string status_message
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/VehicleControlMode.msg
+15
View File
@@ -0,0 +1,15 @@
# =========================================================
# 车辆内部控制模式
# 作用：车端电脑与车辆控制器之间约定当前车辆控制状态
# =========================================================
uint8 MODE_UNSPECIFIED=0
uint8 DISABLED=1
uint8 MANUAL=2
uint8 AUTO=3
uint8 CALIBRATION=4
uint8 ESTOP=5
uint8 FAULT=6
# 当前模式
uint8 value
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/VehicleHealthStatus.msg
+40
View File
@@ -0,0 +1,40 @@
# =========================================================
# 车辆内部健康诊断状态
# 发送方：车端电脑 / 车辆控制器
# 接收方：车端电脑内部监控或车间电脑桥接层
# =========================================================
# 状态时间戳
int64 hardware_timestamp_us
# 车端电脑进程是否在线
bool vehicle_agent_online
# 车辆主控制器是否在线
bool vehicle_controller_online
# 驱动控制器是否在线
bool drive_controller_online
# 转向控制器是否在线
bool steering_controller_online
# 传感器总线是否在线
bool sensor_bus_online
# CAN 或厂商控制链路是否在线
bool vehicle_bus_online
# 外部真值链路是否在线
bool external_truth_link_online
# 通信质量
float64 vehicle_bus_rx_hz
float64 vehicle_bus_drop_ratio
float64 command_latency_ms
float64 telemetry_latency_ms
# 车端电脑资源
float64 cpu_load_ratio
float64 memory_used_ratio
float64 disk_used_ratio
float64 temperature_c
# 诊断摘要
bool healthy
string diagnostic_code
string diagnostic_message
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/VehicleInternalState.msg
+35
View File
@@ -0,0 +1,35 @@
# =========================================================
# 车辆内部综合状态
# 发送方：车辆控制器 / Isaac 底盘执行器适配层
# 接收方：车端电脑
# =========================================================
# 状态时间戳
int64 hardware_timestamp_us
# 当前控制模式
vehicle_internal_interfaces/VehicleControlMode current_mode
# 车辆是否已上使能
bool vehicle_enabled
# 急停是否触发
bool estop_engaged
# 是否处于低速标定模式
bool calibration_low_speed_mode
# 是否存在故障
bool fault_active
# 主故障码
string primary_fault_code
# 主故障说明
string primary_fault_message
# 当前底盘状态
vehicle_internal_interfaces/AckermannActuatorState ackermann_state
# 电源状态
float64 battery_voltage_v
float64 battery_current_amp
float64 battery_soc
# 车辆运动状态
float64 yaw_rate_rads
float64 lateral_accel_ms2
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/VehicleSafetyCommand.msg
+30
View File
@@ -0,0 +1,30 @@
# =========================================================
# 车辆内部安全命令
# 发送方：车端电脑
# 接收方：车辆安全控制器 / 底盘控制器
# =========================================================
# 命令时间戳
int64 command_timestamp_us
# 命令 ID
string command_id
# 命令来源
string source
# 上使能车辆
bool enable_vehicle
# 下使能车辆
bool disable_vehicle
# 触发急停
bool engage_estop
# 解除急停
bool release_estop
# 清除可恢复故障
bool clear_faults
# 切换到标定低速安全模式
bool enter_calibration_low_speed_mode
# 退出标定低速安全模式
bool exit_calibration_low_speed_mode
# 操作原因
string reason
agv_calib_brain/src/communication/vehicle_internal_interfaces/msg/VehicleTimeSyncStatus.msg
+27
View File
@@ -0,0 +1,27 @@
# =========================================================
# 车辆内部时间同步状态
# 发送方：车端电脑
# 接收方：车间电脑桥接层 / 车端诊断
# =========================================================
# 状态时间戳
int64 hardware_timestamp_us
# 同步源，例如 ptp / ntp / external_truth / sim_clock
string sync_source
# 是否认为已同步
bool synchronized
# 车端系统时钟相对传感器硬件时钟偏差
float64 system_to_sensor_offset_ms
# 车端系统时钟相对外部真值时钟偏差
float64 system_to_external_truth_offset_ms
# 同步抖动
float64 jitter_ms
# 近期最大时间同步误差
float64 max_offset_ms
# 时间同步链路延迟
float64 sync_transport_latency_ms
# 说明
string status_message
agv_calib_brain/src/communication/vehicle_internal_interfaces/package.xml
+21
View File
@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<package format="3">
<name>vehicle_internal_interfaces</name>
<version>0.0.1</version>
<description>ROS 2 interfaces for vehicle-computer to vehicle-controller internal communication.</description>
<maintainer email="user@example.com">user</maintainer>
<license>Proprietary</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>calibration_vehicle_profile_interfaces</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/CMakeLists.txt
+73
View File
@@ -0,0 +1,73 @@
cmake_minimum_required(VERSION 3.8)
project(calibration_chassis_interfaces)
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(action_msgs REQUIRED)
find_package(calibration_common_interfaces REQUIRED)
find_package(calibration_vehicle_profile_interfaces REQUIRED)
set(msg_files
"msg/ChassisWorkMode.msg"
"msg/ChassisWorkModeRequest.msg"
"msg/ChassisReadinessResponse.msg"
"msg/ChassisCapabilityRequest.msg"
"msg/ChassisCapabilityResponse.msg"
"msg/StraightLineCommand.msg"
"msg/ArcCommand.msg"
"msg/InPlaceRotationCommand.msg"
"msg/SteeringSweepCommand.msg"
"msg/LateralTranslationCommand.msg"
"msg/DiagonalMotionCommand.msg"
"msg/ModuleAlignmentCheckCommand.msg"
"msg/CoordinatedSteeringCommand.msg"
"msg/MotionPrimitiveTaskPurpose.msg"
"msg/ChassisMotionPrimitiveType.msg"
"msg/MotionPrimitiveRequest.msg"
"msg/StreamChassisTelemetryRequest.msg"
"msg/WheelModuleState.msg"
"msg/ChassisTelemetry.msg"
"msg/CommonChassisCalibrationParams.msg"
"msg/AckermannCalibrationParams.msg"
"msg/DifferentialCalibrationParams.msg"
"msg/SingleSteerWheelCalibrationParams.msg"
"msg/SteeringModuleCalibrationParam.msg"
"msg/MultiSteerWheelCalibrationParams.msg"
"msg/ChassisSpecificParamsType.msg"
"msg/ChassisCalibrationParameterSet.msg"
"msg/ChassisValidationSummary.msg"
"msg/CommitChassisCalibrationParametersRequest.msg"
"msg/GetAppliedChassisCalibrationParametersRequest.msg"
"msg/AppliedChassisCalibrationParametersResponse.msg"
"msg/ChassisJobResult.msg"
)
set(srv_files
"srv/Heartbeat.srv"
"srv/GetChassisReadiness.srv"
"srv/SetChassisWorkMode.srv"
"srv/GetChassisCapability.srv"
"srv/StartMotionPrimitive.srv"
"srv/GetChassisJobStatus.srv"
"srv/GetChassisJobResult.srv"
"srv/CancelChassisJob.srv"
"srv/StreamChassisTelemetry.srv"
"srv/CommitChassisCalibrationParameters.srv"
"srv/GetAppliedChassisCalibrationParameters.srv"
"srv/EmergencyBrake.srv"
)
set(action_files
"action/ExecuteMotionPrimitive.action"
)
rosidl_generate_interfaces(${PROJECT_NAME}
${msg_files}
${srv_files}
${action_files}
DEPENDENCIES std_msgs action_msgs calibration_common_interfaces calibration_vehicle_profile_interfaces
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/FILE_TREE.txt
+51
View File
@@ -0,0 +1,51 @@
calibration_chassis_interfaces/
CMakeLists.txt
package.xml
msg/
AckermannCalibrationParams.msg
AppliedChassisCalibrationParametersResponse.msg
ArcCommand.msg
ChassisCalibrationParameterSet.msg
ChassisCapabilityRequest.msg
ChassisCapabilityResponse.msg
ChassisJobResult.msg
ChassisMotionPrimitiveType.msg
ChassisReadinessResponse.msg
ChassisSpecificParamsType.msg
ChassisTelemetry.msg
ChassisValidationSummary.msg
ChassisWorkMode.msg
ChassisWorkModeRequest.msg
CommitChassisCalibrationParametersRequest.msg
CommonChassisCalibrationParams.msg
CoordinatedSteeringCommand.msg
DiagonalMotionCommand.msg
DifferentialCalibrationParams.msg
GetAppliedChassisCalibrationParametersRequest.msg
InPlaceRotationCommand.msg
LateralTranslationCommand.msg
ModuleAlignmentCheckCommand.msg
MotionPrimitiveRequest.msg
MotionPrimitiveTaskPurpose.msg
MultiSteerWheelCalibrationParams.msg
SingleSteerWheelCalibrationParams.msg
SteeringModuleCalibrationParam.msg
SteeringSweepCommand.msg
StraightLineCommand.msg
StreamChassisTelemetryRequest.msg
WheelModuleState.msg
srv/
CancelChassisJob.srv
CommitChassisCalibrationParameters.srv
EmergencyBrake.srv
GetAppliedChassisCalibrationParameters.srv
GetChassisCapability.srv
GetChassisJobResult.srv
GetChassisJobStatus.srv
GetChassisReadiness.srv
Heartbeat.srv
SetChassisWorkMode.srv
StartMotionPrimitive.srv
StreamChassisTelemetry.srv
action/
ExecuteMotionPrimitive.action
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/action/ExecuteMotionPrimitive.action
+22
View File
@@ -0,0 +1,22 @@
# =========================================================
# 底盘动作原语执行 Action
# 作用：执行一个完整的底盘动作原语任务
# 来源：
# 1) StartMotionPrimitive
# 2) GetChassisJobStatus
# 3) GetChassisJobResult
# 4) CancelChassisJob
# 说明：
# 1) goal 对应 MotionPrimitiveRequest
# 2) result 对应 ChassisJobResult
# 3) feedback 统一复用 common 的 JobStatus
# =========================================================
# Goal：底盘动作原语请求
MotionPrimitiveRequest goal
---
# Result：底盘动作任务最终结果
ChassisJobResult result
---
# Feedback：执行过程中的状态反馈
calibration_common_interfaces/JobStatus feedback
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AckermannCalibrationParams.msg
+25
View File
@@ -0,0 +1,25 @@
# =========================================================
# 阿克曼专属参数
# 作用：适用于阿克曼车
# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真
# =========================================================
bool has_front_left_steer_zero_offset_deg
# 左前舵角零偏
float64 front_left_steer_zero_offset_deg
bool has_front_right_steer_zero_offset_deg
# 右前舵角零偏
float64 front_right_steer_zero_offset_deg
bool has_rear_left_wheel_radius_m
# 左后轮有效半径
float64 rear_left_wheel_radius_m
bool has_rear_right_wheel_radius_m
# 右后轮有效半径
float64 rear_right_wheel_radius_m
bool has_steering_ratio
# 转向传动比
float64 steering_ratio
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/AppliedChassisCalibrationParametersResponse.msg
+16
View File
@@ -0,0 +1,16 @@
# =========================================================
# 查询当前已生效底盘参数响应
# =========================================================
# 是否成功
bool success
# 错误码
calibration_common_interfaces/ErrorCode error_code
# 说明
string message
# 当前版本
string parameter_version
# 当前生效参数
ChassisCalibrationParameterSet params
# 生效时间
int64 applied_timestamp_us
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ArcCommand.msg
+13
View File
@@ -0,0 +1,13 @@
# =========================================================
# 圆弧动作命令
# 作用：用于验证曲线行驶能力和曲率误差
# =========================================================
# 目标线速度（m/s）
float64 target_speed_ms
# 目标半径（m）
float64 radius_m
# 圆弧扫角（度）
float64 sweep_angle_deg
# 是否顺时针
bool clockwise
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCalibrationParameterSet.msg
+24
View File
@@ -0,0 +1,24 @@
# =========================================================
# 底盘标定参数总包
# 作用：统一表达不同底盘类型的标定结果
# 说明：
# 1) proto 中 specific_params 为 oneof
# 2) ROS2 中通过 selected_specific_params + 全部 payload 字段共同表达
# =========================================================
# 底盘类型
calibration_vehicle_profile_interfaces/ChassisType chassis_type
# 通用参数
CommonChassisCalibrationParams common
# 当前生效的专属参数类型
ChassisSpecificParamsType selected_specific_params
# 阿克曼参数
AckermannCalibrationParams ackermann
# 差速参数
DifferentialCalibrationParams differential
# 单舵轮参数
SingleSteerWheelCalibrationParams single_steer
# 多舵轮参数
MultiSteerWheelCalibrationParams multi_steer
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityRequest.msg
+7
View File
@@ -0,0 +1,7 @@
# =========================================================
# 查询底盘能力请求
# 作用：运行前确认当前车辆支持哪些动作
# =========================================================
# 请求头
calibration_common_interfaces/RequestHeader header
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisCapabilityResponse.msg
+33
View File
@@ -0,0 +1,33 @@
# =========================================================
# 查询底盘能力响应
# 作用：返回当前车辆底盘形式和支持的动作原语
# =========================================================
# 是否成功
bool success
# 错误码
calibration_common_interfaces/ErrorCode error_code
# 说明
string message
# 底盘类型
calibration_vehicle_profile_interfaces/ChassisType chassis_type
# 是否支持直线动作
bool supports_straight_line
# 是否支持圆弧动作
bool supports_arc
# 是否支持原地旋转
bool supports_in_place_rotation
# 是否支持舵角扫动
bool supports_steer_sweep
# 是否支持倒车动作
bool supports_reverse_motion
# 是否支持横移动作
bool supports_lateral_translation
# 是否支持斜移动作
bool supports_diagonal_motion
# 是否支持模块零位检查
bool supports_module_alignment_check
# 是否支持模块协同转向
bool supports_coordinated_steering
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisJobResult.msg
+29
View File
@@ -0,0 +1,29 @@
# =========================================================
# 底盘任务结果
# 作用：返回底盘动作执行后的分析摘要，供编排器判断是否可进入下一步
# =========================================================
# 是否成功
bool success
# 错误码
calibration_common_interfaces/ErrorCode error_code
# 说明
string message
# 任务 ID
string job_id
# 数据质量是否达标
bool data_quality_passed
# 是否适合写入参数
bool suitable_for_commit
# 推荐参数版本
string recommended_parameter_version
# 估计的直线跑偏
float64 estimated_straight_line_bias
# 验证摘要
ChassisValidationSummary validation_summary
# 本轮估计参数
ChassisCalibrationParameterSet estimated_params
# 关联产物
calibration_common_interfaces/FileReference[] artifacts
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisMotionPrimitiveType.msg
+17
View File
@@ -0,0 +1,17 @@
# =========================================================
# 底盘动作原语类型
# 作用：补足 proto oneof primitive 的 ROS2 表达
# =========================================================
uint8 CHASSIS_MOTION_PRIMITIVE_UNSPECIFIED=0
uint8 STRAIGHT_LINE=1
uint8 ARC=2
uint8 IN_PLACE_ROTATION=3
uint8 STEERING_SWEEP=4
uint8 LATERAL_TRANSLATION=5
uint8 DIAGONAL_MOTION=6
uint8 MODULE_ALIGNMENT=7
uint8 COORDINATED_STEERING=8
# 当前动作原语取值
uint8 value
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisReadinessResponse.msg
+28
View File
@@ -0,0 +1,28 @@
# =========================================================
# 底盘服务就绪响应
# 作用：返回当前底盘服务是否满足执行条件
# =========================================================
# 是否成功
bool success
# 错误码
calibration_common_interfaces/ErrorCode error_code
# 说明
string message
# 服务本身是否就绪
bool agent_ready
# 底盘驱动是否在线
bool chassis_driver_online
# 运控链路是否可执行动作
bool motion_control_ready
# 急停是否释放
bool estop_released
# 当前车辆是否允许移动
bool vehicle_safe_to_move
# 是否可回传遥测
bool telemetry_available
# 不满足项
calibration_common_interfaces/ReadinessIssue[] issues
# 检查时间
int64 checked_timestamp_us
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisSpecificParamsType.msg
+13
View File
@@ -0,0 +1,13 @@
# =========================================================
# 底盘专属参数类型
# 作用：补足 proto oneof specific_params 的 ROS2 表达
# =========================================================
uint8 CHASSIS_SPECIFIC_PARAMS_UNSPECIFIED=0
uint8 ACKERMANN=1
uint8 DIFFERENTIAL=2
uint8 SINGLE_STEER=3
uint8 MULTI_STEER=4
# 当前专属参数类型取值
uint8 value
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisTelemetry.msg
+37
View File
@@ -0,0 +1,37 @@
# =========================================================
# 底盘遥测数据
# 作用：回传原始底盘状态供 Linux 做误差分析
# 发送方：Windows 车端代理
# 接收方：Ubuntu 车间电脑
# =========================================================
# 硬件时间戳
int64 hardware_timestamp_us
# 当前底盘类型
calibration_vehicle_profile_interfaces/ChassisType chassis_type
# 车端里程计 X（m）
float64 odom_x_m
# 车端里程计 Y（m）
float64 odom_y_m
# 车端里程计偏航角（rad）
float64 odom_yaw_rad
# 实际线速度（m/s）
float64 linear_velocity_ms
# 实际角速度（rad/s）
float64 angular_velocity_rads
# 所有轮 / 舵模块状态
WheelModuleState[] modules
# 是否急停
bool estop_engaged
# 驱动器错误码
uint32 driver_error_code
# 当前任务 ID
string active_job_id
# 估计横向滑移
float64 lateral_slip_estimate
# 估计曲率
float64 curvature_estimate
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisValidationSummary.msg
+21
View File
@@ -0,0 +1,21 @@
# =========================================================
# 底盘验证摘要
# 作用：返回底盘专项自动验收的关键指标
# =========================================================
# 最大横向误差
float64 max_lateral_error_m
# 最大航向误差
float64 max_yaw_error_rad
# 横向误差均方根
float64 rms_lateral_error_m
# 航向误差均方根
float64 rms_yaw_error_rad
# 重复性误差
float64 repeatability_error_m
# 曲率误差
float64 curvature_error
# 模块一致性误差
float64 module_consistency_error
# 自动验收是否通过
bool auto_acceptance_passed
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkMode.msg
+14
View File
@@ -0,0 +1,14 @@
# =========================================================
# 底盘工作模式
# 来源：ChassisWorkModeRequest.Mode
# 作用：切换底盘到标定准备、直接执行、验证等模式
# =========================================================
uint8 CHASSIS_MODE_UNSPECIFIED=0
uint8 NORMAL_MODE=1
uint8 CALIBRATION_READY_MODE=2
uint8 DIRECT_EXECUTION_MODE=3
uint8 VALIDATION_MODE=4
# 当前模式取值
uint8 value
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ChassisWorkModeRequest.msg
+11
View File
@@ -0,0 +1,11 @@
# =========================================================
# 底盘工作模式请求
# 作用：切换底盘到不同工作模式
# =========================================================
# 请求头
calibration_common_interfaces/RequestHeader header
# 目标模式
ChassisWorkMode target_mode
# 切换原因
string reason
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommitChassisCalibrationParametersRequest.msg
+17
View File
@@ -0,0 +1,17 @@
# =========================================================
# 写入底盘标定参数请求
# 作用：将 Linux 求解结果写入车端
# =========================================================
# 请求头
calibration_common_interfaces/RequestHeader header
# 参数版本号
string parameter_version
# 参数总包
ChassisCalibrationParameterSet params
# 写入原因
string commit_reason
# 参数摘要
calibration_common_interfaces/FileDigest digest
# 是否持久化
bool persistent_write
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CommonChassisCalibrationParams.msg
+29
View File
@@ -0,0 +1,29 @@
# =========================================================
# 通用底盘参数
# 作用：不同底盘形式都可能共用的几何 / 补偿参数
# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真
# =========================================================
bool has_effective_wheel_base_m
# 有效轴距（m）
float64 effective_wheel_base_m
bool has_effective_track_width_m
# 有效轮距（m）
float64 effective_track_width_m
bool has_longitudinal_scale
# 纵向里程比例补偿
float64 longitudinal_scale
bool has_lateral_scale
# 横向里程比例补偿
float64 lateral_scale
bool has_yaw_scale
# 航向比例补偿
float64 yaw_scale
bool has_straight_line_bias
# 直线跑偏补偿
float64 straight_line_bias
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/CoordinatedSteeringCommand.msg
+11
View File
@@ -0,0 +1,11 @@
# =========================================================
# 模块协同转向命令
# 作用：用于多舵轮模块协同转向与一致性测试
# =========================================================
# 参与测试的模块 ID 列表
string[] module_ids
# 目标舵角（度）
float64 target_angle_deg
# 保持时长（秒）
float64 hold_time_sec
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DiagonalMotionCommand.msg
+11
View File
@@ -0,0 +1,11 @@
# =========================================================
# 斜移动作命令
# 作用：用于多舵轮底盘斜向运动能力测试
# =========================================================
# 目标速度（m/s）
float64 target_speed_ms
# 目标距离（m）
float64 target_distance_m
# 目标运动方向角（度）
float64 heading_deg
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/DifferentialCalibrationParams.msg
+25
View File
@@ -0,0 +1,25 @@
# =========================================================
# 差速专属参数
# 作用：适用于差速 AGV
# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真
# =========================================================
bool has_left_wheel_radius_m
# 左轮有效半径
float64 left_wheel_radius_m
bool has_right_wheel_radius_m
# 右轮有效半径
float64 right_wheel_radius_m
bool has_axle_track_width_m
# 驱动轮间距
float64 axle_track_width_m
bool has_left_encoder_scale
# 左编码器比例系数
float64 left_encoder_scale
bool has_right_encoder_scale
# 右编码器比例系数
float64 right_encoder_scale
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/GetAppliedChassisCalibrationParametersRequest.msg
+6
View File
@@ -0,0 +1,6 @@
# =========================================================
# 查询当前已生效底盘参数请求
# =========================================================
# 请求头
calibration_common_interfaces/RequestHeader header
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/InPlaceRotationCommand.msg
+9
View File
@@ -0,0 +1,9 @@
# =========================================================
# 原地旋转动作命令
# 作用：用于差速、多舵轮等底盘的旋转能力测试
# =========================================================
# 目标旋转角（度）
float64 target_yaw_deg
# 目标角速度（度/秒）
float64 target_angular_vel_deg_s
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/LateralTranslationCommand.msg
+11
View File
@@ -0,0 +1,11 @@
# =========================================================
# 横移动作命令
# 作用：用于多舵轮底盘横向平移能力测试
# =========================================================
# 目标横移速度（m/s）
float64 target_speed_ms
# 目标横移距离（m）
float64 target_distance_m
# 是否向左移动
bool move_left
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/ModuleAlignmentCheckCommand.msg
+11
View File
@@ -0,0 +1,11 @@
# =========================================================
# 模块零位检查命令
# 作用：用于多舵轮每个舵轮模块零位与安装状态检查
# =========================================================
# 待检查模块 ID 列表
string[] module_ids
# 目标零位角（度）
float64 target_zero_deg
# 允许误差（度）
float64 tolerance_deg
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveRequest.msg
+44
View File
@@ -0,0 +1,44 @@
# =========================================================
# 底盘动作原语请求
# 作用：让车端执行一个标准化动作
# 发送方：Ubuntu 车间电脑
# 接收方：Windows 车端代理
# 说明：
# 1) proto 中使用 oneof primitive
# 2) ROS2 中通过 selected_primitive + 全部 payload 字段共同表达
# =========================================================
# 请求头
calibration_common_interfaces/RequestHeader header
# 测试用例 ID
string test_case_id
# 任务目的
MotionPrimitiveTaskPurpose task_purpose
# 当前生效的动作原语类型
ChassisMotionPrimitiveType selected_primitive
# 直线动作
StraightLineCommand straight_line
# 圆弧动作
ArcCommand arc
# 原地旋转
InPlaceRotationCommand in_place_rotation
# 舵角扫动
SteeringSweepCommand steering_sweep
# 横移
LateralTranslationCommand lateral_translation
# 斜移
DiagonalMotionCommand diagonal_motion
# 模块零位检查
ModuleAlignmentCheckCommand module_alignment
# 模块协同转向
CoordinatedSteeringCommand coordinated_steering
# 结束后是否刹停
bool brake_when_finished
# 任务超时时间
float64 timeout_sec
# 来源迭代号
string source_iteration_id
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MotionPrimitiveTaskPurpose.msg
+13
View File
@@ -0,0 +1,13 @@
# =========================================================
# 底盘动作任务目的
# 来源：MotionPrimitiveRequest.TaskPurpose
# 作用：区分数据采集、参数验证和直接检查
# =========================================================
uint8 CHASSIS_TASK_PURPOSE_UNSPECIFIED=0
uint8 DATA_COLLECTION=1
uint8 VALIDATION=2
uint8 DIRECT_CHECK=3
# 当前任务目的取值
uint8 value
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/MultiSteerWheelCalibrationParams.msg
+7
View File
@@ -0,0 +1,7 @@
# =========================================================
# 多舵轮专属参数
# 作用：适用于多舵轮 AGV
# =========================================================
# 各舵轮模块参数
SteeringModuleCalibrationParam[] modules
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SingleSteerWheelCalibrationParams.msg
+21
View File
@@ -0,0 +1,21 @@
# =========================================================
# 单舵轮专属参数
# 作用：适用于单舵轮 AGV
# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真
# =========================================================
bool has_drive_wheel_radius_m
# 驱动轮有效半径
float64 drive_wheel_radius_m
bool has_steer_zero_offset_deg
# 舵角零偏
float64 steer_zero_offset_deg
bool has_steering_ratio
# 转向比
float64 steering_ratio
bool has_drive_encoder_scale
# 驱动编码器比例
float64 drive_encoder_scale
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringModuleCalibrationParam.msg
+24
View File
@@ -0,0 +1,24 @@
# =========================================================
# 多舵轮单模块参数
# 作用：用于多舵轮车每个模块的独立参数
# 说明：proto optional 在 ROS2 中用 has_xxx + xxx 保真
# =========================================================
# 模块 ID
string module_id
bool has_wheel_radius_m
# 模块轮半径
float64 wheel_radius_m
bool has_steer_zero_offset_deg
# 模块舵角零偏
float64 steer_zero_offset_deg
bool has_module_pos_x_m
# 模块在 base_link 下的 X
float64 module_pos_x_m
bool has_module_pos_y_m
# 模块在 base_link 下的 Y
float64 module_pos_y_m
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/SteeringSweepCommand.msg
+13
View File
@@ -0,0 +1,13 @@
# =========================================================
# 舵角扫动命令
# 作用：用于阿克曼、单舵轮、多舵轮的转向零位和转向响应测试
# =========================================================
# 目标舵角（度）
float64 target_angle_deg
# 扫动幅值（度）
float64 sweep_amplitude_deg
# 扫动频率（Hz）
float64 sweep_frequency_hz
# 持续时长（秒）
float64 duration_sec
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StraightLineCommand.msg
+11
View File
@@ -0,0 +1,11 @@
# =========================================================
# 直线动作命令
# 作用：用于验证直线行驶能力和线速度误差
# =========================================================
# 目标线速度（m/s）
float64 target_speed_ms
# 目标距离（m）
float64 target_distance_m
# 是否倒车
bool reverse
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/StreamChassisTelemetryRequest.msg
+17
View File
@@ -0,0 +1,17 @@
# =========================================================
# 底盘遥测流请求
# 作用：指定底盘遥测内容和频率
# =========================================================
# 请求头
calibration_common_interfaces/RequestHeader header
# 期望上报频率
uint32 expected_hz
# 是否包含里程计
bool include_odom
# 是否包含轮组状态
bool include_wheel_state
# 是否包含驱动器状态
bool include_driver_state
# 是否包含模块级状态
bool include_module_state
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/msg/WheelModuleState.msg
+15
View File
@@ -0,0 +1,15 @@
# =========================================================
# 单个轮 / 舵模块状态
# 作用：兼容差速、单舵轮、多舵轮、阿克曼等形式
# =========================================================
# 模块 ID，例如 fl / fr / drive_center
string module_id
# 编码器累计脉冲
int64 encoder_ticks
# 当前轮速（RPM）
float64 wheel_speed_rpm
# 当前舵角（度），无舵角则可置 0
float64 steer_angle_deg
# 电机电流（A）
float64 motor_current_amp
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<package format="3">
<name>calibration_chassis_interfaces</name>
<version>0.0.1</version>
<description>ROS 2 interface package generated from chassis_calibration.proto.</description>
<maintainer email="user@example.com">user</maintainer>
<license>Proprietary</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>builtin_interfaces</depend>
<depend>std_msgs</depend>
<depend>action_msgs</depend>
<depend>calibration_common_interfaces</depend>
<depend>calibration_vehicle_profile_interfaces</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
agv_calib_brain/src/win_ubuntu_bridge/proto/chassis_calibration.proto → agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/proto/chassis_calibration.proto
+116 -10
View File
@@ -5,13 +5,22 @@ package agv.calibration.chassis;
import "calibration_common.proto";
import "vehicle_profile.proto";
// =========================================================
// 单位约定：
// 1) 所有“状态量、结果量、误差量、位姿量”中的角度统一使用 rad
// 2) 所有“人工输入更直观的命令量、限幅量、目标角度”允许使用 deg
// 3) 字段名必须显式带单位后缀，例如 _rad / _deg / _m / _ms
// 4) 服务端与客户端都必须严格按字段后缀做单位转换，禁止隐式假定
// =========================================================
// =========================================================
// 文件作用：底盘标定执行代理协议
// 运行关系：
// Ubuntu 车间电脑(Linux) -> Windows 车端代理
// 说明：
// 1) Linux 负责底盘参数求解、误差分析、验证判断
// 1) Linux 负责底盘参数求解、误差分析、自动验收、验证判断
// 2) Windows 车端只负责执行动作、回传原始状态、写入参数
// 3) 该协议同时覆盖：底盘预检 / 数据采集 / 参数验证 / 参数写入
// =========================================================
// =========================================================
@@ -24,6 +33,10 @@ service AgvCalibChassisService {
rpc Heartbeat(.agv.calibration.common.HeartbeatRequest)
returns (.agv.calibration.common.HeartbeatResponse);
// 查询底盘服务当前是否具备执行条件
rpc GetChassisReadiness(.agv.calibration.common.AgentReadinessRequest)
returns (ChassisReadinessResponse);
// 设置底盘标定工作模式
rpc SetChassisWorkMode(ChassisWorkModeRequest)
returns (.agv.calibration.common.StandardResponse);
@@ -67,7 +80,7 @@ service AgvCalibChassisService {
// =========================================================
// 底盘工作模式
// 作用：切换底盘到标定准备、直接执行等模式
// 作用：切换底盘到标定准备、直接执行、验证等模式
// =========================================================
message ChassisWorkModeRequest {
.agv.calibration.common.RequestHeader header = 1; // 请求头
@@ -84,6 +97,25 @@ message ChassisWorkModeRequest {
string reason = 3; // 切换原因
}
// =========================================================
// 底盘服务就绪响应
// 作用：返回当前底盘服务是否满足执行条件
// =========================================================
message ChassisReadinessResponse {
bool success = 1; // 是否成功
.agv.calibration.common.ErrorCode error_code = 2; // 错误码
string message = 3; // 说明
bool agent_ready = 4; // 服务本身是否就绪
bool chassis_driver_online = 5; // 底盘驱动是否在线
bool motion_control_ready = 6; // 运控链路是否可执行动作
bool estop_released = 7; // 急停是否释放
bool vehicle_safe_to_move = 8; // 当前车辆是否允许移动
bool telemetry_available = 9; // 是否可回传遥测
repeated .agv.calibration.common.ReadinessIssue issues = 10; // 不满足项
int64 checked_timestamp_us = 11; // 检查时间
}
// =========================================================
// 查询底盘能力请求
// 作用：运行前确认当前车辆支持哪些动作
@@ -101,11 +133,17 @@ message ChassisCapabilityResponse {
.agv.calibration.common.ErrorCode error_code = 2; // 错误码
string message = 3; // 说明
.agv.calibration.vehicle.profile.ChassisType chassis_type = 4; // 底盘类型
bool supports_straight_line = 5; // 是否支持直线动作
bool supports_arc = 6; // 是否支持圆弧动作
bool supports_in_place_rotation = 7; // 是否支持原地旋转
bool supports_steer_sweep = 8; // 是否支持舵角扫动
bool supports_reverse_motion = 9; // 是否支持倒车动作
bool supports_lateral_translation = 10; // 是否支持横移动作
bool supports_diagonal_motion = 11; // 是否支持斜移动作
bool supports_module_alignment_check = 12; // 是否支持模块零位检查
bool supports_coordinated_steering = 13; // 是否支持模块协同转向
}
// =========================================================
@@ -149,6 +187,46 @@ message SteeringSweepCommand {
double duration_sec = 4; // 持续时长（秒）
}
// =========================================================
// 横移动作命令
// 作用：用于多舵轮底盘横向平移能力测试
// =========================================================
message LateralTranslationCommand {
double target_speed_ms = 1; // 目标横移速度（m/s）
double target_distance_m = 2; // 目标横移距离（m）
bool move_left = 3; // 是否向左移动
}
// =========================================================
// 斜移动作命令
// 作用：用于多舵轮底盘斜向运动能力测试
// =========================================================
message DiagonalMotionCommand {
double target_speed_ms = 1; // 目标速度（m/s）
double target_distance_m = 2; // 目标距离（m）
double heading_deg = 3; // 目标运动方向角（度）
}
// =========================================================
// 模块零位检查命令
// 作用：用于多舵轮每个舵轮模块零位与安装状态检查
// =========================================================
message ModuleAlignmentCheckCommand {
repeated string module_ids = 1; // 待检查模块 ID 列表
double target_zero_deg = 2; // 目标零位角（度）
double tolerance_deg = 3; // 允许误差（度）
}
// =========================================================
// 模块协同转向命令
// 作用：用于多舵轮模块协同转向与一致性测试
// =========================================================
message CoordinatedSteeringCommand {
repeated string module_ids = 1; // 参与测试的模块 ID 列表
double target_angle_deg = 2; // 目标舵角（度）
double hold_time_sec = 3; // 保持时长（秒）
}
// =========================================================
// 底盘动作原语请求
// 作用：让车端执行一个标准化动作
@@ -171,13 +249,17 @@ message MotionPrimitiveRequest {
oneof primitive {
StraightLineCommand straight_line = 4; // 直线动作
ArcCommand arc = 5; // 圆弧动作
InPlaceRotationCommand in_place_rotation = 6;// 原地旋转
InPlaceRotationCommand in_place_rotation = 6; // 原地旋转
SteeringSweepCommand steering_sweep = 7; // 舵角扫动
LateralTranslationCommand lateral_translation = 8; // 横移
DiagonalMotionCommand diagonal_motion = 9; // 斜移
ModuleAlignmentCheckCommand module_alignment = 10; // 模块零位检查
CoordinatedSteeringCommand coordinated_steering = 11; // 模块协同转向
}
bool brake_when_finished = 8; // 结束后是否刹停
double timeout_sec = 9; // 任务超时时间
string source_iteration_id = 10; // 来源迭代号
bool brake_when_finished = 12; // 结束后是否刹停
double timeout_sec = 13; // 任务超时时间
string source_iteration_id = 14; // 来源迭代号
}
// =========================================================
@@ -190,6 +272,7 @@ message StreamChassisTelemetryRequest {
bool include_odom = 3; // 是否包含里程计
bool include_wheel_state = 4; // 是否包含轮组状态
bool include_driver_state = 5; // 是否包含驱动器状态
bool include_module_state = 6; // 是否包含模块级状态
}
// =========================================================
@@ -213,15 +296,21 @@ message WheelModuleState {
message ChassisTelemetry {
int64 hardware_timestamp_us = 1; // 硬件时间戳
.agv.calibration.vehicle.profile.ChassisType chassis_type = 2; // 当前底盘类型
double odom_x_m = 3; // 车端里程计 X（m）
double odom_y_m = 4; // 车端里程计 Y（m）
double odom_yaw_rad = 5; // 车端里程计偏航角（rad）
double linear_velocity_ms = 6; // 实际线速度（m/s）
double angular_velocity_rads = 7; // 实际角速度（rad/s）
repeated WheelModuleState modules = 8; // 所有轮 / 舵模块状态
bool estop_engaged = 9; // 是否急停
uint32 driver_error_code = 10; // 驱动器错误码
string active_job_id = 11; // 当前任务 ID
double lateral_slip_estimate = 12; // 估计横向滑移
double curvature_estimate = 13; // 估计曲率
}
// =========================================================
@@ -302,12 +391,27 @@ message ChassisCalibrationParameterSet {
oneof specific_params {
AckermannCalibrationParams ackermann = 3; // 阿克曼参数
DifferentialCalibrationParams differential = 4;// 差速参数
DifferentialCalibrationParams differential = 4; // 差速参数
SingleSteerWheelCalibrationParams single_steer = 5; // 单舵轮参数
MultiSteerWheelCalibrationParams multi_steer = 6; // 多舵轮参数
}
}
// =========================================================
// 底盘验证摘要
// 作用：返回底盘专项自动验收的关键指标
// =========================================================
message ChassisValidationSummary {
double max_lateral_error_m = 1; // 最大横向误差
double max_yaw_error_rad = 2; // 最大航向误差
double rms_lateral_error_m = 3; // 横向误差均方根
double rms_yaw_error_rad = 4; // 航向误差均方根
double repeatability_error_m = 5; // 重复性误差
double curvature_error = 6; // 曲率误差
double module_consistency_error = 7; // 模块一致性误差
bool auto_acceptance_passed = 8; // 自动验收是否通过
}
// =========================================================
// 写入底盘标定参数请求
// 作用：将 Linux 求解结果写入车端
@@ -349,11 +453,13 @@ message ChassisJobResult {
.agv.calibration.common.ErrorCode error_code = 2; // 错误码
string message = 3; // 说明
string job_id = 4; // 任务 ID
bool data_quality_passed = 5; // 数据质量是否达标
bool suitable_for_commit = 6; // 是否适合写入参数
string recommended_parameter_version = 7; // 推荐参数版本
double max_lateral_error_m = 8; // 最大横向误差
double max_yaw_error_rad = 9; // 最大航向误差
double estimated_straight_line_bias = 10; // 估计的直线跑偏
double estimated_straight_line_bias = 8; // 估计的直线跑偏
ChassisValidationSummary validation_summary = 9; // 验证摘要
ChassisCalibrationParameterSet estimated_params = 10; // 本轮估计参数
repeated .agv.calibration.common.FileReference artifacts = 11; // 关联产物
}
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CancelChassisJob.srv
+7
View File
@@ -0,0 +1,7 @@
# =========================================================
# 取消底盘任务
# =========================================================
calibration_common_interfaces/JobQuery request
---
calibration_common_interfaces/StandardResponse response
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/srv/CommitChassisCalibrationParameters.srv
+7
View File
@@ -0,0 +1,7 @@
# =========================================================
# 写入底盘标定结果
# =========================================================
CommitChassisCalibrationParametersRequest request
---
calibration_common_interfaces/StandardResponse response
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/srv/EmergencyBrake.srv
+12
View File
@@ -0,0 +1,12 @@
# =========================================================
# 紧急刹停
# 对应 proto：
# rpc EmergencyBrake(.agv.calibration.common.Empty)
# returns (.agv.calibration.common.StandardResponse);
# 说明：
# 1) 不自定义 Empty.msg
# 2) 这里使用空请求 section
# =========================================================
---
calibration_common_interfaces/StandardResponse response
agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_chassis_interfaces/srv/GetAppliedChassisCalibrationParameters.srv
+7
View File
@@ -0,0 +1,7 @@
# =========================================================
# 查询当前已生效底盘标定参数
# =========================================================
GetAppliedChassisCalibrationParametersRequest request
---
AppliedChassisCalibrationParametersResponse response

Some files were not shown because too many files have changed in this diff Show More