feat:完善结构与仿真环境
This commit is contained in:
li-shihao-code
@@ -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,840 @@
|
||||
{
|
||||
"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/telemetry",
|
||||
"chassis_telemetry": "/chassis/telemetry",
|
||||
"control_telemetry": "/control/telemetry",
|
||||
"sensor_telemetry": "/sensor_calibration/telemetry"
|
||||
},
|
||||
"chassis_calibration": {
|
||||
"chassis_type": "ackermann",
|
||||
"straight_track_length_m": 5.0,
|
||||
"straight_track_width_m": 0.7,
|
||||
"arc_track_radius_m": 1.6,
|
||||
"wheel_radius_m": 0.1,
|
||||
"wheel_track_m": 0.52,
|
||||
"wheel_base_m": 0.8
|
||||
},
|
||||
"control_calibration": {
|
||||
"reference_path": [
|
||||
{
|
||||
"x_m": -2.5,
|
||||
"y_m": 0.0,
|
||||
"yaw_rad": 0.0,
|
||||
"target_speed_ms": 0.3
|
||||
},
|
||||
{
|
||||
"x_m": 2.5,
|
||||
"y_m": 0.0,
|
||||
"yaw_rad": 0.0,
|
||||
"target_speed_ms": 0.3
|
||||
}
|
||||
],
|
||||
"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
|
||||
},
|
||||
"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": 0.82,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.32,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": -1.5707963267948966,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"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.3,
|
||||
"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": 0.46,
|
||||
"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": 0.24,
|
||||
"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,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/telemetry
|
||||
```
|
||||
|
||||
## 执行端到端验收测试
|
||||
|
||||
仿真环境启动后，执行验收：
|
||||
|
||||
```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
|
||||
```
|
||||
Executable
+499
@@ -0,0 +1,499 @@
|
||||
#!/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
|
||||
ISAAC_WAIT_SEC="${ISAAC_WAIT_SEC:-180}"
|
||||
WORKSHOP_WARMUP_SEC="${WORKSHOP_WARMUP_SEC:-5}"
|
||||
START_GRACE_SEC="${START_GRACE_SEC:-2}"
|
||||
SMOKE_TASKS=""
|
||||
|
||||
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，直接继续启动后续链路。
|
||||
--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 本脚本各后台进程日志目录。
|
||||
|
||||
示例:
|
||||
./run_isaac_real_sim_test.sh --headless
|
||||
./run_isaac_real_sim_test.sh --headless --keep-running
|
||||
./run_isaac_real_sim_test.sh --no-smoke
|
||||
EOF
|
||||
}
|
||||
|
||||
log() {
|
||||
echo "[INFO] $*"
|
||||
}
|
||||
|
||||
warn() {
|
||||
echo "[WARN] $*" >&2
|
||||
}
|
||||
|
||||
die() {
|
||||
echo "[ERROR] $*" >&2
|
||||
exit 1
|
||||
}
|
||||
|
||||
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
|
||||
;;
|
||||
--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
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
sleep 3
|
||||
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
|
||||
|
||||
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} 仍未退出，发送 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
|
||||
}
|
||||
|
||||
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}"
|
||||
|
||||
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}'
|
||||
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/telemetry" "${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}'
|
||||
exec 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
|
||||
"
|
||||
|
||||
wait_for_ros_service "/workshop_v2/create_session" 60 || 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 [[ -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
|
||||
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}"
|
||||
+38
-120
@@ -1,141 +1,59 @@
|
||||
# 🧠 AGV 标定中央大脑
|
||||
# 源码目录结构
|
||||
|
||||
**环境**: Ubuntu 22.04 + ROS 2 Humble | **语言**: C++ | **通信**: gRPC over Wi-Fi 6
|
||||
`src` 按“仿真、核心逻辑、通信、现场部署”的边界组织，而不是按临时实验文件组织。
|
||||
|
||||
> 标定车间的"发令大脑"，通过局域网跨平台遥控 Windows 车端执行动作并拉取遥测数据。
|
||||
## 目录说明
|
||||
|
||||
---
|
||||
- `apps/`
|
||||
面向操作人员的工具和界面原型。
|
||||
|
||||
## 📋 目录
|
||||
- `communication/`
|
||||
ROS 2 接口包、TCP 帧协议、车间工控机到车端电脑的 gateway。这个层应该同时服务于仿真和现场部署。
|
||||
|
||||
1. [系统依赖安装](#1-系统依赖一键安装)
|
||||
2. [VS Code 插件配置](#2-vs-code-核心插件配置)
|
||||
3. [解决 IntelliSense 报错](#3-解决-vs-code-红色波浪线)
|
||||
4. [编译与运行](#4-编译与运行)
|
||||
- `core/`
|
||||
标定流程和标定算法，包括 `workshop_orchestrator`、底盘标定、运控标定、传感器标定、车辆参数管理等可复用核心逻辑。
|
||||
|
||||
---
|
||||
- `simulation/`
|
||||
部署前仿真验证代码。Isaac 车间、仿真车辆、仿真车端 agent、仿真传感器、仿真标定靶和旧版仿真包都放在这里。
|
||||
|
||||
## 1. 系统依赖一键安装
|
||||
- `deployment/`
|
||||
部署 profile 和从仿真迁移到现场前的检查清单。这里放配置基准，不放算法实现。
|
||||
|
||||
在 Ubuntu 22.04 终端执行以下命令：
|
||||
- `docs/`
|
||||
源码树内的设计说明、边界说明和迁移规则。
|
||||
|
||||
- `site_deployment/`
|
||||
真实现场部署代码，例如真实车端电脑适配器、真实车辆 SDK、PLC/CAN 或厂商控制器对接代码。
|
||||
|
||||
## 边界规则
|
||||
|
||||
- Isaac API 只放在 `simulation/`。
|
||||
- 真实车辆 SDK、PLC、CAN、厂商控制器相关代码只放在 `site_deployment/`。
|
||||
- ROS 2 接口、TCP 协议和 gateway 放在 `communication/`。
|
||||
- 编排流程和标定算法放在 `core/`。
|
||||
- 部署 profile 放在 `deployment/`。
|
||||
- 设计说明和迁移边界说明放在 `docs/`。
|
||||
|
||||
## 主要入口
|
||||
|
||||
Isaac 车间仿真：
|
||||
|
||||
```bash
|
||||
sudo apt update
|
||||
sudo apt install -y build-essential cmake pkg-config gdb
|
||||
sudo apt install -y protobuf-compiler-grpc libgrpc++-dev libprotobuf-dev protobuf-compiler
|
||||
python3 src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py
|
||||
```
|
||||
|
||||
> ⚠️ **警告**: 严禁自行去 GitHub 源码编译 gRPC，直接使用 Ubuntu 官方 APT 源即可，避免浪费时间与报错。
|
||||
|
||||
---
|
||||
|
||||
## 2. VS Code 核心插件配置
|
||||
|
||||
打开 VS Code → 扩展商店 (Extensions)，**必须安装**以下 4 个插件：
|
||||
|
||||
| 插件名称 | 开发者 | 用途 |
|
||||
|---------|--------|------|
|
||||
| **C/C++** | Microsoft | 代码补全与 GDB 调试 |
|
||||
| **CMake Tools** | Microsoft | 底部快速构建状态栏 |
|
||||
| **ROS** | Microsoft | 自动识别 `colcon` 工作空间 |
|
||||
| **vscode-proto3** | zxh404 | `.proto` 文件语法高亮 |
|
||||
|
||||
---
|
||||
|
||||
## 3. 解决 VS Code 红色波浪线 (IntelliSense 报错)
|
||||
|
||||
**问题原因**: gRPC 生成的 `.pb.h` 文件在 `colcon build` 阶段动态生成于 `build/` 目录，VS Code 初始无法识别。
|
||||
|
||||
**修复步骤**:
|
||||
|
||||
1. 按 `Ctrl+Shift+P` → 输入 `C/C++: Edit Configurations (JSON)`
|
||||
2. 确保 `c_cpp_properties.json` 包含以下配置：
|
||||
|
||||
```json
|
||||
{
|
||||
"configurations": [
|
||||
{
|
||||
"name": "ROS2",
|
||||
"includePath": [
|
||||
"${workspaceFolder}/**",
|
||||
"/opt/ros/humble/include/**",
|
||||
"${workspaceFolder}/build/agv_calib_brain/grpc_gen/**"
|
||||
],
|
||||
"compilerPath": "/usr/bin/gcc",
|
||||
"cStandard": "c17",
|
||||
"cppStandard": "c++17",
|
||||
"intelliSenseMode": "linux-gcc-x64"
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
> 💡 **提示**: `grpc_gen` 是 CMakeLists 中配置的自动生成源码路径，请根据实际情况微调。
|
||||
|
||||
---
|
||||
|
||||
## 4. 编译与运行 (CMake 自动化)
|
||||
|
||||
### 4.0 最小联调闭环
|
||||
|
||||
当前仓库已经补齐了一个最小可运行闭环：
|
||||
|
||||
- `vehicle_profile_manager`：提供默认车辆画像
|
||||
- `external_localization_service`：提供外部真值校核的最小执行端
|
||||
- `workshop_orchestrator_v2`：负责编排会话、计划和报告
|
||||
|
||||
启动顺序：
|
||||
仿真车端 agent：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py
|
||||
python3 src/simulation/vehicle_agent_sim/scripts/isaac_vehicle_agent_sim.py
|
||||
```
|
||||
|
||||
如果只想单独跑编排器：
|
||||
按 profile 启动完整仿真链路：
|
||||
|
||||
```bash
|
||||
ros2 launch workshop_orchestrator_v2 workshop_orchestrator_v2.launch.py
|
||||
python3 src/simulation/tools/launch_sim_stack.py
|
||||
```
|
||||
|
||||
可先调用这些接口做联调：
|
||||
这条链路中，车间工控机与车端电脑之间的底盘、运控、外部真值位姿和传感器数据都通过 TCP/WiFi6 仿真边界传输；Isaac topic 只留在仿真内部。
|
||||
|
||||
- `/vehicle_profile_manager/get_vehicle_profile`
|
||||
- `/vehicle_profile_manager/evaluate_vehicle_calibration_applicability`
|
||||
- `/external_localization/get_readiness`
|
||||
- `/external_localization/execute_task`
|
||||
- `/workshop_v2/create_session`
|
||||
- `/workshop_v2/execute_session`
|
||||
- `/workshop_v2/get_report`
|
||||
|
||||
最小会话建议至少包含：
|
||||
|
||||
- `session.config.localization_source_id = demo_vehicle_001`
|
||||
- `session.config.workcell_zone_id = demo_workcell`
|
||||
- 一个 `requested_tasks`，其中 `stage_type = EXTERNAL_REFERENCE_READY_CHECK_STAGE`
|
||||
- 该任务的 `task_params` 至少包含：
|
||||
- `external.static_sample_count`
|
||||
- `external.dynamic_sample_count`
|
||||
- `external.max_position_stddev_m`
|
||||
- `external.max_yaw_stddev_rad`
|
||||
- `external.max_tracking_loss_ratio`
|
||||
- `external.max_time_sync_offset_ms`
|
||||
- `external.timeout_sec`
|
||||
|
||||
|
||||
> ✨ **无需手动执行 `protoc`** —— CMakeLists.txt 已配置自动化脚本，编译时自动生成 C++ 网络源码。
|
||||
|
||||
### 4.1 编译
|
||||
|
||||
```bash
|
||||
# 回到工作空间根目录（如 ~/agv_ws）
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --packages-select agv_calib_brain --symlink-install
|
||||
```
|
||||
|
||||
### 4.2 运行
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
ros2 run agv_calib_brain brain_node
|
||||
```
|
||||
车间 gateway 和 `workshop_orchestrator` 仍然按 ROS 2 包名启动；源码分别在 `communication/` 和 `core/` 下。
|
||||
|
||||
-24
@@ -1,24 +0,0 @@
|
||||
#include "chassis_calibration_service/chassis_calibration_common.hpp"
|
||||
|
||||
namespace chassis_calibration_service
|
||||
{
|
||||
|
||||
bool DifferentialChassisAlgorithm::run(
|
||||
const ChassisCalibrationInput & input,
|
||||
ChassisCalibrationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// TODO: 在这里填写差速底盘标定算法。
|
||||
// 算法工程师应从这里读取并计算：
|
||||
// - 任务请求：input.request（request_id、任务目的、selected_primitive、straight_line 参数、timeout 等）
|
||||
// - 底盘反馈：由上层扩展到 ChassisCalibrationInput 中的实时数据（轮速、左右电机反馈、IMU、里程计、定位等）
|
||||
// - 输出：output.response.result（success、error_code、validation_summary、estimated_params、artifacts 等）
|
||||
fill_common_result(input, output);
|
||||
output.response.result.message = "differential chassis template executed.";
|
||||
output.response.result.recommended_parameter_version = "differential_template_v1";
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace chassis_calibration_service
|
||||
-33
@@ -1,33 +0,0 @@
|
||||
#include "control_calibration_service/control_calibration_common.hpp"
|
||||
|
||||
namespace control_calibration_service
|
||||
{
|
||||
|
||||
bool PurePursuitControlCalibrationAlgorithm::run(
|
||||
const ControlCalibrationInput & input,
|
||||
ControlCalibrationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// Pure Pursuit 控制标定模板：
|
||||
// 适合基于参考轨迹和前视点策略的横向控制评估。
|
||||
// 算法工程师通常会在这里处理：
|
||||
// 1. 参考轨迹读取：
|
||||
// - input.reference_trajectory
|
||||
// - input.reference_stop_at_end / input.reference_timeout_sec
|
||||
// 2. 观测输入：
|
||||
// - input.control_telemetry_history 中的横向误差、航向误差、转向输出
|
||||
// - input.chassis_telemetry_history 中的速度、姿态和底盘运动状态
|
||||
// - input.truth_source_diagnostics / 真值历史，用于判断轨迹对齐是否可靠
|
||||
// 3. 参数输出：
|
||||
// - 可将前视距离、速度相关增益等写入 output.response.result.estimated_parameter_set
|
||||
// 4. 验收输出：
|
||||
// - 将最大误差、均方误差、振荡情况、自动验收结论写入 validation_summary
|
||||
fill_common_result(input, output);
|
||||
output.response.result.message = "pure pursuit control calibration template executed.";
|
||||
output.response.result.recommended_parameter_version = "pure_pursuit_template_v1";
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace control_calibration_service
|
||||
-120
@@ -1,120 +0,0 @@
|
||||
#include "external_localization_service/external_localization_service_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "calibration_common_interfaces/msg/error_code.hpp"
|
||||
#include "calibration_common_interfaces/msg/job_state.hpp"
|
||||
|
||||
namespace external_localization_service
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
int64_t now_us()
|
||||
{
|
||||
return std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch())
|
||||
.count();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
using calibration_common_interfaces::msg::ErrorCode;
|
||||
using calibration_common_interfaces::msg::JobState;
|
||||
|
||||
ExternalLocalizationServiceNode::ExternalLocalizationServiceNode(const rclcpp::NodeOptions & options)
|
||||
: Node("external_localization_service", options)
|
||||
{
|
||||
readiness_service_ = create_service<ReadinessSrv>(
|
||||
"/external_localization/get_readiness",
|
||||
std::bind(&ExternalLocalizationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
|
||||
this,
|
||||
"/external_localization/execute_task",
|
||||
std::bind(&ExternalLocalizationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&ExternalLocalizationServiceNode::handle_cancel, this, std::placeholders::_1),
|
||||
std::bind(&ExternalLocalizationServiceNode::handle_accepted, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void ExternalLocalizationServiceNode::handle_readiness(
|
||||
const std::shared_ptr<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> response)
|
||||
{
|
||||
(void)request;
|
||||
|
||||
// 这里先保留最小 readiness 逻辑。
|
||||
// 后续若接入真实外部定位设备/真值源桥接程序，可在这里增加：
|
||||
// - 真值源在线检查
|
||||
// - 同步状态检查
|
||||
// - 覆盖范围检查
|
||||
// - 观测质量检查
|
||||
// - 切源稳定性检查
|
||||
response->response.success = true;
|
||||
response->response.error_code.code = ErrorCode::OK;
|
||||
response->response.message = "external_localization_service is ready.";
|
||||
response->response.agent_ready = true;
|
||||
response->response.ready_for_reference_validation = true;
|
||||
response->response.checked_timestamp_us = now_us();
|
||||
response->response.validation_summary.time_sync_ok = true;
|
||||
response->response.validation_summary.coverage_ok = true;
|
||||
response->response.validation_summary.quality_ok = true;
|
||||
response->response.validation_summary.tracking_stable = true;
|
||||
response->response.validation_summary.recommended_as_truth_source = true;
|
||||
response->response.validation_summary.position_stddev_m = 0.0;
|
||||
response->response.validation_summary.yaw_stddev_rad = 0.0;
|
||||
response->response.validation_summary.tracking_loss_ratio = 0.0;
|
||||
response->response.validation_summary.time_sync_offset_ms = 0.0;
|
||||
}
|
||||
|
||||
rclcpp_action::GoalResponse ExternalLocalizationServiceNode::handle_goal(
|
||||
const rclcpp_action::GoalUUID & /*uuid*/,
|
||||
std::shared_ptr<const ExecuteTask::Goal> goal)
|
||||
{
|
||||
std::string reject_reason;
|
||||
if (!executor_.validate_goal(*goal, reject_reason)) {
|
||||
RCLCPP_WARN(get_logger(), "Reject external_localization goal: %s", reject_reason.c_str());
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse ExternalLocalizationServiceNode::handle_cancel(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> /*goal_handle*/)
|
||||
{
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void ExternalLocalizationServiceNode::handle_accepted(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
std::thread(std::bind(&ExternalLocalizationServiceNode::execute_goal, this, goal_handle)).detach();
|
||||
}
|
||||
|
||||
void ExternalLocalizationServiceNode::execute_goal(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
auto feedback = std::make_shared<ExecuteTask::Feedback>();
|
||||
feedback->feedback.job_id = goal_handle->get_goal()->goal.header.request_id;
|
||||
feedback->feedback.state.state = JobState::RUNNING;
|
||||
feedback->feedback.progress = 0.5;
|
||||
feedback->feedback.error_code.code = ErrorCode::OK;
|
||||
feedback->feedback.message = "external localization task is running.";
|
||||
feedback->feedback.server_timestamp_us = now_us();
|
||||
feedback->feedback.safe_to_retry = false;
|
||||
goal_handle->publish_feedback(feedback);
|
||||
|
||||
auto result = std::make_shared<ExecuteTask::Result>();
|
||||
std::string failure_reason;
|
||||
if (!executor_.build_result(*goal_handle->get_goal(), *result, failure_reason)) {
|
||||
result->result.success = false;
|
||||
result->result.error_code.code = ErrorCode::INVALID_ARGUMENT;
|
||||
result->result.message = failure_reason;
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
|
||||
goal_handle->succeed(result);
|
||||
}
|
||||
|
||||
} // namespace external_localization_service
|
||||
-66
@@ -1,66 +0,0 @@
|
||||
#include "external_localization_service/external_localization_common.hpp"
|
||||
|
||||
namespace external_localization_service
|
||||
{
|
||||
|
||||
bool MarkerAlignmentAlgorithm::run(
|
||||
const ExternalLocalizationInput & input,
|
||||
ExternalLocalizationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// 标靶对齐模板：
|
||||
// 【本文件负责什么】
|
||||
// - 负责标靶检测结果读取、坐标系对齐求解和残差统计相关算法实现。
|
||||
// - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。
|
||||
//
|
||||
// 【建议优先读取的输入】
|
||||
// 1. input.marker_alignment_task
|
||||
// - target_board_id、min_valid_observation_count、timeout_sec 等关键约束。
|
||||
// 2. input.latest_external_localization_telemetry / input.external_localization_telemetry_history
|
||||
// - 读取观测位姿、标准差、丢失率、时间同步偏差和质量评分。
|
||||
// 3. input.marker_alignment_diagnostics
|
||||
// - 读取标靶检测失败、角点不足、姿态求解不稳定等问题描述。
|
||||
// 4. input.sensor_quality_diagnostics
|
||||
// - 如果标靶检测依赖相机 / LiDAR 质量，可在这里读取辅助质量信息。
|
||||
//
|
||||
// 【必写输出】
|
||||
// 1. output.response.result.result.workshop_to_localization
|
||||
// - 这是标靶对齐最核心的输出结果。
|
||||
// 2. output.response.result.result.residual_error_m / residual_error_rad
|
||||
// - 写回对齐残差，供 orchestrator 判断是否自动验收。
|
||||
// 3. output.response.result.validation_summary
|
||||
// - 写位置标准差、航向标准差、时间同步偏差等摘要。
|
||||
//
|
||||
// 【可选输出】
|
||||
// - output.response.result.artifacts
|
||||
// 可挂标靶检测日志、可视化结果、拟合报告、残差统计文件等。
|
||||
//
|
||||
// 【常见失败原因】
|
||||
// - 有效观测数不足、标靶检测失败、姿态求解不稳定、时间同步异常、质量评分过低。
|
||||
//
|
||||
// 【在这里添加真实算法】
|
||||
// - 请在 fill_external_localization_common_success(...) 之前或之后补充真实对齐求解逻辑。
|
||||
// - 当前文件仅提供交付模板，不包含真实外部定位算法。
|
||||
|
||||
fill_external_localization_common_success(
|
||||
input,
|
||||
output,
|
||||
"标靶对齐完成。",
|
||||
"demo_external_marker_alignment_v1");
|
||||
|
||||
output.response.result.result.workshop_frame_id = "workshop";
|
||||
output.response.result.result.localization_frame_id = "localization";
|
||||
output.response.result.result.workshop_to_localization.z_m = 0.0;
|
||||
output.response.result.result.position_repeatability_m = 0.0;
|
||||
output.response.result.result.yaw_repeatability_rad = 0.0;
|
||||
output.response.result.result.residual_error_m = 0.0;
|
||||
output.response.result.result.residual_error_rad = 0.0;
|
||||
output.response.result.result.tracking_loss_ratio = 0.0;
|
||||
output.response.result.result.time_sync_offset_ms = 0.0;
|
||||
output.response.result.result.validated_as_truth_source = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace external_localization_service
|
||||
-65
@@ -1,65 +0,0 @@
|
||||
#include "external_localization_service/external_localization_common.hpp"
|
||||
|
||||
namespace external_localization_service
|
||||
{
|
||||
|
||||
bool ReferencePoseCollectionAlgorithm::run(
|
||||
const ExternalLocalizationInput & input,
|
||||
ExternalLocalizationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// 参考位姿采集模板：
|
||||
// 【本文件负责什么】
|
||||
// - 负责参考位姿采集与静态重复性分析相关算法实现。
|
||||
// - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。
|
||||
//
|
||||
// 【建议优先读取的输入】
|
||||
// 1. input.reference_pose_collection_task
|
||||
// - sample_count、require_vehicle_static、timeout_sec、min_sample_interval_sec 等采样约束。
|
||||
// 2. input.latest_external_localization_telemetry / input.external_localization_telemetry_history
|
||||
// - 每帧外部定位位姿、位置标准差、航向标准差、时间同步偏差、质量评分。
|
||||
// 3. input.latest_chassis_telemetry / input.chassis_telemetry_history
|
||||
// - 用于判断车辆是否真实静止，避免采入无效参考位姿。
|
||||
// 4. input.truth_source_diagnostics / input.acquisition_diagnostics
|
||||
// - 记录时间同步异常、观测缺失、采样不足、落盘失败等问题。
|
||||
//
|
||||
// 【必写输出】
|
||||
// 1. output.response.result.validation_summary
|
||||
// - 写位置标准差、航向标准差、时间同步偏差、是否推荐为真值源等摘要。
|
||||
// 2. output.response.result.result
|
||||
// - 写 workshop_frame_id / localization_frame_id / repeatability / residual 等结果。
|
||||
// 3. output.response.result.suitable_for_commit
|
||||
// - 明确当前采集结果是否建议进入下一阶段。
|
||||
//
|
||||
// 【可选输出】
|
||||
// - output.response.result.artifacts
|
||||
// 可挂采样日志、原始位姿文件、统计报告等文件引用。
|
||||
//
|
||||
// 【常见失败原因】
|
||||
// - 车辆未静止、有效样本不足、时间同步超标、外部定位观测丢失、采样频率不足。
|
||||
//
|
||||
// 【在这里添加真实算法】
|
||||
// - 请在 fill_external_localization_common_success(...) 之前或之后补充真实采样与统计逻辑。
|
||||
// - 当前文件仅提供交付模板，不包含真实外部定位算法。
|
||||
|
||||
fill_external_localization_common_success(
|
||||
input,
|
||||
output,
|
||||
"参考位姿采集完成。",
|
||||
"demo_external_reference_pose_collection_v1");
|
||||
|
||||
output.response.result.result.workshop_frame_id = "workshop";
|
||||
output.response.result.result.localization_frame_id = "localization";
|
||||
output.response.result.result.position_repeatability_m = 0.0;
|
||||
output.response.result.result.yaw_repeatability_rad = 0.0;
|
||||
output.response.result.result.residual_error_m = 0.0;
|
||||
output.response.result.result.residual_error_rad = 0.0;
|
||||
output.response.result.result.tracking_loss_ratio = 0.0;
|
||||
output.response.result.result.time_sync_offset_ms = 0.0;
|
||||
output.response.result.result.validated_as_truth_source = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace external_localization_service
|
||||
-77
@@ -1,77 +0,0 @@
|
||||
#include "external_localization_service/external_localization_common.hpp"
|
||||
|
||||
namespace external_localization_service
|
||||
{
|
||||
|
||||
bool TruthSourceValidationAlgorithm::run(
|
||||
const ExternalLocalizationInput & input,
|
||||
ExternalLocalizationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// 真值源验证模板：
|
||||
// 【本文件负责什么】
|
||||
// - 负责静态重复性、动态稳定性、时间同步与丢失率等真值源验证算法实现。
|
||||
// - 后续算法工程师应主要修改本文件，不要改 node 层和模板分发层。
|
||||
//
|
||||
// 【建议优先读取的输入】
|
||||
// 1. input.truth_source_validation_task
|
||||
// - static_sample_count、dynamic_sample_count、require_short_motion_segment 等任务要求。
|
||||
// 2. input.external_localization_telemetry_history
|
||||
// - 外部定位历史观测窗口，是稳定性、同步性、重复性分析的核心输入。
|
||||
// 3. input.chassis_telemetry_history / input.control_telemetry_history
|
||||
// - 如果要求短运动段验证，需要结合车辆实际运动状态和控制输出做时序对齐。
|
||||
// 4. input.sensor_telemetry_history
|
||||
// - 用于判断辅助传感器质量是否影响外部定位观测可信度。
|
||||
// 5. input.truth_source_diagnostics
|
||||
// - 记录时间同步超标、观测丢失、切源异常等问题。
|
||||
//
|
||||
// 【必写输出】
|
||||
// 1. output.response.result.validation_summary
|
||||
// - 这是 orchestrator 自动验收最关键的摘要区域。
|
||||
// 2. output.response.result.result
|
||||
// - 写 repeatability、tracking_loss_ratio、time_sync_offset_ms 等核心结果。
|
||||
// 3. output.response.result.data_quality_passed / suitable_for_commit
|
||||
// - 明确当前真值源是否可进入后续标定闭环。
|
||||
//
|
||||
// 【可选输出】
|
||||
// - output.response.result.artifacts
|
||||
// 可挂稳定性分析报告、同步统计图、丢失率分析文件等。
|
||||
//
|
||||
// 【常见失败原因】
|
||||
// - 动态窗口不足、时间同步偏差超阈值、观测丢失率过高、重复性不满足要求。
|
||||
//
|
||||
// 【在这里添加真实算法】
|
||||
// - 请在 fill_external_localization_common_success(...) 之前或之后补充真实验证逻辑。
|
||||
// - 当前文件仅提供交付模板，不包含真实外部定位算法。
|
||||
|
||||
fill_external_localization_common_success(
|
||||
input,
|
||||
output,
|
||||
"真值源验证完成。",
|
||||
"demo_external_truth_source_validation_v1");
|
||||
|
||||
output.response.result.validation_summary.time_sync_ok = true;
|
||||
output.response.result.validation_summary.coverage_ok = true;
|
||||
output.response.result.validation_summary.quality_ok = true;
|
||||
output.response.result.validation_summary.tracking_stable = true;
|
||||
output.response.result.validation_summary.recommended_as_truth_source = true;
|
||||
output.response.result.validation_summary.position_stddev_m = 0.0;
|
||||
output.response.result.validation_summary.yaw_stddev_rad = 0.0;
|
||||
output.response.result.validation_summary.tracking_loss_ratio = 0.0;
|
||||
output.response.result.validation_summary.time_sync_offset_ms = 0.0;
|
||||
|
||||
output.response.result.result.workshop_frame_id = "workshop";
|
||||
output.response.result.result.localization_frame_id = "localization";
|
||||
output.response.result.result.position_repeatability_m = 0.0;
|
||||
output.response.result.result.yaw_repeatability_rad = 0.0;
|
||||
output.response.result.result.residual_error_m = 0.0;
|
||||
output.response.result.result.residual_error_rad = 0.0;
|
||||
output.response.result.result.tracking_loss_ratio = 0.0;
|
||||
output.response.result.result.time_sync_offset_ms = 0.0;
|
||||
output.response.result.result.validated_as_truth_source = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace external_localization_service
|
||||
-167
@@ -1,167 +0,0 @@
|
||||
#include "sensor_calibration_service/sensor_calibration_service_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "calibration_common_interfaces/msg/error_code.hpp"
|
||||
#include "calibration_common_interfaces/msg/job_state.hpp"
|
||||
#include "calibration_sensor_interfaces/msg/sensor_calibration_job_result.hpp"
|
||||
#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp"
|
||||
|
||||
namespace sensor_calibration_service
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
int64_t now_us()
|
||||
{
|
||||
return std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch())
|
||||
.count();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
using calibration_common_interfaces::msg::ErrorCode;
|
||||
using calibration_common_interfaces::msg::JobState;
|
||||
|
||||
SensorCalibrationServiceNode::SensorCalibrationServiceNode(const rclcpp::NodeOptions & options)
|
||||
: Node("sensor_calibration_service", options)
|
||||
{
|
||||
readiness_service_ = create_service<ReadinessSrv>(
|
||||
"/sensor_calibration/get_readiness",
|
||||
std::bind(&SensorCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
|
||||
this,
|
||||
"/sensor_calibration/execute_task",
|
||||
std::bind(&SensorCalibrationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&SensorCalibrationServiceNode::handle_cancel, this, std::placeholders::_1),
|
||||
std::bind(&SensorCalibrationServiceNode::handle_accepted, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void SensorCalibrationServiceNode::handle_readiness(
|
||||
const std::shared_ptr<ReadinessSrv::Request> request,
|
||||
std::shared_ptr<ReadinessSrv::Response> response)
|
||||
{
|
||||
(void)request;
|
||||
response->response.success = true;
|
||||
response->response.error_code.code = ErrorCode::OK;
|
||||
response->response.message = "sensor_calibration_service is ready.";
|
||||
response->response.agent_ready = true;
|
||||
response->response.capture_pipeline_ready = true;
|
||||
response->response.storage_ready = true;
|
||||
response->response.telemetry_ready = true;
|
||||
response->response.vehicle_safe_to_move = true;
|
||||
response->response.arm_ready = true;
|
||||
response->response.ready_sensor_ids.push_back("demo_sensor_001");
|
||||
response->response.checked_timestamp_us = now_us();
|
||||
}
|
||||
|
||||
rclcpp_action::GoalResponse SensorCalibrationServiceNode::handle_goal(
|
||||
const rclcpp_action::GoalUUID & /*uuid*/,
|
||||
std::shared_ptr<const ExecuteTask::Goal> goal)
|
||||
{
|
||||
if (goal->goal.header.request_id.empty()) {
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse SensorCalibrationServiceNode::handle_cancel(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> /*goal_handle*/)
|
||||
{
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void SensorCalibrationServiceNode::handle_accepted(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
std::thread(std::bind(&SensorCalibrationServiceNode::execute_goal, this, goal_handle)).detach();
|
||||
}
|
||||
|
||||
void SensorCalibrationServiceNode::execute_goal(
|
||||
const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
|
||||
{
|
||||
// 先回一帧 RUNNING feedback，告诉 orchestrator 当前任务已经进入执行阶段。
|
||||
auto feedback = std::make_shared<ExecuteTask::Feedback>();
|
||||
feedback->feedback.state.state = JobState::RUNNING;
|
||||
goal_handle->publish_feedback(feedback);
|
||||
|
||||
// ===== 组装算法输入上下文 =====
|
||||
// 当前模板阶段先把“算法最常用的任务侧输入”显式展开。
|
||||
// 后续如果要接真实车辆画像、已生效参数查询、历史遥测缓存、外部定位缓存，
|
||||
// 也应继续在这里补齐并写入 SensorCalibrationInput。
|
||||
SensorCalibrationAlgorithmTemplate::Input input;
|
||||
input.request = *goal_handle->get_goal();
|
||||
input.task_type = goal_handle->get_goal()->goal.selected_task;
|
||||
input.task_subtype = goal_handle->get_goal()->goal.task_subtype;
|
||||
input.target_sensor_id = resolve_target_sensor_id(*goal_handle->get_goal());
|
||||
input.camera_intrinsic_task = goal_handle->get_goal()->goal.camera_intrinsic;
|
||||
input.imu_intrinsic_task = goal_handle->get_goal()->goal.imu_intrinsic;
|
||||
input.sensor_to_base_extrinsic_task = goal_handle->get_goal()->goal.sensor_to_base_extrinsic;
|
||||
input.hand_eye_task = goal_handle->get_goal()->goal.hand_eye;
|
||||
input.required_image_count = goal_handle->get_goal()->goal.camera_intrinsic.required_image_count;
|
||||
input.required_static_segment_count = goal_handle->get_goal()->goal.imu_intrinsic.required_static_segment_count;
|
||||
input.required_motion_segment_count = goal_handle->get_goal()->goal.imu_intrinsic.required_motion_segment_count;
|
||||
input.required_sample_count = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.required_sample_count;
|
||||
input.required_pose_count = goal_handle->get_goal()->goal.hand_eye.required_pose_count;
|
||||
switch (goal_handle->get_goal()->goal.selected_task.value) {
|
||||
case TaskType::CAMERA_INTRINSIC:
|
||||
input.reference_timeout_sec = goal_handle->get_goal()->goal.camera_intrinsic.timeout_sec;
|
||||
break;
|
||||
case TaskType::IMU_INTRINSIC:
|
||||
input.reference_timeout_sec = goal_handle->get_goal()->goal.imu_intrinsic.timeout_sec;
|
||||
break;
|
||||
case TaskType::SENSOR_TO_BASE_EXTRINSIC:
|
||||
input.reference_timeout_sec = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.timeout_sec;
|
||||
break;
|
||||
case TaskType::HAND_EYE:
|
||||
input.reference_timeout_sec = goal_handle->get_goal()->goal.hand_eye.timeout_sec;
|
||||
break;
|
||||
default:
|
||||
input.reference_timeout_sec = 0.0;
|
||||
break;
|
||||
}
|
||||
input.reference_board_id = goal_handle->get_goal()->goal.camera_intrinsic.target_board_id;
|
||||
input.reference_base_frame_id = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.base_frame_id;
|
||||
input.reference_arm_id = goal_handle->get_goal()->goal.hand_eye.arm_id;
|
||||
input.sensor_history_available = false;
|
||||
input.chassis_history_available = false;
|
||||
input.control_history_available = false;
|
||||
input.truth_history_available = false;
|
||||
|
||||
SensorCalibrationAlgorithmTemplate::Output output;
|
||||
std::string failure_reason;
|
||||
if (!algorithm_.run(input, output, failure_reason)) {
|
||||
auto result = std::make_shared<ExecuteTask::Result>();
|
||||
result->result.success = false;
|
||||
result->result.error_code.code = ErrorCode::INVALID_STATE;
|
||||
result->result.message = failure_reason;
|
||||
result->result.job_id = goal_handle->get_goal()->goal.header.request_id;
|
||||
result->result.data_quality_passed = false;
|
||||
result->result.suitable_for_commit = false;
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
|
||||
auto result = std::make_shared<ExecuteTask::Result>(output.response);
|
||||
goal_handle->succeed(result);
|
||||
}
|
||||
|
||||
std::string SensorCalibrationServiceNode::resolve_target_sensor_id(const ExecuteTask::Goal & goal) const
|
||||
{
|
||||
switch (goal.goal.selected_task.value) {
|
||||
case TaskType::CAMERA_INTRINSIC:
|
||||
return goal.goal.camera_intrinsic.sensor_id;
|
||||
case TaskType::IMU_INTRINSIC:
|
||||
return goal.goal.imu_intrinsic.sensor_id;
|
||||
case TaskType::SENSOR_TO_BASE_EXTRINSIC:
|
||||
return goal.goal.sensor_to_base_extrinsic.sensor_id;
|
||||
case TaskType::HAND_EYE:
|
||||
return goal.goal.hand_eye.sensor_id;
|
||||
default:
|
||||
return "";
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sensor_calibration_service
|
||||
-76
@@ -1,76 +0,0 @@
|
||||
#include "sensor_calibration_service/sensor_calibration_common.hpp"
|
||||
|
||||
namespace sensor_calibration_service
|
||||
{
|
||||
|
||||
bool FrontCameraExtrinsicCalibrationAlgorithm::run(
|
||||
const SensorCalibrationInput & input,
|
||||
SensorCalibrationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// 前视相机到 base_link 外参标定模板。
|
||||
fill_common_result(input, output);
|
||||
output.response.result.message = "front camera extrinsic calibration template executed.";
|
||||
output.response.result.recommended_parameter_version = "front_camera_extrinsic_template_v1";
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DownwardCameraExtrinsicCalibrationAlgorithm::run(
|
||||
const SensorCalibrationInput & input,
|
||||
SensorCalibrationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// 下视相机到 base_link 外参标定模板。
|
||||
fill_common_result(input, output);
|
||||
output.response.result.message = "downward camera extrinsic calibration template executed.";
|
||||
output.response.result.recommended_parameter_version = "downward_camera_extrinsic_template_v1";
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ImuExtrinsicCalibrationAlgorithm::run(
|
||||
const SensorCalibrationInput & input,
|
||||
SensorCalibrationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// IMU 到 base_link 外参标定模板。
|
||||
fill_common_result(input, output);
|
||||
output.response.result.message = "imu extrinsic calibration template executed.";
|
||||
output.response.result.recommended_parameter_version = "imu_extrinsic_template_v1";
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Lidar2DExtrinsicCalibrationAlgorithm::run(
|
||||
const SensorCalibrationInput & input,
|
||||
SensorCalibrationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// 2D 激光雷达到 base_link 外参标定模板。
|
||||
fill_common_result(input, output);
|
||||
output.response.result.message = "2d lidar extrinsic calibration template executed.";
|
||||
output.response.result.recommended_parameter_version = "lidar_2d_extrinsic_template_v1";
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Lidar3DExtrinsicCalibrationAlgorithm::run(
|
||||
const SensorCalibrationInput & input,
|
||||
SensorCalibrationOutput & output,
|
||||
std::string & failure_reason) const
|
||||
{
|
||||
(void)failure_reason;
|
||||
|
||||
// 3D 激光雷达到 base_link 外参标定模板。
|
||||
fill_common_result(input, output);
|
||||
output.response.result.message = "3d lidar extrinsic calibration template executed.";
|
||||
output.response.result.recommended_parameter_version = "lidar_3d_extrinsic_template_v1";
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace sensor_calibration_service
|
||||
-180
@@ -1,180 +0,0 @@
|
||||
#include "vehicle_profile_manager/vehicle_profile_manager_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
|
||||
#include "calibration_common_interfaces/msg/error_code.hpp"
|
||||
#include "calibration_vehicle_profile_interfaces/msg/chassis_type.hpp"
|
||||
#include "calibration_vehicle_profile_interfaces/msg/workflow_stage_type.hpp"
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
namespace vehicle_profile_manager
|
||||
{
|
||||
|
||||
using calibration_common_interfaces::msg::ErrorCode;
|
||||
using calibration_vehicle_profile_interfaces::msg::ChassisType;
|
||||
using calibration_vehicle_profile_interfaces::msg::WorkflowStageType;
|
||||
|
||||
VehicleProfileManagerNode::VehicleProfileManagerNode(const rclcpp::NodeOptions & options)
|
||||
: Node("vehicle_profile_manager", options)
|
||||
{
|
||||
get_profile_service_ = create_service<GetProfileSrv>(
|
||||
"/vehicle_profile_manager/get_vehicle_profile",
|
||||
std::bind(
|
||||
&VehicleProfileManagerNode::handle_get_profile, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
register_service_ = create_service<RegisterSrv>(
|
||||
"/vehicle_profile_manager/register_or_update_vehicle_profile",
|
||||
std::bind(
|
||||
&VehicleProfileManagerNode::handle_register, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
applicability_service_ = create_service<ApplicabilitySrv>(
|
||||
"/vehicle_profile_manager/evaluate_vehicle_calibration_applicability",
|
||||
std::bind(
|
||||
&VehicleProfileManagerNode::handle_applicability, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
heartbeat_service_ = create_service<HeartbeatSrv>(
|
||||
"/vehicle_profile_manager/heartbeat",
|
||||
std::bind(
|
||||
&VehicleProfileManagerNode::handle_heartbeat, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
load_demo_profile();
|
||||
|
||||
RCLCPP_INFO(get_logger(), "VehicleProfileManagerNode 启动，已预载 demo 车辆画像。");
|
||||
}
|
||||
|
||||
void VehicleProfileManagerNode::handle_get_profile(
|
||||
const std::shared_ptr<GetProfileSrv::Request> request,
|
||||
std::shared_ptr<GetProfileSrv::Response> response)
|
||||
{
|
||||
const auto & vehicle_id = request->request.vehicle_id;
|
||||
auto it = profiles_.find(vehicle_id);
|
||||
if (it == profiles_.end()) {
|
||||
response->response.success = false;
|
||||
response->response.error_code.code = ErrorCode::INVALID_ARGUMENT;
|
||||
response->response.message = "找不到 vehicle_id=[" + vehicle_id + "] 的车辆画像。";
|
||||
return;
|
||||
}
|
||||
response->response.success = true;
|
||||
response->response.error_code.code = ErrorCode::OK;
|
||||
response->response.message = "查询成功。";
|
||||
response->response.profile = it->second;
|
||||
}
|
||||
|
||||
void VehicleProfileManagerNode::handle_register(
|
||||
const std::shared_ptr<RegisterSrv::Request> request,
|
||||
std::shared_ptr<RegisterSrv::Response> response)
|
||||
{
|
||||
const auto & vehicle_id = request->request.profile.base_info.vehicle_id;
|
||||
if (vehicle_id.empty()) {
|
||||
response->response.success = false;
|
||||
response->response.error_code.code = ErrorCode::INVALID_ARGUMENT;
|
||||
response->response.message = "vehicle_id 不能为空。";
|
||||
return;
|
||||
}
|
||||
profiles_[vehicle_id] = request->request.profile;
|
||||
RCLCPP_INFO(get_logger(), "已注册/更新车辆画像 vehicle_id=[%s]", vehicle_id.c_str());
|
||||
response->response.success = true;
|
||||
response->response.error_code.code = ErrorCode::OK;
|
||||
response->response.message = "注册/更新成功。";
|
||||
}
|
||||
|
||||
void VehicleProfileManagerNode::handle_applicability(
|
||||
const std::shared_ptr<ApplicabilitySrv::Request> request,
|
||||
std::shared_ptr<ApplicabilitySrv::Response> response)
|
||||
{
|
||||
const auto & profile = request->request.profile_snapshot;
|
||||
auto stages = evaluate_supported_stages(profile);
|
||||
|
||||
response->response.success = true;
|
||||
response->response.error_code.code = ErrorCode::OK;
|
||||
response->response.message = "适用性评估完成。";
|
||||
response->response.overall_supported = !stages.empty();
|
||||
response->response.recommended_workflow_stages = stages;
|
||||
}
|
||||
|
||||
void VehicleProfileManagerNode::handle_heartbeat(
|
||||
const std::shared_ptr<HeartbeatSrv::Request> /*request*/,
|
||||
std::shared_ptr<HeartbeatSrv::Response> response)
|
||||
{
|
||||
const auto now_us = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
response->response.success = true;
|
||||
response->response.error_code.code = ErrorCode::OK;
|
||||
response->response.message = "vehicle_profile_manager 在线。";
|
||||
response->response.server_timestamp_us = now_us;
|
||||
response->response.vehicle_ready = true;
|
||||
}
|
||||
|
||||
std::vector<WorkflowStageType>
|
||||
VehicleProfileManagerNode::evaluate_supported_stages(const VehicleProfile & profile) const
|
||||
{
|
||||
std::vector<WorkflowStageType> stages;
|
||||
|
||||
auto make_stage = [](uint8_t v) {
|
||||
WorkflowStageType s;
|
||||
s.value = v;
|
||||
return s;
|
||||
};
|
||||
|
||||
// 预检和画像校验始终支持。
|
||||
stages.push_back(make_stage(WorkflowStageType::PROFILE_VALIDATION_STAGE));
|
||||
stages.push_back(make_stage(WorkflowStageType::WORKSHOP_PRECHECK_STAGE));
|
||||
|
||||
// 底盘标定：底盘类型已指定时支持。
|
||||
if (profile.chassis_type.value != ChassisType::CHASSIS_TYPE_UNSPECIFIED) {
|
||||
stages.push_back(make_stage(WorkflowStageType::CHASSIS_CALIBRATION_STAGE));
|
||||
stages.push_back(make_stage(WorkflowStageType::CONTROL_CALIBRATION_STAGE));
|
||||
}
|
||||
|
||||
// 传感器标定：有传感器配置时支持。
|
||||
if (!profile.sensors.empty()) {
|
||||
stages.push_back(make_stage(WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE));
|
||||
stages.push_back(make_stage(WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE));
|
||||
}
|
||||
|
||||
// 手眼标定：有机械臂且有传感器时支持。
|
||||
if (profile.arm_profile.has_mechanical_arm && !profile.sensors.empty()) {
|
||||
stages.push_back(make_stage(WorkflowStageType::HAND_EYE_CALIBRATION_STAGE));
|
||||
}
|
||||
|
||||
// 最终阶段始终加入。
|
||||
stages.push_back(make_stage(WorkflowStageType::PARAMETER_COMMIT_STAGE));
|
||||
stages.push_back(make_stage(WorkflowStageType::REPORT_ARCHIVE_STAGE));
|
||||
|
||||
return stages;
|
||||
}
|
||||
|
||||
void VehicleProfileManagerNode::load_demo_profile()
|
||||
{
|
||||
VehicleProfile demo;
|
||||
|
||||
// 基础信息
|
||||
demo.base_info.vehicle_id = "demo_agv_001";
|
||||
demo.base_info.vehicle_name = "Demo AGV";
|
||||
demo.base_info.model_name = "DemoModel-X1";
|
||||
demo.base_info.manufacturer = "Demo Manufacturer";
|
||||
|
||||
// 底盘类型：差速
|
||||
demo.chassis_type.value = ChassisType::DIFFERENTIAL;
|
||||
|
||||
// base_link
|
||||
demo.base_link_frame = "base_link";
|
||||
|
||||
// 画像版本
|
||||
demo.profile_version = "demo_v1";
|
||||
|
||||
// 启用的工作流阶段
|
||||
demo.enabled_workflow_stages = evaluate_supported_stages(demo);
|
||||
|
||||
profiles_[demo.base_info.vehicle_id] = demo;
|
||||
RCLCPP_INFO(get_logger(), "已预载 demo 车辆画像 vehicle_id=[%s]",
|
||||
demo.base_info.vehicle_id.c_str());
|
||||
}
|
||||
|
||||
} // namespace vehicle_profile_manager
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_profile_manager::VehicleProfileManagerNode)
|
||||
-13
@@ -1,13 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package="workshop_orchestrator_v2",
|
||||
executable="workshop_orchestrator_v2_node",
|
||||
name="workshop_orchestrator_v2",
|
||||
output="screen",
|
||||
)
|
||||
])
|
||||
@@ -1,153 +0,0 @@
|
||||
#include "workshop_orchestrator_v2/precheck_runner.hpp"
|
||||
|
||||
#include "calibration_workshop_orchestration_interfaces/msg/precheck_item.hpp"
|
||||
|
||||
namespace workshop_orchestrator_v2
|
||||
{
|
||||
|
||||
bool PrecheckRunner::has_metadata_key(const StagePlan & stage, const std::string & key) const
|
||||
{
|
||||
for (const auto & kv : stage.metadata) {
|
||||
if (kv.key == key) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool PrecheckRunner::has_metadata_prefix(const StagePlan & stage, const std::string & prefix) const
|
||||
{
|
||||
for (const auto & kv : stage.metadata) {
|
||||
if (kv.key.rfind(prefix, 0) == 0) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
WorkshopPrecheckResponse PrecheckRunner::run(const WorkshopSession & session) const
|
||||
{
|
||||
WorkshopPrecheckResponse response;
|
||||
response.success = true;
|
||||
response.error_code = make_error_code(ErrorCode::OK);
|
||||
response.checked_timestamp_us = now_us();
|
||||
|
||||
auto append_item = [&](const std::string & code,
|
||||
const std::string & name,
|
||||
bool passed,
|
||||
const std::string & message,
|
||||
uint8_t stage_type,
|
||||
uint8_t module_type) {
|
||||
calibration_workshop_orchestration_interfaces::msg::PrecheckItem item;
|
||||
item.item_code = code;
|
||||
item.display_name = name;
|
||||
item.passed = passed;
|
||||
item.blocking = true;
|
||||
item.error_code = make_error_code(passed ? ErrorCode::OK : ErrorCode::INVALID_STATE);
|
||||
item.message = message;
|
||||
item.related_stage_type = make_stage_type(stage_type);
|
||||
item.related_module_type = make_module_type(module_type);
|
||||
response.items.push_back(item);
|
||||
if (!passed) {
|
||||
response.blocking_issue_count += 1;
|
||||
response.all_passed = false;
|
||||
}
|
||||
};
|
||||
|
||||
response.all_passed = true;
|
||||
response.blocking_issue_count = 0;
|
||||
|
||||
append_item(
|
||||
"plan_not_empty",
|
||||
"Execution plan exists",
|
||||
!session.stage_plan.empty(),
|
||||
session.stage_plan.empty() ? "Session has no executable stages." : "Session has executable stages.",
|
||||
WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED,
|
||||
CalibrationModuleType::CALIBRATION_MODULE_UNSPECIFIED);
|
||||
|
||||
for (const auto & stage : session.stage_plan) {
|
||||
if (stage.module_type.value == CalibrationModuleType::CHASSIS_MODULE) {
|
||||
const bool passed =
|
||||
has_metadata_key(stage, metadata_keys::CHASSIS_PRIMITIVE_TYPE) &&
|
||||
has_metadata_key(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_DISTANCE_M) &&
|
||||
has_metadata_key(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_SPEED_MS);
|
||||
append_item(
|
||||
stage.stage_id + ".metadata",
|
||||
stage.display_name + " metadata",
|
||||
passed,
|
||||
passed ? "底盘阶段输入完整。" : "底盘阶段缺少 primitive_type 或直线动作参数。",
|
||||
stage.stage_type.value,
|
||||
stage.module_type.value);
|
||||
} else if (stage.module_type.value == CalibrationModuleType::CONTROL_MODULE) {
|
||||
const bool passed =
|
||||
has_metadata_key(stage, metadata_keys::CONTROL_TASK_TYPE) &&
|
||||
has_metadata_prefix(stage, metadata_keys::CONTROL_TRAJECTORY_PREFIX);
|
||||
append_item(
|
||||
stage.stage_id + ".metadata",
|
||||
stage.display_name + " metadata",
|
||||
passed,
|
||||
passed ? "运控阶段输入完整。" : "运控阶段缺少 control.task_type 或轨迹点输入。",
|
||||
stage.stage_type.value,
|
||||
stage.module_type.value);
|
||||
} else if (
|
||||
stage.module_type.value == CalibrationModuleType::SENSOR_INTRINSIC_MODULE ||
|
||||
stage.module_type.value == CalibrationModuleType::SENSOR_EXTRINSIC_MODULE ||
|
||||
stage.module_type.value == CalibrationModuleType::HAND_EYE_MODULE) {
|
||||
bool passed =
|
||||
has_metadata_key(stage, metadata_keys::SENSOR_ID) &&
|
||||
has_metadata_key(stage, metadata_keys::SENSOR_TASK_SUBTYPE);
|
||||
std::string message = passed ? "传感器阶段基础输入完整。" : "传感器阶段缺少 sensor.sensor_id 或 sensor.task_subtype。";
|
||||
|
||||
if (passed && stage.module_type.value == CalibrationModuleType::SENSOR_INTRINSIC_MODULE) {
|
||||
const bool has_image_count = has_metadata_key(stage, metadata_keys::CAMERA_INTRINSIC_REQUIRED_IMAGE_COUNT);
|
||||
const bool has_board = has_metadata_key(stage, metadata_keys::CAMERA_INTRINSIC_TARGET_BOARD_ID);
|
||||
passed = has_image_count || has_board;
|
||||
message = passed ? "传感器内参阶段输入完整。" : "传感器内参阶段缺少图像数或标定板信息。";
|
||||
}
|
||||
|
||||
if (passed && stage.module_type.value == CalibrationModuleType::SENSOR_EXTRINSIC_MODULE) {
|
||||
passed =
|
||||
has_metadata_key(stage, metadata_keys::SENSOR_EXTRINSIC_BASE_FRAME_ID) &&
|
||||
has_metadata_key(stage, metadata_keys::SENSOR_EXTRINSIC_REQUIRED_SAMPLE_COUNT);
|
||||
message = passed ? "传感器外参阶段输入完整。" : "传感器外参阶段缺少 base_frame_id 或 required_sample_count。";
|
||||
}
|
||||
|
||||
if (passed && stage.module_type.value == CalibrationModuleType::HAND_EYE_MODULE) {
|
||||
passed =
|
||||
has_metadata_key(stage, metadata_keys::HAND_EYE_ARM_ID) &&
|
||||
has_metadata_key(stage, metadata_keys::HAND_EYE_REQUIRED_POSE_COUNT);
|
||||
message = passed ? "手眼阶段输入完整。" : "手眼阶段缺少 arm_id 或 required_pose_count。";
|
||||
}
|
||||
|
||||
append_item(
|
||||
stage.stage_id + ".metadata",
|
||||
stage.display_name + " metadata",
|
||||
passed,
|
||||
message,
|
||||
stage.stage_type.value,
|
||||
stage.module_type.value);
|
||||
} else if (stage.module_type.value == CalibrationModuleType::EXTERNAL_LOCALIZATION_MODULE) {
|
||||
const bool passed =
|
||||
has_metadata_key(stage, metadata_keys::EXTERNAL_STATIC_SAMPLE_COUNT) &&
|
||||
has_metadata_key(stage, metadata_keys::EXTERNAL_DYNAMIC_SAMPLE_COUNT) &&
|
||||
has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_POSITION_STDDEV_M) &&
|
||||
has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_YAW_STDDEV_RAD) &&
|
||||
has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_TRACKING_LOSS_RATIO) &&
|
||||
has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_TIME_SYNC_OFFSET_MS) &&
|
||||
has_metadata_key(stage, metadata_keys::EXTERNAL_TIMEOUT_SEC);
|
||||
append_item(
|
||||
stage.stage_id + ".metadata",
|
||||
stage.display_name + " metadata",
|
||||
passed,
|
||||
passed ? "external 阶段输入完整。" : "external 阶段缺少真值校核阈值配置。",
|
||||
stage.stage_type.value,
|
||||
stage.module_type.value);
|
||||
}
|
||||
}
|
||||
|
||||
response.ready_for_start = response.all_passed;
|
||||
response.message = response.ready_for_start ? "Precheck passed." : "Precheck failed.";
|
||||
return response;
|
||||
}
|
||||
|
||||
} // namespace workshop_orchestrator_v2
|
||||
@@ -1,5 +1,4 @@
|
||||
|
||||
# workshop_ui_pyside6_config_aligned
|
||||
# 操作员界面原型
|
||||
|
||||
这版 PySide6 原型的目标不是单纯展示界面，而是：
|
||||
|
||||
@@ -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>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user