Compare commits
10
Commits
efc644a77d
...
f2e5a4bb7d
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
f2e5a4bb7d | ||
|
|
a713b5d9ab | ||
|
|
d708e1bbea | ||
|
|
93fa1b64b3 | ||
|
|
fd04f869b1 | ||
|
|
6593ab0e67 | ||
|
|
df884252ff | ||
|
|
1cc1ed1544 | ||
|
|
f82c3ad12a | ||
|
|
672c77726d |
@@ -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 环境配置、基础仿真运行 |
|
||||
@@ -0,0 +1,2 @@
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
@@ -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>
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:ac7fb8e48883155d1feffe880c55a972c5562b34bc340f0002ff80965d20cfac
|
||||
size 75606548
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 73 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 14 KiB |
@@ -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"
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
+168
@@ -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"
|
||||
}
|
||||
}
|
||||
+168
@@ -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"
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
@@ -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` 描述,支持多车型
|
||||
@@ -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
|
||||
```
|
||||
@@ -0,0 +1,50 @@
|
||||
# 自动化标定流程图(当前实现状态)
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A[启动 minimal_workshop_demo.launch.py] --> B[vehicle_profile_manager\n最小画像服务]
|
||||
A --> C[external_localization_service\n最小外部真值校核服务]
|
||||
A --> D[chassis_calibration_service\n最小底盘 stub]
|
||||
A --> E[control_calibration_service\n最小运控 stub]
|
||||
A --> F[sensor_calibration_service\n最小传感器 stub]
|
||||
A --> G[workshop_orchestrator_v2\n总控编排器]
|
||||
|
||||
B --> H[create_session]
|
||||
H --> I[PlanBuilder 生成 stage_plan]
|
||||
I --> J[execute_session]
|
||||
J --> K[PrecheckRunner 预检]
|
||||
K --> L{是否通过预检}
|
||||
L -- 否 --> M[生成失败报告\nfinish_session_precheck_failed]
|
||||
L -- 是 --> N[按阶段执行]
|
||||
|
||||
N --> O[外部真值校核\nexternal_localization_client]
|
||||
N --> P[底盘标定\nchassis_gateway_client]
|
||||
N --> Q[运控参数调优\ncontrol_gateway_client]
|
||||
N --> R[传感器标定\nsensor_gateway_client]
|
||||
|
||||
O --> O1[external_localization_service\nreadiness + action]
|
||||
P --> P1[chassis_calibration_service\nreadiness + action]
|
||||
Q --> Q1[control_calibration_service\nreadiness + action]
|
||||
R --> R1[sensor_calibration_service\nreadiness + action]
|
||||
|
||||
O1 --> S[收集 StageResultSummary]
|
||||
P1 --> S
|
||||
Q1 --> S
|
||||
R1 --> S
|
||||
|
||||
S --> T[ReportBuilder 生成 WorkshopReport]
|
||||
T --> U[get_report 查询最终报告]
|
||||
|
||||
classDef done fill:#d1fae5,stroke:#059669,color:#064e3b;
|
||||
classDef stub fill:#fef3c7,stroke:#d97706,color:#92400e;
|
||||
classDef core fill:#dbeafe,stroke:#2563eb,color:#1e3a8a;
|
||||
|
||||
class B,C,G,H,I,J,K,L,M,N,O,P,Q,R,S,T,U core;
|
||||
class D,E,F,O1,P1,Q1,R1 stub;
|
||||
```
|
||||
|
||||
## 说明
|
||||
|
||||
- **核心编排已完成**:session、plan、precheck、stage dispatch、report。
|
||||
- **专项执行端目前是最小 stub**:能接 readiness / action,主要用于跑通链路。
|
||||
- **当前可验证闭环**:`create_session -> execute_session -> get_report`。
|
||||
@@ -18,7 +18,22 @@ BUILD_ARGS=(
|
||||
)
|
||||
|
||||
# 如需部分包编译,把 xxx 换成包名后取消下一行注释
|
||||
# BUILD_ARGS+=(--packages-select autoware_pose_initializer)
|
||||
BUILD_ARGS+=(--packages-select calibration_chassis_interfaces \
|
||||
calibration_control_interfaces \
|
||||
calibration_common_interfaces \
|
||||
calibration_external_localization_interfaces \
|
||||
calibration_sensor_interfaces \
|
||||
calibration_vehicle_profile_interfaces \
|
||||
calibration_workshop_orchestration_interfaces \
|
||||
workshop_orchestrator_v2 \
|
||||
vehicle_profile_manager \
|
||||
external_localization_service \
|
||||
chassis_calibration_service \
|
||||
control_calibration_service \
|
||||
sensor_calibration_service \
|
||||
vehicle_agent_gateway \
|
||||
win_ubuntu_bridge
|
||||
)
|
||||
|
||||
log "Starting colcon build..."
|
||||
if AUTOWARE_COMPILE_WITH_CUDA=1 "${BUILD_ARGS[@]}"; then
|
||||
@@ -33,5 +48,3 @@ if AUTOWARE_COMPILE_WITH_CUDA=1 "${BUILD_ARGS[@]}"; then
|
||||
else
|
||||
warn "Build failed, skip sourcing."
|
||||
fi
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
# 标定系统分层架构图
|
||||
|
||||
```mermaid
|
||||
flowchart TB
|
||||
UI[UI / 上层系统] --> ORCH[workshop_orchestrator_v2\n总控编排器]
|
||||
|
||||
ORCH -->|create_session| PLAN[PlanBuilder\n生成阶段计划]
|
||||
ORCH -->|execute_session| PRECHECK[PrecheckRunner\n执行前检查]
|
||||
ORCH -->|stage dispatch| GW[各专项 Gateway Client]
|
||||
ORCH -->|report| RPT[ReportBuilder\n生成最终报告]
|
||||
|
||||
GW --> EXT[external_localization_service]
|
||||
GW --> CHA[chassis_calibration_service]
|
||||
GW --> CON[control_calibration_service]
|
||||
GW --> SEN[sensor_calibration_service]
|
||||
GW --> VPM[vehicle_profile_manager]
|
||||
|
||||
subgraph SERVICE[专项包:ROS 接口层]
|
||||
EXT1[action/service/readiness]
|
||||
CHA1[action/service/readiness]
|
||||
CON1[action/service/readiness]
|
||||
SEN1[action/service/readiness]
|
||||
VPM1[profile query / update / applicability]
|
||||
end
|
||||
|
||||
EXT --> EXT1
|
||||
CHA --> CHA1
|
||||
CON --> CON1
|
||||
SEN --> SEN1
|
||||
VPM --> VPM1
|
||||
|
||||
subgraph ALG[专项包:算法执行层]
|
||||
EXTALG[外部真值校核算法]
|
||||
CHAALG[底盘标定算法]
|
||||
CONALG[运控评估算法]
|
||||
SENALG[传感器标定算法]
|
||||
end
|
||||
|
||||
EXT1 --> EXTALG
|
||||
CHA1 --> CHAALG
|
||||
CON1 --> CONALG
|
||||
SEN1 --> SENALG
|
||||
|
||||
EXTALG -->|结果翻译| EXT1
|
||||
CHAALG -->|结果翻译| CHA1
|
||||
CONALG -->|结果翻译| CON1
|
||||
SENALG -->|结果翻译| SEN1
|
||||
|
||||
classDef core fill:#dbeafe,stroke:#2563eb,color:#1e3a8a;
|
||||
classDef service fill:#fef3c7,stroke:#d97706,color:#92400e;
|
||||
classDef algo fill:#d1fae5,stroke:#059669,color:#064e3b;
|
||||
|
||||
class ORCH,PLAN,PRECHECK,RPT,GW core;
|
||||
class EXT,CHA,CON,SEN,VPM,EXT1,CHA1,CON1,SEN1,VPM1 service;
|
||||
class EXTALG,CHAALG,CONALG,SENALG algo;
|
||||
```
|
||||
|
||||
## 说明
|
||||
|
||||
- **总控编排器** 只管流程,不直接承载专项算法。
|
||||
- **专项包** 对外暴露固定 ROS 接口,内部再挂算法执行层。
|
||||
- **算法层** 可以独立替换,只要输入/输出协议不变。
|
||||
- **topic / service / action** 用来解耦流程和算法实现。
|
||||
@@ -0,0 +1,49 @@
|
||||
# 标定系统架构对照图
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
subgraph NOW[当前实现]
|
||||
UI1[UI / 上层系统] --> ORCH1[workshop_orchestrator_v2\n总控编排器]
|
||||
ORCH1 --> PLAN1[PlanBuilder / PrecheckRunner / ReportBuilder]
|
||||
ORCH1 --> GW1[Gateway Clients]
|
||||
GW1 --> EXT1[external_localization_service\n最小 stub]
|
||||
GW1 --> CHA1[chassis_calibration_service\n最小 stub]
|
||||
GW1 --> CON1[control_calibration_service\n最小 stub]
|
||||
GW1 --> SEN1[sensor_calibration_service\n最小 stub]
|
||||
ORCH1 --> REP1[get_report]
|
||||
end
|
||||
|
||||
subgraph FUTURE[未来算法接入]
|
||||
UI2[UI / 上层系统] --> ORCH2[workshop_orchestrator_v2\n总控编排器]
|
||||
ORCH2 --> PLAN2[PlanBuilder / PrecheckRunner / ReportBuilder]
|
||||
ORCH2 --> GW2[Gateway Clients]
|
||||
GW2 --> EXT2[external_localization_service\nROS 接口层]
|
||||
GW2 --> CHA2[chassis_calibration_service\nROS 接口层]
|
||||
GW2 --> CON2[control_calibration_service\nROS 接口层]
|
||||
GW2 --> SEN2[sensor_calibration_service\nROS 接口层]
|
||||
EXT2 --> EXTALG[外部真值校核算法]
|
||||
CHA2 --> CHAALG[底盘标定算法]
|
||||
CON2 --> CONALG[运控评估算法]
|
||||
SEN2 --> SENALG[传感器标定算法]
|
||||
EXTALG --> EXT2
|
||||
CHAALG --> CHA2
|
||||
CONALG --> CON2
|
||||
SENALG --> SEN2
|
||||
ORCH2 --> REP2[get_report]
|
||||
end
|
||||
|
||||
classDef core fill:#dbeafe,stroke:#2563eb,color:#1e3a8a;
|
||||
classDef stub fill:#fef3c7,stroke:#d97706,color:#92400e;
|
||||
classDef algo fill:#d1fae5,stroke:#059669,color:#064e3b;
|
||||
|
||||
class ORCH1,PLAN1,GW1,REP1,ORCH2,PLAN2,GW2,REP2 core;
|
||||
class EXT1,CHA1,CON1,SEN1 stub;
|
||||
class EXT2,CHA2,CON2,SEN2 algo;
|
||||
class EXTALG,CHAALG,CONALG,SENALG algo;
|
||||
```
|
||||
|
||||
## 读图方式
|
||||
|
||||
- **左边当前实现**:专项包先用最小 stub 跑通流程。
|
||||
- **右边未来接入**:专项包保留 ROS 接口,内部替换成真实算法。
|
||||
- **总控不变**:orchestrator 始终只负责编排,不直接承载算法。
|
||||
Executable
+661
@@ -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}"
|
||||
+280
@@ -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 "本地数据输入回归测试通过"
|
||||
@@ -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/` 下。
|
||||
@@ -1,56 +0,0 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# NO CHECKED-IN PROTOBUF GENCODE
|
||||
# source: agv_calib_control.proto
|
||||
# Protobuf Python Version: 6.31.1
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import runtime_version as _runtime_version
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
from google.protobuf.internal import builder as _builder
|
||||
_runtime_version.ValidateProtobufRuntimeVersion(
|
||||
_runtime_version.Domain.PUBLIC,
|
||||
6,
|
||||
31,
|
||||
1,
|
||||
'',
|
||||
'agv_calib_control.proto'
|
||||
)
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x17\x61gv_calib_control.proto\x12\x17\x61gv.calibration.control\"\x07\n\x05\x45mpty\"4\n\x10StandardResponse\x12\x0f\n\x07success\x18\x01 \x01(\x08\x12\x0f\n\x07message\x18\x02 \x01(\t\"\x8b\x01\n\x0bModeRequest\x12>\n\x0btarget_mode\x18\x01 \x01(\x0e\x32).agv.calibration.control.ModeRequest.Mode\"<\n\x04Mode\x12\x0f\n\x0bNORMAL_MODE\x10\x00\x12\x12\n\x0eOPEN_LOOP_MODE\x10\x01\x12\x0f\n\x0bTUNING_MODE\x10\x02\"p\n\x0fOpenLoopRequest\x12\x16\n\x0eleft_motor_cmd\x18\x01 \x01(\x01\x12\x17\n\x0fright_motor_cmd\x18\x02 \x01(\x01\x12\x16\n\x0esteering_angle\x18\x03 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x04 \x01(\x01\"h\n\x0fTrajectoryPoint\x12\x0b\n\x03x_m\x18\x01 \x01(\x01\x12\x0b\n\x03y_m\x18\x02 \x01(\x01\x12\x0f\n\x07yaw_rad\x18\x03 \x01(\x01\x12\x17\n\x0ftarget_speed_ms\x18\x04 \x01(\x01\x12\x11\n\tcurvature\x18\x05 \x01(\x01\"a\n\x11TrajectoryRequest\x12\x14\n\x0ctest_case_id\x18\x01 \x01(\t\x12\x36\n\x04path\x18\x02 \x03(\x0b\x32(.agv.calibration.control.TrajectoryPoint\"G\n\x13StepResponseRequest\x12\x1a\n\x12target_velocity_ms\x18\x01 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x02 \x01(\x01\"\xf9\x05\n\rControlParams\x12$\n\x17wheel_radius_left_ratio\x18\x01 \x01(\x01H\x00\x88\x01\x01\x12%\n\x18wheel_radius_right_ratio\x18\x02 \x01(\x01H\x01\x88\x01\x01\x12$\n\x17\x65\x66\x66\x65\x63tive_track_width_m\x18\x03 \x01(\x01H\x02\x88\x01\x01\x12%\n\x18steering_zero_offset_deg\x18\x04 \x01(\x01H\x03\x88\x01\x01\x12\x1b\n\x0epid_kp_lateral\x18\x05 \x01(\x01H\x04\x88\x01\x01\x12\x1b\n\x0epid_ki_lateral\x18\x06 \x01(\x01H\x05\x88\x01\x01\x12\x1b\n\x0epid_kd_lateral\x18\x07 \x01(\x01H\x06\x88\x01\x01\x12\x1b\n\x0epid_kp_heading\x18\x08 \x01(\x01H\x07\x88\x01\x01\x12\x1b\n\x0epid_ki_heading\x18\t \x01(\x01H\x08\x88\x01\x01\x12\x1b\n\x0epid_kd_heading\x18\n \x01(\x01H\t\x88\x01\x01\x12%\n\x18pure_pursuit_lookahead_m\x18\x0b \x01(\x01H\n\x88\x01\x01\x12!\n\x14mpc_weight_q_lateral\x18\x0c \x01(\x01H\x0b\x88\x01\x01\x12\"\n\x15mpc_weight_r_steering\x18\r \x01(\x01H\x0c\x88\x01\x01\x42\x1a\n\x18_wheel_radius_left_ratioB\x1b\n\x19_wheel_radius_right_ratioB\x1a\n\x18_effective_track_width_mB\x1b\n\x19_steering_zero_offset_degB\x11\n\x0f_pid_kp_lateralB\x11\n\x0f_pid_ki_lateralB\x11\n\x0f_pid_kd_lateralB\x11\n\x0f_pid_kp_headingB\x11\n\x0f_pid_ki_headingB\x11\n\x0f_pid_kd_headingB\x1b\n\x19_pure_pursuit_lookahead_mB\x17\n\x15_mpc_weight_q_lateralB\x18\n\x16_mpc_weight_r_steering\"\xad\x02\n\rTelemetryData\x12\x1d\n\x15hardware_timestamp_us\x18\x01 \x01(\x03\x12\x10\n\x08odom_x_m\x18\x02 \x01(\x01\x12\x10\n\x08odom_y_m\x18\x03 \x01(\x01\x12\x14\n\x0codom_yaw_rad\x18\x04 \x01(\x01\x12\x1e\n\x16\x66\x65\x65\x64\x62\x61\x63k_linear_vel_ms\x18\x05 \x01(\x01\x12!\n\x19\x66\x65\x65\x64\x62\x61\x63k_angular_vel_rads\x18\x06 \x01(\x01\x12\x1e\n\x16left_motor_current_amp\x18\x07 \x01(\x01\x12\x1f\n\x17right_motor_current_amp\x18\x08 \x01(\x01\x12\"\n\x1asteering_motor_current_amp\x18\t \x01(\x01\x12\x1b\n\x13\x63md_steering_output\x18\n \x01(\x01\x32\xd1\x06\n\x16\x41gvCalibControlService\x12\x61\n\x0eSetControlMode\x12$.agv.calibration.control.ModeRequest\x1a).agv.calibration.control.StandardResponse\x12Z\n\rEmergencyStop\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12i\n\x12\x45xecuteOpenLoopCmd\x12(.agv.calibration.control.OpenLoopRequest\x1a).agv.calibration.control.StandardResponse\x12m\n\x14\x46ollowTestTrajectory\x12*.agv.calibration.control.TrajectoryRequest\x1a).agv.calibration.control.StandardResponse\x12n\n\x13\x45xecuteStepResponse\x12,.agv.calibration.control.StepResponseRequest\x1a).agv.calibration.control.StandardResponse\x12k\n\x16InjectTuningParameters\x12&.agv.calibration.control.ControlParams\x1a).agv.calibration.control.StandardResponse\x12\x64\n\x17\x43ommitControlParameters\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12[\n\x0fStreamTelemetry\x12\x1e.agv.calibration.control.Empty\x1a&.agv.calibration.control.TelemetryData0\x01\x62\x06proto3')
|
||||
|
||||
_globals = globals()
|
||||
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
|
||||
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'agv_calib_control_pb2', _globals)
|
||||
if not _descriptor._USE_C_DESCRIPTORS:
|
||||
DESCRIPTOR._loaded_options = None
|
||||
_globals['_EMPTY']._serialized_start=52
|
||||
_globals['_EMPTY']._serialized_end=59
|
||||
_globals['_STANDARDRESPONSE']._serialized_start=61
|
||||
_globals['_STANDARDRESPONSE']._serialized_end=113
|
||||
_globals['_MODEREQUEST']._serialized_start=116
|
||||
_globals['_MODEREQUEST']._serialized_end=255
|
||||
_globals['_MODEREQUEST_MODE']._serialized_start=195
|
||||
_globals['_MODEREQUEST_MODE']._serialized_end=255
|
||||
_globals['_OPENLOOPREQUEST']._serialized_start=257
|
||||
_globals['_OPENLOOPREQUEST']._serialized_end=369
|
||||
_globals['_TRAJECTORYPOINT']._serialized_start=371
|
||||
_globals['_TRAJECTORYPOINT']._serialized_end=475
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_start=477
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_end=574
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_start=576
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_end=647
|
||||
_globals['_CONTROLPARAMS']._serialized_start=650
|
||||
_globals['_CONTROLPARAMS']._serialized_end=1411
|
||||
_globals['_TELEMETRYDATA']._serialized_start=1414
|
||||
_globals['_TELEMETRYDATA']._serialized_end=1715
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_start=1718
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_end=2567
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@@ -1,444 +0,0 @@
|
||||
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
|
||||
"""Client and server classes corresponding to protobuf-defined services."""
|
||||
import grpc
|
||||
import warnings
|
||||
|
||||
import agv_calib_control_pb2 as agv__calib__control__pb2
|
||||
|
||||
GRPC_GENERATED_VERSION = '1.78.0'
|
||||
GRPC_VERSION = grpc.__version__
|
||||
_version_not_supported = False
|
||||
|
||||
try:
|
||||
from grpc._utilities import first_version_is_lower
|
||||
_version_not_supported = first_version_is_lower(GRPC_VERSION, GRPC_GENERATED_VERSION)
|
||||
except ImportError:
|
||||
_version_not_supported = True
|
||||
|
||||
if _version_not_supported:
|
||||
raise RuntimeError(
|
||||
f'The grpc package installed is at version {GRPC_VERSION},'
|
||||
+ ' but the generated code in agv_calib_control_pb2_grpc.py depends on'
|
||||
+ f' grpcio>={GRPC_GENERATED_VERSION}.'
|
||||
+ f' Please upgrade your grpc module to grpcio>={GRPC_GENERATED_VERSION}'
|
||||
+ f' or downgrade your generated code using grpcio-tools<={GRPC_VERSION}.'
|
||||
)
|
||||
|
||||
|
||||
class AgvCalibControlServiceStub(object):
|
||||
"""=========================================================
|
||||
核心服务:AGV 运控大脑(PID/MPC)参数自动化寻优调教代理
|
||||
[部署端 Server]:Windows车端 (满血保留自身算法,负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]:Linux标定服务器 (上帝视角,负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def __init__(self, channel):
|
||||
"""Constructor.
|
||||
|
||||
Args:
|
||||
channel: A grpc.Channel.
|
||||
"""
|
||||
self.SetControlMode = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
request_serializer=agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.EmergencyStop = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteOpenLoopCmd = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
request_serializer=agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.FollowTestTrajectory = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
request_serializer=agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteStepResponse = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
request_serializer=agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.InjectTuningParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
request_serializer=agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.CommitControlParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.StreamTelemetry = channel.unary_stream(
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.TelemetryData.FromString,
|
||||
_registered_method=True)
|
||||
|
||||
|
||||
class AgvCalibControlServiceServicer(object):
|
||||
"""=========================================================
|
||||
核心服务:AGV 运控大脑(PID/MPC)参数自动化寻优调教代理
|
||||
[部署端 Server]:Windows车端 (满血保留自身算法,负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]:Linux标定服务器 (上帝视角,负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def SetControlMode(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第一步:权限接管与生命周期安全管控
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]:要求切断避障,但保留底层 PID/MPC 算法就绪
|
||||
🚙 [Windows 返回 -> Linux]:回复模式切换成功,准备好接考题
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def EmergencyStop(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]:断网或飞车时的最高级别急停,无视一切直接刹车
|
||||
🚙 [Windows 返回 -> Linux]:返回底层抱死结果
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteOpenLoopCmd(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第二步:运动考题下发 (开环排雷 / 闭环寻优 / 波峰对齐)
|
||||
---------------------------------------------------------
|
||||
【场景A: 纯物理开环备用】
|
||||
💻 [Linux 发送 -> Windows]:要求切断算法盲跑,多用于摸底或辅助验证
|
||||
🚙 [Windows 返回 -> Linux]:确认已按指定 RPM/PWM 运转
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def FollowTestTrajectory(self, request, context):
|
||||
"""【场景B: 算法闭环调优】
|
||||
💻 [Linux 发送 -> Windows]:下发一条由几百个点组成的测试轨迹(如 S型贝塞尔曲线)
|
||||
🚙 [Windows 返回 -> Linux]:收到轨迹后,车端立刻使用它自带的 PID/MPC 算法努力贴合轨迹跑圈
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteStepResponse(self, request, context):
|
||||
"""【场景C: 波峰时序对齐】
|
||||
💻 [Linux 发送 -> Windows]:下发极短促的阶跃加速指令,人为制造绝对速度波峰
|
||||
🚙 [Windows 返回 -> Linux]:确认加速。(Linux 借此波峰算出网络的绝对 Time Offset)
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def InjectTuningParameters(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第三步:运控参数 AI 寻优:动态热注入与最终固化
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]:Linux 发现上一圈跑得差,AI算出了新的 PID/前瞻距离,要求立即热注入
|
||||
🚙 [Windows 返回 -> Linux]:车端将新参数瞬间覆写进运行内存(不重启系统),随时准备用新参数重跑
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def CommitControlParameters(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]:Linux 判定误差极小,调优结束,命令固化目前内存里的最高分参数
|
||||
🚙 [Windows 返回 -> Linux]:车端将这组完美参数永久覆写进硬盘的 config.yaml 或系统注册表
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def StreamTelemetry(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第四步:高频数字孪生体感上报 (50Hz)
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]:空包触发,命令车端开始疯狂推流
|
||||
🚙 [Windows 持续流式返回 -> Linux]:以 50Hz 频率,持续上报自己的里程计坐标、速度和单调时间戳
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
|
||||
def add_AgvCalibControlServiceServicer_to_server(servicer, server):
|
||||
rpc_method_handlers = {
|
||||
'SetControlMode': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.SetControlMode,
|
||||
request_deserializer=agv__calib__control__pb2.ModeRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'EmergencyStop': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.EmergencyStop,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteOpenLoopCmd': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteOpenLoopCmd,
|
||||
request_deserializer=agv__calib__control__pb2.OpenLoopRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'FollowTestTrajectory': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.FollowTestTrajectory,
|
||||
request_deserializer=agv__calib__control__pb2.TrajectoryRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteStepResponse': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteStepResponse,
|
||||
request_deserializer=agv__calib__control__pb2.StepResponseRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'InjectTuningParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.InjectTuningParameters,
|
||||
request_deserializer=agv__calib__control__pb2.ControlParams.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'CommitControlParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.CommitControlParameters,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'StreamTelemetry': grpc.unary_stream_rpc_method_handler(
|
||||
servicer.StreamTelemetry,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.TelemetryData.SerializeToString,
|
||||
),
|
||||
}
|
||||
generic_handler = grpc.method_handlers_generic_handler(
|
||||
'agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
server.add_generic_rpc_handlers((generic_handler,))
|
||||
server.add_registered_method_handlers('agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
|
||||
|
||||
# This class is part of an EXPERIMENTAL API.
|
||||
class AgvCalibControlService(object):
|
||||
"""=========================================================
|
||||
核心服务:AGV 运控大脑(PID/MPC)参数自动化寻优调教代理
|
||||
[部署端 Server]:Windows车端 (满血保留自身算法,负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]:Linux标定服务器 (上帝视角,负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
@staticmethod
|
||||
def SetControlMode(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def EmergencyStop(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteOpenLoopCmd(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def FollowTestTrajectory(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteStepResponse(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def InjectTuningParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def CommitControlParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def StreamTelemetry(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_stream(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.TelemetryData.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
@@ -1,51 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(agv_calib_core)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED) # 🚨 核心依赖:寻找组件库
|
||||
find_package(behaviortree_cpp_v3 REQUIRED)
|
||||
find_package(ament_index_cpp REQUIRED)
|
||||
find_package(win_ubuntu_bridge REQUIRED)
|
||||
|
||||
# 1. 编译大脑为动态链接库 (SHARED) 组件
|
||||
add_library(brain_node SHARED src/brain_node.cpp)
|
||||
|
||||
# 2. 将 include 暴露给编译器
|
||||
target_include_directories(brain_node PUBLIC
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||
)
|
||||
|
||||
ament_target_dependencies(brain_node
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
rclcpp_components
|
||||
behaviortree_cpp_v3
|
||||
ament_index_cpp
|
||||
win_ubuntu_bridge
|
||||
)
|
||||
|
||||
# 3. 注册插件
|
||||
rclcpp_components_register_node(brain_node
|
||||
PLUGIN "agv_calib_core::BrainNode"
|
||||
EXECUTABLE brain_node_exe
|
||||
)
|
||||
|
||||
# 4. 安装工程中所有的核心文件夹 (不可遗漏!)
|
||||
install(TARGETS brain_node brain_node_exe
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/ DESTINATION include)
|
||||
install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config)
|
||||
install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
|
||||
install(DIRECTORY behavior_trees/ DESTINATION share/${PROJECT_NAME}/behavior_trees)
|
||||
|
||||
ament_package()
|
||||
@@ -1,23 +0,0 @@
|
||||
<root main_tree_to_execute="MainCalibrationFlow">
|
||||
<BehaviorTree ID="MainCalibrationFlow">
|
||||
<Sequence name="全自动标定主干">
|
||||
|
||||
<Sequence name="Phase_1_Chassis">
|
||||
<MockConnectAGV />
|
||||
<MockCallChassisAlgo />
|
||||
</Sequence>
|
||||
|
||||
<Sequence name="Phase_2_Control">
|
||||
<MockTuneControl />
|
||||
<MockCallControlAlgo />
|
||||
</Sequence>
|
||||
|
||||
<Sequence name="Phase_3_Sensor">
|
||||
<MockMoveAndCapture />
|
||||
<MockCallSensorAlgo />
|
||||
<MockCommitAllParams />
|
||||
</Sequence>
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -1,9 +0,0 @@
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence name="全自动标定总流程">
|
||||
<SetChassisMode target_mode="1" />
|
||||
<TriggerCapture sensor_id="cam_front" capture_code_out="{shared_code}" />
|
||||
<DownloadData sensor_id="cam_front" capture_code_in="{shared_code}" save_dir="/tmp/calib_data" saved_path_out="{saved_image_path}" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -1,6 +0,0 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
# 动态指定要加载的 XML 剧本文件名
|
||||
tree_xml_filename: "main_tree.xml"
|
||||
# 行为树的 Tick 循环频率 (毫秒)
|
||||
tick_rate_ms: 50
|
||||
@@ -1,25 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <behaviortree_cpp_v3/bt_factory.h>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <string>
|
||||
|
||||
namespace agv_calib_core {
|
||||
|
||||
// 继承 Node,化身为标准的 ROS 2 Component
|
||||
class BrainNode : public rclcpp::Node {
|
||||
public:
|
||||
explicit BrainNode(const rclcpp::NodeOptions & options);
|
||||
~BrainNode() override;
|
||||
|
||||
private:
|
||||
// 行为树专属的后台执行线程 (极其重要!绝不能阻塞 ROS 2 容器主线程)
|
||||
void execute_behavior_tree();
|
||||
|
||||
std::thread bt_thread_;
|
||||
std::atomic<bool> is_running_;
|
||||
};
|
||||
|
||||
} // namespace agv_calib_core
|
||||
@@ -1,109 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <behaviortree_cpp_v3/action_node.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
// 引入底层 win_ubuntu_bridge 接口
|
||||
#include "win_ubuntu_bridge/srv/set_diagnostic_mode.hpp"
|
||||
#include "win_ubuntu_bridge/srv/trigger_sync_capture.hpp"
|
||||
#include "win_ubuntu_bridge/action/download_sensor_data.hpp"
|
||||
|
||||
namespace agv_calib_core {
|
||||
|
||||
class SetChassisModeNode : public BT::SyncActionNode {
|
||||
public:
|
||||
SetChassisModeNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node)
|
||||
: BT::SyncActionNode(name, config), node_(node) {
|
||||
client_ = node_->create_client<win_ubuntu_bridge::srv::SetDiagnosticMode>("/chassis_gateway/set_diagnostic_mode");
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() { return { BT::InputPort<int>("target_mode") }; }
|
||||
|
||||
BT::NodeStatus tick() override {
|
||||
int mode; if (!getInput("target_mode", mode)) return BT::NodeStatus::FAILURE;
|
||||
RCLCPP_INFO(node_->get_logger(), "🌲 [BT] 下发底盘夺权指令,模式: %d", mode);
|
||||
if (!client_->wait_for_service(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE;
|
||||
|
||||
auto req = std::make_shared<win_ubuntu_bridge::srv::SetDiagnosticMode::Request>(); req->target_mode = mode;
|
||||
auto future = client_->async_send_request(req);
|
||||
|
||||
// 🚨 这里阻塞等待完全没问题!因为外层 BT 跑在独立线程,根本不影响 ROS 2 Executor 的回调!
|
||||
if (future.wait_for(std::chrono::seconds(3)) == std::future_status::ready) {
|
||||
auto res = future.get();
|
||||
if (res->success) return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
private:
|
||||
rclcpp::Node* node_; rclcpp::Client<win_ubuntu_bridge::srv::SetDiagnosticMode>::SharedPtr client_;
|
||||
};
|
||||
|
||||
class TriggerCaptureNode : public BT::SyncActionNode {
|
||||
public:
|
||||
TriggerCaptureNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node)
|
||||
: BT::SyncActionNode(name, config), node_(node) {
|
||||
client_ = node_->create_client<win_ubuntu_bridge::srv::TriggerSyncCapture>("/sensor_gateway/trigger_sync_capture");
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return { BT::InputPort<std::string>("sensor_id"), BT::OutputPort<int64_t>("capture_code_out") };
|
||||
}
|
||||
|
||||
BT::NodeStatus tick() override {
|
||||
std::string sensor_id; getInput("sensor_id", sensor_id);
|
||||
RCLCPP_INFO(node_->get_logger(), "📷 [BT] 冻结 %s 数据...", sensor_id.c_str());
|
||||
if (!client_->wait_for_service(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE;
|
||||
|
||||
auto req = std::make_shared<win_ubuntu_bridge::srv::TriggerSyncCapture::Request>(); req->sensor_ids.push_back(sensor_id);
|
||||
auto future = client_->async_send_request(req);
|
||||
if (future.wait_for(std::chrono::seconds(3)) == std::future_status::ready) {
|
||||
auto res = future.get();
|
||||
if (res->success) { setOutput("capture_code_out", res->capture_timestamp_us); return BT::NodeStatus::SUCCESS; }
|
||||
}
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
private:
|
||||
rclcpp::Node* node_; rclcpp::Client<win_ubuntu_bridge::srv::TriggerSyncCapture>::SharedPtr client_;
|
||||
};
|
||||
|
||||
class DownloadDataNode : public BT::StatefulActionNode {
|
||||
public:
|
||||
DownloadDataNode(const std::string& name, const BT::NodeConfiguration& config, rclcpp::Node* node)
|
||||
: BT::StatefulActionNode(name, config), node_(node) {
|
||||
action_client_ = rclcpp_action::create_client<win_ubuntu_bridge::action::DownloadSensorData>(node_, "/sensor_gateway/download_sensor_data");
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return { BT::InputPort<std::string>("sensor_id"), BT::InputPort<int64_t>("capture_code_in"),
|
||||
BT::InputPort<std::string>("save_dir"), BT::OutputPort<std::string>("saved_path_out") };
|
||||
}
|
||||
|
||||
BT::NodeStatus onStart() override {
|
||||
int64_t code; std::string sensor; std::string save_dir;
|
||||
if (!getInput("capture_code_in", code) || !getInput("sensor_id", sensor) || !getInput("save_dir", save_dir)) return BT::NodeStatus::FAILURE;
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "📥 [BT] 挂起下载任务,取件码: %ld", code);
|
||||
if (!action_client_->wait_for_action_server(std::chrono::seconds(2))) return BT::NodeStatus::FAILURE;
|
||||
|
||||
auto goal_msg = win_ubuntu_bridge::action::DownloadSensorData::Goal();
|
||||
goal_msg.capture_timestamp_us = code; goal_msg.sensor_id = sensor;
|
||||
goal_msg.data_type = win_ubuntu_bridge::action::DownloadSensorData::Goal::DATA_TYPE_IMAGE; goal_msg.save_directory = save_dir;
|
||||
|
||||
auto send_goal_options = rclcpp_action::Client<win_ubuntu_bridge::action::DownloadSensorData>::SendGoalOptions();
|
||||
send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle<win_ubuntu_bridge::action::DownloadSensorData>::WrappedResult & result) {
|
||||
if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->success) {
|
||||
RCLCPP_INFO(node_->get_logger(), "✅ [BT] 落盘成功!路径: %s", result.result->saved_file_path.c_str());
|
||||
setOutput("saved_path_out", result.result->saved_file_path); done_ = true; success_ = true;
|
||||
} else { done_ = true; success_ = false; }
|
||||
};
|
||||
action_client_->async_send_goal(goal_msg, send_goal_options); done_ = false; return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
BT::NodeStatus onRunning() override { if (done_) return success_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; return BT::NodeStatus::RUNNING; }
|
||||
void onHalted() override { }
|
||||
private:
|
||||
rclcpp::Node* node_; rclcpp_action::Client<win_ubuntu_bridge::action::DownloadSensorData>::SharedPtr action_client_;
|
||||
bool done_ = false; bool success_ = false;
|
||||
};
|
||||
|
||||
} // namespace agv_calib_core
|
||||
@@ -1,31 +0,0 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import ComposableNodeContainer
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取 yaml 文件的绝对路径
|
||||
config_file = os.path.join(
|
||||
get_package_share_directory('agv_calib_core'),
|
||||
'config',
|
||||
'brain_params.yaml'
|
||||
)
|
||||
|
||||
# 建立多线程容器加载大脑组件 (MT 代表 Multi-Threaded Executor)
|
||||
container = ComposableNodeContainer(
|
||||
name='brain_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container_mt',
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
package='agv_calib_core',
|
||||
plugin='agv_calib_core::BrainNode',
|
||||
name='brain_node',
|
||||
parameters=[config_file] # 🚨 动态挂载 YAML 参数表!
|
||||
)
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
return LaunchDescription([container])
|
||||
@@ -1,22 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>agv_calib_core</name>
|
||||
<version>1.0.0</version>
|
||||
<description>行为树总控大脑</description>
|
||||
<maintainer email="2469171725@qq.com">nvidia</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>behaviortree_cpp_v3</depend>
|
||||
<depend>ament_index_cpp</depend>
|
||||
<depend>win_ubuntu_bridge</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,87 +0,0 @@
|
||||
#include "agv_calib_core/brain_node.hpp"
|
||||
#include "agv_calib_core/bt_ros2_nodes.hpp"
|
||||
|
||||
#include <behaviortree_cpp_v3/bt_factory.h>
|
||||
#include <behaviortree_cpp_v3/loggers/bt_cout_logger.h>
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
|
||||
namespace agv_calib_core {
|
||||
|
||||
BrainNode::BrainNode(const rclcpp::NodeOptions & options)
|
||||
: Node("brain_node", options), is_running_(false) {
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "👑 AGV 标定中央大脑 (Component 版) 正在挂载...");
|
||||
|
||||
// 1. 从 YAML 配置文件读取动态参数!绝不硬编码!
|
||||
this->declare_parameter<std::string>("tree_xml_filename", "main_tree.xml");
|
||||
this->declare_parameter<int>("tick_rate_ms", 50);
|
||||
|
||||
// 2. 启动专属的后台守护线程去运行行为树。将主线程交还给容器处理网络回调!
|
||||
is_running_ = true;
|
||||
bt_thread_ = std::thread(&BrainNode::execute_behavior_tree, this);
|
||||
}
|
||||
|
||||
BrainNode::~BrainNode() {
|
||||
is_running_ = false;
|
||||
if (bt_thread_.joinable()) {
|
||||
bt_thread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
void BrainNode::execute_behavior_tree() {
|
||||
// 稍微延时 0.5 秒,确保节点完全被容器接管,再发起 Client 寻址
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
|
||||
BT::BehaviorTreeFactory factory;
|
||||
|
||||
// 注册业务积木,传递 this 裸指针给所有积木
|
||||
factory.registerBuilder<SetChassisModeNode>("SetChassisMode",
|
||||
[this](const std::string& name, const BT::NodeConfiguration& config) {
|
||||
return std::make_unique<SetChassisModeNode>(name, config, this);
|
||||
});
|
||||
|
||||
factory.registerBuilder<TriggerCaptureNode>("TriggerCapture",
|
||||
[this](const std::string& name, const BT::NodeConfiguration& config) {
|
||||
return std::make_unique<TriggerCaptureNode>(name, config, this);
|
||||
});
|
||||
|
||||
factory.registerBuilder<DownloadDataNode>("DownloadData",
|
||||
[this](const std::string& name, const BT::NodeConfiguration& config) {
|
||||
return std::make_unique<DownloadDataNode>(name, config, this);
|
||||
});
|
||||
|
||||
try {
|
||||
std::string xml_filename = this->get_parameter("tree_xml_filename").as_string();
|
||||
int tick_rate = this->get_parameter("tick_rate_ms").as_int();
|
||||
|
||||
std::string pkg_path = ament_index_cpp::get_package_share_directory("agv_calib_core");
|
||||
std::string xml_file = pkg_path + "/behavior_trees/" + xml_filename;
|
||||
|
||||
auto tree = factory.createTreeFromFile(xml_file);
|
||||
BT::StdCoutLogger logger_cout(tree);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "📜 XML 剧本 [%s] 加载完毕,开始全自动流水线...", xml_filename.c_str());
|
||||
|
||||
// 按照 YAML 配置的频率持续 Tick
|
||||
BT::NodeStatus status = BT::NodeStatus::RUNNING;
|
||||
while (rclcpp::ok() && is_running_ && status == BT::NodeStatus::RUNNING) {
|
||||
status = tree.tickRoot();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(tick_rate));
|
||||
}
|
||||
|
||||
if (status == BT::NodeStatus::SUCCESS) {
|
||||
RCLCPP_INFO(this->get_logger(), "🎉 标定流水线全流程完美结束!");
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "⚠️ 流水线未成功完成 (可能被中止)。");
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "❌ 行为树崩溃: %s", e.what());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace agv_calib_core
|
||||
|
||||
// 🚨 终极一步:将该类注册为 ROS 2 Component (插件)!
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(agv_calib_core::BrainNode)
|
||||
@@ -1,56 +0,0 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# NO CHECKED-IN PROTOBUF GENCODE
|
||||
# source: agv_calib_control.proto
|
||||
# Protobuf Python Version: 6.31.1
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import runtime_version as _runtime_version
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
from google.protobuf.internal import builder as _builder
|
||||
_runtime_version.ValidateProtobufRuntimeVersion(
|
||||
_runtime_version.Domain.PUBLIC,
|
||||
6,
|
||||
31,
|
||||
1,
|
||||
'',
|
||||
'agv_calib_control.proto'
|
||||
)
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x17\x61gv_calib_control.proto\x12\x17\x61gv.calibration.control\"\x07\n\x05\x45mpty\"4\n\x10StandardResponse\x12\x0f\n\x07success\x18\x01 \x01(\x08\x12\x0f\n\x07message\x18\x02 \x01(\t\"\x8b\x01\n\x0bModeRequest\x12>\n\x0btarget_mode\x18\x01 \x01(\x0e\x32).agv.calibration.control.ModeRequest.Mode\"<\n\x04Mode\x12\x0f\n\x0bNORMAL_MODE\x10\x00\x12\x12\n\x0eOPEN_LOOP_MODE\x10\x01\x12\x0f\n\x0bTUNING_MODE\x10\x02\"p\n\x0fOpenLoopRequest\x12\x16\n\x0eleft_motor_cmd\x18\x01 \x01(\x01\x12\x17\n\x0fright_motor_cmd\x18\x02 \x01(\x01\x12\x16\n\x0esteering_angle\x18\x03 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x04 \x01(\x01\"h\n\x0fTrajectoryPoint\x12\x0b\n\x03x_m\x18\x01 \x01(\x01\x12\x0b\n\x03y_m\x18\x02 \x01(\x01\x12\x0f\n\x07yaw_rad\x18\x03 \x01(\x01\x12\x17\n\x0ftarget_speed_ms\x18\x04 \x01(\x01\x12\x11\n\tcurvature\x18\x05 \x01(\x01\"a\n\x11TrajectoryRequest\x12\x14\n\x0ctest_case_id\x18\x01 \x01(\t\x12\x36\n\x04path\x18\x02 \x03(\x0b\x32(.agv.calibration.control.TrajectoryPoint\"G\n\x13StepResponseRequest\x12\x1a\n\x12target_velocity_ms\x18\x01 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x02 \x01(\x01\"\xf9\x05\n\rControlParams\x12$\n\x17wheel_radius_left_ratio\x18\x01 \x01(\x01H\x00\x88\x01\x01\x12%\n\x18wheel_radius_right_ratio\x18\x02 \x01(\x01H\x01\x88\x01\x01\x12$\n\x17\x65\x66\x66\x65\x63tive_track_width_m\x18\x03 \x01(\x01H\x02\x88\x01\x01\x12%\n\x18steering_zero_offset_deg\x18\x04 \x01(\x01H\x03\x88\x01\x01\x12\x1b\n\x0epid_kp_lateral\x18\x05 \x01(\x01H\x04\x88\x01\x01\x12\x1b\n\x0epid_ki_lateral\x18\x06 \x01(\x01H\x05\x88\x01\x01\x12\x1b\n\x0epid_kd_lateral\x18\x07 \x01(\x01H\x06\x88\x01\x01\x12\x1b\n\x0epid_kp_heading\x18\x08 \x01(\x01H\x07\x88\x01\x01\x12\x1b\n\x0epid_ki_heading\x18\t \x01(\x01H\x08\x88\x01\x01\x12\x1b\n\x0epid_kd_heading\x18\n \x01(\x01H\t\x88\x01\x01\x12%\n\x18pure_pursuit_lookahead_m\x18\x0b \x01(\x01H\n\x88\x01\x01\x12!\n\x14mpc_weight_q_lateral\x18\x0c \x01(\x01H\x0b\x88\x01\x01\x12\"\n\x15mpc_weight_r_steering\x18\r \x01(\x01H\x0c\x88\x01\x01\x42\x1a\n\x18_wheel_radius_left_ratioB\x1b\n\x19_wheel_radius_right_ratioB\x1a\n\x18_effective_track_width_mB\x1b\n\x19_steering_zero_offset_degB\x11\n\x0f_pid_kp_lateralB\x11\n\x0f_pid_ki_lateralB\x11\n\x0f_pid_kd_lateralB\x11\n\x0f_pid_kp_headingB\x11\n\x0f_pid_ki_headingB\x11\n\x0f_pid_kd_headingB\x1b\n\x19_pure_pursuit_lookahead_mB\x17\n\x15_mpc_weight_q_lateralB\x18\n\x16_mpc_weight_r_steering\"\xad\x02\n\rTelemetryData\x12\x1d\n\x15hardware_timestamp_us\x18\x01 \x01(\x03\x12\x10\n\x08odom_x_m\x18\x02 \x01(\x01\x12\x10\n\x08odom_y_m\x18\x03 \x01(\x01\x12\x14\n\x0codom_yaw_rad\x18\x04 \x01(\x01\x12\x1e\n\x16\x66\x65\x65\x64\x62\x61\x63k_linear_vel_ms\x18\x05 \x01(\x01\x12!\n\x19\x66\x65\x65\x64\x62\x61\x63k_angular_vel_rads\x18\x06 \x01(\x01\x12\x1e\n\x16left_motor_current_amp\x18\x07 \x01(\x01\x12\x1f\n\x17right_motor_current_amp\x18\x08 \x01(\x01\x12\"\n\x1asteering_motor_current_amp\x18\t \x01(\x01\x12\x1b\n\x13\x63md_steering_output\x18\n \x01(\x01\x32\xd1\x06\n\x16\x41gvCalibControlService\x12\x61\n\x0eSetControlMode\x12$.agv.calibration.control.ModeRequest\x1a).agv.calibration.control.StandardResponse\x12Z\n\rEmergencyStop\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12i\n\x12\x45xecuteOpenLoopCmd\x12(.agv.calibration.control.OpenLoopRequest\x1a).agv.calibration.control.StandardResponse\x12m\n\x14\x46ollowTestTrajectory\x12*.agv.calibration.control.TrajectoryRequest\x1a).agv.calibration.control.StandardResponse\x12n\n\x13\x45xecuteStepResponse\x12,.agv.calibration.control.StepResponseRequest\x1a).agv.calibration.control.StandardResponse\x12k\n\x16InjectTuningParameters\x12&.agv.calibration.control.ControlParams\x1a).agv.calibration.control.StandardResponse\x12\x64\n\x17\x43ommitControlParameters\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12[\n\x0fStreamTelemetry\x12\x1e.agv.calibration.control.Empty\x1a&.agv.calibration.control.TelemetryData0\x01\x62\x06proto3')
|
||||
|
||||
_globals = globals()
|
||||
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
|
||||
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'agv_calib_control_pb2', _globals)
|
||||
if not _descriptor._USE_C_DESCRIPTORS:
|
||||
DESCRIPTOR._loaded_options = None
|
||||
_globals['_EMPTY']._serialized_start=52
|
||||
_globals['_EMPTY']._serialized_end=59
|
||||
_globals['_STANDARDRESPONSE']._serialized_start=61
|
||||
_globals['_STANDARDRESPONSE']._serialized_end=113
|
||||
_globals['_MODEREQUEST']._serialized_start=116
|
||||
_globals['_MODEREQUEST']._serialized_end=255
|
||||
_globals['_MODEREQUEST_MODE']._serialized_start=195
|
||||
_globals['_MODEREQUEST_MODE']._serialized_end=255
|
||||
_globals['_OPENLOOPREQUEST']._serialized_start=257
|
||||
_globals['_OPENLOOPREQUEST']._serialized_end=369
|
||||
_globals['_TRAJECTORYPOINT']._serialized_start=371
|
||||
_globals['_TRAJECTORYPOINT']._serialized_end=475
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_start=477
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_end=574
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_start=576
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_end=647
|
||||
_globals['_CONTROLPARAMS']._serialized_start=650
|
||||
_globals['_CONTROLPARAMS']._serialized_end=1411
|
||||
_globals['_TELEMETRYDATA']._serialized_start=1414
|
||||
_globals['_TELEMETRYDATA']._serialized_end=1715
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_start=1718
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_end=2567
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@@ -1,444 +0,0 @@
|
||||
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
|
||||
"""Client and server classes corresponding to protobuf-defined services."""
|
||||
import grpc
|
||||
import warnings
|
||||
|
||||
import agv_calib_control_pb2 as agv__calib__control__pb2
|
||||
|
||||
GRPC_GENERATED_VERSION = '1.78.0'
|
||||
GRPC_VERSION = grpc.__version__
|
||||
_version_not_supported = False
|
||||
|
||||
try:
|
||||
from grpc._utilities import first_version_is_lower
|
||||
_version_not_supported = first_version_is_lower(GRPC_VERSION, GRPC_GENERATED_VERSION)
|
||||
except ImportError:
|
||||
_version_not_supported = True
|
||||
|
||||
if _version_not_supported:
|
||||
raise RuntimeError(
|
||||
f'The grpc package installed is at version {GRPC_VERSION},'
|
||||
+ ' but the generated code in agv_calib_control_pb2_grpc.py depends on'
|
||||
+ f' grpcio>={GRPC_GENERATED_VERSION}.'
|
||||
+ f' Please upgrade your grpc module to grpcio>={GRPC_GENERATED_VERSION}'
|
||||
+ f' or downgrade your generated code using grpcio-tools<={GRPC_VERSION}.'
|
||||
)
|
||||
|
||||
|
||||
class AgvCalibControlServiceStub(object):
|
||||
"""=========================================================
|
||||
核心服务:AGV 运控大脑(PID/MPC)参数自动化寻优调教代理
|
||||
[部署端 Server]:Windows车端 (满血保留自身算法,负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]:Linux标定服务器 (上帝视角,负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def __init__(self, channel):
|
||||
"""Constructor.
|
||||
|
||||
Args:
|
||||
channel: A grpc.Channel.
|
||||
"""
|
||||
self.SetControlMode = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
request_serializer=agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.EmergencyStop = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteOpenLoopCmd = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
request_serializer=agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.FollowTestTrajectory = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
request_serializer=agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteStepResponse = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
request_serializer=agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.InjectTuningParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
request_serializer=agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.CommitControlParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.StreamTelemetry = channel.unary_stream(
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.TelemetryData.FromString,
|
||||
_registered_method=True)
|
||||
|
||||
|
||||
class AgvCalibControlServiceServicer(object):
|
||||
"""=========================================================
|
||||
核心服务:AGV 运控大脑(PID/MPC)参数自动化寻优调教代理
|
||||
[部署端 Server]:Windows车端 (满血保留自身算法,负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]:Linux标定服务器 (上帝视角,负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def SetControlMode(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第一步:权限接管与生命周期安全管控
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]:要求切断避障,但保留底层 PID/MPC 算法就绪
|
||||
🚙 [Windows 返回 -> Linux]:回复模式切换成功,准备好接考题
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def EmergencyStop(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]:断网或飞车时的最高级别急停,无视一切直接刹车
|
||||
🚙 [Windows 返回 -> Linux]:返回底层抱死结果
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteOpenLoopCmd(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第二步:运动考题下发 (开环排雷 / 闭环寻优 / 波峰对齐)
|
||||
---------------------------------------------------------
|
||||
【场景A: 纯物理开环备用】
|
||||
💻 [Linux 发送 -> Windows]:要求切断算法盲跑,多用于摸底或辅助验证
|
||||
🚙 [Windows 返回 -> Linux]:确认已按指定 RPM/PWM 运转
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def FollowTestTrajectory(self, request, context):
|
||||
"""【场景B: 算法闭环调优】
|
||||
💻 [Linux 发送 -> Windows]:下发一条由几百个点组成的测试轨迹(如 S型贝塞尔曲线)
|
||||
🚙 [Windows 返回 -> Linux]:收到轨迹后,车端立刻使用它自带的 PID/MPC 算法努力贴合轨迹跑圈
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteStepResponse(self, request, context):
|
||||
"""【场景C: 波峰时序对齐】
|
||||
💻 [Linux 发送 -> Windows]:下发极短促的阶跃加速指令,人为制造绝对速度波峰
|
||||
🚙 [Windows 返回 -> Linux]:确认加速。(Linux 借此波峰算出网络的绝对 Time Offset)
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def InjectTuningParameters(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第三步:运控参数 AI 寻优:动态热注入与最终固化
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]:Linux 发现上一圈跑得差,AI算出了新的 PID/前瞻距离,要求立即热注入
|
||||
🚙 [Windows 返回 -> Linux]:车端将新参数瞬间覆写进运行内存(不重启系统),随时准备用新参数重跑
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def CommitControlParameters(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]:Linux 判定误差极小,调优结束,命令固化目前内存里的最高分参数
|
||||
🚙 [Windows 返回 -> Linux]:车端将这组完美参数永久覆写进硬盘的 config.yaml 或系统注册表
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def StreamTelemetry(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第四步:高频数字孪生体感上报 (50Hz)
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]:空包触发,命令车端开始疯狂推流
|
||||
🚙 [Windows 持续流式返回 -> Linux]:以 50Hz 频率,持续上报自己的里程计坐标、速度和单调时间戳
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
|
||||
def add_AgvCalibControlServiceServicer_to_server(servicer, server):
|
||||
rpc_method_handlers = {
|
||||
'SetControlMode': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.SetControlMode,
|
||||
request_deserializer=agv__calib__control__pb2.ModeRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'EmergencyStop': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.EmergencyStop,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteOpenLoopCmd': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteOpenLoopCmd,
|
||||
request_deserializer=agv__calib__control__pb2.OpenLoopRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'FollowTestTrajectory': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.FollowTestTrajectory,
|
||||
request_deserializer=agv__calib__control__pb2.TrajectoryRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteStepResponse': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteStepResponse,
|
||||
request_deserializer=agv__calib__control__pb2.StepResponseRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'InjectTuningParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.InjectTuningParameters,
|
||||
request_deserializer=agv__calib__control__pb2.ControlParams.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'CommitControlParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.CommitControlParameters,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'StreamTelemetry': grpc.unary_stream_rpc_method_handler(
|
||||
servicer.StreamTelemetry,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.TelemetryData.SerializeToString,
|
||||
),
|
||||
}
|
||||
generic_handler = grpc.method_handlers_generic_handler(
|
||||
'agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
server.add_generic_rpc_handlers((generic_handler,))
|
||||
server.add_registered_method_handlers('agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
|
||||
|
||||
# This class is part of an EXPERIMENTAL API.
|
||||
class AgvCalibControlService(object):
|
||||
"""=========================================================
|
||||
核心服务:AGV 运控大脑(PID/MPC)参数自动化寻优调教代理
|
||||
[部署端 Server]:Windows车端 (满血保留自身算法,负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]:Linux标定服务器 (上帝视角,负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
@staticmethod
|
||||
def SetControlMode(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def EmergencyStop(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteOpenLoopCmd(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def FollowTestTrajectory(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteStepResponse(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def InjectTuningParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def CommitControlParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def StreamTelemetry(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_stream(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.TelemetryData.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
@@ -1,26 +0,0 @@
|
||||
import grpc
|
||||
from concurrent import futures
|
||||
import time
|
||||
import agv_calib_control_pb2 as pb2
|
||||
import agv_calib_control_pb2_grpc as pb2_grpc
|
||||
|
||||
# 扮演 Windows 车端的角色
|
||||
class FakeAgvServer(pb2_grpc.AgvCalibControlServiceServicer):
|
||||
def SetControlMode(self, request, context):
|
||||
print(f"\n[🚙 Windows 假车端] 收到 Linux 夺权指令! 目标模式: {request.target_mode}")
|
||||
time.sleep(0.5) # 假装底层继电器切换花了点时间
|
||||
print("[🚙 Windows 假车端] 避障已切断,乖乖交出控制权!")
|
||||
|
||||
# 返回成功回执给 Linux
|
||||
return pb2.StandardResponse(success=True, message="Windows: 已交出底盘控制权!")
|
||||
|
||||
def serve():
|
||||
server = grpc.server(futures.ThreadPoolExecutor(max_workers=10))
|
||||
pb2_grpc.add_AgvCalibControlServiceServicer_to_server(FakeAgvServer(), server)
|
||||
server.add_insecure_port('[::]:50051')
|
||||
print("🚀 [Windows 假车端] 已启动,正在监听 50051 端口,等待 Linux 大脑连接...")
|
||||
server.start()
|
||||
server.wait_for_termination()
|
||||
|
||||
if __name__ == '__main__':
|
||||
serve()
|
||||
@@ -0,0 +1,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]` 行。
|
||||
@@ -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}手眼标定"
|
||||
@@ -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)
|
||||
@@ -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())
|
||||
@@ -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"]))
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -0,0 +1 @@
|
||||
PySide6>=6.6.0
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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 车端应把这些内部消息映射到实际车辆接口。
|
||||
+35
@@ -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
|
||||
+33
@@ -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
|
||||
+33
@@ -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
|
||||
+15
@@ -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
|
||||
+40
@@ -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
|
||||
+35
@@ -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
|
||||
+30
@@ -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
|
||||
+27
@@ -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
|
||||
@@ -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>
|
||||
+73
@@ -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()
|
||||
+51
@@ -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
|
||||
+22
@@ -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
|
||||
+25
@@ -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
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
# =========================================================
|
||||
# 查询当前已生效底盘参数响应
|
||||
# =========================================================
|
||||
|
||||
# 是否成功
|
||||
bool success
|
||||
# 错误码
|
||||
calibration_common_interfaces/ErrorCode error_code
|
||||
# 说明
|
||||
string message
|
||||
# 当前版本
|
||||
string parameter_version
|
||||
# 当前生效参数
|
||||
ChassisCalibrationParameterSet params
|
||||
# 生效时间
|
||||
int64 applied_timestamp_us
|
||||
+13
@@ -0,0 +1,13 @@
|
||||
# =========================================================
|
||||
# 圆弧动作命令
|
||||
# 作用:用于验证曲线行驶能力和曲率误差
|
||||
# =========================================================
|
||||
|
||||
# 目标线速度(m/s)
|
||||
float64 target_speed_ms
|
||||
# 目标半径(m)
|
||||
float64 radius_m
|
||||
# 圆弧扫角(度)
|
||||
float64 sweep_angle_deg
|
||||
# 是否顺时针
|
||||
bool clockwise
|
||||
+24
@@ -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
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
# =========================================================
|
||||
# 查询底盘能力请求
|
||||
# 作用:运行前确认当前车辆支持哪些动作
|
||||
# =========================================================
|
||||
|
||||
# 请求头
|
||||
calibration_common_interfaces/RequestHeader header
|
||||
+33
@@ -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
|
||||
+29
@@ -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
|
||||
+17
@@ -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
|
||||
+28
@@ -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
|
||||
+13
@@ -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
|
||||
+37
@@ -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
|
||||
+21
@@ -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
|
||||
+14
@@ -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
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
# =========================================================
|
||||
# 底盘工作模式请求
|
||||
# 作用:切换底盘到不同工作模式
|
||||
# =========================================================
|
||||
|
||||
# 请求头
|
||||
calibration_common_interfaces/RequestHeader header
|
||||
# 目标模式
|
||||
ChassisWorkMode target_mode
|
||||
# 切换原因
|
||||
string reason
|
||||
+17
@@ -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
|
||||
+29
@@ -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
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
# =========================================================
|
||||
# 模块协同转向命令
|
||||
# 作用:用于多舵轮模块协同转向与一致性测试
|
||||
# =========================================================
|
||||
|
||||
# 参与测试的模块 ID 列表
|
||||
string[] module_ids
|
||||
# 目标舵角(度)
|
||||
float64 target_angle_deg
|
||||
# 保持时长(秒)
|
||||
float64 hold_time_sec
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
# =========================================================
|
||||
# 斜移动作命令
|
||||
# 作用:用于多舵轮底盘斜向运动能力测试
|
||||
# =========================================================
|
||||
|
||||
# 目标速度(m/s)
|
||||
float64 target_speed_ms
|
||||
# 目标距离(m)
|
||||
float64 target_distance_m
|
||||
# 目标运动方向角(度)
|
||||
float64 heading_deg
|
||||
+25
@@ -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
|
||||
+6
@@ -0,0 +1,6 @@
|
||||
# =========================================================
|
||||
# 查询当前已生效底盘参数请求
|
||||
# =========================================================
|
||||
|
||||
# 请求头
|
||||
calibration_common_interfaces/RequestHeader header
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
# =========================================================
|
||||
# 原地旋转动作命令
|
||||
# 作用:用于差速、多舵轮等底盘的旋转能力测试
|
||||
# =========================================================
|
||||
|
||||
# 目标旋转角(度)
|
||||
float64 target_yaw_deg
|
||||
# 目标角速度(度/秒)
|
||||
float64 target_angular_vel_deg_s
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
# =========================================================
|
||||
# 横移动作命令
|
||||
# 作用:用于多舵轮底盘横向平移能力测试
|
||||
# =========================================================
|
||||
|
||||
# 目标横移速度(m/s)
|
||||
float64 target_speed_ms
|
||||
# 目标横移距离(m)
|
||||
float64 target_distance_m
|
||||
# 是否向左移动
|
||||
bool move_left
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
# =========================================================
|
||||
# 模块零位检查命令
|
||||
# 作用:用于多舵轮每个舵轮模块零位与安装状态检查
|
||||
# =========================================================
|
||||
|
||||
# 待检查模块 ID 列表
|
||||
string[] module_ids
|
||||
# 目标零位角(度)
|
||||
float64 target_zero_deg
|
||||
# 允许误差(度)
|
||||
float64 tolerance_deg
|
||||
+44
@@ -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
|
||||
+13
@@ -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
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
# =========================================================
|
||||
# 多舵轮专属参数
|
||||
# 作用:适用于多舵轮 AGV
|
||||
# =========================================================
|
||||
|
||||
# 各舵轮模块参数
|
||||
SteeringModuleCalibrationParam[] modules
|
||||
+21
@@ -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
|
||||
+24
@@ -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
|
||||
+13
@@ -0,0 +1,13 @@
|
||||
# =========================================================
|
||||
# 舵角扫动命令
|
||||
# 作用:用于阿克曼、单舵轮、多舵轮的转向零位和转向响应测试
|
||||
# =========================================================
|
||||
|
||||
# 目标舵角(度)
|
||||
float64 target_angle_deg
|
||||
# 扫动幅值(度)
|
||||
float64 sweep_amplitude_deg
|
||||
# 扫动频率(Hz)
|
||||
float64 sweep_frequency_hz
|
||||
# 持续时长(秒)
|
||||
float64 duration_sec
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
# =========================================================
|
||||
# 直线动作命令
|
||||
# 作用:用于验证直线行驶能力和线速度误差
|
||||
# =========================================================
|
||||
|
||||
# 目标线速度(m/s)
|
||||
float64 target_speed_ms
|
||||
# 目标距离(m)
|
||||
float64 target_distance_m
|
||||
# 是否倒车
|
||||
bool reverse
|
||||
+17
@@ -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
|
||||
+15
@@ -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
|
||||
+25
@@ -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>
|
||||
+114
-8
@@ -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; // 保持时长(秒)
|
||||
}
|
||||
|
||||
// =========================================================
|
||||
// 底盘动作原语请求
|
||||
// 作用:让车端执行一个标准化动作
|
||||
@@ -173,11 +251,15 @@ message MotionPrimitiveRequest {
|
||||
ArcCommand arc = 5; // 圆弧动作
|
||||
InPlaceRotationCommand in_place_rotation = 6; // 原地旋转
|
||||
SteeringSweepCommand steering_sweep = 7; // 舵角扫动
|
||||
LateralTranslationCommand lateral_translation = 8; // 横移
|
||||
DiagonalMotionCommand diagonal_motion = 9; // 斜移
|
||||
ModuleAlignmentCheckCommand module_alignment = 10; // 模块零位检查
|
||||
CoordinatedSteeringCommand coordinated_steering = 11; // 模块协同转向
|
||||
}
|
||||
|
||||
bool brake_when_finished = 8; // 结束后是否刹停
|
||||
double timeout_sec = 9; // 任务超时时间
|
||||
string source_iteration_id = 10; // 来源迭代号
|
||||
bool brake_when_finished = 12; // 结束后是否刹停
|
||||
double timeout_sec = 13; // 任务超时时间
|
||||
string source_iteration_id = 14; // 来源迭代号
|
||||
}
|
||||
|
||||
// =========================================================
|
||||
@@ -190,6 +272,7 @@ message StreamChassisTelemetryRequest {
|
||||
bool include_odom = 3; // 是否包含里程计
|
||||
bool include_wheel_state = 4; // 是否包含轮组状态
|
||||
bool include_driver_state = 5; // 是否包含驱动器状态
|
||||
bool include_module_state = 6; // 是否包含模块级状态
|
||||
}
|
||||
|
||||
// =========================================================
|
||||
@@ -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; // 估计曲率
|
||||
}
|
||||
|
||||
// =========================================================
|
||||
@@ -308,6 +397,21 @@ message ChassisCalibrationParameterSet {
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================
|
||||
// 底盘验证摘要
|
||||
// 作用:返回底盘专项自动验收的关键指标
|
||||
// =========================================================
|
||||
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; // 关联产物
|
||||
}
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
# =========================================================
|
||||
# 取消底盘任务
|
||||
# =========================================================
|
||||
|
||||
calibration_common_interfaces/JobQuery request
|
||||
---
|
||||
calibration_common_interfaces/StandardResponse response
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
# =========================================================
|
||||
# 写入底盘标定结果
|
||||
# =========================================================
|
||||
|
||||
CommitChassisCalibrationParametersRequest request
|
||||
---
|
||||
calibration_common_interfaces/StandardResponse response
|
||||
+12
@@ -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
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
# =========================================================
|
||||
# 查询当前已生效底盘标定参数
|
||||
# =========================================================
|
||||
|
||||
GetAppliedChassisCalibrationParametersRequest request
|
||||
---
|
||||
AppliedChassisCalibrationParametersResponse response
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user