更新完善
This commit is contained in:
li-shihao-code
@@ -637,7 +637,7 @@
|
||||
}
|
||||
},
|
||||
"ros_topics": {
|
||||
"cmd_vel": "/cmd_vel",
|
||||
"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",
|
||||
@@ -645,7 +645,7 @@
|
||||
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
|
||||
"lidar_2d_scan": "/sensor/lidar_2d/scan",
|
||||
"imu": "/sensor/imu/data",
|
||||
"external_telemetry": "/isaac/external_localization/telemetry",
|
||||
"external_telemetry": "/isaac/external_localization/vehicle/pose",
|
||||
"chassis_telemetry": "/chassis/telemetry",
|
||||
"control_telemetry": "/control/telemetry",
|
||||
"sensor_telemetry": "/sensor_calibration/telemetry"
|
||||
|
||||
@@ -0,0 +1,184 @@
|
||||
{
|
||||
"vehicle": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"vehicle_source": "urdf_direct",
|
||||
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"usd_path": null,
|
||||
"stage_prim_path": "/ackermann_front_steer_rear_drive",
|
||||
"drive_wheels": [
|
||||
"rear_left_wheel_link",
|
||||
"rear_right_wheel_link"
|
||||
],
|
||||
"steer_wheels": [
|
||||
"front_left_wheel_link",
|
||||
"front_right_wheel_link"
|
||||
],
|
||||
"initial_pose": {
|
||||
"x_m": 0.0,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
"workshop": {
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"room_length_m": 10.0,
|
||||
"room_width_m": 6.0,
|
||||
"room_height_m": 3.5,
|
||||
"checkerboard": {
|
||||
"rows": 6,
|
||||
"cols": 9,
|
||||
"square_size_m": 0.12,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
|
||||
"reserved_wall_zones": {},
|
||||
"layout": []
|
||||
},
|
||||
"down_camera_intrinsic_target": {
|
||||
"enabled": false,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
|
||||
"pattern": "charuco",
|
||||
"squares_x": 30,
|
||||
"squares_y": 10,
|
||||
"square_size_x_m": 0.09000000000000001,
|
||||
"square_size_y_m": 0.09,
|
||||
"layout": []
|
||||
}
|
||||
},
|
||||
"ros_topics": {
|
||||
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
|
||||
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
|
||||
"lidar_prefix": "/AutoCalib_Workshop/lidar",
|
||||
"front_camera_image": "/sensor/front_camera/image_raw",
|
||||
"down_camera_image": "/sensor/down_camera/image_raw",
|
||||
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
|
||||
"lidar_2d_scan": "/sensor/lidar_2d/scan",
|
||||
"imu": "/sensor/imu/data",
|
||||
"external_telemetry": "/isaac/external_localization/vehicle/pose",
|
||||
"chassis_telemetry": "/chassis/telemetry",
|
||||
"control_telemetry": "/control/telemetry",
|
||||
"sensor_telemetry": "/sensor_calibration/telemetry"
|
||||
},
|
||||
"chassis_calibration": {
|
||||
"chassis_type": "ackermann",
|
||||
"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": 1.12,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.18,
|
||||
"roll_rad": 1.5707963267948966,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": -1.5707963267948966
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_down_camera",
|
||||
"sensor_type": "down_camera",
|
||||
"frame_id": "down_camera_link",
|
||||
"image_topic": "/sensor/down_camera/image_raw",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_3d",
|
||||
"sensor_type": "lidar_3d",
|
||||
"frame_id": "lidar_3d_link",
|
||||
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
|
||||
"mount_pose": {
|
||||
"x_m": 0.55,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_2d",
|
||||
"sensor_type": "lidar_2d",
|
||||
"frame_id": "lidar_2d_link",
|
||||
"scan_topic": "/sensor/lidar_2d/scan",
|
||||
"mount_pose": {
|
||||
"x_m": 0.7,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.0,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_imu",
|
||||
"sensor_type": "imu",
|
||||
"frame_id": "imu_link",
|
||||
"imu_topic": "/sensor/imu/data",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.26,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
}
|
||||
],
|
||||
"lidar_2d_calibration_targets": {
|
||||
"enabled": false,
|
||||
"target_width_m": 0.82,
|
||||
"target_height_m": 1.6,
|
||||
"target_thickness_m": 0.04,
|
||||
"target_bottom_z_m": 0.08,
|
||||
"slope_angle_deg": 45.0,
|
||||
"targets": []
|
||||
},
|
||||
"orchestrator_session_config_hint": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"localization_source_id": "isaac_sim_truth_source",
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"reference_target_id": "isaac_external_truth"
|
||||
}
|
||||
}
|
||||
+184
@@ -0,0 +1,184 @@
|
||||
{
|
||||
"vehicle": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"vehicle_source": "urdf_direct",
|
||||
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"usd_path": null,
|
||||
"stage_prim_path": "/ackermann_front_steer_rear_drive",
|
||||
"drive_wheels": [
|
||||
"rear_left_wheel_link",
|
||||
"rear_right_wheel_link"
|
||||
],
|
||||
"steer_wheels": [
|
||||
"front_left_wheel_link",
|
||||
"front_right_wheel_link"
|
||||
],
|
||||
"initial_pose": {
|
||||
"x_m": 0.0,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
"workshop": {
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"room_length_m": 10.0,
|
||||
"room_width_m": 6.0,
|
||||
"room_height_m": 3.5,
|
||||
"checkerboard": {
|
||||
"rows": 6,
|
||||
"cols": 9,
|
||||
"square_size_m": 0.12,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
|
||||
"reserved_wall_zones": {},
|
||||
"layout": []
|
||||
},
|
||||
"down_camera_intrinsic_target": {
|
||||
"enabled": false,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
|
||||
"pattern": "charuco",
|
||||
"squares_x": 30,
|
||||
"squares_y": 10,
|
||||
"square_size_x_m": 0.09000000000000001,
|
||||
"square_size_y_m": 0.09,
|
||||
"layout": []
|
||||
}
|
||||
},
|
||||
"ros_topics": {
|
||||
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
|
||||
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
|
||||
"lidar_prefix": "/AutoCalib_Workshop/lidar",
|
||||
"front_camera_image": "/sensor/front_camera/image_raw",
|
||||
"down_camera_image": "/sensor/down_camera/image_raw",
|
||||
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
|
||||
"lidar_2d_scan": "/sensor/lidar_2d/scan",
|
||||
"imu": "/sensor/imu/data",
|
||||
"external_telemetry": "/isaac/external_localization/vehicle/pose",
|
||||
"chassis_telemetry": "/chassis/telemetry",
|
||||
"control_telemetry": "/control/telemetry",
|
||||
"sensor_telemetry": "/sensor_calibration/telemetry"
|
||||
},
|
||||
"chassis_calibration": {
|
||||
"chassis_type": "ackermann",
|
||||
"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": 1.12,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.18,
|
||||
"roll_rad": 1.5707963267948966,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": -1.5707963267948966
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_down_camera",
|
||||
"sensor_type": "down_camera",
|
||||
"frame_id": "down_camera_link",
|
||||
"image_topic": "/sensor/down_camera/image_raw",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_3d",
|
||||
"sensor_type": "lidar_3d",
|
||||
"frame_id": "lidar_3d_link",
|
||||
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
|
||||
"mount_pose": {
|
||||
"x_m": 0.55,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_2d",
|
||||
"sensor_type": "lidar_2d",
|
||||
"frame_id": "lidar_2d_link",
|
||||
"scan_topic": "/sensor/lidar_2d/scan",
|
||||
"mount_pose": {
|
||||
"x_m": 0.7,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.0,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_imu",
|
||||
"sensor_type": "imu",
|
||||
"frame_id": "imu_link",
|
||||
"imu_topic": "/sensor/imu/data",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.26,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
}
|
||||
],
|
||||
"lidar_2d_calibration_targets": {
|
||||
"enabled": false,
|
||||
"target_width_m": 0.82,
|
||||
"target_height_m": 1.6,
|
||||
"target_thickness_m": 0.04,
|
||||
"target_bottom_z_m": 0.08,
|
||||
"slope_angle_deg": 45.0,
|
||||
"targets": []
|
||||
},
|
||||
"orchestrator_session_config_hint": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"localization_source_id": "isaac_sim_truth_source",
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"reference_target_id": "isaac_external_truth"
|
||||
}
|
||||
}
|
||||
+184
@@ -0,0 +1,184 @@
|
||||
{
|
||||
"vehicle": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"vehicle_source": "urdf_direct",
|
||||
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"usd_path": null,
|
||||
"stage_prim_path": "/ackermann_front_steer_rear_drive",
|
||||
"drive_wheels": [
|
||||
"rear_left_wheel_link",
|
||||
"rear_right_wheel_link"
|
||||
],
|
||||
"steer_wheels": [
|
||||
"front_left_wheel_link",
|
||||
"front_right_wheel_link"
|
||||
],
|
||||
"initial_pose": {
|
||||
"x_m": 0.0,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
"workshop": {
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"room_length_m": 10.0,
|
||||
"room_width_m": 6.0,
|
||||
"room_height_m": 3.5,
|
||||
"checkerboard": {
|
||||
"rows": 6,
|
||||
"cols": 9,
|
||||
"square_size_m": 0.12,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
|
||||
"reserved_wall_zones": {},
|
||||
"layout": []
|
||||
},
|
||||
"down_camera_intrinsic_target": {
|
||||
"enabled": false,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
|
||||
"pattern": "charuco",
|
||||
"squares_x": 30,
|
||||
"squares_y": 10,
|
||||
"square_size_x_m": 0.09000000000000001,
|
||||
"square_size_y_m": 0.09,
|
||||
"layout": []
|
||||
}
|
||||
},
|
||||
"ros_topics": {
|
||||
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
|
||||
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
|
||||
"lidar_prefix": "/AutoCalib_Workshop/lidar",
|
||||
"front_camera_image": "/sensor/front_camera/image_raw",
|
||||
"down_camera_image": "/sensor/down_camera/image_raw",
|
||||
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
|
||||
"lidar_2d_scan": "/sensor/lidar_2d/scan",
|
||||
"imu": "/sensor/imu/data",
|
||||
"external_telemetry": "/isaac/external_localization/vehicle/pose",
|
||||
"chassis_telemetry": "/chassis/telemetry",
|
||||
"control_telemetry": "/control/telemetry",
|
||||
"sensor_telemetry": "/sensor_calibration/telemetry"
|
||||
},
|
||||
"chassis_calibration": {
|
||||
"chassis_type": "ackermann",
|
||||
"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": 1.12,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.18,
|
||||
"roll_rad": 1.5707963267948966,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": -1.5707963267948966
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_down_camera",
|
||||
"sensor_type": "down_camera",
|
||||
"frame_id": "down_camera_link",
|
||||
"image_topic": "/sensor/down_camera/image_raw",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_3d",
|
||||
"sensor_type": "lidar_3d",
|
||||
"frame_id": "lidar_3d_link",
|
||||
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
|
||||
"mount_pose": {
|
||||
"x_m": 0.55,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_2d",
|
||||
"sensor_type": "lidar_2d",
|
||||
"frame_id": "lidar_2d_link",
|
||||
"scan_topic": "/sensor/lidar_2d/scan",
|
||||
"mount_pose": {
|
||||
"x_m": 0.7,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.0,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_imu",
|
||||
"sensor_type": "imu",
|
||||
"frame_id": "imu_link",
|
||||
"imu_topic": "/sensor/imu/data",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.26,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
}
|
||||
],
|
||||
"lidar_2d_calibration_targets": {
|
||||
"enabled": false,
|
||||
"target_width_m": 0.82,
|
||||
"target_height_m": 1.6,
|
||||
"target_thickness_m": 0.04,
|
||||
"target_bottom_z_m": 0.08,
|
||||
"slope_angle_deg": 45.0,
|
||||
"targets": []
|
||||
},
|
||||
"orchestrator_session_config_hint": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"localization_source_id": "isaac_sim_truth_source",
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"reference_target_id": "isaac_external_truth"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,184 @@
|
||||
{
|
||||
"vehicle": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"vehicle_source": "urdf_direct",
|
||||
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"usd_path": null,
|
||||
"stage_prim_path": "/ackermann_front_steer_rear_drive",
|
||||
"drive_wheels": [
|
||||
"rear_left_wheel_link",
|
||||
"rear_right_wheel_link"
|
||||
],
|
||||
"steer_wheels": [
|
||||
"front_left_wheel_link",
|
||||
"front_right_wheel_link"
|
||||
],
|
||||
"initial_pose": {
|
||||
"x_m": 0.0,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
"workshop": {
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"room_length_m": 10.0,
|
||||
"room_width_m": 6.0,
|
||||
"room_height_m": 3.5,
|
||||
"checkerboard": {
|
||||
"rows": 6,
|
||||
"cols": 9,
|
||||
"square_size_m": 0.12,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
|
||||
"reserved_wall_zones": {},
|
||||
"layout": []
|
||||
},
|
||||
"down_camera_intrinsic_target": {
|
||||
"enabled": false,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
|
||||
"pattern": "charuco",
|
||||
"squares_x": 30,
|
||||
"squares_y": 10,
|
||||
"square_size_x_m": 0.09000000000000001,
|
||||
"square_size_y_m": 0.09,
|
||||
"layout": []
|
||||
}
|
||||
},
|
||||
"ros_topics": {
|
||||
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
|
||||
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
|
||||
"lidar_prefix": "/AutoCalib_Workshop/lidar",
|
||||
"front_camera_image": "/sensor/front_camera/image_raw",
|
||||
"down_camera_image": "/sensor/down_camera/image_raw",
|
||||
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
|
||||
"lidar_2d_scan": "/sensor/lidar_2d/scan",
|
||||
"imu": "/sensor/imu/data",
|
||||
"external_telemetry": "/isaac/external_localization/vehicle/pose",
|
||||
"chassis_telemetry": "/chassis/telemetry",
|
||||
"control_telemetry": "/control/telemetry",
|
||||
"sensor_telemetry": "/sensor_calibration/telemetry"
|
||||
},
|
||||
"chassis_calibration": {
|
||||
"chassis_type": "ackermann",
|
||||
"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": 1.12,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.18,
|
||||
"roll_rad": 1.5707963267948966,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": -1.5707963267948966
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_down_camera",
|
||||
"sensor_type": "down_camera",
|
||||
"frame_id": "down_camera_link",
|
||||
"image_topic": "/sensor/down_camera/image_raw",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_3d",
|
||||
"sensor_type": "lidar_3d",
|
||||
"frame_id": "lidar_3d_link",
|
||||
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
|
||||
"mount_pose": {
|
||||
"x_m": 0.55,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_2d",
|
||||
"sensor_type": "lidar_2d",
|
||||
"frame_id": "lidar_2d_link",
|
||||
"scan_topic": "/sensor/lidar_2d/scan",
|
||||
"mount_pose": {
|
||||
"x_m": 0.7,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.0,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_imu",
|
||||
"sensor_type": "imu",
|
||||
"frame_id": "imu_link",
|
||||
"imu_topic": "/sensor/imu/data",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.26,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
}
|
||||
],
|
||||
"lidar_2d_calibration_targets": {
|
||||
"enabled": false,
|
||||
"target_width_m": 0.82,
|
||||
"target_height_m": 1.6,
|
||||
"target_thickness_m": 0.04,
|
||||
"target_bottom_z_m": 0.08,
|
||||
"slope_angle_deg": 45.0,
|
||||
"targets": []
|
||||
},
|
||||
"orchestrator_session_config_hint": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"localization_source_id": "isaac_sim_truth_source",
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"reference_target_id": "isaac_external_truth"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,184 @@
|
||||
{
|
||||
"vehicle": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"vehicle_source": "urdf_direct",
|
||||
"source_urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"urdf_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf",
|
||||
"usd_path": null,
|
||||
"stage_prim_path": "/ackermann_front_steer_rear_drive",
|
||||
"drive_wheels": [
|
||||
"rear_left_wheel_link",
|
||||
"rear_right_wheel_link"
|
||||
],
|
||||
"steer_wheels": [
|
||||
"front_left_wheel_link",
|
||||
"front_right_wheel_link"
|
||||
],
|
||||
"initial_pose": {
|
||||
"x_m": 0.0,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
"workshop": {
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"room_length_m": 10.0,
|
||||
"room_width_m": 6.0,
|
||||
"room_height_m": 3.5,
|
||||
"checkerboard": {
|
||||
"rows": 6,
|
||||
"cols": 9,
|
||||
"square_size_m": 0.12,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/checkerboard.png",
|
||||
"reserved_wall_zones": {},
|
||||
"layout": []
|
||||
},
|
||||
"down_camera_intrinsic_target": {
|
||||
"enabled": false,
|
||||
"texture_path": "/home/nvidia/study/AutoCalib-Workshop/agv_calib_brain/.isaac_cache/down_camera_charuco.png",
|
||||
"pattern": "charuco",
|
||||
"squares_x": 30,
|
||||
"squares_y": 10,
|
||||
"square_size_x_m": 0.09000000000000001,
|
||||
"square_size_y_m": 0.09,
|
||||
"layout": []
|
||||
}
|
||||
},
|
||||
"ros_topics": {
|
||||
"cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
|
||||
"camera_image": "/AutoCalib_Workshop/camera/image_raw",
|
||||
"lidar_prefix": "/AutoCalib_Workshop/lidar",
|
||||
"front_camera_image": "/sensor/front_camera/image_raw",
|
||||
"down_camera_image": "/sensor/down_camera/image_raw",
|
||||
"lidar_3d_pointcloud": "/sensor/lidar_3d/pointcloud",
|
||||
"lidar_2d_scan": "/sensor/lidar_2d/scan",
|
||||
"imu": "/sensor/imu/data",
|
||||
"external_telemetry": "/isaac/external_localization/vehicle/pose",
|
||||
"chassis_telemetry": "/chassis/telemetry",
|
||||
"control_telemetry": "/control/telemetry",
|
||||
"sensor_telemetry": "/sensor_calibration/telemetry"
|
||||
},
|
||||
"chassis_calibration": {
|
||||
"chassis_type": "ackermann",
|
||||
"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": 1.12,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.18,
|
||||
"roll_rad": 1.5707963267948966,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": -1.5707963267948966
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_down_camera",
|
||||
"sensor_type": "down_camera",
|
||||
"frame_id": "down_camera_link",
|
||||
"image_topic": "/sensor/down_camera/image_raw",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_3d",
|
||||
"sensor_type": "lidar_3d",
|
||||
"frame_id": "lidar_3d_link",
|
||||
"pointcloud_topic": "/sensor/lidar_3d/pointcloud",
|
||||
"mount_pose": {
|
||||
"x_m": 0.55,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.2,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_lidar_2d",
|
||||
"sensor_type": "lidar_2d",
|
||||
"frame_id": "lidar_2d_link",
|
||||
"scan_topic": "/sensor/lidar_2d/scan",
|
||||
"mount_pose": {
|
||||
"x_m": 0.7,
|
||||
"y_m": 0.0,
|
||||
"z_m": 1.0,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"sensor_id": "demo_imu",
|
||||
"sensor_type": "imu",
|
||||
"frame_id": "imu_link",
|
||||
"imu_topic": "/sensor/imu/data",
|
||||
"mount_pose": {
|
||||
"x_m": 0.4,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.26,
|
||||
"roll_rad": 0.0,
|
||||
"pitch_rad": 0.0,
|
||||
"yaw_rad": 0.0
|
||||
}
|
||||
}
|
||||
],
|
||||
"lidar_2d_calibration_targets": {
|
||||
"enabled": false,
|
||||
"target_width_m": 0.82,
|
||||
"target_height_m": 1.6,
|
||||
"target_thickness_m": 0.04,
|
||||
"target_bottom_z_m": 0.08,
|
||||
"slope_angle_deg": 45.0,
|
||||
"targets": []
|
||||
},
|
||||
"orchestrator_session_config_hint": {
|
||||
"vehicle_id": "demo_agv_001",
|
||||
"localization_source_id": "isaac_sim_truth_source",
|
||||
"workcell_zone_id": "isaac_workcell_zone_a",
|
||||
"reference_target_id": "isaac_external_truth"
|
||||
}
|
||||
}
|
||||
@@ -11,6 +11,7 @@ set -Eeuo pipefail
|
||||
WORKSPACE_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}"
|
||||
RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/local_data_input_smoke_$(date +%Y%m%d_%H%M%S)}"
|
||||
export ROS_LOG_DIR
|
||||
|
||||
SITE_PROFILE="src/deployment/profiles/site_template.yaml"
|
||||
SESSION_ID="session_001"
|
||||
@@ -200,6 +201,9 @@ if [[ "${SKIP_GENERATE}" -eq 0 ]]; then
|
||||
--file external.external_observation_files=external/external_observations.csv \
|
||||
--file chassis.chassis_motion_data_files=chassis/chassis_motion.csv \
|
||||
--file control.control_evaluation_data_files=control/control_eval.csv \
|
||||
--file control.reference_signal_files=control/reference_signal.csv \
|
||||
--file control.chassis_response_files=control/chassis_response.csv \
|
||||
--file control.truth_trajectory_files=control/truth_trajectory.csv \
|
||||
--file sensor.image_sample_files=sensor/front_camera/images.yaml \
|
||||
--file sensor.target_detection_files=sensor/front_camera/charuco_detections.json \
|
||||
--finalize
|
||||
|
||||
@@ -1,34 +1,71 @@
|
||||
# 操作员界面原型
|
||||
# 标定车间现场操作台
|
||||
|
||||
这版 PySide6 原型的目标不是单纯展示界面，而是：
|
||||
这个 PySide6 界面面向真实部署使用，不再只是静态原型。它围绕车间电脑侧的实际流程组织：
|
||||
|
||||
- 让 UI 直接生成接近 `WorkshopSessionConfig` 的数据结构
|
||||
- 让每个勾选项直接映射成 `RequestedCalibrationTask`
|
||||
- 让你一眼看出：当前界面上的输入，最后会如何进入主控消息
|
||||
- 后台加载固定车间部署配置
|
||||
- 选择、加载或保存可复用的车辆画像文件
|
||||
- 检查未替换的占位配置
|
||||
- 生成本轮任务文件
|
||||
- 在界面中选择底盘类型、底盘标定参数、运控算法和传感器标定项目
|
||||
- 启动或停止 ROS 现场服务
|
||||
- 执行本轮标定流程
|
||||
- 在“车间定位”窗口显示 3D 车间图、车辆实时位置、本轮计划轨迹和定位质量信息
|
||||
- 解析并展示最终 report、阶段结果和 metadata
|
||||
|
||||
## 运行方法
|
||||
|
||||
```bash
|
||||
pip install -r requirements.txt
|
||||
python main.py
|
||||
source install/setup.bash
|
||||
python3 src/apps/operator_ui/main.py
|
||||
```
|
||||
|
||||
## 这版重点
|
||||
如果环境里还没有界面依赖：
|
||||
|
||||
### 固定步骤输入
|
||||
直接映射到 `WorkshopSessionConfig`：
|
||||
```bash
|
||||
pip install -r src/apps/operator_ui/requirements.txt
|
||||
```
|
||||
|
||||
- `localization_source_id`
|
||||
- `workcell_zone_id`
|
||||
- `reference_target_id`
|
||||
## 真实部署前需要先确认
|
||||
|
||||
### 具体标定项
|
||||
直接映射到 `requested_tasks[]`：
|
||||
1. 把 `src/deployment/profiles/site_template.yaml` 复制成唯一车间的部署配置文件，并在程序默认配置里固定使用。
|
||||
2. 替换所有 `replace_with_*`、`measured_on_site`、`session_xxx` 占位项。
|
||||
3. 为首次标定的车型新建并保存车辆画像，至少填写车辆长宽高、轴距、轮距、轮半径、底盘类型、传感器 ID 和运控默认限制；同车型新车可直接选择已有车辆画像。
|
||||
4. 按现场测量并填写车间长、宽、高，单位为米。
|
||||
5. 确认车间定位输出话题是 `/workshop/external_localization/vehicle/pose`。
|
||||
6. 确认车上 Windows 程序的 IP 和端口。
|
||||
7. 真实采集完成后，把采集数据索引文件转成算法数据参数文件，再在界面里填入。
|
||||
|
||||
- `stage_type`
|
||||
- `task_code`
|
||||
- `target_id`
|
||||
- `task_params`
|
||||
## 界面操作顺序
|
||||
|
||||
### 导出 JSON
|
||||
可以把右侧预览直接导出成 JSON 文件，便于和后端 / ROS 接口一起核对。
|
||||
1. 查看“现场检查”页，确认固定车间配置和关键项没有失败。
|
||||
2. 在右上角查看车间尺寸是否已从固定部署配置加载。
|
||||
3. 在“车辆画像”里选择已有画像；需要新车型时点击“新建画像”，在弹出的画像窗口里填写车辆尺寸、底盘几何、运控限制，并勾选车上安装的传感器后填写编号；手眼相机需要选择眼在手上或眼在手外。
|
||||
4. 在“本轮要标定的内容”里选择：
|
||||
- 底盘类型和要标定的底盘参数
|
||||
- 运控轴向、控制算法和评估项目
|
||||
- 传感器内参、外参或手眼任务；传感器 ID 来自车辆画像
|
||||
5. 点击“生成本轮任务文件”，检查“本轮任务预览”里的任务列表。
|
||||
6. 查看“车间定位”窗口，确认 3D 车间图中的本轮计划轨迹、车辆位置和质量信息在更新。
|
||||
7. 点击“启动现场服务”，等待日志中各节点启动。
|
||||
8. 点击“开始执行本轮标定”，在“报告”页查看最终结果。
|
||||
9. 验证结束后点击“停止现场服务”。
|
||||
|
||||
## 注意事项
|
||||
|
||||
- “跳过网络质量检查（仅调试）”只适合本地联调，真实部署默认不要勾选。
|
||||
- 车上 Windows 程序链路是真实部署的必需链路，界面固定启用，不再让操作人员选择。
|
||||
- 车辆 ID 表示当前这辆车；车辆画像表示同一车型的可复用静态配置，两者不要混用。
|
||||
- 底盘标定和运控参数标定都依赖车辆画像；画像会写入本轮任务文件，smoke 执行时会按当前车辆 ID 注册这份画像快照。
|
||||
- 主界面只保留车辆画像选择；画像明细在独立画像窗口里新建、编辑和保存。
|
||||
- 画像文件固定保存在 `src/deployment/profiles/vehicle_profiles/`，保存时按“画像名称.ymal”生成文件名。
|
||||
- 车间长、宽、高是固定现场配置，只能从现场配置文件读取，不允许操作员在界面手动输入。
|
||||
- 现场只有一个车间，所以界面不提供现场配置文件选择入口；部署配置由程序后台固定加载。
|
||||
- 车间长、宽、高会写入本轮任务文件，并通过 `workshop_geometry.*` 进入总控报告 metadata。
|
||||
- 车间定位输出话题、车间定位系统名称、当前标定工位从现场配置文件读取，不作为普通操作项显示。
|
||||
- “车间定位”窗口只显示 3D 位置、本轮计划轨迹和定位数据，不允许修改固定 topic 或定位系统名称。
|
||||
- 3D 车间图支持鼠标左键拖动旋转视角，左键双击恢复默认视角。
|
||||
- 本轮计划轨迹用绿色点显示，来自当前勾选的底盘动作和运控轨迹参数，不要求操作员额外输入。
|
||||
- 标定流程运行时，界面订阅 `/workshop_v2/events`，上一项任务完成后会自动切到下一项需要动车采集的轨迹。
|
||||
- 界面会把明细选择写入本轮任务文件，并让标定流程读取这些任务。
|
||||
- 界面通过现有 CLI 和 ROS launch 运行流程，没有绕过总控。
|
||||
- 如果 report 显示失败，优先查看“阶段”表里的 `summary` 和日志中的 `[STAGE]` 行。
|
||||
|
||||
@@ -0,0 +1,377 @@
|
||||
"""操作台固定配置和任务选项。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[3]
|
||||
DEFAULT_SITE_PROFILE = REPO_ROOT / "src/deployment/profiles/site_template.yaml"
|
||||
DEFAULT_VEHICLE_PROFILE_DIR = REPO_ROOT / "src/deployment/profiles/vehicle_profiles"
|
||||
DEFAULT_VEHICLE_PROFILE = DEFAULT_VEHICLE_PROFILE_DIR / "ackermann_default.yaml"
|
||||
VEHICLE_PROFILE_SAVE_EXTENSION = ".ymal"
|
||||
DEFAULT_SESSION_CONFIG = Path("/tmp/agv_calib_operator_ui/workshop_session_config.yaml")
|
||||
PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx")
|
||||
|
||||
TASKS = [
|
||||
("external", "检查车间定位"),
|
||||
("chassis", "底盘标定"),
|
||||
("control", "运控参数标定"),
|
||||
("sensor_intrinsic", "传感器内参标定"),
|
||||
("sensor_extrinsic", "传感器外参标定"),
|
||||
("hand_eye", "手眼标定"),
|
||||
]
|
||||
|
||||
CHASSIS_TYPES = [
|
||||
("ackermann", "阿克曼"),
|
||||
("differential", "差速轮"),
|
||||
("single_steer_wheel", "单舵轮"),
|
||||
("multi_steer_wheel", "多舵轮"),
|
||||
]
|
||||
|
||||
STAGE_EXTERNAL = "EXTERNAL_REFERENCE_READY_CHECK_STAGE"
|
||||
STAGE_CHASSIS = "CHASSIS_CALIBRATION_STAGE"
|
||||
STAGE_CONTROL = "CONTROL_CALIBRATION_STAGE"
|
||||
STAGE_SENSOR_INTRINSIC = "SENSOR_INTRINSIC_CALIBRATION_STAGE"
|
||||
STAGE_SENSOR_EXTRINSIC = "SENSOR_EXTRINSIC_CALIBRATION_STAGE"
|
||||
STAGE_HAND_EYE = "HAND_EYE_CALIBRATION_STAGE"
|
||||
|
||||
STAGE_DISPLAY_NAMES = {
|
||||
STAGE_EXTERNAL: "车间定位检查",
|
||||
STAGE_CHASSIS: "底盘标定",
|
||||
STAGE_CONTROL: "运控参数标定",
|
||||
STAGE_SENSOR_INTRINSIC: "传感器内参标定",
|
||||
STAGE_SENSOR_EXTRINSIC: "传感器外参标定",
|
||||
STAGE_HAND_EYE: "手眼标定",
|
||||
}
|
||||
|
||||
STAGE_ID_PREFIX_BY_STAGE_TYPE = {
|
||||
STAGE_EXTERNAL: "stage_external_reference",
|
||||
STAGE_CHASSIS: "stage_chassis_calibration",
|
||||
STAGE_CONTROL: "stage_control_calibration",
|
||||
STAGE_SENSOR_INTRINSIC: "stage_sensor_intrinsic_calibration",
|
||||
STAGE_SENSOR_EXTRINSIC: "stage_sensor_extrinsic_calibration",
|
||||
STAGE_HAND_EYE: "stage_hand_eye_calibration",
|
||||
}
|
||||
|
||||
WORKSHOP_EVENT_STAGE_STARTED = 5
|
||||
WORKSHOP_EVENT_STAGE_COMPLETED = 6
|
||||
WORKSHOP_EVENT_STAGE_FAILED = 7
|
||||
WORKSHOP_EVENT_REPORT_READY = 12
|
||||
|
||||
POLICY_DISPLAY_NAMES = {
|
||||
"REQUIRED": "必做",
|
||||
"OPTIONAL": "可选",
|
||||
"SKIP_IF_UNSUPPORTED": "不支持则跳过",
|
||||
}
|
||||
|
||||
LOCALIZATION_DISPLAY_ROWS = [
|
||||
"状态",
|
||||
"更新时间",
|
||||
"X(m)",
|
||||
"Y(m)",
|
||||
"Z(m)",
|
||||
"Yaw(deg)",
|
||||
"质量分数",
|
||||
"位置标准差(m)",
|
||||
"航向标准差(deg)",
|
||||
"跟踪丢失率",
|
||||
"时间同步偏差(ms)",
|
||||
"目标数量",
|
||||
"定位系统",
|
||||
"任务 ID",
|
||||
]
|
||||
|
||||
CHASSIS_PARAMETER_OPTIONS = {
|
||||
"ackermann": [
|
||||
{
|
||||
"code": "chassis.ackermann.rear_wheel_radius",
|
||||
"label": "后轮有效半径",
|
||||
"target": "rear_drive_wheels",
|
||||
"params": {
|
||||
"primitive_type": "straight_line",
|
||||
"straight_line.target_distance_m": "0.5",
|
||||
"straight_line.target_speed_ms": "0.1",
|
||||
"straight_line.reverse": "false",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.ackermann.steering_zero",
|
||||
"label": "前轮舵角零偏",
|
||||
"target": "front_steering",
|
||||
"params": {
|
||||
"primitive_type": "steering_sweep",
|
||||
"steering_sweep.target_angle_deg": "0.0",
|
||||
"steering_sweep.sweep_amplitude_deg": "12.0",
|
||||
"steering_sweep.sweep_frequency_hz": "0.2",
|
||||
"steering_sweep.duration_sec": "8.0",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.ackermann.steering_ratio",
|
||||
"label": "转向传动比/等效轴距",
|
||||
"target": "front_steering",
|
||||
"params": {
|
||||
"primitive_type": "arc",
|
||||
"arc.target_speed_ms": "0.1",
|
||||
"arc.radius_m": "1.2",
|
||||
"arc.sweep_angle_deg": "90.0",
|
||||
"arc.clockwise": "false",
|
||||
},
|
||||
},
|
||||
],
|
||||
"differential": [
|
||||
{
|
||||
"code": "chassis.differential.wheel_radius",
|
||||
"label": "左右轮有效半径",
|
||||
"target": "drive_wheels",
|
||||
"params": {
|
||||
"primitive_type": "straight_line",
|
||||
"straight_line.target_distance_m": "0.5",
|
||||
"straight_line.target_speed_ms": "0.1",
|
||||
"straight_line.reverse": "false",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.differential.track_width",
|
||||
"label": "驱动轮轮距",
|
||||
"target": "drive_wheels",
|
||||
"params": {
|
||||
"primitive_type": "in_place_rotation",
|
||||
"in_place_rotation.target_yaw_deg": "360.0",
|
||||
"in_place_rotation.target_angular_vel_deg_s": "20.0",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.differential.encoder_scale",
|
||||
"label": "左右编码器比例",
|
||||
"target": "wheel_encoders",
|
||||
"params": {
|
||||
"primitive_type": "straight_line",
|
||||
"straight_line.target_distance_m": "0.5",
|
||||
"straight_line.target_speed_ms": "0.08",
|
||||
"straight_line.reverse": "false",
|
||||
},
|
||||
},
|
||||
],
|
||||
"single_steer_wheel": [
|
||||
{
|
||||
"code": "chassis.single_steer.drive_wheel_radius",
|
||||
"label": "驱动轮有效半径",
|
||||
"target": "steer_drive_module",
|
||||
"params": {
|
||||
"primitive_type": "straight_line",
|
||||
"straight_line.target_distance_m": "0.5",
|
||||
"straight_line.target_speed_ms": "0.1",
|
||||
"straight_line.reverse": "false",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.single_steer.steer_zero",
|
||||
"label": "舵轮零位",
|
||||
"target": "steer_drive_module",
|
||||
"params": {
|
||||
"primitive_type": "steering_sweep",
|
||||
"steering_sweep.target_angle_deg": "0.0",
|
||||
"steering_sweep.sweep_amplitude_deg": "15.0",
|
||||
"steering_sweep.sweep_frequency_hz": "0.2",
|
||||
"steering_sweep.duration_sec": "8.0",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.single_steer.steering_ratio",
|
||||
"label": "转向比例",
|
||||
"target": "steer_drive_module",
|
||||
"params": {
|
||||
"primitive_type": "arc",
|
||||
"arc.target_speed_ms": "0.1",
|
||||
"arc.radius_m": "1.0",
|
||||
"arc.sweep_angle_deg": "90.0",
|
||||
"arc.clockwise": "false",
|
||||
},
|
||||
},
|
||||
],
|
||||
"multi_steer_wheel": [
|
||||
{
|
||||
"code": "chassis.multi_steer.module_wheel_radius",
|
||||
"label": "各模块轮半径",
|
||||
"target": "steering_modules",
|
||||
"params": {
|
||||
"primitive_type": "straight_line",
|
||||
"straight_line.target_distance_m": "0.5",
|
||||
"straight_line.target_speed_ms": "0.1",
|
||||
"straight_line.reverse": "false",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.multi_steer.module_zero",
|
||||
"label": "各模块舵角零位",
|
||||
"target": "steering_modules",
|
||||
"params": {
|
||||
"primitive_type": "module_alignment",
|
||||
"module_alignment.module_ids": "module_1,module_2,module_3,module_4",
|
||||
"module_alignment.target_zero_deg": "0.0",
|
||||
"module_alignment.tolerance_deg": "0.5",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "chassis.multi_steer.coordinated_steering",
|
||||
"label": "多模块协同转向一致性",
|
||||
"target": "steering_modules",
|
||||
"params": {
|
||||
"primitive_type": "coordinated_steering",
|
||||
"coordinated_steering.module_ids": "module_1,module_2,module_3,module_4",
|
||||
"coordinated_steering.target_angle_deg": "30.0",
|
||||
"coordinated_steering.hold_time_sec": "3.0",
|
||||
},
|
||||
},
|
||||
],
|
||||
}
|
||||
|
||||
CONTROL_PARAMETER_OPTIONS = [
|
||||
{
|
||||
"code": "control.lateral.mpc",
|
||||
"label": "横向 MPC",
|
||||
"target": "lateral_controller",
|
||||
"axis": "lateral",
|
||||
"algorithm": "mpc",
|
||||
"params": {
|
||||
"control.task_type": "trajectory_tracking",
|
||||
"control.stop_at_end": "true",
|
||||
"control.timeout_sec": "60.0",
|
||||
"trajectory_tracking.trajectory_id": "ui_lateral_mpc_eval",
|
||||
"trajectory_tracking.segment_index": "0",
|
||||
"trajectory_tracking.total_segments": "1",
|
||||
"trajectory_tracking.is_final_segment": "true",
|
||||
"trajectory_tracking.max_external_pose_age_ms": "200.0",
|
||||
"trajectory_tracking.min_external_pose_quality_score": "0.5",
|
||||
"traj_pt_0_x_m": "0.0",
|
||||
"traj_pt_0_y_m": "0.0",
|
||||
"traj_pt_0_yaw_rad": "0.0",
|
||||
"traj_pt_0_speed_ms": "0.1",
|
||||
"traj_pt_1_x_m": "1.0",
|
||||
"traj_pt_1_y_m": "0.0",
|
||||
"traj_pt_1_yaw_rad": "0.0",
|
||||
"traj_pt_1_speed_ms": "0.1",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "control.lateral.lqr",
|
||||
"label": "横向 LQR",
|
||||
"target": "lateral_controller",
|
||||
"axis": "lateral",
|
||||
"algorithm": "lqr",
|
||||
"params": {
|
||||
"control.task_type": "trajectory_tracking",
|
||||
"control.stop_at_end": "true",
|
||||
"control.timeout_sec": "60.0",
|
||||
"trajectory_tracking.trajectory_id": "ui_lateral_lqr_eval",
|
||||
"traj_pt_0_x_m": "0.0",
|
||||
"traj_pt_0_y_m": "0.0",
|
||||
"traj_pt_0_yaw_rad": "0.0",
|
||||
"traj_pt_0_speed_ms": "0.1",
|
||||
"traj_pt_1_x_m": "1.0",
|
||||
"traj_pt_1_y_m": "0.0",
|
||||
"traj_pt_1_yaw_rad": "0.0",
|
||||
"traj_pt_1_speed_ms": "0.1",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "control.lateral.pure_pursuit",
|
||||
"label": "横向 Pure Pursuit",
|
||||
"target": "lateral_controller",
|
||||
"axis": "lateral",
|
||||
"algorithm": "pure_pursuit",
|
||||
"params": {
|
||||
"control.task_type": "trajectory_tracking",
|
||||
"control.stop_at_end": "true",
|
||||
"control.timeout_sec": "60.0",
|
||||
"trajectory_tracking.trajectory_id": "ui_lateral_pp_eval",
|
||||
"traj_pt_0_x_m": "0.0",
|
||||
"traj_pt_0_y_m": "0.0",
|
||||
"traj_pt_0_yaw_rad": "0.0",
|
||||
"traj_pt_0_speed_ms": "0.1",
|
||||
"traj_pt_1_x_m": "1.0",
|
||||
"traj_pt_1_y_m": "0.0",
|
||||
"traj_pt_1_yaw_rad": "0.0",
|
||||
"traj_pt_1_speed_ms": "0.1",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "control.longitudinal.pid",
|
||||
"label": "纵向 PID",
|
||||
"target": "longitudinal_controller",
|
||||
"axis": "longitudinal",
|
||||
"algorithm": "pid",
|
||||
"params": {
|
||||
"control.task_type": "velocity_step",
|
||||
"velocity_step.target_velocity_ms": "0.1",
|
||||
"velocity_step.hold_time_sec": "0.5",
|
||||
"velocity_step.settle_before_step_sec": "0.1",
|
||||
"control.timeout_sec": "10.0",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "control.longitudinal.mpc",
|
||||
"label": "纵向 MPC",
|
||||
"target": "longitudinal_controller",
|
||||
"axis": "longitudinal",
|
||||
"algorithm": "mpc",
|
||||
"params": {
|
||||
"control.task_type": "accel_decel",
|
||||
"accel_decel.start_velocity_ms": "0.0",
|
||||
"accel_decel.target_velocity_ms": "0.15",
|
||||
"accel_decel.target_accel_ms2": "0.1",
|
||||
"accel_decel.hold_time_sec": "0.5",
|
||||
"control.timeout_sec": "10.0",
|
||||
},
|
||||
},
|
||||
{
|
||||
"code": "control.stop_accuracy",
|
||||
"label": "停车精度",
|
||||
"target": "stop_controller",
|
||||
"axis": "combined",
|
||||
"algorithm": "stop_accuracy",
|
||||
"params": {
|
||||
"control.task_type": "stop_accuracy",
|
||||
"stop_accuracy.target_stop_x_m": "0.0",
|
||||
"stop_accuracy.target_stop_y_m": "0.0",
|
||||
"stop_accuracy.target_stop_yaw_rad": "0.0",
|
||||
"control.timeout_sec": "10.0",
|
||||
},
|
||||
},
|
||||
]
|
||||
|
||||
SENSOR_TASK_OPTIONS = [
|
||||
("front_camera", "前视相机", "front_camera_intrinsic", "前视相机内参", STAGE_SENSOR_INTRINSIC),
|
||||
("front_camera", "前视相机", "front_camera_extrinsic", "前视相机到车体外参", STAGE_SENSOR_EXTRINSIC),
|
||||
("down_camera", "下视相机", "downward_camera_intrinsic", "下视相机内参", STAGE_SENSOR_INTRINSIC),
|
||||
("down_camera", "下视相机", "downward_camera_extrinsic", "下视相机到车体外参", STAGE_SENSOR_EXTRINSIC),
|
||||
("lidar_2d", "2D 激光雷达", "lidar_2d_extrinsic", "2D 激光雷达到车体外参", STAGE_SENSOR_EXTRINSIC),
|
||||
("lidar_3d", "3D 激光雷达", "lidar_3d_extrinsic", "3D 激光雷达到车体外参", STAGE_SENSOR_EXTRINSIC),
|
||||
("imu", "IMU", "imu_intrinsic", "IMU 内参", STAGE_SENSOR_INTRINSIC),
|
||||
("imu", "IMU", "imu_extrinsic", "IMU 到车体外参", STAGE_SENSOR_EXTRINSIC),
|
||||
("arm_camera", "手眼相机", "eye_in_hand", "眼在手上手眼标定", STAGE_HAND_EYE),
|
||||
("arm_camera", "手眼相机", "eye_to_hand", "眼在手外手眼标定", STAGE_HAND_EYE),
|
||||
]
|
||||
|
||||
TASK_DISPLAY_NAMES = {"external": "车间定位可用性检查"}
|
||||
for options in CHASSIS_PARAMETER_OPTIONS.values():
|
||||
for option in options:
|
||||
TASK_DISPLAY_NAMES[str(option["code"])] = str(option["label"])
|
||||
for option in CONTROL_PARAMETER_OPTIONS:
|
||||
TASK_DISPLAY_NAMES[str(option["code"])] = str(option["label"])
|
||||
for sensor_key, sensor_label, subtype, label, _stage_type in SENSOR_TASK_OPTIONS:
|
||||
TASK_DISPLAY_NAMES[f"sensor.{sensor_key}.{subtype}"] = label
|
||||
|
||||
SENSOR_STAGE_DISPLAY_NAMES: dict[str, str] = {}
|
||||
for sensor_key, sensor_label, subtype, _label, stage_type in SENSOR_TASK_OPTIONS:
|
||||
task_code = f"sensor.{sensor_key}.{subtype}"
|
||||
if stage_type == STAGE_SENSOR_INTRINSIC:
|
||||
SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}内参标定"
|
||||
elif stage_type == STAGE_SENSOR_EXTRINSIC:
|
||||
SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}外参标定"
|
||||
elif stage_type == STAGE_HAND_EYE:
|
||||
SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}手眼标定"
|
||||
|
||||
|
||||
@@ -0,0 +1,272 @@
|
||||
"""车间 3D 定位显示控件。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
from PySide6.QtCore import QPointF, Qt
|
||||
from PySide6.QtGui import QBrush, QColor, QFont, QPainter, QPen, QPolygonF
|
||||
from PySide6.QtWidgets import QWidget
|
||||
|
||||
|
||||
class WorkshopLocalization3DView(QWidget):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
self.setMinimumHeight(360)
|
||||
self.room_length_m = 0.0
|
||||
self.room_width_m = 0.0
|
||||
self.room_height_m = 0.0
|
||||
self.pose: dict[str, float] | None = None
|
||||
self.pose_valid = False
|
||||
self.quality_score: float | None = None
|
||||
self.status_text = "等待定位数据"
|
||||
self.trajectory_segments: list[list[tuple[float, float, float]]] = []
|
||||
self.trajectory_label = "本轮计划轨迹"
|
||||
self.view_yaw_rad = math.radians(45.0)
|
||||
self.view_pitch_rad = math.radians(35.0)
|
||||
self.view_drag_last_pos: QPointF | None = None
|
||||
self.setCursor(Qt.OpenHandCursor)
|
||||
|
||||
def set_workshop_geometry(self, geometry: dict[str, str]) -> None:
|
||||
try:
|
||||
self.room_length_m = float(geometry.get("length_m", "0"))
|
||||
self.room_width_m = float(geometry.get("width_m", "0"))
|
||||
self.room_height_m = float(geometry.get("height_m", "0"))
|
||||
except ValueError:
|
||||
self.room_length_m = 0.0
|
||||
self.room_width_m = 0.0
|
||||
self.room_height_m = 0.0
|
||||
self.update()
|
||||
|
||||
def set_status(self, text: str) -> None:
|
||||
self.status_text = text
|
||||
self.update()
|
||||
|
||||
def set_pose(self, x_m: float, y_m: float, z_m: float, yaw_rad: float, valid: bool, quality_score: float | None) -> None:
|
||||
self.pose = {"x": x_m, "y": y_m, "z": z_m, "yaw": yaw_rad}
|
||||
self.pose_valid = valid
|
||||
self.quality_score = quality_score
|
||||
self.update()
|
||||
|
||||
def set_trajectory(self, segments: list[list[tuple[float, float, float]]], label: str = "本轮计划轨迹") -> None:
|
||||
self.trajectory_segments = segments
|
||||
self.trajectory_label = label
|
||||
self.update()
|
||||
|
||||
def _geometry_ready(self) -> bool:
|
||||
return self.room_length_m > 0.0 and self.room_width_m > 0.0 and self.room_height_m > 0.0
|
||||
|
||||
def paintEvent(self, event) -> None:
|
||||
painter = QPainter(self)
|
||||
painter.setRenderHint(QPainter.Antialiasing, True)
|
||||
painter.fillRect(self.rect(), QColor("#f8fafc"))
|
||||
|
||||
if not self._geometry_ready():
|
||||
painter.setPen(QColor("#991b1b"))
|
||||
painter.setFont(QFont("", 12, QFont.Bold))
|
||||
painter.drawText(self.rect(), Qt.AlignCenter, "车间尺寸未配置，无法显示 3D 定位图")
|
||||
return
|
||||
|
||||
length = self.room_length_m
|
||||
width = self.room_width_m
|
||||
height = self.room_height_m
|
||||
margin = 32.0
|
||||
raw_points = [
|
||||
self._raw_project(x, y, z)
|
||||
for x in (-length / 2.0, length / 2.0)
|
||||
for y in (-width / 2.0, width / 2.0)
|
||||
for z in (0.0, height)
|
||||
]
|
||||
min_x = min(point.x() for point in raw_points)
|
||||
max_x = max(point.x() for point in raw_points)
|
||||
min_y = min(point.y() for point in raw_points)
|
||||
max_y = max(point.y() for point in raw_points)
|
||||
span_x = max(max_x - min_x, 1e-6)
|
||||
span_y = max(max_y - min_y, 1e-6)
|
||||
scale = min((self.width() - margin * 2.0) / span_x, (self.height() - margin * 2.0) / span_y)
|
||||
raw_center = QPointF((min_x + max_x) / 2.0, (min_y + max_y) / 2.0)
|
||||
screen_center = QPointF(self.width() / 2.0, self.height() / 2.0 + 18.0)
|
||||
|
||||
def project(x: float, y: float, z: float) -> QPointF:
|
||||
raw = self._raw_project(x, y, z)
|
||||
return QPointF(
|
||||
(raw.x() - raw_center.x()) * scale + screen_center.x(),
|
||||
(raw.y() - raw_center.y()) * scale + screen_center.y(),
|
||||
)
|
||||
|
||||
floor = [
|
||||
project(-length / 2.0, -width / 2.0, 0.0),
|
||||
project(length / 2.0, -width / 2.0, 0.0),
|
||||
project(length / 2.0, width / 2.0, 0.0),
|
||||
project(-length / 2.0, width / 2.0, 0.0),
|
||||
]
|
||||
top = [
|
||||
project(-length / 2.0, -width / 2.0, height),
|
||||
project(length / 2.0, -width / 2.0, height),
|
||||
project(length / 2.0, width / 2.0, height),
|
||||
project(-length / 2.0, width / 2.0, height),
|
||||
]
|
||||
|
||||
painter.setPen(QPen(QColor("#94a3b8"), 1))
|
||||
painter.setBrush(QBrush(QColor("#e0f2fe")))
|
||||
painter.drawPolygon(QPolygonF(floor))
|
||||
painter.setBrush(Qt.NoBrush)
|
||||
|
||||
grid_pen = QPen(QColor("#cbd5e1"), 1)
|
||||
grid_pen.setStyle(Qt.DotLine)
|
||||
painter.setPen(grid_pen)
|
||||
grid_count = 8
|
||||
for i in range(1, grid_count):
|
||||
x = -length / 2.0 + length * i / grid_count
|
||||
painter.drawLine(project(x, -width / 2.0, 0.0), project(x, width / 2.0, 0.0))
|
||||
y = -width / 2.0 + width * i / grid_count
|
||||
painter.drawLine(project(-length / 2.0, y, 0.0), project(length / 2.0, y, 0.0))
|
||||
|
||||
painter.setPen(QPen(QColor("#475569"), 2))
|
||||
painter.drawPolygon(QPolygonF(floor))
|
||||
top_pen = QPen(QColor("#64748b"), 1)
|
||||
top_pen.setStyle(Qt.DashLine)
|
||||
painter.setPen(top_pen)
|
||||
painter.drawPolygon(QPolygonF(top))
|
||||
painter.setPen(QPen(QColor("#64748b"), 1))
|
||||
for bottom, upper in zip(floor, top):
|
||||
painter.drawLine(bottom, upper)
|
||||
|
||||
self._draw_axes(painter, project, min(length, width, height) * 0.22)
|
||||
self._draw_trajectory(painter, project)
|
||||
if self.pose is not None:
|
||||
self._draw_vehicle(painter, project)
|
||||
|
||||
painter.setPen(QColor("#0f172a"))
|
||||
painter.setFont(QFont("", 10, QFont.Bold))
|
||||
painter.drawText(16, 24, f"车间 {length:g} x {width:g} x {height:g} m")
|
||||
painter.setFont(QFont("", 9))
|
||||
if self.pose is None:
|
||||
painter.drawText(16, 44, self.status_text)
|
||||
else:
|
||||
quality = "-" if self.quality_score is None else f"{self.quality_score:.3f}"
|
||||
painter.drawText(
|
||||
16,
|
||||
44,
|
||||
f"x={self.pose['x']:.3f} m, y={self.pose['y']:.3f} m, yaw={math.degrees(self.pose['yaw']):.2f} deg, 质量={quality}",
|
||||
)
|
||||
|
||||
def _raw_project(self, x: float, y: float, z: float) -> QPointF:
|
||||
yaw_cos = math.cos(self.view_yaw_rad)
|
||||
yaw_sin = math.sin(self.view_yaw_rad)
|
||||
pitch_sin = math.sin(self.view_pitch_rad)
|
||||
pitch_cos = math.cos(self.view_pitch_rad)
|
||||
rotated_x = x * yaw_cos - y * yaw_sin
|
||||
rotated_y = x * yaw_sin + y * yaw_cos
|
||||
return QPointF(rotated_x, rotated_y * pitch_sin - z * pitch_cos)
|
||||
|
||||
def mousePressEvent(self, event) -> None:
|
||||
if event.button() == Qt.LeftButton:
|
||||
self.view_drag_last_pos = event.position()
|
||||
self.setCursor(Qt.ClosedHandCursor)
|
||||
|
||||
def mouseMoveEvent(self, event) -> None:
|
||||
if self.view_drag_last_pos is None or not (event.buttons() & Qt.LeftButton):
|
||||
return
|
||||
current_pos = event.position()
|
||||
delta = current_pos - self.view_drag_last_pos
|
||||
self.view_drag_last_pos = current_pos
|
||||
self.view_yaw_rad += delta.x() * 0.01
|
||||
min_pitch = math.radians(8.0)
|
||||
max_pitch = math.radians(78.0)
|
||||
self.view_pitch_rad = min(max_pitch, max(min_pitch, self.view_pitch_rad - delta.y() * 0.008))
|
||||
self.update()
|
||||
|
||||
def mouseReleaseEvent(self, event) -> None:
|
||||
if event.button() == Qt.LeftButton:
|
||||
self.view_drag_last_pos = None
|
||||
self.setCursor(Qt.OpenHandCursor)
|
||||
|
||||
def mouseDoubleClickEvent(self, event) -> None:
|
||||
if event.button() == Qt.LeftButton:
|
||||
self.view_yaw_rad = math.radians(45.0)
|
||||
self.view_pitch_rad = math.radians(35.0)
|
||||
self.update()
|
||||
|
||||
def _draw_axes(self, painter: QPainter, project, axis_len: float) -> None:
|
||||
origin = project(0.0, 0.0, 0.0)
|
||||
axes = [
|
||||
(project(axis_len, 0.0, 0.0), QColor("#dc2626"), "X"),
|
||||
(project(0.0, axis_len, 0.0), QColor("#16a34a"), "Y"),
|
||||
(project(0.0, 0.0, axis_len), QColor("#2563eb"), "Z"),
|
||||
]
|
||||
for end, color, label in axes:
|
||||
painter.setPen(QPen(color, 2))
|
||||
painter.drawLine(origin, end)
|
||||
painter.drawText(end + QPointF(4.0, -4.0), label)
|
||||
|
||||
def _draw_trajectory(self, painter: QPainter, project) -> None:
|
||||
if not self.trajectory_segments:
|
||||
return
|
||||
path_pen = QPen(QColor("#22c55e"), 3)
|
||||
marker_pen = QPen(QColor("#166534"), 1)
|
||||
marker_brush = QBrush(QColor("#22c55e"))
|
||||
label_drawn = False
|
||||
for segment in self.trajectory_segments:
|
||||
if len(segment) < 2:
|
||||
continue
|
||||
points = [project(x, y, z) for x, y, z in segment]
|
||||
painter.setPen(path_pen)
|
||||
for index in range(len(points) - 1):
|
||||
painter.drawLine(points[index], points[index + 1])
|
||||
painter.setPen(marker_pen)
|
||||
painter.setBrush(marker_brush)
|
||||
for point in points:
|
||||
painter.drawEllipse(point, 3.5, 3.5)
|
||||
painter.setBrush(Qt.NoBrush)
|
||||
if not label_drawn:
|
||||
painter.setPen(QColor("#166534"))
|
||||
painter.setFont(QFont("", 9, QFont.Bold))
|
||||
painter.drawText(points[0] + QPointF(6.0, -6.0), self.trajectory_label)
|
||||
label_drawn = True
|
||||
|
||||
def _draw_vehicle(self, painter: QPainter, project) -> None:
|
||||
if self.pose is None:
|
||||
return
|
||||
length = min(max(self.room_length_m * 0.08, 0.55), 1.20)
|
||||
width = length * 0.58
|
||||
height = min(max(self.room_height_m * 0.08, 0.25), 0.55)
|
||||
x = self.pose["x"]
|
||||
y = self.pose["y"]
|
||||
yaw = self.pose["yaw"]
|
||||
cos_yaw = math.cos(yaw)
|
||||
sin_yaw = math.sin(yaw)
|
||||
|
||||
def corner(dx: float, dy: float, z: float) -> QPointF:
|
||||
return project(
|
||||
x + dx * cos_yaw - dy * sin_yaw,
|
||||
y + dx * sin_yaw + dy * cos_yaw,
|
||||
z,
|
||||
)
|
||||
|
||||
bottom = [
|
||||
corner(length / 2.0, width / 2.0, 0.0),
|
||||
corner(length / 2.0, -width / 2.0, 0.0),
|
||||
corner(-length / 2.0, -width / 2.0, 0.0),
|
||||
corner(-length / 2.0, width / 2.0, 0.0),
|
||||
]
|
||||
upper = [
|
||||
corner(length / 2.0, width / 2.0, height),
|
||||
corner(length / 2.0, -width / 2.0, height),
|
||||
corner(-length / 2.0, -width / 2.0, height),
|
||||
corner(-length / 2.0, width / 2.0, height),
|
||||
]
|
||||
color = QColor("#22c55e") if self.pose_valid else QColor("#ef4444")
|
||||
color.setAlpha(190)
|
||||
painter.setPen(QPen(QColor("#14532d") if self.pose_valid else QColor("#7f1d1d"), 2))
|
||||
painter.setBrush(QBrush(color))
|
||||
painter.drawPolygon(QPolygonF(upper))
|
||||
painter.setBrush(Qt.NoBrush)
|
||||
painter.drawPolygon(QPolygonF(bottom))
|
||||
painter.drawPolygon(QPolygonF(upper))
|
||||
for p0, p1 in zip(bottom, upper):
|
||||
painter.drawLine(p0, p1)
|
||||
center = project(x, y, height + 0.03)
|
||||
front = project(x + math.cos(yaw) * length * 0.75, y + math.sin(yaw) * length * 0.75, height + 0.03)
|
||||
painter.setPen(QPen(QColor("#0f172a"), 3))
|
||||
painter.drawLine(center, front)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,210 @@
|
||||
"""操作台进程启动、日志和报告展示。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import sys
|
||||
|
||||
from PySide6.QtCore import QProcess
|
||||
from PySide6.QtWidgets import QMessageBox
|
||||
|
||||
try:
|
||||
from .profile_io import launch_arg, shell_join
|
||||
from .ui_helpers import table_item
|
||||
except ImportError:
|
||||
from profile_io import launch_arg, shell_join
|
||||
from ui_helpers import table_item
|
||||
|
||||
|
||||
class ProcessReportMixin:
|
||||
def build_launch_command(self) -> str:
|
||||
args = [
|
||||
"ros2",
|
||||
"launch",
|
||||
"win_ubuntu_bridge",
|
||||
"minimal_workshop_demo.launch.py",
|
||||
launch_arg("external_telemetry_topic", self.external_topic_edit.text().strip()),
|
||||
launch_arg("expected_reference_source_name", self.reference_source_edit.text().strip()),
|
||||
launch_arg("expected_workcell_zone_id", self.workcell_zone_edit.text().strip()),
|
||||
launch_arg("use_gateway", "true"),
|
||||
launch_arg("chassis_host", self.vehicle_host_edit.text().strip()),
|
||||
launch_arg("control_host", self.vehicle_host_edit.text().strip()),
|
||||
launch_arg("sensor_storage_root", self.sensor_storage_edit.text().strip()),
|
||||
launch_arg("sensor_registry", self.sensor_registry_edit.text().strip()),
|
||||
]
|
||||
if self.data_input_edit.text().strip():
|
||||
args.append(launch_arg("data_input_params_file", self.data_input_edit.text().strip()))
|
||||
if self.dataset_index_edit.text().strip():
|
||||
args.append(launch_arg("dataset_index_file", self.dataset_index_edit.text().strip()))
|
||||
return "source install/setup.bash && " + shell_join(args)
|
||||
|
||||
def build_smoke_command(self) -> str:
|
||||
args = [
|
||||
sys.executable,
|
||||
"src/simulation/tools/smoke_test_workshop_orchestrator.py",
|
||||
"--site-profile",
|
||||
self.profile_path_edit.text().strip(),
|
||||
"--tasks",
|
||||
self.selected_tasks_csv(),
|
||||
"--session-config-file",
|
||||
self.session_config_output_edit.text().strip(),
|
||||
"--no-publish-fake-external-telemetry",
|
||||
"--external-telemetry-topic",
|
||||
self.external_topic_edit.text().strip(),
|
||||
"--reference-source-name",
|
||||
self.reference_source_edit.text().strip(),
|
||||
"--workcell-zone-id",
|
||||
self.workcell_zone_edit.text().strip(),
|
||||
"--vehicle-id",
|
||||
self.vehicle_id_edit.text().strip(),
|
||||
"--action-timeout-sec",
|
||||
"240",
|
||||
"--service-timeout-sec",
|
||||
"10",
|
||||
]
|
||||
if self.vehicle_profile_path_edit.text().strip():
|
||||
args.extend(["--vehicle-profile-file", self.vehicle_profile_path_edit.text().strip()])
|
||||
if self.skip_wifi6_check.isChecked():
|
||||
args.append("--disable-wifi6-precheck")
|
||||
return "source install/setup.bash && " + shell_join(args)
|
||||
|
||||
def refresh_command_preview(self) -> None:
|
||||
if not hasattr(self, "launch_command_preview"):
|
||||
return
|
||||
self.launch_command_preview.setPlainText(self.build_launch_command())
|
||||
self.smoke_command_preview.setPlainText(self.build_smoke_command())
|
||||
self.refresh_deployment_checks()
|
||||
self.refresh_task_preview_from_config()
|
||||
|
||||
def start_launch_stack(self) -> None:
|
||||
if self.launch_process.state() != QProcess.NotRunning:
|
||||
QMessageBox.warning(self, "现场服务已运行", "现场服务进程已经在运行。")
|
||||
return
|
||||
command = self.build_launch_command()
|
||||
self.append_log("[现场服务] 启动：" + command)
|
||||
self.stack_state.setText("现场服务：运行中")
|
||||
self.launch_process.start("bash", ["-lc", command])
|
||||
|
||||
def start_smoke_test(self) -> None:
|
||||
if self.smoke_process.state() != QProcess.NotRunning:
|
||||
QMessageBox.warning(self, "标定流程已运行", "标定流程已经在运行。")
|
||||
return
|
||||
if not self.selected_tasks():
|
||||
QMessageBox.warning(self, "任务为空", "请至少选择一个任务。")
|
||||
return
|
||||
if not self.build_session_config():
|
||||
return
|
||||
if self.launch_process.state() == QProcess.NotRunning:
|
||||
reply = QMessageBox.question(
|
||||
self,
|
||||
"现场服务未运行",
|
||||
"当前界面没有检测到由本界面启动的现场服务，仍然继续执行本轮标定吗？",
|
||||
)
|
||||
if reply != QMessageBox.Yes:
|
||||
return
|
||||
self.reset_report()
|
||||
if self.external_check.isChecked():
|
||||
self.update_localization_state("检查中", "running")
|
||||
else:
|
||||
self.update_localization_state("本轮未检查", "neutral")
|
||||
self.trajectory_runtime_active = True
|
||||
self.trajectory_last_completed_stage_id = ""
|
||||
self.refresh_planned_trajectory()
|
||||
command = self.build_smoke_command()
|
||||
self.append_log("[标定流程] 启动：" + command)
|
||||
self.smoke_state.setText("标定流程：运行中")
|
||||
self.smoke_process.start("bash", ["-lc", command])
|
||||
|
||||
def stop_process(self, process: QProcess, name: str) -> None:
|
||||
if process.state() == QProcess.NotRunning:
|
||||
self.append_log(f"[{name}] 当前没有运行中的进程。")
|
||||
return
|
||||
self.append_log(f"[{name}] 正在停止。")
|
||||
process.terminate()
|
||||
if not process.waitForFinished(3000):
|
||||
process.kill()
|
||||
process.waitForFinished(2000)
|
||||
|
||||
def _read_process_output(self, process: QProcess, name: str) -> None:
|
||||
chunks = [
|
||||
bytes(process.readAllStandardOutput()).decode(errors="replace"),
|
||||
bytes(process.readAllStandardError()).decode(errors="replace"),
|
||||
]
|
||||
for chunk in chunks:
|
||||
if not chunk:
|
||||
continue
|
||||
for line in chunk.splitlines():
|
||||
self.append_log(f"[{name}] {line}")
|
||||
self.parse_report_line(line)
|
||||
|
||||
def _process_finished(self, name: str, code: int, status: QProcess.ExitStatus) -> None:
|
||||
status_text = "正常退出" if status == QProcess.NormalExit and code == 0 else f"退出码 {code}"
|
||||
self.append_log(f"[{name}] {status_text}")
|
||||
if name == "现场服务":
|
||||
self.stack_state.setText("现场服务：未启动")
|
||||
else:
|
||||
self.trajectory_runtime_active = False
|
||||
self.trajectory_last_completed_stage_id = ""
|
||||
self.refresh_planned_trajectory()
|
||||
self.smoke_state.setText("标定流程：空闲")
|
||||
|
||||
def append_log(self, text: str) -> None:
|
||||
self.log_view.appendPlainText(text)
|
||||
scrollbar = self.log_view.verticalScrollBar()
|
||||
scrollbar.setValue(scrollbar.maximum())
|
||||
|
||||
def reset_report(self) -> None:
|
||||
self.report_metadata.clear()
|
||||
self.report_stages.clear()
|
||||
self.report_summary_label.setText("等待本轮标定报告")
|
||||
self.stage_table.setRowCount(0)
|
||||
self.metadata_table.setRowCount(0)
|
||||
|
||||
def parse_report_line(self, line: str) -> None:
|
||||
if line.startswith("[REPORT] "):
|
||||
self.report_summary_label.setText(line.replace("[REPORT] ", "", 1))
|
||||
return
|
||||
metadata_match = re.match(r"^\[REPORT_METADATA\]\s+([^=]+)=(.*)$", line)
|
||||
if metadata_match:
|
||||
self.report_metadata[metadata_match.group(1)] = metadata_match.group(2)
|
||||
self.refresh_metadata_table()
|
||||
return
|
||||
stage_match = re.match(
|
||||
r"^\[STAGE\]\s+id=(.*?)\s+success=(.*?)\s+auto_acceptance=(.*?)\s+state=(.*?)\s+summary=(.*)$",
|
||||
line,
|
||||
)
|
||||
if stage_match:
|
||||
stage_id = stage_match.group(1)
|
||||
success_text = stage_match.group(2)
|
||||
summary = stage_match.group(5)
|
||||
self.report_stages.append({
|
||||
"stage_id": stage_id,
|
||||
"success": success_text,
|
||||
"auto_acceptance": stage_match.group(3),
|
||||
"state": stage_match.group(4),
|
||||
"summary": summary,
|
||||
})
|
||||
if "external_reference" in stage_id or stage_id.endswith("_external"):
|
||||
if success_text == "True":
|
||||
self.update_localization_state("检查通过", "ok", summary)
|
||||
else:
|
||||
self.update_localization_state("检查失败", "fail", summary)
|
||||
self.refresh_stage_table()
|
||||
|
||||
def refresh_metadata_table(self) -> None:
|
||||
items = list(self.report_metadata.items())
|
||||
self.metadata_table.setRowCount(len(items))
|
||||
for row, (key, value) in enumerate(items):
|
||||
self.metadata_table.setItem(row, 0, table_item(key))
|
||||
self.metadata_table.setItem(row, 1, table_item(value))
|
||||
|
||||
def refresh_stage_table(self) -> None:
|
||||
self.stage_table.setRowCount(len(self.report_stages))
|
||||
for row, stage in enumerate(self.report_stages):
|
||||
success = stage["success"] == "True"
|
||||
accepted = stage["auto_acceptance"] == "True"
|
||||
self.stage_table.setItem(row, 0, table_item(stage["stage_id"]))
|
||||
self.stage_table.setItem(row, 1, table_item(stage["success"], "ok" if success else "fail"))
|
||||
self.stage_table.setItem(row, 2, table_item(stage["auto_acceptance"], "ok" if accepted else "fail"))
|
||||
self.stage_table.setItem(row, 3, table_item(stage["state"]))
|
||||
self.stage_table.setItem(row, 4, table_item(stage["summary"]))
|
||||
@@ -0,0 +1,382 @@
|
||||
"""操作台现场配置和车辆画像处理。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from PySide6.QtWidgets import QFileDialog, QMessageBox
|
||||
|
||||
try:
|
||||
from .constants import DEFAULT_SESSION_CONFIG, DEFAULT_SITE_PROFILE, DEFAULT_VEHICLE_PROFILE, DEFAULT_VEHICLE_PROFILE_DIR, REPO_ROOT
|
||||
from .profile_io import (
|
||||
bool_from_text,
|
||||
collect_placeholders,
|
||||
dump_yaml,
|
||||
load_yaml,
|
||||
read_nested,
|
||||
resolve_profile_path,
|
||||
resolve_vehicle_profile_path,
|
||||
vehicle_profile_path_from_name,
|
||||
)
|
||||
from .vehicle_profile_dialog import open_vehicle_profile_dialog as show_vehicle_profile_dialog
|
||||
except ImportError:
|
||||
from constants import DEFAULT_SESSION_CONFIG, DEFAULT_SITE_PROFILE, DEFAULT_VEHICLE_PROFILE, DEFAULT_VEHICLE_PROFILE_DIR, REPO_ROOT
|
||||
from profile_io import (
|
||||
bool_from_text,
|
||||
collect_placeholders,
|
||||
dump_yaml,
|
||||
load_yaml,
|
||||
read_nested,
|
||||
resolve_profile_path,
|
||||
resolve_vehicle_profile_path,
|
||||
vehicle_profile_path_from_name,
|
||||
)
|
||||
from vehicle_profile_dialog import open_vehicle_profile_dialog as show_vehicle_profile_dialog
|
||||
|
||||
|
||||
class ProfileHandlersMixin:
|
||||
def browse_profile(self) -> None:
|
||||
path, _ = QFileDialog.getOpenFileName(self, "选择现场配置文件", str(REPO_ROOT), "YAML Files (*.yaml *.yml)")
|
||||
if path:
|
||||
self.profile_path_edit.setText(path)
|
||||
self.load_profile()
|
||||
|
||||
def refresh_vehicle_profile_combo(self) -> None:
|
||||
if not hasattr(self, "vehicle_profile_combo"):
|
||||
return
|
||||
current_path = self.vehicle_profile_path_edit.text().strip() if hasattr(self, "vehicle_profile_path_edit") else ""
|
||||
self.vehicle_profile_combo.blockSignals(True)
|
||||
self.vehicle_profile_combo.clear()
|
||||
DEFAULT_VEHICLE_PROFILE_DIR.mkdir(parents=True, exist_ok=True)
|
||||
profile_paths: list[Path] = []
|
||||
for pattern in ("*.yaml", "*.yml", "*.ymal"):
|
||||
profile_paths.extend(DEFAULT_VEHICLE_PROFILE_DIR.glob(pattern))
|
||||
for profile_path in sorted(set(profile_paths)):
|
||||
display_name = profile_path.stem
|
||||
try:
|
||||
profile = load_yaml(profile_path)
|
||||
display_name = str(profile.get("display_name") or profile.get("profile_name") or profile_path.stem)
|
||||
except Exception:
|
||||
pass
|
||||
self.vehicle_profile_combo.addItem(display_name, str(profile_path))
|
||||
self.vehicle_profile_combo.blockSignals(False)
|
||||
if current_path:
|
||||
resolved = str(resolve_vehicle_profile_path(current_path))
|
||||
for index in range(self.vehicle_profile_combo.count()):
|
||||
if str(resolve_vehicle_profile_path(str(self.vehicle_profile_combo.itemData(index)))) == resolved:
|
||||
self.vehicle_profile_combo.setCurrentIndex(index)
|
||||
break
|
||||
|
||||
def browse_vehicle_profile(self) -> None:
|
||||
path, _ = QFileDialog.getOpenFileName(
|
||||
self,
|
||||
"选择车辆画像文件",
|
||||
str(DEFAULT_VEHICLE_PROFILE_DIR),
|
||||
"YAML Files (*.yaml *.yml)",
|
||||
)
|
||||
if path:
|
||||
self.vehicle_profile_path_edit.setText(path)
|
||||
self.load_vehicle_profile_from_current_path()
|
||||
|
||||
def open_new_vehicle_profile_dialog(self) -> None:
|
||||
self.open_vehicle_profile_dialog(new_profile=True)
|
||||
|
||||
def open_edit_vehicle_profile_dialog(self) -> None:
|
||||
self.open_vehicle_profile_dialog(new_profile=False)
|
||||
|
||||
def open_vehicle_profile_dialog(self, new_profile: bool) -> None:
|
||||
show_vehicle_profile_dialog(self, new_profile)
|
||||
|
||||
def load_selected_vehicle_profile(self, _index: int | None = None) -> None:
|
||||
path = self.vehicle_profile_combo.currentData()
|
||||
if path:
|
||||
self.vehicle_profile_path_edit.setText(str(path))
|
||||
self.load_vehicle_profile_from_current_path()
|
||||
|
||||
def load_vehicle_profile_from_current_path(self) -> None:
|
||||
path = resolve_vehicle_profile_path(self.vehicle_profile_path_edit.text().strip() or str(DEFAULT_VEHICLE_PROFILE))
|
||||
try:
|
||||
profile = load_yaml(path)
|
||||
except Exception as exc:
|
||||
QMessageBox.warning(self, "车辆画像加载失败", f"无法加载车辆画像文件：{exc}")
|
||||
return
|
||||
self.vehicle_profile_data = profile
|
||||
self.vehicle_profile_path_edit.setText(str(path))
|
||||
self.vehicle_profile_name_edit.setText(str(profile.get("profile_name") or profile.get("display_name") or path.stem))
|
||||
self.vehicle_model_name_edit.setText(str(profile.get("model_name") or ""))
|
||||
self.vehicle_manufacturer_edit.setText(str(profile.get("manufacturer") or ""))
|
||||
self.apply_vehicle_profile_to_ui(profile)
|
||||
self.refresh_vehicle_profile_combo()
|
||||
self.append_log(f"[车辆画像] 已加载：{path}")
|
||||
self.refresh_all()
|
||||
|
||||
def apply_vehicle_profile_to_ui(self, profile: dict[str, Any]) -> None:
|
||||
geometry = profile.get("geometry", {})
|
||||
if not isinstance(geometry, dict):
|
||||
geometry = {}
|
||||
self.vehicle_length_edit.setText(str(geometry.get("length_m", "")))
|
||||
self.vehicle_width_edit.setText(str(geometry.get("width_m", "")))
|
||||
self.vehicle_height_edit.setText(str(geometry.get("height_m", "")))
|
||||
self.vehicle_ground_clearance_edit.setText(str(geometry.get("ground_clearance_m", "")))
|
||||
|
||||
chassis = profile.get("chassis", {})
|
||||
if not isinstance(chassis, dict):
|
||||
chassis = {}
|
||||
self.vehicle_wheel_base_edit.setText(str(chassis.get("wheel_base_m", "")))
|
||||
self.vehicle_track_width_edit.setText(str(chassis.get("track_width_m", "")))
|
||||
self.vehicle_wheel_radius_edit.setText(str(chassis.get("wheel_radius_m", "")))
|
||||
self.vehicle_max_steering_angle_edit.setText(str(chassis.get("max_steering_angle_rad", "")))
|
||||
self.vehicle_min_turning_radius_edit.setText(str(chassis.get("min_turning_radius_m", "")))
|
||||
chassis_type = str(chassis.get("chassis_type") or profile.get("vehicle_class") or "")
|
||||
if chassis_type and hasattr(self, "chassis_type_combo"):
|
||||
index = self.chassis_type_combo.findData(chassis_type)
|
||||
if index >= 0:
|
||||
self.chassis_type_combo.setCurrentIndex(index)
|
||||
self.rebuild_chassis_parameter_checks()
|
||||
|
||||
controllers = profile.get("controllers", {})
|
||||
if not isinstance(controllers, dict):
|
||||
controllers = {}
|
||||
self.vehicle_max_speed_edit.setText(str(controllers.get("max_speed_mps", "")))
|
||||
self.vehicle_max_accel_edit.setText(str(controllers.get("max_accel_mps2", "")))
|
||||
self.vehicle_control_frequency_edit.setText(str(controllers.get("control_frequency_hz", "")))
|
||||
|
||||
sensors = profile.get("sensors", {})
|
||||
if isinstance(sensors, dict) and hasattr(self, "sensor_id_edits"):
|
||||
for edit in self.sensor_id_edits.values():
|
||||
edit.clear()
|
||||
for sensor_key, edit in self.sensor_id_edits.items():
|
||||
sensor_profile = sensors.get(sensor_key, {})
|
||||
if (
|
||||
isinstance(sensor_profile, dict)
|
||||
and bool_from_text(sensor_profile.get("enabled", True))
|
||||
and sensor_profile.get("sensor_id")
|
||||
):
|
||||
edit.setText(str(sensor_profile["sensor_id"]))
|
||||
sensor_ids = [
|
||||
edit.text().strip()
|
||||
for edit in self.sensor_id_edits.values()
|
||||
if edit.text().strip()
|
||||
]
|
||||
self.sensor_registry_edit.setText(",".join(sensor_ids))
|
||||
sensor_display_names = {
|
||||
"front_camera": "前视相机",
|
||||
"down_camera": "下视相机",
|
||||
"lidar_2d": "2D 雷达",
|
||||
"lidar_3d": "3D 雷达",
|
||||
"imu": "IMU",
|
||||
"arm_camera": "手眼相机",
|
||||
}
|
||||
summary_parts = [
|
||||
f"{sensor_display_names.get(sensor_key, sensor_key)}：{edit.text().strip()}"
|
||||
for sensor_key, edit in self.sensor_id_edits.items()
|
||||
if edit.text().strip()
|
||||
]
|
||||
if hasattr(self, "sensor_profile_summary_label"):
|
||||
summary = "；".join(summary_parts) if summary_parts else "当前车辆画像没有启用传感器"
|
||||
self.sensor_profile_summary_label.setText(f"当前画像传感器：{summary}")
|
||||
if hasattr(self, "sensor_task_checks"):
|
||||
for (sensor_key, _subtype), check in self.sensor_task_checks.items():
|
||||
available = bool(self.sensor_id_edits.get(sensor_key) and self.sensor_id_edits[sensor_key].text().strip())
|
||||
check.setEnabled(available)
|
||||
if not available:
|
||||
check.setChecked(False)
|
||||
|
||||
def current_vehicle_profile_payload(self) -> dict[str, Any]:
|
||||
chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") if hasattr(self, "chassis_type_combo") else "ackermann"
|
||||
loaded_sensors = self.vehicle_profile_data.get("sensors", {}) if isinstance(self.vehicle_profile_data, dict) else {}
|
||||
if not isinstance(loaded_sensors, dict):
|
||||
loaded_sensors = {}
|
||||
sensor_payload: dict[str, dict[str, Any]] = {}
|
||||
if hasattr(self, "sensor_id_edits"):
|
||||
for sensor_key, edit in self.sensor_id_edits.items():
|
||||
existing = loaded_sensors.get(sensor_key, {})
|
||||
if not isinstance(existing, dict):
|
||||
existing = {}
|
||||
sensor_id = edit.text().strip()
|
||||
if not sensor_id:
|
||||
continue
|
||||
mount_type = existing.get("camera_mount_type", "eye_in_hand" if sensor_key == "arm_camera" else "unspecified")
|
||||
sensor_payload[sensor_key] = {
|
||||
"sensor_id": sensor_id,
|
||||
"sensor_name": existing.get("sensor_name", sensor_key),
|
||||
"frame_id": existing.get("frame_id", f"{sensor_key}_link"),
|
||||
"sensor_type": existing.get("sensor_type", sensor_key),
|
||||
"camera_mount_type": mount_type,
|
||||
"enabled": existing.get("enabled", True),
|
||||
"needs_intrinsic_calibration": existing.get("needs_intrinsic_calibration", sensor_key in {"front_camera", "down_camera", "imu"}),
|
||||
"needs_extrinsic_calibration": existing.get("needs_extrinsic_calibration", True),
|
||||
}
|
||||
loaded_chassis = self.vehicle_profile_data.get("chassis", {}) if isinstance(self.vehicle_profile_data, dict) else {}
|
||||
if not isinstance(loaded_chassis, dict):
|
||||
loaded_chassis = {}
|
||||
loaded_controllers = self.vehicle_profile_data.get("controllers", {}) if isinstance(self.vehicle_profile_data, dict) else {}
|
||||
if not isinstance(loaded_controllers, dict):
|
||||
loaded_controllers = {}
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"profile_name": self.vehicle_profile_name_edit.text().strip() or "unnamed_vehicle_profile",
|
||||
"display_name": self.vehicle_profile_name_edit.text().strip() or "未命名车辆画像",
|
||||
"model_name": self.vehicle_model_name_edit.text().strip(),
|
||||
"manufacturer": self.vehicle_manufacturer_edit.text().strip(),
|
||||
"profile_version": self.vehicle_profile_data.get("profile_version", "v1") if isinstance(self.vehicle_profile_data, dict) else "v1",
|
||||
"description": self.vehicle_profile_data.get("description", "") if isinstance(self.vehicle_profile_data, dict) else "",
|
||||
"base_link_frame": read_nested(self.site_profile, ("frames", "base_link"), "base_link"),
|
||||
"geometry": {
|
||||
"length_m": self.vehicle_length_edit.text().strip(),
|
||||
"width_m": self.vehicle_width_edit.text().strip(),
|
||||
"height_m": self.vehicle_height_edit.text().strip(),
|
||||
"ground_clearance_m": self.vehicle_ground_clearance_edit.text().strip(),
|
||||
},
|
||||
"chassis": {
|
||||
"chassis_type": chassis_type,
|
||||
"wheel_base_m": self.vehicle_wheel_base_edit.text().strip() or read_nested(self.site_profile, ("vehicle_agent", "wheel_base_m"), ""),
|
||||
"track_width_m": self.vehicle_track_width_edit.text().strip() or loaded_chassis.get("track_width_m", ""),
|
||||
"wheel_radius_m": self.vehicle_wheel_radius_edit.text().strip() or loaded_chassis.get("wheel_radius_m", ""),
|
||||
"max_steering_angle_rad": self.vehicle_max_steering_angle_edit.text().strip() or read_nested(self.site_profile, ("vehicle_agent", "max_steering_angle_rad"), ""),
|
||||
"min_turning_radius_m": self.vehicle_min_turning_radius_edit.text().strip() or loaded_chassis.get("min_turning_radius_m", ""),
|
||||
},
|
||||
"sensors": sensor_payload,
|
||||
"controllers": {
|
||||
"lateral_default": loaded_controllers.get("lateral_default", "mpc"),
|
||||
"longitudinal_default": loaded_controllers.get("longitudinal_default", "pid"),
|
||||
"max_speed_mps": self.vehicle_max_speed_edit.text().strip(),
|
||||
"max_accel_mps2": self.vehicle_max_accel_edit.text().strip(),
|
||||
"control_frequency_hz": self.vehicle_control_frequency_edit.text().strip(),
|
||||
},
|
||||
}
|
||||
|
||||
def save_vehicle_profile_to_path(self, path: Path) -> None:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
payload = self.current_vehicle_profile_payload()
|
||||
path.write_text(dump_yaml(payload), encoding="utf-8")
|
||||
self.vehicle_profile_data = payload
|
||||
self.vehicle_profile_path_edit.setText(str(path))
|
||||
self.refresh_vehicle_profile_combo()
|
||||
self.append_log(f"[车辆画像] 已保存：{path}")
|
||||
self.refresh_all()
|
||||
|
||||
def save_vehicle_profile(self) -> None:
|
||||
self.save_vehicle_profile_to_path(vehicle_profile_path_from_name(self.vehicle_profile_name_edit.text()))
|
||||
|
||||
def save_vehicle_profile_as(self) -> None:
|
||||
self.save_vehicle_profile()
|
||||
|
||||
def browse_data_input(self) -> None:
|
||||
path, _ = QFileDialog.getOpenFileName(self, "选择算法数据参数文件", str(REPO_ROOT), "YAML Files (*.yaml *.yml)")
|
||||
if path:
|
||||
self.data_input_edit.setText(path)
|
||||
self.refresh_all()
|
||||
|
||||
def browse_dataset_index(self) -> None:
|
||||
path, _ = QFileDialog.getOpenFileName(self, "选择采集数据索引文件", str(REPO_ROOT), "YAML Files (*.yaml *.yml)")
|
||||
if path:
|
||||
self.dataset_index_edit.setText(path)
|
||||
self.refresh_all()
|
||||
|
||||
def browse_session_config_output(self) -> None:
|
||||
path, _ = QFileDialog.getSaveFileName(self, "选择本轮任务文件保存位置", str(DEFAULT_SESSION_CONFIG), "YAML Files (*.yaml *.yml)")
|
||||
if path:
|
||||
self.session_config_output_edit.setText(path)
|
||||
self.refresh_command_preview()
|
||||
|
||||
def load_profile(self) -> None:
|
||||
path = resolve_profile_path(self.profile_path_edit.text().strip())
|
||||
try:
|
||||
self.site_profile = load_yaml(path)
|
||||
except Exception as exc:
|
||||
self.site_profile = {}
|
||||
self.profile_state_label.setText(f"加载失败：{exc}")
|
||||
self.append_log(f"[界面] 现场配置文件加载失败: {exc}")
|
||||
self.refresh_all()
|
||||
return
|
||||
|
||||
self.profile_path_edit.setText(str(path))
|
||||
self._apply_profile_defaults(path)
|
||||
unresolved = collect_placeholders(self.site_profile)
|
||||
self.profile_state_label.setText(f"已加载，占位项 {len(unresolved)} 个")
|
||||
self.append_log(f"[界面] 已加载现场配置文件: {path}")
|
||||
self.refresh_all()
|
||||
self.restart_localization_monitor()
|
||||
self.restart_workshop_event_monitor()
|
||||
|
||||
def _apply_profile_defaults(self, profile_path: Path) -> None:
|
||||
profile = self.site_profile
|
||||
self.vehicle_id_edit.setText(read_nested(profile, ("vehicle_id",), "demo_agv_001"))
|
||||
self.update_workshop_dimension_state()
|
||||
self.vehicle_host_edit.setText(read_nested(profile, ("workshop_pc", "gateway", "vehicle_host"), "127.0.0.1"))
|
||||
self.vehicle_port_edit.setText(read_nested(profile, ("workshop_pc", "gateway", "vehicle_port"), "9000"))
|
||||
self.external_topic_edit.setText(
|
||||
read_nested(profile, ("external_pose_bridge", "source_topic"))
|
||||
or read_nested(profile, ("external_localization", "output_topic"), "/workshop/external_localization/vehicle/pose")
|
||||
)
|
||||
self.reference_source_edit.setText(
|
||||
read_nested(profile, ("external_pose_bridge", "reference_source_name"))
|
||||
or read_nested(profile, ("external_localization", "reference_source_name"), "workshop_four_lidar_ball_truth")
|
||||
)
|
||||
self.workcell_zone_edit.setText(read_nested(profile, ("external_localization", "workcell_zone_id"), "workcell_zone_a"))
|
||||
self.update_localization_state("待检查", "warn")
|
||||
self.data_input_edit.setText(read_nested(profile, ("algorithm_data_inputs", "ros_params_file"), ""))
|
||||
self.dataset_index_edit.setText(
|
||||
read_nested(profile, ("external_localization", "recording_dataset_index_path"))
|
||||
or read_nested(profile, ("chassis_calibration", "recording_dataset_index_path"))
|
||||
or read_nested(profile, ("sensor_calibration", "recording_dataset_index_path"), "")
|
||||
)
|
||||
self.sensor_storage_edit.setText(
|
||||
read_nested(profile, ("sensor_calibration", "recording_session_dir"), "/tmp/agv_sensor_calibration")
|
||||
)
|
||||
sensor_ids = [
|
||||
read_nested(profile, ("vehicle_sensor_agent", "front_camera_sensor_id")),
|
||||
read_nested(profile, ("vehicle_sensor_agent", "down_camera_sensor_id")),
|
||||
read_nested(profile, ("vehicle_sensor_agent", "lidar_3d_sensor_id")),
|
||||
read_nested(profile, ("vehicle_sensor_agent", "lidar_2d_sensor_id")),
|
||||
read_nested(profile, ("vehicle_sensor_agent", "imu_sensor_id")),
|
||||
]
|
||||
sensor_ids = [value for value in sensor_ids if value]
|
||||
self.sensor_registry_edit.setText(",".join(sensor_ids) if sensor_ids else "demo_front_camera")
|
||||
if hasattr(self, "sensor_id_edits"):
|
||||
sensor_defaults = {
|
||||
"front_camera": read_nested(profile, ("vehicle_sensor_agent", "front_camera_sensor_id"), "demo_front_camera"),
|
||||
"down_camera": read_nested(profile, ("vehicle_sensor_agent", "down_camera_sensor_id"), "demo_down_camera"),
|
||||
"lidar_2d": read_nested(profile, ("vehicle_sensor_agent", "lidar_2d_sensor_id"), "demo_lidar_2d"),
|
||||
"lidar_3d": read_nested(profile, ("vehicle_sensor_agent", "lidar_3d_sensor_id"), "demo_lidar_3d"),
|
||||
"imu": read_nested(profile, ("vehicle_sensor_agent", "imu_sensor_id"), "demo_imu"),
|
||||
"arm_camera": "demo_arm_camera",
|
||||
}
|
||||
for sensor_key, sensor_id in sensor_defaults.items():
|
||||
self.sensor_id_edits[sensor_key].setText(sensor_id)
|
||||
sensor_display_names = {
|
||||
"front_camera": "前视相机",
|
||||
"down_camera": "下视相机",
|
||||
"lidar_2d": "2D 雷达",
|
||||
"lidar_3d": "3D 雷达",
|
||||
"imu": "IMU",
|
||||
"arm_camera": "手眼相机",
|
||||
}
|
||||
summary_parts = [
|
||||
f"{sensor_display_names.get(sensor_key, sensor_key)}：{edit.text().strip()}"
|
||||
for sensor_key, edit in self.sensor_id_edits.items()
|
||||
if edit.text().strip()
|
||||
]
|
||||
if hasattr(self, "sensor_profile_summary_label"):
|
||||
summary = "；".join(summary_parts) if summary_parts else "当前车辆画像没有启用传感器"
|
||||
self.sensor_profile_summary_label.setText(f"当前画像传感器：{summary}")
|
||||
chassis_type = (
|
||||
read_nested(profile, ("chassis_calibration", "chassis_type"))
|
||||
or read_nested(profile, ("control_calibration", "chassis_type"))
|
||||
or "ackermann"
|
||||
)
|
||||
if hasattr(self, "chassis_type_combo"):
|
||||
index = self.chassis_type_combo.findData(chassis_type)
|
||||
if index >= 0:
|
||||
self.chassis_type_combo.setCurrentIndex(index)
|
||||
self.rebuild_chassis_parameter_checks()
|
||||
output_path = read_nested(profile, ("orchestrator_session", "generated_session_config_file"), "")
|
||||
self.session_config_output_edit.setText(output_path if output_path else str(DEFAULT_SESSION_CONFIG))
|
||||
if not Path(self.session_config_output_edit.text()).is_absolute():
|
||||
self.session_config_output_edit.setText(str((profile_path.parent / self.session_config_output_edit.text()).resolve(strict=False)))
|
||||
vehicle_profile_file = read_nested(profile, ("vehicle_profile", "profile_file"), str(DEFAULT_VEHICLE_PROFILE))
|
||||
self.vehicle_profile_path_edit.setText(str(resolve_vehicle_profile_path(vehicle_profile_file)))
|
||||
if Path(self.vehicle_profile_path_edit.text()).exists():
|
||||
self.load_vehicle_profile_from_current_path()
|
||||
@@ -0,0 +1,134 @@
|
||||
"""操作台配置文件读写工具。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import re
|
||||
import shlex
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
try:
|
||||
import yaml
|
||||
except ImportError:
|
||||
yaml = None
|
||||
|
||||
try:
|
||||
from .constants import (
|
||||
DEFAULT_VEHICLE_PROFILE_DIR,
|
||||
PLACEHOLDER_MARKERS,
|
||||
REPO_ROOT,
|
||||
VEHICLE_PROFILE_SAVE_EXTENSION,
|
||||
)
|
||||
except ImportError:
|
||||
from constants import (
|
||||
DEFAULT_VEHICLE_PROFILE_DIR,
|
||||
PLACEHOLDER_MARKERS,
|
||||
REPO_ROOT,
|
||||
VEHICLE_PROFILE_SAVE_EXTENSION,
|
||||
)
|
||||
|
||||
|
||||
def has_placeholder(value: Any) -> bool:
|
||||
return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS)
|
||||
|
||||
def read_nested(data: dict[str, Any], path: tuple[str, ...], default: str = "") -> str:
|
||||
cursor: Any = data
|
||||
for key in path:
|
||||
if not isinstance(cursor, dict):
|
||||
return default
|
||||
cursor = cursor.get(key)
|
||||
if cursor in (None, "") or has_placeholder(cursor):
|
||||
return default
|
||||
return str(cursor)
|
||||
|
||||
def collect_placeholders(value: Any, prefix: str = "") -> list[str]:
|
||||
unresolved: list[str] = []
|
||||
if isinstance(value, dict):
|
||||
for key, child in value.items():
|
||||
child_prefix = f"{prefix}.{key}" if prefix else str(key)
|
||||
unresolved.extend(collect_placeholders(child, child_prefix))
|
||||
return unresolved
|
||||
if isinstance(value, list):
|
||||
for index, child in enumerate(value):
|
||||
child_prefix = f"{prefix}[{index}]"
|
||||
unresolved.extend(collect_placeholders(child, child_prefix))
|
||||
return unresolved
|
||||
if has_placeholder(value):
|
||||
unresolved.append(f"{prefix}: {value}")
|
||||
return unresolved
|
||||
|
||||
def load_yaml(path: Path) -> dict[str, Any]:
|
||||
if yaml is None:
|
||||
raise RuntimeError("缺少 PyYAML，无法加载现场配置文件。")
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
data = yaml.safe_load(stream) or {}
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"{path} 顶层必须是 YAML map。")
|
||||
return data
|
||||
|
||||
class NoAliasDumper(yaml.SafeDumper if yaml is not None else object):
|
||||
def ignore_aliases(self, data: Any) -> bool:
|
||||
return True
|
||||
|
||||
def dump_yaml(data: Any) -> str:
|
||||
if yaml is None:
|
||||
return json.dumps(data, ensure_ascii=False, indent=2)
|
||||
return yaml.dump(data, Dumper=NoAliasDumper, sort_keys=False, allow_unicode=True)
|
||||
|
||||
def resolve_profile_path(raw_path: str) -> Path:
|
||||
path = Path(raw_path).expanduser()
|
||||
if path.is_absolute():
|
||||
return path.resolve(strict=False)
|
||||
return (REPO_ROOT / path).resolve(strict=False)
|
||||
|
||||
def resolve_vehicle_profile_path(raw_path: str) -> Path:
|
||||
path = Path(raw_path).expanduser()
|
||||
if path.is_absolute():
|
||||
return path.resolve(strict=False)
|
||||
return (REPO_ROOT / path).resolve(strict=False)
|
||||
|
||||
def relative_to_repo(path: Path) -> str:
|
||||
try:
|
||||
return str(path.resolve(strict=False).relative_to(REPO_ROOT))
|
||||
except ValueError:
|
||||
return str(path.resolve(strict=False))
|
||||
|
||||
def vehicle_profile_filename(profile_name: str) -> str:
|
||||
name = re.sub(r"[^\w\-]+", "_", profile_name.strip() or "vehicle_profile")
|
||||
name = name.strip("_") or "vehicle_profile"
|
||||
return f"{name}{VEHICLE_PROFILE_SAVE_EXTENSION}"
|
||||
|
||||
def vehicle_profile_path_from_name(profile_name: str) -> Path:
|
||||
return DEFAULT_VEHICLE_PROFILE_DIR / vehicle_profile_filename(profile_name)
|
||||
|
||||
def shell_join(args: list[str]) -> str:
|
||||
return " ".join(shlex.quote(str(arg)) for arg in args if str(arg) != "")
|
||||
|
||||
def launch_arg(name: str, value: str) -> str:
|
||||
return f"{name}:={value}"
|
||||
|
||||
def is_positive_number(raw_value: str) -> bool:
|
||||
try:
|
||||
return float(raw_value) > 0.0
|
||||
except ValueError:
|
||||
return False
|
||||
|
||||
def float_or_none(raw_value: Any) -> float | None:
|
||||
try:
|
||||
return float(raw_value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
def bool_from_text(raw_value: Any) -> bool:
|
||||
return str(raw_value).strip().lower() in {"1", "true", "yes", "y", "on"}
|
||||
|
||||
def task_params_map(task: dict[str, Any]) -> dict[str, str]:
|
||||
params: dict[str, str] = {}
|
||||
for item in task.get("task_params", []):
|
||||
if not isinstance(item, dict):
|
||||
continue
|
||||
key = str(item.get("key", "")).strip()
|
||||
if key:
|
||||
params[key] = str(item.get("value", ""))
|
||||
return params
|
||||
@@ -0,0 +1,255 @@
|
||||
"""操作台 ROS 监听和实时定位状态。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import time
|
||||
|
||||
try:
|
||||
import rclpy
|
||||
except ImportError:
|
||||
rclpy = None
|
||||
|
||||
try:
|
||||
if rclpy is None:
|
||||
raise ImportError
|
||||
from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry
|
||||
except ImportError:
|
||||
ExternalLocalizationTelemetry = None
|
||||
|
||||
try:
|
||||
if rclpy is None:
|
||||
raise ImportError
|
||||
from calibration_workshop_orchestration_interfaces.msg import WorkshopEvent
|
||||
except ImportError:
|
||||
WorkshopEvent = None
|
||||
|
||||
try:
|
||||
from .constants import (
|
||||
LOCALIZATION_DISPLAY_ROWS,
|
||||
WORKSHOP_EVENT_REPORT_READY,
|
||||
WORKSHOP_EVENT_STAGE_COMPLETED,
|
||||
WORKSHOP_EVENT_STAGE_FAILED,
|
||||
WORKSHOP_EVENT_STAGE_STARTED,
|
||||
)
|
||||
from .profile_io import is_positive_number
|
||||
from .ui_helpers import table_item
|
||||
except ImportError:
|
||||
from constants import (
|
||||
LOCALIZATION_DISPLAY_ROWS,
|
||||
WORKSHOP_EVENT_REPORT_READY,
|
||||
WORKSHOP_EVENT_STAGE_COMPLETED,
|
||||
WORKSHOP_EVENT_STAGE_FAILED,
|
||||
WORKSHOP_EVENT_STAGE_STARTED,
|
||||
)
|
||||
from profile_io import is_positive_number
|
||||
from ui_helpers import table_item
|
||||
|
||||
|
||||
class RuntimeMonitorMixin:
|
||||
def update_localization_state(self, state_text: str, status: str = "neutral", detail: str = "") -> None:
|
||||
if not hasattr(self, "localization_state"):
|
||||
return
|
||||
zone = self.workcell_zone_edit.text().strip() or "未配置工位"
|
||||
topic = self.external_topic_edit.text().strip() or "未配置"
|
||||
source = self.reference_source_edit.text().strip() or "未配置"
|
||||
self.localization_state.setText(f"车间定位：{state_text} · {zone}")
|
||||
tooltip = f"定位话题：{topic}\n定位系统：{source}\n标定工位：{zone}"
|
||||
if detail:
|
||||
tooltip += f"\n最近结果：{detail}"
|
||||
self.localization_state.setToolTip(tooltip)
|
||||
style = {
|
||||
"ok": ("#166534", "#dcfce7", "#86efac"),
|
||||
"warn": ("#92400e", "#fef3c7", "#fbbf24"),
|
||||
"fail": ("#991b1b", "#fee2e2", "#fca5a5"),
|
||||
"running": ("#1d4ed8", "#dbeafe", "#93c5fd"),
|
||||
"neutral": ("#374151", "#f3f4f6", "#d1d5db"),
|
||||
}.get(status, ("#374151", "#f3f4f6", "#d1d5db"))
|
||||
self.localization_state.setStyleSheet(
|
||||
"QLabel {"
|
||||
f"color: {style[0]}; background: {style[1]}; border: 1px solid {style[2]}; "
|
||||
"border-radius: 4px; padding: 4px 8px;"
|
||||
"}"
|
||||
)
|
||||
|
||||
def update_workshop_dimension_state(self) -> None:
|
||||
if not hasattr(self, "workshop_dimension_state"):
|
||||
return
|
||||
geometry = self.workshop_geometry_payload()
|
||||
geometry_ok = all(is_positive_number(geometry[key]) for key in ("length_m", "width_m", "height_m"))
|
||||
if geometry_ok:
|
||||
text = f"车间尺寸：{geometry['length_m']} x {geometry['width_m']} x {geometry['height_m']} m"
|
||||
color, background, border = "#166534", "#dcfce7", "#86efac"
|
||||
else:
|
||||
text = "车间尺寸：配置缺失"
|
||||
color, background, border = "#991b1b", "#fee2e2", "#fca5a5"
|
||||
self.workshop_dimension_state.setText(text)
|
||||
self.workshop_dimension_state.setToolTip(
|
||||
"车间尺寸来自现场配置文件 workshop_geometry，操作员界面不允许手动修改。"
|
||||
)
|
||||
if hasattr(self, "localization_3d_view"):
|
||||
self.localization_3d_view.set_workshop_geometry(geometry)
|
||||
self.workshop_dimension_state.setStyleSheet(
|
||||
"QLabel {"
|
||||
f"color: {color}; background: {background}; border: 1px solid {border}; "
|
||||
"border-radius: 4px; padding: 4px 8px;"
|
||||
"}"
|
||||
)
|
||||
|
||||
def set_localization_display(self, values: dict[str, str], status: str | None = None) -> None:
|
||||
if not hasattr(self, "localization_table"):
|
||||
return
|
||||
for row, name in enumerate(LOCALIZATION_DISPLAY_ROWS):
|
||||
self.localization_table.setItem(row, 0, table_item(name))
|
||||
self.localization_table.setItem(row, 1, table_item(values.get(name, "-"), status if name == "状态" else None))
|
||||
|
||||
def set_localization_message(self, summary: str, status_text: str = "-", status: str = "warn") -> None:
|
||||
if hasattr(self, "localization_summary_label"):
|
||||
self.localization_summary_label.setText(summary)
|
||||
if hasattr(self, "localization_3d_view"):
|
||||
self.localization_3d_view.set_status(summary)
|
||||
self.set_localization_display({
|
||||
"状态": status_text,
|
||||
"定位系统": self.reference_source_edit.text().strip() or "-",
|
||||
}, status)
|
||||
|
||||
def ensure_ros_monitor_node(self):
|
||||
if rclpy is None:
|
||||
return None
|
||||
if not rclpy.ok():
|
||||
rclpy.init(args=None)
|
||||
self.localization_rclpy_initialized = True
|
||||
if self.localization_node is None:
|
||||
self.localization_node = rclpy.create_node("operator_ui_runtime_monitor")
|
||||
if not self.localization_spin_timer.isActive():
|
||||
self.localization_spin_timer.start(50)
|
||||
return self.localization_node
|
||||
|
||||
def restart_localization_monitor(self) -> None:
|
||||
topic = self.external_topic_edit.text().strip()
|
||||
source = self.reference_source_edit.text().strip()
|
||||
if not topic:
|
||||
self.set_localization_message("现场配置文件没有定位话题。", "配置缺失", "fail")
|
||||
self.update_localization_state("配置缺失", "fail")
|
||||
return
|
||||
self.localization_last_sample_wall_time = 0.0
|
||||
if rclpy is None or ExternalLocalizationTelemetry is None:
|
||||
self.set_localization_message("未连接 ROS 环境，无法订阅车间定位数据。", "未连接", "warn")
|
||||
self.update_localization_state("未连接", "warn")
|
||||
return
|
||||
try:
|
||||
node = self.ensure_ros_monitor_node()
|
||||
if node is None:
|
||||
return
|
||||
if self.localization_subscription is not None:
|
||||
node.destroy_subscription(self.localization_subscription)
|
||||
self.localization_subscription = None
|
||||
self.localization_subscription = node.create_subscription(
|
||||
ExternalLocalizationTelemetry,
|
||||
topic,
|
||||
self.handle_localization_message,
|
||||
10,
|
||||
)
|
||||
self.set_localization_message(f"正在监听车间定位数据：{source or topic}", "等待数据", "warn")
|
||||
self.update_localization_state("等待数据", "warn")
|
||||
except Exception as exc:
|
||||
self.set_localization_message(f"车间定位订阅失败：{exc}", "订阅失败", "fail")
|
||||
self.update_localization_state("订阅失败", "fail", str(exc))
|
||||
|
||||
def restart_workshop_event_monitor(self) -> None:
|
||||
if rclpy is None or WorkshopEvent is None:
|
||||
return
|
||||
try:
|
||||
node = self.ensure_ros_monitor_node()
|
||||
if node is None:
|
||||
return
|
||||
if self.workshop_event_subscription is not None:
|
||||
node.destroy_subscription(self.workshop_event_subscription)
|
||||
self.workshop_event_subscription = None
|
||||
self.workshop_event_subscription = node.create_subscription(
|
||||
WorkshopEvent,
|
||||
"/workshop_v2/events",
|
||||
self.handle_workshop_event,
|
||||
50,
|
||||
)
|
||||
except Exception as exc:
|
||||
self.append_log(f"[界面] 订阅总控事件失败: {exc}")
|
||||
|
||||
def poll_localization_messages(self) -> None:
|
||||
if rclpy is None or self.localization_node is None:
|
||||
return
|
||||
try:
|
||||
rclpy.spin_once(self.localization_node, timeout_sec=0.0)
|
||||
except Exception as exc:
|
||||
self.localization_spin_timer.stop()
|
||||
self.set_localization_message(f"车间定位读取失败：{exc}", "读取失败", "fail")
|
||||
self.update_localization_state("读取失败", "fail", str(exc))
|
||||
return
|
||||
if self.localization_last_sample_wall_time > 0.0:
|
||||
age_sec = time.time() - self.localization_last_sample_wall_time
|
||||
if age_sec > 2.0:
|
||||
self.update_localization_state("数据超时", "warn", f"{age_sec:.1f}s 未更新")
|
||||
|
||||
def handle_localization_message(self, msg) -> None:
|
||||
self.localization_last_sample_wall_time = time.time()
|
||||
pose = msg.workshop_pose
|
||||
yaw_deg = math.degrees(float(pose.yaw_rad))
|
||||
yaw_stddev_deg = math.degrees(float(msg.yaw_stddev_rad))
|
||||
valid = bool(msg.pose_valid)
|
||||
status_text = "正常" if valid else "位姿无效"
|
||||
status = "ok" if valid else "fail"
|
||||
source = msg.reference_source_name or self.reference_source_edit.text().strip()
|
||||
values = {
|
||||
"状态": status_text,
|
||||
"更新时间": time.strftime("%H:%M:%S"),
|
||||
"X(m)": f"{float(pose.x_m):.3f}",
|
||||
"Y(m)": f"{float(pose.y_m):.3f}",
|
||||
"Z(m)": f"{float(pose.z_m):.3f}",
|
||||
"Yaw(deg)": f"{yaw_deg:.2f}",
|
||||
"质量分数": f"{float(msg.quality_score):.3f}",
|
||||
"位置标准差(m)": f"{float(msg.position_stddev_m):.4f}",
|
||||
"航向标准差(deg)": f"{yaw_stddev_deg:.3f}",
|
||||
"跟踪丢失率": f"{float(msg.tracking_loss_ratio):.3f}",
|
||||
"时间同步偏差(ms)": f"{float(msg.time_sync_offset_ms):.2f}",
|
||||
"目标数量": str(int(msg.observed_target_count)),
|
||||
"定位系统": source or "-",
|
||||
"任务 ID": msg.active_job_id or "-",
|
||||
}
|
||||
self.set_localization_display(values, status)
|
||||
if hasattr(self, "localization_summary_label"):
|
||||
self.localization_summary_label.setText(
|
||||
f"x={values['X(m)']} m, y={values['Y(m)']} m, yaw={values['Yaw(deg)']} deg, 质量={values['质量分数']}"
|
||||
)
|
||||
if hasattr(self, "localization_3d_view"):
|
||||
self.localization_3d_view.set_pose(
|
||||
float(pose.x_m),
|
||||
float(pose.y_m),
|
||||
float(pose.z_m),
|
||||
float(pose.yaw_rad),
|
||||
valid,
|
||||
float(msg.quality_score),
|
||||
)
|
||||
self.update_localization_state("实时正常" if valid else "位姿无效", status)
|
||||
|
||||
def handle_workshop_event(self, msg) -> None:
|
||||
event_type = int(msg.event_type.value)
|
||||
stage_id = str(msg.stage_id)
|
||||
if event_type == WORKSHOP_EVENT_STAGE_STARTED:
|
||||
self.trajectory_runtime_active = True
|
||||
if not self.show_stage_trajectory(stage_id, "正在执行轨迹"):
|
||||
self.show_next_trajectory_after_stage(stage_id)
|
||||
return
|
||||
if event_type == WORKSHOP_EVENT_STAGE_COMPLETED:
|
||||
self.trajectory_runtime_active = True
|
||||
self.trajectory_last_completed_stage_id = stage_id
|
||||
self.show_next_trajectory_after_stage(stage_id)
|
||||
return
|
||||
if event_type == WORKSHOP_EVENT_STAGE_FAILED:
|
||||
self.trajectory_runtime_active = True
|
||||
if not self.show_stage_trajectory(stage_id, "失败阶段轨迹"):
|
||||
self.show_next_trajectory_after_stage(stage_id)
|
||||
return
|
||||
if event_type == WORKSHOP_EVENT_REPORT_READY:
|
||||
self.trajectory_runtime_active = False
|
||||
self.trajectory_last_completed_stage_id = ""
|
||||
self.show_all_planned_trajectories()
|
||||
@@ -0,0 +1,516 @@
|
||||
"""操作台本轮任务和会话文件生成。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from PySide6.QtWidgets import QMessageBox
|
||||
|
||||
try:
|
||||
from .constants import (
|
||||
CHASSIS_PARAMETER_OPTIONS,
|
||||
CONTROL_PARAMETER_OPTIONS,
|
||||
DEFAULT_VEHICLE_PROFILE,
|
||||
POLICY_DISPLAY_NAMES,
|
||||
REPO_ROOT,
|
||||
SENSOR_TASK_OPTIONS,
|
||||
STAGE_CHASSIS,
|
||||
STAGE_CONTROL,
|
||||
STAGE_EXTERNAL,
|
||||
STAGE_HAND_EYE,
|
||||
STAGE_SENSOR_EXTRINSIC,
|
||||
STAGE_SENSOR_INTRINSIC,
|
||||
)
|
||||
from .profile_io import (
|
||||
bool_from_text,
|
||||
collect_placeholders,
|
||||
dump_yaml,
|
||||
float_or_none,
|
||||
is_positive_number,
|
||||
load_yaml,
|
||||
read_nested,
|
||||
resolve_profile_path,
|
||||
resolve_vehicle_profile_path,
|
||||
task_params_map,
|
||||
)
|
||||
from .ui_helpers import table_item, task_item_display_name, task_stage_display_name, task_stage_id
|
||||
except ImportError:
|
||||
from constants import (
|
||||
CHASSIS_PARAMETER_OPTIONS,
|
||||
CONTROL_PARAMETER_OPTIONS,
|
||||
DEFAULT_VEHICLE_PROFILE,
|
||||
POLICY_DISPLAY_NAMES,
|
||||
REPO_ROOT,
|
||||
SENSOR_TASK_OPTIONS,
|
||||
STAGE_CHASSIS,
|
||||
STAGE_CONTROL,
|
||||
STAGE_EXTERNAL,
|
||||
STAGE_HAND_EYE,
|
||||
STAGE_SENSOR_EXTRINSIC,
|
||||
STAGE_SENSOR_INTRINSIC,
|
||||
)
|
||||
from profile_io import (
|
||||
bool_from_text,
|
||||
collect_placeholders,
|
||||
dump_yaml,
|
||||
float_or_none,
|
||||
is_positive_number,
|
||||
load_yaml,
|
||||
read_nested,
|
||||
resolve_profile_path,
|
||||
resolve_vehicle_profile_path,
|
||||
task_params_map,
|
||||
)
|
||||
from ui_helpers import table_item, task_item_display_name, task_stage_display_name, task_stage_id
|
||||
|
||||
|
||||
class SessionBuilderMixin:
|
||||
def selected_tasks(self) -> list[str]:
|
||||
tasks: list[str] = []
|
||||
if self.external_check.isChecked():
|
||||
tasks.append("external")
|
||||
if self.selected_chassis_tasks():
|
||||
tasks.append("chassis")
|
||||
if self.selected_control_tasks():
|
||||
tasks.append("control")
|
||||
sensor_stages = {task["stage_type"] for task in self.selected_sensor_tasks()}
|
||||
if STAGE_SENSOR_INTRINSIC in sensor_stages:
|
||||
tasks.append("sensor_intrinsic")
|
||||
if STAGE_SENSOR_EXTRINSIC in sensor_stages:
|
||||
tasks.append("sensor_extrinsic")
|
||||
if STAGE_HAND_EYE in sensor_stages:
|
||||
tasks.append("hand_eye")
|
||||
return tasks
|
||||
|
||||
def selected_tasks_csv(self) -> str:
|
||||
tasks = self.selected_tasks()
|
||||
return ",".join(tasks) if tasks else "external"
|
||||
|
||||
def refresh_all(self) -> None:
|
||||
self.refresh_command_preview()
|
||||
self.refresh_task_preview_from_config()
|
||||
|
||||
def make_task(
|
||||
self,
|
||||
stage_type: str,
|
||||
task_code: str,
|
||||
target_id: str,
|
||||
params: dict[str, Any],
|
||||
reason: str,
|
||||
) -> dict[str, Any]:
|
||||
return {
|
||||
"stage_type": stage_type,
|
||||
"enabled": True,
|
||||
"require_manual_approval": False,
|
||||
"execution_policy": "REQUIRED",
|
||||
"reason": reason,
|
||||
"task_code": task_code,
|
||||
"target_id": target_id,
|
||||
"task_params": [
|
||||
{"key": str(key), "value": str(value)}
|
||||
for key, value in params.items()
|
||||
],
|
||||
}
|
||||
|
||||
def workshop_geometry_payload(self) -> dict[str, str]:
|
||||
return {
|
||||
"length_m": read_nested(self.site_profile, ("workshop_geometry", "length_m"), ""),
|
||||
"width_m": read_nested(self.site_profile, ("workshop_geometry", "width_m"), ""),
|
||||
"height_m": read_nested(self.site_profile, ("workshop_geometry", "height_m"), ""),
|
||||
}
|
||||
|
||||
def refresh_planned_trajectory(self) -> None:
|
||||
self.trajectory_task_records = self.planned_trajectory_task_records()
|
||||
if self.trajectory_runtime_active:
|
||||
self.show_next_trajectory_after_stage(self.trajectory_last_completed_stage_id)
|
||||
else:
|
||||
self.show_all_planned_trajectories()
|
||||
|
||||
def planned_trajectory_segments(self) -> list[list[tuple[float, float, float]]]:
|
||||
return [
|
||||
segment
|
||||
for record in self.trajectory_task_records
|
||||
for segment in record["segments"]
|
||||
]
|
||||
|
||||
def planned_trajectory_task_records(self) -> list[dict[str, Any]]:
|
||||
tasks = self.build_requested_tasks()
|
||||
self.trajectory_stage_order = {
|
||||
task_stage_id(task): order
|
||||
for order, task in enumerate(tasks)
|
||||
}
|
||||
records: list[dict[str, Any]] = []
|
||||
for order, task in enumerate(tasks):
|
||||
params = task_params_map(task)
|
||||
segments = self.trajectory_segments_from_task_params(params)
|
||||
if not segments:
|
||||
continue
|
||||
records.append({
|
||||
"order": order,
|
||||
"stage_id": task_stage_id(task),
|
||||
"display_name": f"{task_stage_display_name(task)}：{task_item_display_name(task)}",
|
||||
"segments": segments,
|
||||
})
|
||||
return records
|
||||
|
||||
def trajectory_segments_from_task_params(self, params: dict[str, str]) -> list[list[tuple[float, float, float]]]:
|
||||
segments: list[list[tuple[float, float, float]]] = []
|
||||
segment = self.trajectory_points_from_explicit_params(params)
|
||||
if segment:
|
||||
segments.append(segment)
|
||||
return segments
|
||||
segment = self.trajectory_points_from_motion_primitive(params)
|
||||
if segment:
|
||||
segments.append(segment)
|
||||
return segments
|
||||
|
||||
def show_all_planned_trajectories(self) -> None:
|
||||
if not hasattr(self, "localization_3d_view"):
|
||||
return
|
||||
self.localization_3d_view.set_trajectory(self.planned_trajectory_segments(), "本轮计划轨迹")
|
||||
|
||||
def show_stage_trajectory(self, stage_id: str, label_prefix: str) -> bool:
|
||||
if not hasattr(self, "localization_3d_view"):
|
||||
return False
|
||||
for record in self.trajectory_task_records:
|
||||
if record["stage_id"] == stage_id:
|
||||
self.localization_3d_view.set_trajectory(
|
||||
record["segments"],
|
||||
f"{label_prefix}：{record['display_name']}",
|
||||
)
|
||||
return True
|
||||
return False
|
||||
|
||||
def show_next_trajectory_after_stage(self, stage_id: str) -> bool:
|
||||
if not hasattr(self, "localization_3d_view"):
|
||||
return False
|
||||
completed_order = self.trajectory_stage_order.get(stage_id, -1) if stage_id else -1
|
||||
for record in self.trajectory_task_records:
|
||||
if int(record["order"]) > completed_order:
|
||||
self.localization_3d_view.set_trajectory(
|
||||
record["segments"],
|
||||
f"下一项轨迹：{record['display_name']}",
|
||||
)
|
||||
return True
|
||||
self.localization_3d_view.set_trajectory([], "后续没有计划轨迹")
|
||||
return False
|
||||
|
||||
def trajectory_points_from_explicit_params(self, params: dict[str, str]) -> list[tuple[float, float, float]]:
|
||||
indexed_points: dict[int, tuple[float, float]] = {}
|
||||
indexes = sorted({
|
||||
int(match.group(1))
|
||||
for key in params
|
||||
if (match := re.fullmatch(r"traj_pt_(\d+)_x_m", key))
|
||||
})
|
||||
for index in indexes:
|
||||
x = float_or_none(params.get(f"traj_pt_{index}_x_m"))
|
||||
y = float_or_none(params.get(f"traj_pt_{index}_y_m"))
|
||||
if x is not None and y is not None:
|
||||
indexed_points[index] = (x, y)
|
||||
return [(x, y, 0.04) for _, (x, y) in sorted(indexed_points.items())]
|
||||
|
||||
def trajectory_points_from_motion_primitive(self, params: dict[str, str]) -> list[tuple[float, float, float]]:
|
||||
primitive_type = params.get("primitive_type", "")
|
||||
if primitive_type == "straight_line":
|
||||
distance = float_or_none(params.get("straight_line.target_distance_m"))
|
||||
if distance is None:
|
||||
return []
|
||||
direction = -1.0 if bool_from_text(params.get("straight_line.reverse")) else 1.0
|
||||
return [(0.0, 0.0, 0.04), (direction * abs(distance), 0.0, 0.04)]
|
||||
if primitive_type == "arc":
|
||||
radius = float_or_none(params.get("arc.radius_m"))
|
||||
sweep_angle_deg = float_or_none(params.get("arc.sweep_angle_deg"))
|
||||
if radius is None or sweep_angle_deg is None or radius <= 0.0:
|
||||
return []
|
||||
clockwise = bool_from_text(params.get("arc.clockwise"))
|
||||
turn_sign = -1.0 if clockwise else 1.0
|
||||
sweep_rad = math.radians(abs(sweep_angle_deg))
|
||||
sample_count = max(8, min(64, int(abs(sweep_angle_deg) / 5.0) + 1))
|
||||
points: list[tuple[float, float, float]] = []
|
||||
for index in range(sample_count):
|
||||
ratio = index / float(sample_count - 1)
|
||||
theta = sweep_rad * ratio
|
||||
x = radius * math.sin(theta)
|
||||
y = turn_sign * radius * (1.0 - math.cos(theta))
|
||||
points.append((x, y, 0.04))
|
||||
return points
|
||||
if primitive_type == "in_place_rotation":
|
||||
radius = 0.25
|
||||
points = []
|
||||
for index in range(25):
|
||||
theta = math.tau * index / 24.0
|
||||
points.append((radius * math.cos(theta), radius * math.sin(theta), 0.04))
|
||||
return points
|
||||
return []
|
||||
|
||||
def external_task(self) -> dict[str, Any]:
|
||||
return self.make_task(
|
||||
STAGE_EXTERNAL,
|
||||
"external",
|
||||
self.reference_target_edit.text().strip() or "site_reference_target",
|
||||
{
|
||||
"external.static_sample_count": "1",
|
||||
"external.dynamic_sample_count": "1",
|
||||
"external.require_short_motion_segment": "false",
|
||||
"external.max_position_stddev_m": "0.05",
|
||||
"external.max_yaw_stddev_rad": "0.05",
|
||||
"external.max_tracking_loss_ratio": "0.05",
|
||||
"external.max_time_sync_offset_ms": "50.0",
|
||||
"external.timeout_sec": "5.0",
|
||||
},
|
||||
"现场界面选择的车间定位可用性检查",
|
||||
)
|
||||
|
||||
def selected_chassis_tasks(self) -> list[dict[str, Any]]:
|
||||
if not hasattr(self, "chassis_type_combo"):
|
||||
return []
|
||||
chassis_type = str(self.chassis_type_combo.currentData() or "ackermann")
|
||||
tasks: list[dict[str, Any]] = []
|
||||
for option in CHASSIS_PARAMETER_OPTIONS[chassis_type]:
|
||||
check = self.chassis_param_checks.get(str(option["code"]))
|
||||
if check is None or not check.isChecked():
|
||||
continue
|
||||
params = {
|
||||
"chassis_type": chassis_type,
|
||||
"calibration_parameter": option["code"],
|
||||
**option["params"],
|
||||
}
|
||||
params.setdefault("brake_when_finished", "true")
|
||||
params.setdefault("timeout_sec", "20.0")
|
||||
tasks.append(self.make_task(
|
||||
STAGE_CHASSIS,
|
||||
str(option["code"]),
|
||||
str(option["target"]),
|
||||
params,
|
||||
"现场 UI 选择的底盘标定参数",
|
||||
))
|
||||
return tasks
|
||||
|
||||
def selected_control_tasks(self) -> list[dict[str, Any]]:
|
||||
if not hasattr(self, "control_param_checks"):
|
||||
return []
|
||||
tasks: list[dict[str, Any]] = []
|
||||
for option in CONTROL_PARAMETER_OPTIONS:
|
||||
check = self.control_param_checks.get(str(option["code"]))
|
||||
if check is None or not check.isChecked():
|
||||
continue
|
||||
params = {
|
||||
"control.axis": option["axis"],
|
||||
"control.algorithm": option["algorithm"],
|
||||
"calibration_parameter": option["code"],
|
||||
**option["params"],
|
||||
}
|
||||
if params.get("control.task_type") == "trajectory_tracking":
|
||||
params.setdefault(
|
||||
"trajectory_tracking.required_external_pose_source_id",
|
||||
self.reference_source_edit.text().strip(),
|
||||
)
|
||||
tasks.append(self.make_task(
|
||||
STAGE_CONTROL,
|
||||
str(option["code"]),
|
||||
str(option["target"]),
|
||||
params,
|
||||
"现场 UI 选择的运控参数标定",
|
||||
))
|
||||
return tasks
|
||||
|
||||
def selected_sensor_tasks(self) -> list[dict[str, Any]]:
|
||||
if not hasattr(self, "sensor_task_checks"):
|
||||
return []
|
||||
tasks: list[dict[str, Any]] = []
|
||||
for sensor_key, sensor_label, subtype, label, stage_type in SENSOR_TASK_OPTIONS:
|
||||
check = self.sensor_task_checks.get((sensor_key, subtype))
|
||||
if check is None or not check.isChecked():
|
||||
continue
|
||||
sensor_edit = self.sensor_id_edits.get(sensor_key)
|
||||
sensor_id = sensor_edit.text().strip() if sensor_edit is not None else ""
|
||||
if not sensor_id:
|
||||
continue
|
||||
params: dict[str, Any] = {
|
||||
"sensor.sensor_id": sensor_id,
|
||||
"sensor.task_subtype": subtype,
|
||||
"calibration_parameter": subtype,
|
||||
}
|
||||
if stage_type == STAGE_SENSOR_INTRINSIC and subtype.endswith("camera_intrinsic"):
|
||||
params.update({
|
||||
"camera_intrinsic.required_image_count": "1",
|
||||
"camera_intrinsic.target_board_id": self.reference_target_edit.text().strip() or "site_reference_target",
|
||||
"camera_intrinsic.timeout_sec": "5.0",
|
||||
})
|
||||
elif stage_type == STAGE_SENSOR_INTRINSIC and subtype == "imu_intrinsic":
|
||||
params.update({
|
||||
"imu_intrinsic.required_static_segment_count": "1",
|
||||
"imu_intrinsic.required_motion_segment_count": "1",
|
||||
"imu_intrinsic.timeout_sec": "5.0",
|
||||
})
|
||||
elif stage_type == STAGE_SENSOR_EXTRINSIC:
|
||||
params.update({
|
||||
"sensor_extrinsic.base_frame_id": read_nested(self.site_profile, ("frames", "base_link"), "base_link"),
|
||||
"sensor_extrinsic.required_sample_count": "1",
|
||||
"sensor_extrinsic.timeout_sec": "5.0",
|
||||
})
|
||||
elif stage_type == STAGE_HAND_EYE:
|
||||
params.update({
|
||||
"hand_eye.arm_id": "demo_arm",
|
||||
"hand_eye.required_pose_count": "1",
|
||||
"hand_eye.timeout_sec": "5.0",
|
||||
})
|
||||
tasks.append(self.make_task(
|
||||
stage_type,
|
||||
f"sensor.{sensor_key}.{subtype}",
|
||||
sensor_id,
|
||||
params,
|
||||
f"现场 UI 选择的传感器标定：{label}",
|
||||
))
|
||||
return tasks
|
||||
|
||||
def build_requested_tasks(self) -> list[dict[str, Any]]:
|
||||
tasks: list[dict[str, Any]] = []
|
||||
if self.external_check.isChecked():
|
||||
tasks.append(self.external_task())
|
||||
tasks.extend(self.selected_chassis_tasks())
|
||||
tasks.extend(self.selected_control_tasks())
|
||||
tasks.extend(self.selected_sensor_tasks())
|
||||
return tasks
|
||||
|
||||
def build_session_payload(self) -> dict[str, Any]:
|
||||
requested_tasks = self.build_requested_tasks()
|
||||
workshop_geometry = self.workshop_geometry_payload()
|
||||
vehicle_profile_file = self.vehicle_profile_path_edit.text().strip()
|
||||
vehicle_profile = self.current_vehicle_profile_payload()
|
||||
vehicle_profile["profile_file"] = vehicle_profile_file
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"source_site_profile": self.profile_path_edit.text().strip(),
|
||||
"vehicle_profile_file": vehicle_profile_file,
|
||||
"vehicle_profile": vehicle_profile,
|
||||
"generated_by": "operator_ui",
|
||||
"workshop_geometry": workshop_geometry,
|
||||
"session_config": {
|
||||
"auto_commit_parameters": False,
|
||||
"require_manual_approval_before_commit": False,
|
||||
"run_validation_after_each_stage": False,
|
||||
"stop_on_first_failure": True,
|
||||
"allow_optional_stage_skip": False,
|
||||
"enable_auto_rollback_on_validation_failure": False,
|
||||
"allow_rebuild_execution_plan": False,
|
||||
"localization_source_id": self.reference_source_edit.text().strip(),
|
||||
"workcell_zone_id": self.workcell_zone_edit.text().strip(),
|
||||
"reference_target_id": self.reference_target_edit.text().strip() or "site_reference_target",
|
||||
"workshop_geometry": workshop_geometry,
|
||||
"vehicle_profile_file": vehicle_profile_file,
|
||||
"vehicle_profile_name": vehicle_profile.get("profile_name", ""),
|
||||
"requested_tasks": requested_tasks,
|
||||
},
|
||||
"requested_tasks": requested_tasks,
|
||||
}
|
||||
|
||||
def refresh_deployment_checks(self) -> None:
|
||||
rows: list[tuple[str, str, str, str]] = []
|
||||
profile_path = resolve_profile_path(self.profile_path_edit.text().strip())
|
||||
unresolved = collect_placeholders(self.site_profile)
|
||||
rows.append(("固定车间配置", "正常" if profile_path.exists() else "失败", "已按部署配置加载" if profile_path.exists() else "部署配置文件不存在", "ok" if profile_path.exists() else "fail"))
|
||||
rows.append(("未替换占位项", "正常" if not unresolved else "警告", f"{len(unresolved)} 个", "ok" if not unresolved else "warn"))
|
||||
rows.append(("车辆 ID", "正常" if self.vehicle_id_edit.text().strip() else "失败", self.vehicle_id_edit.text().strip(), "ok" if self.vehicle_id_edit.text().strip() else "fail"))
|
||||
vehicle_profile_path = resolve_vehicle_profile_path(self.vehicle_profile_path_edit.text().strip() or str(DEFAULT_VEHICLE_PROFILE))
|
||||
rows.append(("车辆画像", "正常" if vehicle_profile_path.exists() else "失败", f"{self.vehicle_profile_name_edit.text().strip() or '-'} · {vehicle_profile_path}", "ok" if vehicle_profile_path.exists() else "fail"))
|
||||
vehicle_geometry_values = [
|
||||
self.vehicle_length_edit.text().strip(),
|
||||
self.vehicle_width_edit.text().strip(),
|
||||
self.vehicle_height_edit.text().strip(),
|
||||
]
|
||||
vehicle_geometry_ok = all(is_positive_number(value) for value in vehicle_geometry_values)
|
||||
rows.append(("车辆外形尺寸", "正常" if vehicle_geometry_ok else "失败", f"长 {vehicle_geometry_values[0] or '-'} m，宽 {vehicle_geometry_values[1] or '-'} m，高 {vehicle_geometry_values[2] or '-'} m", "ok" if vehicle_geometry_ok else "fail"))
|
||||
chassis_geometry_values = [
|
||||
self.vehicle_wheel_base_edit.text().strip(),
|
||||
self.vehicle_track_width_edit.text().strip(),
|
||||
self.vehicle_wheel_radius_edit.text().strip(),
|
||||
]
|
||||
chassis_geometry_ok = all(is_positive_number(value) for value in chassis_geometry_values)
|
||||
rows.append(("底盘几何画像", "正常" if chassis_geometry_ok else "失败", f"轴距 {chassis_geometry_values[0] or '-'} m，轮距 {chassis_geometry_values[1] or '-'} m，轮半径 {chassis_geometry_values[2] or '-'} m", "ok" if chassis_geometry_ok else "fail"))
|
||||
geometry = self.workshop_geometry_payload()
|
||||
geometry_ok = all(is_positive_number(geometry[key]) for key in ("length_m", "width_m", "height_m"))
|
||||
geometry_detail = f"长 {geometry['length_m'] or '-'} m，宽 {geometry['width_m'] or '-'} m，高 {geometry['height_m'] or '-'} m"
|
||||
rows.append(("车间尺寸", "正常" if geometry_ok else "失败", geometry_detail, "ok" if geometry_ok else "fail"))
|
||||
rows.append(("车上 Windows 程序", "正常" if self.vehicle_host_edit.text().strip() else "失败", f"{self.vehicle_host_edit.text()}:{self.vehicle_port_edit.text()}", "ok" if self.vehicle_host_edit.text().strip() else "fail"))
|
||||
data_input = self.data_input_edit.text().strip()
|
||||
rows.append(("算法数据参数文件", "正常" if data_input and Path(data_input).exists() else "警告", data_input or "未填写", "ok" if data_input and Path(data_input).exists() else "warn"))
|
||||
dataset_index = self.dataset_index_edit.text().strip()
|
||||
rows.append(("采集数据索引文件", "正常" if dataset_index and Path(dataset_index).exists() else "警告", dataset_index or "未填写", "ok" if dataset_index and Path(dataset_index).exists() else "warn"))
|
||||
rows.append(("任务选择", "正常" if self.selected_tasks() else "失败", self.selected_tasks_csv(), "ok" if self.selected_tasks() else "fail"))
|
||||
|
||||
self.check_table.setRowCount(len(rows))
|
||||
for row, (name, status, detail, color) in enumerate(rows):
|
||||
self.check_table.setItem(row, 0, table_item(name))
|
||||
self.check_table.setItem(row, 1, table_item(status, color))
|
||||
self.check_table.setItem(row, 2, table_item(detail))
|
||||
|
||||
unresolved_signature = "\n".join(unresolved)
|
||||
if unresolved and unresolved_signature != self.last_unresolved_signature:
|
||||
self.append_log("[界面] 现场配置文件仍有占位项，真实部署前需要替换。前几项：")
|
||||
for item in unresolved[:8]:
|
||||
self.append_log(f" - {item}")
|
||||
self.last_unresolved_signature = unresolved_signature
|
||||
|
||||
def build_session_config(self) -> bool:
|
||||
output_path = Path(self.session_config_output_edit.text().strip()).expanduser()
|
||||
if not output_path.is_absolute():
|
||||
output_path = (REPO_ROOT / output_path).resolve(strict=False)
|
||||
output_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
payload = self.build_session_payload()
|
||||
if not payload["requested_tasks"]:
|
||||
QMessageBox.warning(self, "任务为空", "请至少选择一个标定任务。")
|
||||
return False
|
||||
geometry = payload.get("workshop_geometry", {})
|
||||
if not all(is_positive_number(str(geometry.get(key, ""))) for key in ("length_m", "width_m", "height_m")):
|
||||
QMessageBox.warning(self, "车间尺寸缺失", "现场配置文件缺少 workshop_geometry.length_m / width_m / height_m。")
|
||||
return False
|
||||
vehicle_profile_file = payload.get("vehicle_profile_file", "")
|
||||
if vehicle_profile_file and not resolve_vehicle_profile_path(str(vehicle_profile_file)).exists():
|
||||
QMessageBox.warning(self, "车辆画像缺失", "当前选择的车辆画像文件不存在。")
|
||||
return False
|
||||
vehicle_profile = payload.get("vehicle_profile", {})
|
||||
vehicle_geometry = vehicle_profile.get("geometry", {}) if isinstance(vehicle_profile, dict) else {}
|
||||
if not all(is_positive_number(str(vehicle_geometry.get(key, ""))) for key in ("length_m", "width_m", "height_m")):
|
||||
QMessageBox.warning(self, "车辆尺寸缺失", "车辆画像缺少 geometry.length_m / width_m / height_m。")
|
||||
return False
|
||||
chassis_geometry = vehicle_profile.get("chassis", {}) if isinstance(vehicle_profile, dict) else {}
|
||||
if not all(is_positive_number(str(chassis_geometry.get(key, ""))) for key in ("wheel_base_m", "track_width_m", "wheel_radius_m")):
|
||||
QMessageBox.warning(self, "底盘画像缺失", "车辆画像缺少 chassis.wheel_base_m / track_width_m / wheel_radius_m。")
|
||||
return False
|
||||
output_path.write_text(dump_yaml(payload), encoding="utf-8")
|
||||
|
||||
self.session_config_output_edit.setText(str(output_path))
|
||||
try:
|
||||
self.generated_session_config = load_yaml(output_path)
|
||||
except Exception as exc:
|
||||
QMessageBox.critical(self, "读取失败", f"本轮任务文件已生成，但读取失败：{exc}")
|
||||
return False
|
||||
self.append_log(f"[本轮任务文件] 已生成：{output_path}")
|
||||
self.refresh_task_preview_from_config()
|
||||
self.refresh_command_preview()
|
||||
return True
|
||||
|
||||
def default_sensor_id(self) -> str:
|
||||
first = self.sensor_registry_edit.text().split(",")[0].strip()
|
||||
return first or "demo_front_camera"
|
||||
|
||||
def refresh_task_preview_from_config(self) -> None:
|
||||
tasks: list[dict[str, Any]] = []
|
||||
preview_config = self.build_session_payload()
|
||||
if preview_config:
|
||||
session_config = preview_config.get("session_config", {})
|
||||
tasks = list(session_config.get("requested_tasks", preview_config.get("requested_tasks", [])))
|
||||
self.task_table.setRowCount(len(tasks))
|
||||
for index, task in enumerate(tasks):
|
||||
execution_policy = str(task.get("execution_policy", ""))
|
||||
self.task_table.setItem(index, 0, table_item(str(index + 1)))
|
||||
self.task_table.setItem(index, 1, table_item(task_stage_display_name(task)))
|
||||
self.task_table.setItem(index, 2, table_item(task_item_display_name(task)))
|
||||
self.task_table.setItem(index, 3, table_item(POLICY_DISPLAY_NAMES.get(execution_policy, execution_policy)))
|
||||
self.session_config_preview.setPlainText(dump_yaml(preview_config) if preview_config else "尚未生成本轮任务文件。")
|
||||
self.refresh_planned_trajectory()
|
||||
@@ -0,0 +1,61 @@
|
||||
"""操作台通用显示工具。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
from PySide6.QtCore import Qt
|
||||
from PySide6.QtGui import QColor
|
||||
from PySide6.QtWidgets import QTableWidgetItem
|
||||
|
||||
try:
|
||||
from .constants import (
|
||||
SENSOR_STAGE_DISPLAY_NAMES,
|
||||
STAGE_DISPLAY_NAMES,
|
||||
STAGE_ID_PREFIX_BY_STAGE_TYPE,
|
||||
TASK_DISPLAY_NAMES,
|
||||
)
|
||||
except ImportError:
|
||||
from constants import (
|
||||
SENSOR_STAGE_DISPLAY_NAMES,
|
||||
STAGE_DISPLAY_NAMES,
|
||||
STAGE_ID_PREFIX_BY_STAGE_TYPE,
|
||||
TASK_DISPLAY_NAMES,
|
||||
)
|
||||
|
||||
|
||||
def table_item(text: str, status: str | None = None) -> QTableWidgetItem:
|
||||
item = QTableWidgetItem(text)
|
||||
item.setFlags(item.flags() ^ Qt.ItemIsEditable)
|
||||
if status == "ok":
|
||||
item.setBackground(QColor("#dcfce7"))
|
||||
elif status == "warn":
|
||||
item.setBackground(QColor("#fef3c7"))
|
||||
elif status == "fail":
|
||||
item.setBackground(QColor("#fee2e2"))
|
||||
return item
|
||||
|
||||
def task_stage_display_name(task: dict[str, Any]) -> str:
|
||||
task_code = str(task.get("task_code", ""))
|
||||
stage_type = str(task.get("stage_type", ""))
|
||||
return SENSOR_STAGE_DISPLAY_NAMES.get(task_code, STAGE_DISPLAY_NAMES.get(stage_type, stage_type))
|
||||
|
||||
def task_item_display_name(task: dict[str, Any]) -> str:
|
||||
task_code = str(task.get("task_code", ""))
|
||||
name = TASK_DISPLAY_NAMES.get(task_code, task_code)
|
||||
if task_code.startswith("sensor."):
|
||||
target_id = str(task.get("target_id", "")).strip()
|
||||
if target_id:
|
||||
return f"{name}（{target_id}）"
|
||||
return name
|
||||
|
||||
def make_task_suffix(task_code: str) -> str:
|
||||
if not task_code:
|
||||
return ""
|
||||
return "_" + "".join(ch if ch.isalnum() else "_" for ch in task_code)
|
||||
|
||||
def task_stage_id(task: dict[str, Any]) -> str:
|
||||
stage_type = str(task.get("stage_type", ""))
|
||||
task_code = str(task.get("task_code", ""))
|
||||
prefix = STAGE_ID_PREFIX_BY_STAGE_TYPE.get(stage_type, "stage_unknown")
|
||||
return prefix + make_task_suffix(task_code)
|
||||
@@ -0,0 +1,278 @@
|
||||
"""车辆画像新建和编辑窗口。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from PySide6.QtWidgets import (
|
||||
QCheckBox,
|
||||
QComboBox,
|
||||
QDialog,
|
||||
QDialogButtonBox,
|
||||
QGridLayout,
|
||||
QLabel,
|
||||
QLineEdit,
|
||||
QMessageBox,
|
||||
QVBoxLayout,
|
||||
)
|
||||
|
||||
try:
|
||||
from .constants import CHASSIS_TYPES
|
||||
from .profile_io import (
|
||||
bool_from_text,
|
||||
dump_yaml,
|
||||
is_positive_number,
|
||||
read_nested,
|
||||
vehicle_profile_filename,
|
||||
vehicle_profile_path_from_name,
|
||||
)
|
||||
except ImportError:
|
||||
from constants import CHASSIS_TYPES
|
||||
from profile_io import (
|
||||
bool_from_text,
|
||||
dump_yaml,
|
||||
is_positive_number,
|
||||
read_nested,
|
||||
vehicle_profile_filename,
|
||||
vehicle_profile_path_from_name,
|
||||
)
|
||||
|
||||
|
||||
def open_vehicle_profile_dialog(window: Any, new_profile: bool) -> None:
|
||||
self = window
|
||||
source_payload = self.current_vehicle_profile_payload()
|
||||
if new_profile:
|
||||
source_payload["profile_name"] = ""
|
||||
source_payload["display_name"] = ""
|
||||
source_payload["manufacturer"] = ""
|
||||
title = "新建车辆画像"
|
||||
else:
|
||||
title = "编辑车辆画像"
|
||||
|
||||
dialog = QDialog(self)
|
||||
dialog.setWindowTitle(title)
|
||||
dialog.resize(760, 560)
|
||||
layout = QVBoxLayout(dialog)
|
||||
grid = QGridLayout()
|
||||
layout.addLayout(grid)
|
||||
|
||||
name_edit = QLineEdit(str(source_payload.get("profile_name", "")))
|
||||
file_name_label = QLabel(vehicle_profile_filename(name_edit.text()))
|
||||
model_edit = QLineEdit(str(source_payload.get("model_name", "")))
|
||||
chassis_type_combo = QComboBox()
|
||||
for value, label in CHASSIS_TYPES:
|
||||
chassis_type_combo.addItem(label, value)
|
||||
chassis_type = str(source_payload.get("chassis", {}).get("chassis_type", "ackermann"))
|
||||
chassis_index = chassis_type_combo.findData(chassis_type)
|
||||
if chassis_index >= 0:
|
||||
chassis_type_combo.setCurrentIndex(chassis_index)
|
||||
|
||||
geometry = source_payload.get("geometry", {})
|
||||
chassis = source_payload.get("chassis", {})
|
||||
controllers = source_payload.get("controllers", {})
|
||||
if not isinstance(geometry, dict):
|
||||
geometry = {}
|
||||
if not isinstance(chassis, dict):
|
||||
chassis = {}
|
||||
if not isinstance(controllers, dict):
|
||||
controllers = {}
|
||||
|
||||
length_edit = QLineEdit(str(geometry.get("length_m", "")))
|
||||
width_edit = QLineEdit(str(geometry.get("width_m", "")))
|
||||
height_edit = QLineEdit(str(geometry.get("height_m", "")))
|
||||
ground_clearance_edit = QLineEdit(str(geometry.get("ground_clearance_m", "")))
|
||||
wheel_base_edit = QLineEdit(str(chassis.get("wheel_base_m", "")))
|
||||
track_width_edit = QLineEdit(str(chassis.get("track_width_m", "")))
|
||||
wheel_radius_edit = QLineEdit(str(chassis.get("wheel_radius_m", "")))
|
||||
max_steering_angle_edit = QLineEdit(str(chassis.get("max_steering_angle_rad", "")))
|
||||
min_turning_radius_edit = QLineEdit(str(chassis.get("min_turning_radius_m", "")))
|
||||
max_speed_edit = QLineEdit(str(controllers.get("max_speed_mps", "")))
|
||||
max_accel_edit = QLineEdit(str(controllers.get("max_accel_mps2", "")))
|
||||
control_frequency_edit = QLineEdit(str(controllers.get("control_frequency_hz", "")))
|
||||
|
||||
name_edit.textChanged.connect(lambda text: file_name_label.setText(vehicle_profile_filename(text)))
|
||||
grid.addWidget(QLabel("保存文件"), 0, 0)
|
||||
grid.addWidget(file_name_label, 0, 1, 1, 2)
|
||||
grid.addWidget(QLabel("画像名称"), 1, 0)
|
||||
grid.addWidget(name_edit, 1, 1)
|
||||
grid.addWidget(QLabel("车型名称"), 1, 2)
|
||||
grid.addWidget(model_edit, 1, 3, 1, 2)
|
||||
grid.addWidget(QLabel("底盘类型"), 2, 0)
|
||||
grid.addWidget(chassis_type_combo, 2, 1, 1, 2)
|
||||
grid.addWidget(QLabel("车长 m"), 3, 0)
|
||||
grid.addWidget(length_edit, 3, 1)
|
||||
grid.addWidget(QLabel("车宽 m"), 3, 2)
|
||||
grid.addWidget(width_edit, 3, 3)
|
||||
grid.addWidget(QLabel("车高 m"), 3, 4)
|
||||
grid.addWidget(height_edit, 3, 5)
|
||||
grid.addWidget(QLabel("离地间隙 m"), 4, 0)
|
||||
grid.addWidget(ground_clearance_edit, 4, 1)
|
||||
grid.addWidget(QLabel("轴距 m"), 4, 2)
|
||||
grid.addWidget(wheel_base_edit, 4, 3)
|
||||
grid.addWidget(QLabel("轮距 m"), 4, 4)
|
||||
grid.addWidget(track_width_edit, 4, 5)
|
||||
grid.addWidget(QLabel("轮半径 m"), 5, 0)
|
||||
grid.addWidget(wheel_radius_edit, 5, 1)
|
||||
grid.addWidget(QLabel("最大转角 rad"), 5, 2)
|
||||
grid.addWidget(max_steering_angle_edit, 5, 3)
|
||||
grid.addWidget(QLabel("最小转弯半径 m"), 5, 4)
|
||||
grid.addWidget(min_turning_radius_edit, 5, 5)
|
||||
grid.addWidget(QLabel("最大速度 m/s"), 6, 0)
|
||||
grid.addWidget(max_speed_edit, 6, 1)
|
||||
grid.addWidget(QLabel("最大加速度 m/s²"), 6, 2)
|
||||
grid.addWidget(max_accel_edit, 6, 3)
|
||||
grid.addWidget(QLabel("控制频率 Hz"), 6, 4)
|
||||
grid.addWidget(control_frequency_edit, 6, 5)
|
||||
|
||||
sensor_fields: dict[str, tuple[QCheckBox, QLineEdit, QComboBox | None]] = {}
|
||||
sensors = source_payload.get("sensors", {})
|
||||
if not isinstance(sensors, dict):
|
||||
sensors = {}
|
||||
sensor_labels = [
|
||||
("front_camera", "前视相机 ID"),
|
||||
("down_camera", "下视相机 ID"),
|
||||
("lidar_3d", "3D 雷达 ID"),
|
||||
("lidar_2d", "2D 雷达 ID"),
|
||||
("imu", "IMU ID"),
|
||||
("arm_camera", "手眼相机 ID"),
|
||||
]
|
||||
start_row = 7
|
||||
for index, (sensor_key, label) in enumerate(sensor_labels):
|
||||
row = start_row + index // 2
|
||||
column = (index % 2) * 3
|
||||
sensor_profile = sensors.get(sensor_key, {})
|
||||
sensor_id = sensor_profile.get("sensor_id", "") if isinstance(sensor_profile, dict) else ""
|
||||
enabled = bool_from_text(sensor_profile.get("enabled", True)) if isinstance(sensor_profile, dict) else False
|
||||
installed_check = QCheckBox(label.replace(" ID", ""))
|
||||
installed_check.setChecked(enabled)
|
||||
edit = QLineEdit(str(sensor_id))
|
||||
edit.setEnabled(enabled)
|
||||
mount_combo: QComboBox | None = None
|
||||
if sensor_key == "arm_camera":
|
||||
mount_combo = QComboBox()
|
||||
mount_combo.addItem("眼在手上", "eye_in_hand")
|
||||
mount_combo.addItem("眼在手外", "eye_to_hand")
|
||||
mount_type = sensor_profile.get("camera_mount_type", "eye_in_hand") if isinstance(sensor_profile, dict) else "eye_in_hand"
|
||||
combo_index = mount_combo.findData(mount_type)
|
||||
mount_combo.setCurrentIndex(combo_index if combo_index >= 0 else 0)
|
||||
mount_combo.setEnabled(enabled)
|
||||
installed_check.stateChanged.connect(
|
||||
lambda _state, line_edit=edit, combo=mount_combo, check=installed_check: (
|
||||
line_edit.setEnabled(check.isChecked()),
|
||||
combo.setEnabled(check.isChecked()),
|
||||
)
|
||||
)
|
||||
else:
|
||||
installed_check.stateChanged.connect(
|
||||
lambda _state, line_edit=edit, check=installed_check: line_edit.setEnabled(check.isChecked())
|
||||
)
|
||||
sensor_fields[sensor_key] = (installed_check, edit, mount_combo)
|
||||
grid.addWidget(installed_check, row, column)
|
||||
if mount_combo is None:
|
||||
grid.addWidget(edit, row, column + 1, 1, 2)
|
||||
else:
|
||||
grid.addWidget(edit, row, column + 1)
|
||||
grid.addWidget(mount_combo, row, column + 2)
|
||||
|
||||
buttons = QDialogButtonBox()
|
||||
save_btn = buttons.addButton("保存", QDialogButtonBox.AcceptRole)
|
||||
close_btn = buttons.addButton("关闭", QDialogButtonBox.RejectRole)
|
||||
layout.addWidget(buttons)
|
||||
|
||||
def build_payload() -> dict[str, Any] | None:
|
||||
required_values = [
|
||||
length_edit.text().strip(),
|
||||
width_edit.text().strip(),
|
||||
height_edit.text().strip(),
|
||||
wheel_base_edit.text().strip(),
|
||||
track_width_edit.text().strip(),
|
||||
wheel_radius_edit.text().strip(),
|
||||
]
|
||||
if not all(is_positive_number(value) for value in required_values):
|
||||
QMessageBox.warning(dialog, "画像不完整", "车辆长宽高、轴距、轮距、轮半径必须填写正数。")
|
||||
return None
|
||||
loaded_sensors = source_payload.get("sensors", {})
|
||||
if not isinstance(loaded_sensors, dict):
|
||||
loaded_sensors = {}
|
||||
sensor_payload: dict[str, dict[str, Any]] = {}
|
||||
for sensor_key, (installed_check, edit, mount_combo) in sensor_fields.items():
|
||||
if not installed_check.isChecked():
|
||||
continue
|
||||
existing = loaded_sensors.get(sensor_key, {})
|
||||
if not isinstance(existing, dict):
|
||||
existing = {}
|
||||
if not edit.text().strip():
|
||||
QMessageBox.warning(dialog, "传感器编号缺失", f"{installed_check.text()}已选择安装，请填写编号。")
|
||||
return None
|
||||
mount_type = existing.get("camera_mount_type", "eye_in_hand" if sensor_key == "arm_camera" else "unspecified")
|
||||
if mount_combo is not None:
|
||||
mount_type = str(mount_combo.currentData() or "eye_in_hand")
|
||||
sensor_payload[sensor_key] = {
|
||||
"sensor_id": edit.text().strip(),
|
||||
"sensor_name": existing.get("sensor_name", sensor_key),
|
||||
"frame_id": existing.get("frame_id", f"{sensor_key}_link"),
|
||||
"sensor_type": existing.get("sensor_type", sensor_key),
|
||||
"camera_mount_type": mount_type,
|
||||
"enabled": True,
|
||||
"needs_intrinsic_calibration": existing.get("needs_intrinsic_calibration", sensor_key in {"front_camera", "down_camera", "imu"}),
|
||||
"needs_extrinsic_calibration": existing.get("needs_extrinsic_calibration", True),
|
||||
}
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"profile_name": name_edit.text().strip() or "unnamed_vehicle_profile",
|
||||
"display_name": name_edit.text().strip() or "未命名车辆画像",
|
||||
"model_name": model_edit.text().strip(),
|
||||
"manufacturer": source_payload.get("manufacturer", ""),
|
||||
"profile_version": source_payload.get("profile_version", "v1"),
|
||||
"description": source_payload.get("description", ""),
|
||||
"base_link_frame": read_nested(self.site_profile, ("frames", "base_link"), "base_link"),
|
||||
"geometry": {
|
||||
"length_m": length_edit.text().strip(),
|
||||
"width_m": width_edit.text().strip(),
|
||||
"height_m": height_edit.text().strip(),
|
||||
"ground_clearance_m": ground_clearance_edit.text().strip(),
|
||||
},
|
||||
"chassis": {
|
||||
"chassis_type": str(chassis_type_combo.currentData() or "ackermann"),
|
||||
"wheel_base_m": wheel_base_edit.text().strip(),
|
||||
"track_width_m": track_width_edit.text().strip(),
|
||||
"wheel_radius_m": wheel_radius_edit.text().strip(),
|
||||
"max_steering_angle_rad": max_steering_angle_edit.text().strip(),
|
||||
"min_turning_radius_m": min_turning_radius_edit.text().strip(),
|
||||
},
|
||||
"sensors": sensor_payload,
|
||||
"controllers": {
|
||||
"lateral_default": controllers.get("lateral_default", "mpc"),
|
||||
"longitudinal_default": controllers.get("longitudinal_default", "pid"),
|
||||
"max_speed_mps": max_speed_edit.text().strip(),
|
||||
"max_accel_mps2": max_accel_edit.text().strip(),
|
||||
"control_frequency_hz": control_frequency_edit.text().strip(),
|
||||
},
|
||||
}
|
||||
|
||||
def save_to_path(target_path: Path) -> bool:
|
||||
payload = build_payload()
|
||||
if payload is None:
|
||||
return False
|
||||
target_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
target_path.write_text(dump_yaml(payload), encoding="utf-8")
|
||||
self.vehicle_profile_data = payload
|
||||
self.vehicle_profile_path_edit.setText(str(target_path))
|
||||
self.vehicle_profile_name_edit.setText(str(payload["profile_name"]))
|
||||
self.vehicle_model_name_edit.setText(str(payload["model_name"]))
|
||||
self.vehicle_manufacturer_edit.setText(str(payload.get("manufacturer", "")))
|
||||
self.apply_vehicle_profile_to_ui(payload)
|
||||
self.refresh_vehicle_profile_combo()
|
||||
self.append_log(f"[车辆画像] 已保存：{target_path}")
|
||||
self.refresh_all()
|
||||
return True
|
||||
|
||||
def save_current() -> None:
|
||||
target_path = vehicle_profile_path_from_name(name_edit.text())
|
||||
if save_to_path(target_path):
|
||||
dialog.accept()
|
||||
|
||||
save_btn.clicked.connect(save_current)
|
||||
close_btn.clicked.connect(dialog.reject)
|
||||
dialog.exec()
|
||||
@@ -182,7 +182,32 @@ void apply_default_hand_eye_metadata(StagePlan & stage, const VehicleProfile & p
|
||||
return;
|
||||
}
|
||||
|
||||
const auto * sensor = find_default_sensor(profile, false, true);
|
||||
const SensorProfile * sensor = nullptr;
|
||||
for (const auto & candidate : profile.sensors) {
|
||||
if (!candidate.enabled || !candidate.selected_for_this_session) {
|
||||
continue;
|
||||
}
|
||||
if (candidate.sensor_type.value != SensorType::ARM_CAMERA) {
|
||||
continue;
|
||||
}
|
||||
if (!candidate.needs_extrinsic_calibration || !candidate.supports_extrinsic_calibration) {
|
||||
continue;
|
||||
}
|
||||
sensor = &candidate;
|
||||
break;
|
||||
}
|
||||
if (!sensor) {
|
||||
for (const auto & candidate : profile.sensors) {
|
||||
if (!candidate.enabled || candidate.sensor_type.value != SensorType::ARM_CAMERA) {
|
||||
continue;
|
||||
}
|
||||
if (!candidate.needs_extrinsic_calibration || !candidate.supports_extrinsic_calibration) {
|
||||
continue;
|
||||
}
|
||||
sensor = &candidate;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!sensor) {
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -68,6 +68,12 @@ WorkshopReport ReportBuilder::build(
|
||||
kv.value = session.vehicle_profile_snapshot.base_info.vehicle_id;
|
||||
report.metadata.push_back(kv);
|
||||
|
||||
for (const auto & profile_kv : session.vehicle_profile_snapshot.metadata) {
|
||||
if (profile_kv.key.rfind("workshop_geometry.", 0) == 0) {
|
||||
report.metadata.push_back(profile_kv);
|
||||
}
|
||||
}
|
||||
|
||||
kv.key = "operator_id";
|
||||
kv.value = session.operator_info.operator_id;
|
||||
report.metadata.push_back(kv);
|
||||
|
||||
+18
-1
@@ -8,6 +8,7 @@
|
||||
#include <sstream> // 拼接 session_id 时要用字符串流。
|
||||
#include <system_error> // 文件系统查询失败时用 error_code 接住错误。
|
||||
#include <thread> // 把整场执行放到后台线程时要用 std::thread。
|
||||
#include <exception> // 执行线程兜底捕获异常，避免节点直接退出。
|
||||
#include <unordered_map> // 保存从 dataset_index.yaml 读取出来的 metadata。
|
||||
|
||||
#include "calibration_workshop_orchestration_interfaces/msg/approval_state.hpp"
|
||||
@@ -978,7 +979,12 @@ void WorkshopOrchestratorV2Node::execute_session(
|
||||
const std::shared_ptr<GoalHandleExecuteWorkshopSession> goal_handle)
|
||||
{
|
||||
const auto goal = goal_handle->get_goal(); // 取出外部发来的“开始跑这台车这次标定”的请求内容。
|
||||
auto context = prepare_session_for_run(goal->goal.session_id); // 把这条任务切到“准备开跑”状态，并记录真正开始时间。
|
||||
SessionExecutionContext context; // 先准备一份兜底上下文；后面即使 prepare 抛异常，也能按失败路径收尾。
|
||||
context.session_id = goal->goal.session_id;
|
||||
context.started_timestamp_us = now_us();
|
||||
|
||||
try {
|
||||
context = prepare_session_for_run(goal->goal.session_id); // 把这条任务切到“准备开跑”状态，并记录真正开始时间。
|
||||
|
||||
const auto precheck = run_precheck_step(context.session_id); // 正式开跑前先检查：这条任务现在有没有条件开始跑。
|
||||
if (!precheck.ready_for_start) { // 如果检查结论是不允许开跑。
|
||||
@@ -1002,6 +1008,17 @@ void WorkshopOrchestratorV2Node::execute_session(
|
||||
}
|
||||
|
||||
finish_session_succeeded(goal_handle, context); // 全部步骤都成功后，按成功路径收尾。
|
||||
} catch (const std::exception & exc) {
|
||||
const std::string failure_reason =
|
||||
std::string("总控执行线程捕获未处理异常: ") + exc.what();
|
||||
RCLCPP_ERROR(get_logger(), "%s", failure_reason.c_str());
|
||||
finish_session_failed(goal_handle, context, failure_reason);
|
||||
} catch (...) {
|
||||
const std::string failure_reason = "总控执行线程捕获未知异常。";
|
||||
RCLCPP_ERROR(get_logger(), "%s", failure_reason.c_str());
|
||||
finish_session_failed(goal_handle, context, failure_reason);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
WorkshopOrchestratorV2Node::SessionExecutionContext WorkshopOrchestratorV2Node::prepare_session_for_run(
|
||||
|
||||
@@ -1,6 +1,15 @@
|
||||
mode: sim
|
||||
vehicle_id: demo_agv_001
|
||||
|
||||
vehicle_profile:
|
||||
profile_file: src/deployment/profiles/vehicle_profiles/ackermann_default.yaml
|
||||
profile_name: ackermann_standard_v1
|
||||
|
||||
workshop_geometry:
|
||||
length_m: 10.0
|
||||
width_m: 6.0
|
||||
height_m: 3.5
|
||||
|
||||
workshop_pc:
|
||||
gateway:
|
||||
vehicle_host: 127.0.0.1
|
||||
|
||||
@@ -1,6 +1,17 @@
|
||||
mode: site
|
||||
vehicle_id: replace_with_real_vehicle_id
|
||||
|
||||
vehicle_profile:
|
||||
# 车辆画像描述同一车型的可复用静态配置；当前车辆的唯一编号仍然使用上面的 vehicle_id。
|
||||
profile_file: src/deployment/profiles/vehicle_profiles/ackermann_default.yaml
|
||||
profile_name: ackermann_standard_v1
|
||||
|
||||
workshop_geometry:
|
||||
# 标定车间有效空间尺寸，单位为米；真实现场尺寸不同就在现场配置文件里改这里。
|
||||
length_m: 10.0
|
||||
width_m: 6.0
|
||||
height_m: 3.5
|
||||
|
||||
workshop_pc:
|
||||
gateway:
|
||||
vehicle_host: 192.168.10.42
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
schema_version: 1
|
||||
profile_name: ackermann_standard_v1
|
||||
display_name: 标准阿克曼底盘画像
|
||||
model_name: ackermann_standard
|
||||
manufacturer: AutoCalib Workshop
|
||||
profile_version: v1
|
||||
description: 可复用的阿克曼车型画像模板；用于同类型车辆首次建档或复用建档。
|
||||
base_link_frame: rear_axle_center
|
||||
|
||||
geometry:
|
||||
length_m: 1.20
|
||||
width_m: 0.65
|
||||
height_m: 1.30
|
||||
ground_clearance_m: 0.08
|
||||
|
||||
chassis:
|
||||
chassis_type: ackermann
|
||||
wheel_base_m: 0.80
|
||||
track_width_m: 0.48
|
||||
wheel_radius_m: 0.10
|
||||
max_steering_angle_rad: 0.60
|
||||
min_turning_radius_m: 1.20
|
||||
|
||||
sensors:
|
||||
front_camera:
|
||||
sensor_id: demo_front_camera
|
||||
sensor_name: 前视相机
|
||||
frame_id: front_camera_link
|
||||
sensor_type: front_camera
|
||||
camera_mount_type: front_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
down_camera:
|
||||
sensor_id: demo_down_camera
|
||||
sensor_name: 下视相机
|
||||
frame_id: down_camera_link
|
||||
sensor_type: down_camera
|
||||
camera_mount_type: downward_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_3d:
|
||||
sensor_id: demo_lidar_3d
|
||||
sensor_name: 3D 激光雷达
|
||||
frame_id: lidar_3d_link
|
||||
sensor_type: lidar_3d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_2d:
|
||||
sensor_id: demo_lidar_2d
|
||||
sensor_name: 2D 激光雷达
|
||||
frame_id: lidar_2d_link
|
||||
sensor_type: lidar_2d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
imu:
|
||||
sensor_id: demo_imu
|
||||
sensor_name: IMU
|
||||
frame_id: imu_link
|
||||
sensor_type: imu
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
|
||||
controllers:
|
||||
lateral_default: mpc
|
||||
longitudinal_default: pid
|
||||
max_speed_mps: 0.30
|
||||
max_accel_mps2: 0.40
|
||||
control_frequency_hz: 50.0
|
||||
@@ -0,0 +1,81 @@
|
||||
schema_version: 1
|
||||
profile_name: ackermann_standard_v1
|
||||
display_name: ackermann_standard_v1
|
||||
model_name: ackermann_standard
|
||||
manufacturer: AutoCalib Workshop
|
||||
profile_version: v1
|
||||
description: 可复用的阿克曼车型画像模板；用于同类型车辆首次建档或复用建档。
|
||||
base_link_frame: rear_axle_center
|
||||
geometry:
|
||||
length_m: '1.2'
|
||||
width_m: '0.65'
|
||||
height_m: '1.3'
|
||||
ground_clearance_m: '0.08'
|
||||
chassis:
|
||||
chassis_type: ackermann
|
||||
wheel_base_m: '0.8'
|
||||
track_width_m: '0.48'
|
||||
wheel_radius_m: '0.1'
|
||||
max_steering_angle_rad: '0.6'
|
||||
min_turning_radius_m: '1.2'
|
||||
sensors:
|
||||
front_camera:
|
||||
sensor_id: demo_front_camera
|
||||
sensor_name: 前视相机
|
||||
frame_id: front_camera_link
|
||||
sensor_type: front_camera
|
||||
camera_mount_type: front_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
down_camera:
|
||||
sensor_id: demo_down_camera
|
||||
sensor_name: 下视相机
|
||||
frame_id: down_camera_link
|
||||
sensor_type: down_camera
|
||||
camera_mount_type: downward_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_3d:
|
||||
sensor_id: demo_lidar_3d
|
||||
sensor_name: 3D 激光雷达
|
||||
frame_id: lidar_3d_link
|
||||
sensor_type: lidar_3d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_2d:
|
||||
sensor_id: demo_lidar_2d
|
||||
sensor_name: 2D 激光雷达
|
||||
frame_id: lidar_2d_link
|
||||
sensor_type: lidar_2d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
imu:
|
||||
sensor_id: demo_imu
|
||||
sensor_name: IMU
|
||||
frame_id: imu_link
|
||||
sensor_type: imu
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
arm_camera:
|
||||
sensor_id: asf
|
||||
sensor_name: arm_camera
|
||||
frame_id: arm_camera_link
|
||||
sensor_type: arm_camera
|
||||
camera_mount_type: eye_in_hand
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
controllers:
|
||||
lateral_default: mpc
|
||||
longitudinal_default: pid
|
||||
max_speed_mps: '0.3'
|
||||
max_accel_mps2: '0.4'
|
||||
control_frequency_hz: '50.0'
|
||||
@@ -0,0 +1,81 @@
|
||||
schema_version: 1
|
||||
profile_name: adad
|
||||
display_name: adad
|
||||
model_name: ackermann_standard
|
||||
manufacturer: AutoCalib Workshop
|
||||
profile_version: v1
|
||||
description: 可复用的阿克曼车型画像模板；用于同类型车辆首次建档或复用建档。
|
||||
base_link_frame: rear_axle_center
|
||||
geometry:
|
||||
length_m: '1.2'
|
||||
width_m: '0.65'
|
||||
height_m: '1.3'
|
||||
ground_clearance_m: '0.08'
|
||||
chassis:
|
||||
chassis_type: ackermann
|
||||
wheel_base_m: '0.8'
|
||||
track_width_m: '0.48'
|
||||
wheel_radius_m: '0.1'
|
||||
max_steering_angle_rad: '0.6'
|
||||
min_turning_radius_m: '1.2'
|
||||
sensors:
|
||||
front_camera:
|
||||
sensor_id: demo_front_camera
|
||||
sensor_name: 前视相机
|
||||
frame_id: front_camera_link
|
||||
sensor_type: front_camera
|
||||
camera_mount_type: front_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
down_camera:
|
||||
sensor_id: demo_down_camera
|
||||
sensor_name: 下视相机
|
||||
frame_id: down_camera_link
|
||||
sensor_type: down_camera
|
||||
camera_mount_type: downward_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_3d:
|
||||
sensor_id: demo_lidar_3d
|
||||
sensor_name: 3D 激光雷达
|
||||
frame_id: lidar_3d_link
|
||||
sensor_type: lidar_3d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_2d:
|
||||
sensor_id: demo_lidar_2d
|
||||
sensor_name: 2D 激光雷达
|
||||
frame_id: lidar_2d_link
|
||||
sensor_type: lidar_2d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
imu:
|
||||
sensor_id: demo_imu
|
||||
sensor_name: IMU
|
||||
frame_id: imu_link
|
||||
sensor_type: imu
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
arm_camera:
|
||||
sensor_id: demo_arm_camera
|
||||
sensor_name: arm_camera
|
||||
frame_id: arm_camera_link
|
||||
sensor_type: arm_camera
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
controllers:
|
||||
lateral_default: mpc
|
||||
longitudinal_default: pid
|
||||
max_speed_mps: '0.3'
|
||||
max_accel_mps2: '0.4'
|
||||
control_frequency_hz: '50.0'
|
||||
@@ -0,0 +1,81 @@
|
||||
schema_version: 1
|
||||
profile_name: unnamed_vehicle_profile
|
||||
display_name: 未命名车辆画像
|
||||
model_name: ackermann_standard
|
||||
manufacturer: AutoCalib Workshop
|
||||
profile_version: v1
|
||||
description: 可复用的阿克曼车型画像模板；用于同类型车辆首次建档或复用建档。
|
||||
base_link_frame: rear_axle_center
|
||||
geometry:
|
||||
length_m: '1.2'
|
||||
width_m: '0.65'
|
||||
height_m: '1.3'
|
||||
ground_clearance_m: '0.08'
|
||||
chassis:
|
||||
chassis_type: ackermann
|
||||
wheel_base_m: '0.8'
|
||||
track_width_m: '0.48'
|
||||
wheel_radius_m: '0.1'
|
||||
max_steering_angle_rad: '0.6'
|
||||
min_turning_radius_m: '1.2'
|
||||
sensors:
|
||||
front_camera:
|
||||
sensor_id: demo_front_camera
|
||||
sensor_name: 前视相机
|
||||
frame_id: front_camera_link
|
||||
sensor_type: front_camera
|
||||
camera_mount_type: front_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
down_camera:
|
||||
sensor_id: demo_down_camera
|
||||
sensor_name: 下视相机
|
||||
frame_id: down_camera_link
|
||||
sensor_type: down_camera
|
||||
camera_mount_type: downward_mounted
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_3d:
|
||||
sensor_id: demo_lidar_3d
|
||||
sensor_name: 3D 激光雷达
|
||||
frame_id: lidar_3d_link
|
||||
sensor_type: lidar_3d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
lidar_2d:
|
||||
sensor_id: demo_lidar_2d
|
||||
sensor_name: 2D 激光雷达
|
||||
frame_id: lidar_2d_link
|
||||
sensor_type: lidar_2d
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
imu:
|
||||
sensor_id: demo_imu
|
||||
sensor_name: IMU
|
||||
frame_id: imu_link
|
||||
sensor_type: imu
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: true
|
||||
needs_extrinsic_calibration: true
|
||||
arm_camera:
|
||||
sensor_id: demo_arm_camera
|
||||
sensor_name: arm_camera
|
||||
frame_id: arm_camera_link
|
||||
sensor_type: arm_camera
|
||||
camera_mount_type: unspecified
|
||||
enabled: true
|
||||
needs_intrinsic_calibration: false
|
||||
needs_extrinsic_calibration: true
|
||||
controllers:
|
||||
lateral_default: mpc
|
||||
longitudinal_default: pid
|
||||
max_speed_mps: '0.3'
|
||||
max_accel_mps2: '0.4'
|
||||
control_frequency_hz: '50.0'
|
||||
@@ -69,6 +69,7 @@ def parse_args() -> argparse.Namespace:
|
||||
)
|
||||
parser.add_argument("--reference-target-id", default="site_reference_target", help="外部真值/传感器默认目标 ID。")
|
||||
parser.add_argument("--sensor-id", default="demo_front_camera", help="默认传感器内参任务 sensor_id。")
|
||||
parser.add_argument("--hand-eye-sensor-id", default="demo_arm_camera", help="默认手眼任务 sensor_id。")
|
||||
parser.add_argument("--sensor-task-subtype", default="front_camera_intrinsic", help="默认传感器内参任务子类型。")
|
||||
parser.add_argument("--output", "-o", default="", help="输出 YAML；不填写时输出到标准输出。")
|
||||
parser.add_argument(
|
||||
@@ -290,9 +291,9 @@ def minimal_task(
|
||||
task_param("sensor_extrinsic.timeout_sec", "5.0"),
|
||||
]
|
||||
elif task_name == "hand_eye":
|
||||
task["target_id"] = args.sensor_id
|
||||
task["target_id"] = args.hand_eye_sensor_id
|
||||
task["task_params"] = [
|
||||
task_param("sensor.sensor_id", args.sensor_id),
|
||||
task_param("sensor.sensor_id", args.hand_eye_sensor_id),
|
||||
task_param("sensor.task_subtype", "eye_in_hand"),
|
||||
task_param("hand_eye.arm_id", "demo_arm"),
|
||||
task_param("hand_eye.required_pose_count", "1"),
|
||||
|
||||
@@ -73,6 +73,16 @@ TASK_STAGE_TYPES = {
|
||||
|
||||
DEFAULT_TASKS = "external,chassis,control,sensor_intrinsic"
|
||||
|
||||
FAKE_EXTERNAL_MODES = (
|
||||
"valid",
|
||||
"invalid_pose",
|
||||
"low_quality",
|
||||
"wrong_source",
|
||||
"time_sync_exceeded",
|
||||
"tracking_unstable",
|
||||
"target_missing",
|
||||
)
|
||||
|
||||
CHASSIS_TYPE_VALUES = {
|
||||
"ackermann": ChassisType.ACKERMANN,
|
||||
"differential": ChassisType.DIFFERENTIAL,
|
||||
@@ -87,6 +97,36 @@ CHASSIS_TYPE_DISPLAY_NAMES = {
|
||||
"multi_steer_wheel": "多舵轮",
|
||||
}
|
||||
|
||||
SENSOR_TYPE_VALUES = {
|
||||
"front_camera": SensorType.FRONT_CAMERA,
|
||||
"arm_camera": SensorType.ARM_CAMERA,
|
||||
"down_camera": SensorType.DOWNWARD_CAMERA,
|
||||
"downward_camera": SensorType.DOWNWARD_CAMERA,
|
||||
"lidar_3d": SensorType.LIDAR_3D,
|
||||
"lidar_2d": SensorType.LIDAR_2D,
|
||||
"imu": SensorType.IMU,
|
||||
}
|
||||
|
||||
CAMERA_MOUNT_TYPE_VALUES = {
|
||||
"front_mounted": CameraMountType.FRONT_MOUNTED,
|
||||
"downward_mounted": CameraMountType.DOWNWARD_MOUNTED,
|
||||
"eye_in_hand": CameraMountType.EYE_IN_HAND,
|
||||
"eye_to_hand": CameraMountType.EYE_TO_HAND,
|
||||
"unspecified": CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED,
|
||||
}
|
||||
|
||||
CONTROL_AXIS_VALUES = {
|
||||
"lateral": ControlAxisType.LATERAL_CONTROL,
|
||||
"longitudinal": ControlAxisType.LONGITUDINAL_CONTROL,
|
||||
}
|
||||
|
||||
CONTROLLER_ALGORITHM_VALUES = {
|
||||
"pid": ControllerAlgorithmType.PID,
|
||||
"mpc": ControllerAlgorithmType.MPC,
|
||||
"lqr": ControllerAlgorithmType.LQR,
|
||||
"pure_pursuit": ControllerAlgorithmType.PURE_PURSUIT,
|
||||
}
|
||||
|
||||
EXECUTION_POLICY_VALUES = {
|
||||
"REQUIRED": StageExecutionPolicy.REQUIRED,
|
||||
"OPTIONAL": StageExecutionPolicy.OPTIONAL,
|
||||
@@ -172,6 +212,7 @@ def make_sensor(sensor_id: str, sensor_type: int, mount_type: int, name: str) ->
|
||||
sensor.enabled = True
|
||||
sensor.needs_intrinsic_calibration = sensor_type in (
|
||||
SensorType.FRONT_CAMERA,
|
||||
SensorType.ARM_CAMERA,
|
||||
SensorType.DOWNWARD_CAMERA,
|
||||
SensorType.IMU,
|
||||
)
|
||||
@@ -195,6 +236,12 @@ def make_vehicle_profile(vehicle_id: str, chassis_type_name: str) -> VehicleProf
|
||||
profile.chassis_type.value = CHASSIS_TYPE_VALUES[chassis_type_name]
|
||||
profile.base_link_frame = "base_link"
|
||||
profile.profile_version = "orchestrator_smoke_v1"
|
||||
profile.arm_profile.has_mechanical_arm = True
|
||||
profile.arm_profile.arm_id = "demo_arm"
|
||||
profile.arm_profile.arm_model = "demo_arm_model"
|
||||
profile.arm_profile.dof = 6
|
||||
profile.arm_profile.arm_base_frame = "arm_base_link"
|
||||
profile.arm_profile.tool_frame = "tool0"
|
||||
|
||||
profile.capabilities.extend([
|
||||
make_capability(CalibrationAbilityType.EXTERNAL_LOCALIZATION_CALIBRATION, "支持外部真值接入"),
|
||||
@@ -209,6 +256,7 @@ def make_vehicle_profile(vehicle_id: str, chassis_type_name: str) -> VehicleProf
|
||||
make_sensor("demo_lidar_3d", SensorType.LIDAR_3D, CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED, "3D 激光雷达"),
|
||||
make_sensor("demo_lidar_2d", SensorType.LIDAR_2D, CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED, "2D 激光雷达"),
|
||||
make_sensor("demo_imu", SensorType.IMU, CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED, "IMU"),
|
||||
make_sensor("demo_arm_camera", SensorType.ARM_CAMERA, CameraMountType.EYE_IN_HAND, "手眼相机"),
|
||||
])
|
||||
|
||||
profile.controllers.extend([
|
||||
@@ -263,6 +311,128 @@ def load_yaml(path: Path) -> dict:
|
||||
return data
|
||||
|
||||
|
||||
def bool_from_profile(value: object, default: bool) -> bool:
|
||||
if isinstance(value, bool):
|
||||
return value
|
||||
if value in (None, ""):
|
||||
return default
|
||||
return str(value).strip().lower() in {"1", "true", "yes", "y", "on"}
|
||||
|
||||
|
||||
def apply_vehicle_profile_file(profile: VehicleProfile, vehicle_profile_file: str, vehicle_id: str) -> None:
|
||||
if not vehicle_profile_file:
|
||||
return
|
||||
path = Path(vehicle_profile_file).expanduser().resolve(strict=False)
|
||||
data = load_yaml(path)
|
||||
|
||||
profile.base_info.vehicle_id = vehicle_id
|
||||
profile.base_info.vehicle_name = str(data.get("display_name") or data.get("profile_name") or profile.base_info.vehicle_name)
|
||||
profile.base_info.model_name = str(data.get("model_name") or profile.base_info.model_name)
|
||||
profile.base_info.manufacturer = str(data.get("manufacturer") or profile.base_info.manufacturer)
|
||||
profile.base_info.description = str(data.get("description") or profile.base_info.description)
|
||||
profile.profile_version = str(data.get("profile_version") or profile.profile_version)
|
||||
profile.base_link_frame = str(data.get("base_link_frame") or profile.base_link_frame)
|
||||
|
||||
chassis = data.get("chassis", {})
|
||||
if not isinstance(chassis, dict):
|
||||
chassis = {}
|
||||
chassis_type = str(chassis.get("chassis_type") or data.get("vehicle_class") or "").strip()
|
||||
if chassis_type in CHASSIS_TYPE_VALUES:
|
||||
profile.chassis_type.value = CHASSIS_TYPE_VALUES[chassis_type]
|
||||
|
||||
geometry = data.get("geometry", {})
|
||||
if not isinstance(geometry, dict):
|
||||
geometry = {}
|
||||
|
||||
sensors = data.get("sensors", {})
|
||||
parsed_sensors: list[SensorProfile] = []
|
||||
if isinstance(sensors, dict):
|
||||
for sensor_key, raw_sensor in sensors.items():
|
||||
if not isinstance(raw_sensor, dict):
|
||||
continue
|
||||
sensor_id = str(raw_sensor.get("sensor_id") or sensor_key)
|
||||
sensor_type_name = str(raw_sensor.get("sensor_type") or sensor_key)
|
||||
sensor_type = SENSOR_TYPE_VALUES.get(sensor_type_name)
|
||||
if sensor_type is None:
|
||||
continue
|
||||
mount_type = CAMERA_MOUNT_TYPE_VALUES.get(
|
||||
str(raw_sensor.get("camera_mount_type") or "unspecified"),
|
||||
CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED,
|
||||
)
|
||||
sensor = make_sensor(
|
||||
sensor_id,
|
||||
sensor_type,
|
||||
mount_type,
|
||||
str(raw_sensor.get("sensor_name") or sensor_key),
|
||||
)
|
||||
sensor.frame_id = str(raw_sensor.get("frame_id") or sensor.frame_id)
|
||||
sensor.enabled = bool_from_profile(raw_sensor.get("enabled"), True)
|
||||
sensor.needs_intrinsic_calibration = bool_from_profile(
|
||||
raw_sensor.get("needs_intrinsic_calibration"),
|
||||
sensor.needs_intrinsic_calibration,
|
||||
)
|
||||
sensor.needs_extrinsic_calibration = bool_from_profile(
|
||||
raw_sensor.get("needs_extrinsic_calibration"),
|
||||
sensor.needs_extrinsic_calibration,
|
||||
)
|
||||
sensor.supports_intrinsic_calibration = sensor.needs_intrinsic_calibration
|
||||
sensor.supports_extrinsic_calibration = sensor.needs_extrinsic_calibration
|
||||
parsed_sensors.append(sensor)
|
||||
if parsed_sensors:
|
||||
profile.sensors.clear()
|
||||
profile.sensors.extend(parsed_sensors)
|
||||
|
||||
controllers = data.get("controllers", {})
|
||||
if isinstance(controllers, dict):
|
||||
lateral_default = CONTROLLER_ALGORITHM_VALUES.get(
|
||||
str(controllers.get("lateral_default") or "mpc"),
|
||||
ControllerAlgorithmType.MPC,
|
||||
)
|
||||
longitudinal_default = CONTROLLER_ALGORITHM_VALUES.get(
|
||||
str(controllers.get("longitudinal_default") or "pid"),
|
||||
ControllerAlgorithmType.PID,
|
||||
)
|
||||
profile.controllers.clear()
|
||||
profile.controller_selections.clear()
|
||||
profile.controllers.extend([
|
||||
make_controller_profile(ControlAxisType.LATERAL_CONTROL, lateral_default),
|
||||
make_controller_profile(ControlAxisType.LONGITUDINAL_CONTROL, longitudinal_default),
|
||||
])
|
||||
profile.controller_selections.extend([
|
||||
make_controller_selection(ControlAxisType.LATERAL_CONTROL, lateral_default, True),
|
||||
make_controller_selection(ControlAxisType.LONGITUDINAL_CONTROL, longitudinal_default, True),
|
||||
])
|
||||
|
||||
profile.metadata.append(kv("vehicle_profile.file", str(path)))
|
||||
profile.metadata.append(kv("vehicle_profile.profile_name", str(data.get("profile_name") or "")))
|
||||
for key in ("length_m", "width_m", "height_m", "ground_clearance_m"):
|
||||
value = geometry.get(key)
|
||||
if value not in (None, ""):
|
||||
profile.metadata.append(kv(f"vehicle.geometry.{key}", str(value)))
|
||||
for key in (
|
||||
"chassis_type",
|
||||
"wheel_base_m",
|
||||
"track_width_m",
|
||||
"wheel_radius_m",
|
||||
"max_steering_angle_rad",
|
||||
"min_turning_radius_m",
|
||||
):
|
||||
value = chassis.get(key)
|
||||
if value not in (None, ""):
|
||||
profile.metadata.append(kv(f"vehicle.chassis.{key}", str(value)))
|
||||
if isinstance(controllers, dict):
|
||||
for key in (
|
||||
"lateral_default",
|
||||
"longitudinal_default",
|
||||
"max_speed_mps",
|
||||
"max_accel_mps2",
|
||||
"control_frequency_hz",
|
||||
):
|
||||
value = controllers.get(key)
|
||||
if value not in (None, ""):
|
||||
profile.metadata.append(kv(f"vehicle.control.{key}", str(value)))
|
||||
|
||||
|
||||
def has_placeholder(value: object) -> bool:
|
||||
return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS)
|
||||
|
||||
@@ -308,6 +478,13 @@ def maybe_apply_site_profile_defaults(args: argparse.Namespace) -> None:
|
||||
or "ackermann"
|
||||
)
|
||||
|
||||
if not args.vehicle_profile_file:
|
||||
raw_path = read_profile_string(profile, ("vehicle_profile", "profile_file"))
|
||||
if raw_path:
|
||||
candidate = resolve_profile_path(raw_path, site_profile_path)
|
||||
if candidate.exists():
|
||||
args.vehicle_profile_file = str(candidate)
|
||||
|
||||
if not args.chassis_action_profile:
|
||||
raw_path = read_profile_string(profile, ("chassis_calibration", "action_profile_file"))
|
||||
if raw_path:
|
||||
@@ -395,7 +572,7 @@ def task_params(task_name: str, args: argparse.Namespace) -> list[KeyValuePair]:
|
||||
]
|
||||
if task_name == "hand_eye":
|
||||
return [
|
||||
kv("sensor.sensor_id", "demo_front_camera"),
|
||||
kv("sensor.sensor_id", "demo_arm_camera"),
|
||||
kv("sensor.task_subtype", "eye_in_hand"),
|
||||
kv("hand_eye.arm_id", "demo_arm"),
|
||||
kv("hand_eye.required_pose_count", "1"),
|
||||
@@ -535,6 +712,69 @@ def make_session_config(tasks: Iterable[str], args: argparse.Namespace) -> Works
|
||||
return config
|
||||
|
||||
|
||||
def make_session_config_from_file(args: argparse.Namespace) -> WorkshopSessionConfig:
|
||||
path = Path(args.session_config_file).expanduser().resolve(strict=False)
|
||||
payload = load_yaml(path)
|
||||
raw_config = payload.get("session_config", payload)
|
||||
if not isinstance(raw_config, dict):
|
||||
raise ValueError(f"{path} 中的 session_config 必须是 YAML map。")
|
||||
|
||||
config = WorkshopSessionConfig()
|
||||
config.auto_commit_parameters = bool(raw_config.get("auto_commit_parameters", False))
|
||||
config.require_manual_approval_before_commit = bool(raw_config.get("require_manual_approval_before_commit", False))
|
||||
config.run_validation_after_each_stage = bool(raw_config.get("run_validation_after_each_stage", False))
|
||||
config.stop_on_first_failure = bool(raw_config.get("stop_on_first_failure", True))
|
||||
config.allow_optional_stage_skip = bool(raw_config.get("allow_optional_stage_skip", False))
|
||||
config.enable_auto_rollback_on_validation_failure = bool(
|
||||
raw_config.get("enable_auto_rollback_on_validation_failure", False)
|
||||
)
|
||||
config.allow_rebuild_execution_plan = bool(raw_config.get("allow_rebuild_execution_plan", False))
|
||||
config.localization_source_id = str(raw_config.get("localization_source_id", args.reference_source_name))
|
||||
config.workcell_zone_id = str(raw_config.get("workcell_zone_id", args.workcell_zone_id))
|
||||
config.reference_target_id = str(raw_config.get("reference_target_id", args.reference_target_id))
|
||||
|
||||
raw_tasks = raw_config.get("requested_tasks", payload.get("requested_tasks", []))
|
||||
if not isinstance(raw_tasks, list) or not raw_tasks:
|
||||
raise ValueError(f"{path} 中缺少 requested_tasks。")
|
||||
for raw_task in raw_tasks:
|
||||
if not isinstance(raw_task, dict):
|
||||
raise ValueError("requested_tasks 中每一项都必须是 YAML map。")
|
||||
config.requested_tasks.append(make_requested_task_from_profile(
|
||||
raw_task,
|
||||
PROFILE_STAGE_TYPE_VALUES.get(
|
||||
str(raw_task.get("stage_type", "")),
|
||||
0,
|
||||
),
|
||||
"UI/现场会话配置",
|
||||
))
|
||||
return config
|
||||
|
||||
|
||||
def read_workshop_geometry_from_file(args: argparse.Namespace) -> dict[str, str]:
|
||||
if not args.session_config_file:
|
||||
return {}
|
||||
path = Path(args.session_config_file).expanduser().resolve(strict=False)
|
||||
payload = load_yaml(path)
|
||||
raw_config = payload.get("session_config", {})
|
||||
geometry = payload.get("workshop_geometry")
|
||||
if not isinstance(geometry, dict) and isinstance(raw_config, dict):
|
||||
geometry = raw_config.get("workshop_geometry")
|
||||
if not isinstance(geometry, dict):
|
||||
return {}
|
||||
return {
|
||||
"length_m": str(geometry.get("length_m", "")).strip(),
|
||||
"width_m": str(geometry.get("width_m", "")).strip(),
|
||||
"height_m": str(geometry.get("height_m", "")).strip(),
|
||||
}
|
||||
|
||||
|
||||
def apply_workshop_geometry_metadata(profile: VehicleProfile, geometry: dict[str, str]) -> None:
|
||||
for key in ("length_m", "width_m", "height_m"):
|
||||
value = geometry.get(key, "")
|
||||
if value:
|
||||
profile.metadata.append(kv(f"workshop_geometry.{key}", value))
|
||||
|
||||
|
||||
class OrchestratorSmoke:
|
||||
def __init__(self, args: argparse.Namespace) -> None:
|
||||
self.args = args
|
||||
@@ -571,6 +811,22 @@ class OrchestratorSmoke:
|
||||
msg.observed_target_count = 4
|
||||
msg.reference_source_name = self.args.reference_source_name
|
||||
msg.active_job_id = "orchestrator_e2e_smoke"
|
||||
|
||||
if self.args.fake_external_mode == "invalid_pose":
|
||||
msg.pose_valid = False
|
||||
elif self.args.fake_external_mode == "low_quality":
|
||||
msg.position_stddev_m = 0.2
|
||||
msg.yaw_stddev_rad = 0.2
|
||||
msg.quality_score = 0.1
|
||||
elif self.args.fake_external_mode == "wrong_source":
|
||||
msg.reference_source_name = f"{self.args.reference_source_name}_unexpected"
|
||||
elif self.args.fake_external_mode == "time_sync_exceeded":
|
||||
msg.time_sync_offset_ms = 500.0
|
||||
elif self.args.fake_external_mode == "tracking_unstable":
|
||||
msg.tracking_loss_ratio = 1.0
|
||||
elif self.args.fake_external_mode == "target_missing":
|
||||
msg.observed_target_count = 0
|
||||
|
||||
self.external_pub.publish(msg)
|
||||
|
||||
def spin_for(self, seconds: float) -> None:
|
||||
@@ -729,13 +985,19 @@ class OrchestratorSmoke:
|
||||
return self.call_service(client, request, "/workshop_v2/get_report")
|
||||
|
||||
def run(self) -> int:
|
||||
tasks = parse_tasks(self.args.tasks)
|
||||
expect_failure = self.args.expected_outcome == "failure"
|
||||
Path(self.args.sensor_storage_root).mkdir(parents=True, exist_ok=True)
|
||||
self.maybe_disable_wifi6_precheck()
|
||||
if self.args.publish_fake_external_telemetry:
|
||||
self.spin_for(self.args.external_warmup_sec)
|
||||
|
||||
profile = make_vehicle_profile(self.args.vehicle_id, self.args.chassis_profile_type)
|
||||
apply_vehicle_profile_file(profile, self.args.vehicle_profile_file, self.args.vehicle_id)
|
||||
if self.args.session_config_file:
|
||||
session_config = make_session_config_from_file(self.args)
|
||||
apply_workshop_geometry_metadata(profile, read_workshop_geometry_from_file(self.args))
|
||||
else:
|
||||
tasks = parse_tasks(self.args.tasks)
|
||||
session_config = make_session_config(tasks, self.args)
|
||||
self.register_vehicle_profile(profile)
|
||||
session_id = self.create_session(profile, session_config)
|
||||
@@ -745,6 +1007,8 @@ class OrchestratorSmoke:
|
||||
report = report_response.report
|
||||
|
||||
if not report_response.success:
|
||||
if expect_failure:
|
||||
return self.verify_expected_failure(session_id, report_response.message, report)
|
||||
action_status = getattr(action_result, "status", None)
|
||||
message = report_response.message or "(orchestrator 未返回失败原因)"
|
||||
print(
|
||||
@@ -755,6 +1019,11 @@ class OrchestratorSmoke:
|
||||
self.print_report(report)
|
||||
return 2
|
||||
|
||||
if expect_failure:
|
||||
print("[FAIL] 预期本次 smoke 失败，但 execute_session 返回成功。", file=sys.stderr)
|
||||
self.print_report(report)
|
||||
return 7
|
||||
|
||||
if not report.overall_success:
|
||||
print("[FAIL] report.overall_success=false", file=sys.stderr)
|
||||
self.print_report(report)
|
||||
@@ -785,6 +1054,74 @@ class OrchestratorSmoke:
|
||||
self.print_report(report)
|
||||
return 0
|
||||
|
||||
def verify_expected_failure(self, session_id: str, message: str, report) -> int:
|
||||
if report.overall_success:
|
||||
print("[FAIL] 预期本次 smoke 失败，但 report.overall_success=true。", file=sys.stderr)
|
||||
self.print_report(report)
|
||||
return 8
|
||||
|
||||
failed_stages = [
|
||||
result
|
||||
for result in report.stage_results
|
||||
if not result.success or not result.auto_acceptance_passed
|
||||
]
|
||||
if not failed_stages and report.stage_results:
|
||||
print("[FAIL] report 失败但所有已执行阶段都显示成功。", file=sys.stderr)
|
||||
self.print_report(report)
|
||||
return 9
|
||||
|
||||
expected_stage = self.args.expected_failed_stage.strip()
|
||||
if not failed_stages and expected_stage:
|
||||
print("[FAIL] 预期指定阶段失败，但报告里没有失败阶段。", file=sys.stderr)
|
||||
self.print_report(report)
|
||||
return 9
|
||||
|
||||
if expected_stage and not any(expected_stage in result.stage_id for result in failed_stages):
|
||||
actual = ", ".join(result.stage_id for result in failed_stages)
|
||||
print(
|
||||
"[FAIL] 失败阶段不符合预期: "
|
||||
f"expected_contains={expected_stage!r}, actual={actual}",
|
||||
file=sys.stderr,
|
||||
)
|
||||
self.print_report(report)
|
||||
return 10
|
||||
|
||||
expected_text = self.args.expected_failure_text.strip()
|
||||
if expected_text:
|
||||
haystack_parts = [message, report.summary]
|
||||
haystack_parts.extend(self.feedback_messages)
|
||||
for result in report.stage_results:
|
||||
haystack_parts.append(result.summary)
|
||||
haystack_parts.append(result.failure_root_cause)
|
||||
haystack = "\n".join(part for part in haystack_parts if part)
|
||||
if expected_text not in haystack:
|
||||
print(
|
||||
"[FAIL] 失败原因不符合预期: "
|
||||
f"expected_contains={expected_text!r}",
|
||||
file=sys.stderr,
|
||||
)
|
||||
self.print_report(report)
|
||||
return 11
|
||||
|
||||
if self.args.expect_stop_after_failure and failed_stages:
|
||||
first_failed_stage_id = failed_stages[0].stage_id
|
||||
if report.stage_results[-1].stage_id != first_failed_stage_id:
|
||||
print(
|
||||
"[FAIL] 失败后仍然继续执行了后续阶段，未满足首错即停预期。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
self.print_report(report)
|
||||
return 12
|
||||
|
||||
service_report = self.fetch_report(session_id)
|
||||
if not service_report.response.success:
|
||||
print(f"[FAIL] 失败后 get_report 不可用: {service_report.response.message}", file=sys.stderr)
|
||||
return 13
|
||||
|
||||
print("[PASS] workshop orchestrator expected failure smoke completed.")
|
||||
self.print_report(report)
|
||||
return 0
|
||||
|
||||
@staticmethod
|
||||
def print_report(report) -> None:
|
||||
print(f"[REPORT] session_id={report.session_id} overall_success={report.overall_success}")
|
||||
@@ -818,7 +1155,13 @@ def parse_args() -> argparse.Namespace:
|
||||
help="可选现场部署 profile；填写后自动读取底盘动作、运控评估和传感器标定 profile 路径",
|
||||
)
|
||||
parser.add_argument("--vehicle-id", default="demo_agv_001")
|
||||
parser.add_argument("--vehicle-profile-file", default="", help="车辆画像 YAML 文件；同车型车辆可复用，vehicle_id 仍按当前车辆传入")
|
||||
parser.add_argument("--tasks", default=DEFAULT_TASKS, help=f"逗号分隔任务列表，默认: {DEFAULT_TASKS}")
|
||||
parser.add_argument(
|
||||
"--session-config-file",
|
||||
default="",
|
||||
help="可选 UI/现场工具生成的会话配置 YAML；填写后直接使用其中的 requested_tasks",
|
||||
)
|
||||
parser.add_argument("--operator-id", default="smoke_operator")
|
||||
parser.add_argument("--workstation-id", default="sim_workstation")
|
||||
parser.add_argument("--workshop-line-id", default="sim_line_001")
|
||||
@@ -827,6 +1170,12 @@ def parse_args() -> argparse.Namespace:
|
||||
parser.add_argument("--reference-target-id", default="sim_reference_target")
|
||||
parser.add_argument("--external-telemetry-topic", default="/isaac/external_localization/vehicle/pose")
|
||||
parser.add_argument("--publish-fake-external-telemetry", action=argparse.BooleanOptionalAction, default=True)
|
||||
parser.add_argument(
|
||||
"--fake-external-mode",
|
||||
choices=FAKE_EXTERNAL_MODES,
|
||||
default="valid",
|
||||
help="假 external 遥测模式；用于验证无效位姿、低质量、错真值源等异常路径",
|
||||
)
|
||||
parser.add_argument("--fake-external-hz", type=float, default=20.0)
|
||||
parser.add_argument("--external-warmup-sec", type=float, default=0.8)
|
||||
parser.add_argument("--sensor-storage-root", default="/tmp/agv_sensor_calibration")
|
||||
@@ -857,6 +1206,28 @@ def parse_args() -> argparse.Namespace:
|
||||
parser.add_argument("--service-timeout-sec", type=float, default=10.0)
|
||||
parser.add_argument("--action-timeout-sec", type=float, default=240.0)
|
||||
parser.add_argument("--strict-stage-count", action=argparse.BooleanOptionalAction, default=True)
|
||||
parser.add_argument(
|
||||
"--expected-outcome",
|
||||
choices=("success", "failure"),
|
||||
default="success",
|
||||
help="预期 smoke 结果；failure 用于验收异常处理路径",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--expected-failed-stage",
|
||||
default="",
|
||||
help="预期失败阶段 ID 的子串；为空表示只要求存在失败阶段",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--expected-failure-text",
|
||||
default="",
|
||||
help="预期失败原因中的子串；为空表示不检查具体文本",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--expect-stop-after-failure",
|
||||
action=argparse.BooleanOptionalAction,
|
||||
default=True,
|
||||
help="预期首个 REQUIRED 阶段失败后不再继续执行后续阶段",
|
||||
)
|
||||
parser.add_argument("--verbose-feedback", action="store_true")
|
||||
parser.add_argument(
|
||||
"--disable-wifi6-precheck",
|
||||
|
||||
+3
-3
@@ -109,11 +109,11 @@ tasks:
|
||||
required_sample_count: 10
|
||||
timeout_sec: 120.0
|
||||
|
||||
- task_code: sensor.front_camera.hand_eye
|
||||
display_name: 前视相机眼在手内
|
||||
- task_code: sensor.arm_camera.hand_eye
|
||||
display_name: 手眼相机眼在手上
|
||||
enabled: true
|
||||
selected_task: hand_eye
|
||||
sensor_id: demo_front_camera
|
||||
sensor_id: demo_arm_camera
|
||||
task_subtype: eye_in_hand
|
||||
hand_eye:
|
||||
arm_id: demo_arm
|
||||
|
||||
+1
-1
@@ -296,7 +296,7 @@ def run_smoke(session_dir: Path, args: argparse.Namespace) -> dict[str, Any]:
|
||||
tasks = [
|
||||
("camera_intrinsic", "front_camera_intrinsic", "demo_front_camera"),
|
||||
("sensor_extrinsic", "lidar_3d_extrinsic", "demo_lidar_3d"),
|
||||
("hand_eye", "eye_in_hand", "demo_front_camera"),
|
||||
("hand_eye", "eye_in_hand", "demo_arm_camera"),
|
||||
]
|
||||
generated: list[dict[str, str]] = []
|
||||
for selected_task, task_subtype, sensor_id in tasks:
|
||||
|
||||
Reference in New Issue
Block a user