diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json index dc532a5..dc46835 100644 --- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json @@ -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" diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json new file mode 100644 index 0000000..4e1c895 --- /dev/null +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json @@ -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" + } +} diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json new file mode 100644 index 0000000..4e1c895 --- /dev/null +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json @@ -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" + } +} diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json new file mode 100644 index 0000000..4e1c895 --- /dev/null +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json @@ -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" + } +} diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json new file mode 100644 index 0000000..4e1c895 --- /dev/null +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json @@ -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" + } +} diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json new file mode 100644 index 0000000..4e1c895 --- /dev/null +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json @@ -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" + } +} diff --git a/agv_calib_brain/run_local_data_input_smoke.sh b/agv_calib_brain/run_local_data_input_smoke.sh index d769511..87d9da2 100755 --- a/agv_calib_brain/run_local_data_input_smoke.sh +++ b/agv_calib_brain/run_local_data_input_smoke.sh @@ -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 diff --git a/agv_calib_brain/src/apps/operator_ui/README.md b/agv_calib_brain/src/apps/operator_ui/README.md index de21973..35bbf5e 100644 --- a/agv_calib_brain/src/apps/operator_ui/README.md +++ b/agv_calib_brain/src/apps/operator_ui/README.md @@ -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]` 行。 diff --git a/agv_calib_brain/src/apps/operator_ui/constants.py b/agv_calib_brain/src/apps/operator_ui/constants.py new file mode 100644 index 0000000..dadf308 --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/constants.py @@ -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}手眼标定" + + diff --git a/agv_calib_brain/src/apps/operator_ui/localization_view.py b/agv_calib_brain/src/apps/operator_ui/localization_view.py new file mode 100644 index 0000000..9bc478b --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/localization_view.py @@ -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) diff --git a/agv_calib_brain/src/apps/operator_ui/main.py b/agv_calib_brain/src/apps/operator_ui/main.py index e097fc7..2bb619b 100644 --- a/agv_calib_brain/src/apps/operator_ui/main.py +++ b/agv_calib_brain/src/apps/operator_ui/main.py @@ -1,680 +1,505 @@ +#!/usr/bin/env python3 +"""真实部署用标定车间操作台。""" -# ========================================================= -# 这个原型界面的目标： -# 1) 不再只做“勾选 UI”，而是让界面直接生成接近 -# WorkshopSessionConfig / RequestedCalibrationTask 的数据结构预览； -# 2) 让你能一眼看到：当前界面上的输入，最后会怎么进入主控消息。 -# 说明： -# - 这里不是 ROS2 运行节点，只是一个 PySide6 原型工具； -# - 右侧 JSON 预览，是为了帮助你核对“UI -> 消息结构”是否对齐。 -# ========================================================= +from __future__ import annotations -import json import signal -from PySide6.QtCore import Qt, QTimer +import sys +from typing import Any + +try: + import rclpy +except ImportError: + rclpy = None + +from PySide6.QtCore import QProcess, QTimer, Qt from PySide6.QtGui import QFont from PySide6.QtWidgets import ( - QApplication, QMainWindow, QWidget, QLabel, QLineEdit, QPushButton, QVBoxLayout, - QHBoxLayout, QGridLayout, QTextEdit, QGroupBox, QCheckBox, QListWidget, - QListWidgetItem, QProgressBar, QMessageBox, QTreeWidget, QTreeWidgetItem, - QSplitter, QTabWidget, QComboBox, QFormLayout, QFileDialog + QApplication, + QCheckBox, + QComboBox, + QFrame, + QGridLayout, + QGroupBox, + QHBoxLayout, + QHeaderView, + QLabel, + QLineEdit, + QMainWindow, + QPlainTextEdit, + QPushButton, + QSplitter, + QTableWidget, + QTabWidget, + QVBoxLayout, + QWidget, ) -# ========================================================= -# 底盘通用参数 -# 这些最终会映射成 RequestedCalibrationTask： -# - stage_type = CHASSIS_CALIBRATION_STAGE -# - task_code = chassis.common.xxx -# ========================================================= -CHASSIS_COMMON = [ - ("chassis.common.effective_wheel_base_m", "有效轴距"), - ("chassis.common.effective_track_width_m", "有效轮距"), - ("chassis.common.longitudinal_scale", "纵向里程比例补偿"), - ("chassis.common.lateral_scale", "横向里程比例补偿"), - ("chassis.common.yaw_scale", "航向比例补偿"), - ("chassis.common.straight_line_bias", "直线跑偏补偿"), -] -# ========================================================= -# 底盘专属参数 -# 这些会根据底盘类型动态显示。 -# ========================================================= -CHASSIS_SPECIFIC = { - "ACKERMANN_CHASSIS": [ - ("chassis.ackermann.front_left_steer_zero_offset_deg", "左前舵角零偏"), - ("chassis.ackermann.front_right_steer_zero_offset_deg", "右前舵角零偏"), - ("chassis.ackermann.rear_left_wheel_radius_m", "左后轮有效半径"), - ("chassis.ackermann.rear_right_wheel_radius_m", "右后轮有效半径"), - ("chassis.ackermann.steering_ratio", "转向传动比"), - ], - "DIFFERENTIAL_CHASSIS": [ - ("chassis.differential.left_wheel_radius_m", "左轮有效半径"), - ("chassis.differential.right_wheel_radius_m", "右轮有效半径"), - ("chassis.differential.axle_track_width_m", "驱动轮间距"), - ("chassis.differential.left_encoder_scale", "左编码器比例系数"), - ("chassis.differential.right_encoder_scale", "右编码器比例系数"), - ], - "SINGLE_STEER_WHEEL_CHASSIS": [ - ("chassis.single_steer.drive_wheel_radius_m", "驱动轮有效半径"), - ("chassis.single_steer.steer_zero_offset_deg", "舵角零偏"), - ("chassis.single_steer.steering_ratio", "转向比"), - ("chassis.single_steer.drive_encoder_scale", "驱动编码器比例"), - ], - "MULTI_STEER_WHEEL_CHASSIS": [ - ("chassis.multi_steer.module_wheel_radius_m", "模块轮半径"), - ("chassis.multi_steer.module_steer_zero_offset_deg", "模块舵角零偏"), - ("chassis.multi_steer.module_pos_x_m", "模块在 base_link 下的 X"), - ("chassis.multi_steer.module_pos_y_m", "模块在 base_link 下的 Y"), - ], -} - -# ========================================================= -# 运控参数 -# 按 控制轴 × 算法类型 组织。 -# 最终会映射成 RequestedCalibrationTask： -# - stage_type = CONTROL_CALIBRATION_STAGE -# - task_code = control.lateral.pid / control.longitudinal.mpc 等 -# - task_params = need_tuning / is_default -# ========================================================= -CONTROL_OPTIONS = { - "lateral": [ - ("control.lateral.pid", "PID"), - ("control.lateral.mpc", "MPC"), - ("control.lateral.lqr", "LQR"), - ("control.lateral.pure_pursuit", "Pure Pursuit"), - ], - "longitudinal": [ - ("control.longitudinal.pid", "PID"), - ("control.longitudinal.mpc", "MPC"), - ], -} - -# ========================================================= -# 传感器标定树 -# 最终会映射成 RequestedCalibrationTask： -# - stage_type = SENSOR_INTRINSIC_CALIBRATION_STAGE / SENSOR_EXTRINSIC_CALIBRATION_STAGE / HAND_EYE_CALIBRATION_STAGE -# - task_code / target_id 由树节点决定 -# ========================================================= -SENSOR_TREE = { - "camera": { - "front_camera": [ - ("sensor.front_camera.camera_intrinsic", "相机内参"), - ("sensor.front_camera.sensor_to_base_extrinsic", "传感器到 base_link 外参"), - ], - "downward_camera": [ - ("sensor.downward_camera.camera_intrinsic", "相机内参"), - ("sensor.downward_camera.sensor_to_base_extrinsic", "传感器到 base_link 外参"), - ], - "arm_camera": [ - ("sensor.arm_camera.camera_intrinsic", "相机内参"), - ("sensor.arm_camera.sensor_to_base_extrinsic", "传感器到 base_link 外参"), - ("sensor.arm_camera.hand_eye", "手眼标定"), - ], - }, - "lidar": { - "lidar_3d": [ - ("sensor.lidar_3d.sensor_to_base_extrinsic", "传感器到 base_link 外参"), - ], - "lidar_2d": [ - ("sensor.lidar_2d.sensor_to_base_extrinsic", "传感器到 base_link 外参"), - ], - }, - "imu": { - "imu_01": [ - ("sensor.imu.imu_intrinsic", "IMU 内参"), - ("sensor.imu.sensor_to_base_extrinsic", "传感器到 base_link 外参"), - ], - } -} - -# ========================================================= -# 常量：用字符串模拟 WorkflowStageType / StageExecutionPolicy -# 作用： -# - 方便你直接在 JSON 预览里看懂每一项会落成什么； -# - 后面接 ROS2 时，再替换成真实枚举/消息值即可。 -# ========================================================= -STAGE_CHASSIS = "CHASSIS_CALIBRATION_STAGE" -STAGE_CONTROL = "CONTROL_CALIBRATION_STAGE" -STAGE_SENSOR_INTRINSIC = "SENSOR_INTRINSIC_CALIBRATION_STAGE" -STAGE_SENSOR_EXTRINSIC = "SENSOR_EXTRINSIC_CALIBRATION_STAGE" -STAGE_HAND_EYE = "HAND_EYE_CALIBRATION_STAGE" -POLICY_REQUIRED = "REQUIRED" -POLICY_OPTIONAL = "OPTIONAL" +try: + from .constants import * + from .localization_view import WorkshopLocalization3DView + from .process_report import ProcessReportMixin + from .profile_handlers import ProfileHandlersMixin + from .profile_io import * + from .runtime_monitor import RuntimeMonitorMixin + from .session_builder import SessionBuilderMixin + from .ui_helpers import * +except ImportError: + from constants import * + from localization_view import WorkshopLocalization3DView + from process_report import ProcessReportMixin + from profile_handlers import ProfileHandlersMixin + from profile_io import * + from runtime_monitor import RuntimeMonitorMixin + from session_builder import SessionBuilderMixin + from ui_helpers import * -class MainWindow(QMainWindow): - def __init__(self): +class OperatorMainWindow( + RuntimeMonitorMixin, + ProfileHandlersMixin, + SessionBuilderMixin, + ProcessReportMixin, + QMainWindow, +): + def __init__(self) -> None: super().__init__() - self.setWindowTitle("自动化标定车间 · UI -> WorkshopSessionConfig 对齐版") - self.resize(1700, 980) + self.setWindowTitle("自动化标定车间现场操作台") + self.setWindowFlags( + self.windowFlags() + | Qt.Window + | Qt.WindowMinimizeButtonHint + | Qt.WindowMaximizeButtonHint + | Qt.WindowCloseButtonHint + ) + self.resize(1520, 920) - # 当前界面右侧显示的“整场任务运行状态”只是原型演示状态，不是 ROS 真状态。 - self.session_state = "DRAFT" - self.current_step_index = -1 - self.steps = [] - self._timers = [] + self.site_profile: dict[str, Any] = {} + self.vehicle_profile_data: dict[str, Any] = {} + self.generated_session_config: dict[str, Any] = {} + self.launch_process = QProcess(self) + self.smoke_process = QProcess(self) + self.report_metadata: dict[str, str] = {} + self.report_stages: list[dict[str, str]] = [] + self.last_unresolved_signature = "" + self.localization_node = None + self.localization_subscription = None + self.workshop_event_subscription = None + self.localization_rclpy_initialized = False + self.localization_last_sample_wall_time = 0.0 + self.localization_spin_timer = QTimer(self) + self.localization_spin_timer.timeout.connect(self.poll_localization_messages) + self.trajectory_task_records: list[dict[str, Any]] = [] + self.trajectory_stage_order: dict[str, int] = {} + self.trajectory_runtime_active = False + self.trajectory_last_completed_stage_id = "" + + self._setup_process(self.launch_process, "现场服务") + self._setup_process(self.smoke_process, "标定流程") + self._build_ui() + self._connect_live_refresh() + self.load_profile() + + def _setup_process(self, process: QProcess, name: str) -> None: + process.setWorkingDirectory(str(REPO_ROOT)) + process.readyReadStandardOutput.connect(lambda p=process, n=name: self._read_process_output(p, n)) + process.readyReadStandardError.connect(lambda p=process, n=name: self._read_process_output(p, n)) + process.finished.connect(lambda code, status, n=name: self._process_finished(n, code, status)) + + def _build_ui(self) -> None: + self.setStyleSheet( + """ + QMainWindow { background: #f6f7f9; color: #111827; } + QGroupBox { border: 1px solid #d1d5db; border-radius: 6px; margin-top: 10px; padding-top: 12px; font-weight: 600; } + QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 4px; color: #1f2937; } + QLineEdit { border: 1px solid #cbd5e1; border-radius: 4px; padding: 6px; background: #ffffff; } + QPushButton { border: 1px solid #cbd5e1; border-radius: 4px; padding: 8px 10px; background: #ffffff; } + QPushButton:hover { background: #f1f5f9; } + QPushButton:disabled { color: #9ca3af; background: #f3f4f6; } + QPlainTextEdit { border: 1px solid #cbd5e1; border-radius: 4px; background: #0f172a; color: #e5e7eb; } + QTableWidget { border: 1px solid #cbd5e1; background: #ffffff; gridline-color: #e5e7eb; } + QHeaderView::section { background: #f3f4f6; padding: 5px; border: 0; border-right: 1px solid #d1d5db; font-weight: 600; } + """ + ) central = QWidget() self.setCentralWidget(central) root = QVBoxLayout(central) - root.setContentsMargins(14, 14, 14, 14) - root.setSpacing(12) + root.setContentsMargins(12, 12, 12, 12) + root.setSpacing(10) - title = QLabel("自动化标定车间 · UI 与消息结构对齐原型") + header = QFrame() + header.setStyleSheet("QFrame { background: #ffffff; border: 1px solid #d1d5db; border-radius: 6px; }") + header_layout = QHBoxLayout(header) + title = QLabel("自动化标定车间现场操作台") title.setFont(QFont("", 18, QFont.Bold)) - desc = QLabel( - "这版界面不只是勾选任务，而是直接生成接近 WorkshopSessionConfig / RequestedCalibrationTask 的数据预览。" - ) - desc.setWordWrap(True) - root.addWidget(title) - root.addWidget(desc) + self.workshop_dimension_state = QLabel("车间尺寸：未加载") + self.workshop_dimension_state.setMinimumWidth(220) + self.workshop_dimension_state.setAlignment(Qt.AlignRight | Qt.AlignVCenter) + self.localization_state = QLabel("车间定位：未加载") + self.localization_state.setMinimumWidth(260) + self.localization_state.setAlignment(Qt.AlignRight | Qt.AlignVCenter) + self.stack_state = QLabel("现场服务：未启动") + self.smoke_state = QLabel("标定流程：空闲") + header_layout.addWidget(title) + header_layout.addStretch(1) + header_layout.addWidget(self.workshop_dimension_state) + header_layout.addWidget(self.localization_state) + header_layout.addWidget(self.stack_state) + header_layout.addWidget(self.smoke_state) + root.addWidget(header) - main_splitter = QSplitter(Qt.Horizontal) - root.addWidget(main_splitter, 1) + splitter = QSplitter(Qt.Horizontal) + root.addWidget(splitter, 1) + splitter.addWidget(self._build_left_panel()) + splitter.addWidget(self._build_right_panel()) + splitter.setSizes([470, 1050]) - left = QWidget() - right = QWidget() - main_splitter.addWidget(left) - main_splitter.addWidget(right) - main_splitter.setSizes([980, 720]) - - left_layout = QVBoxLayout(left) - right_layout = QVBoxLayout(right) - - # ========================================================= - # 任务基础信息 - # 这些字段不会直接进入 WorkshopSessionConfig， - # 但它们通常属于 CreateWorkshopSessionRequest 的上层输入。 - # ========================================================= - base_box = QGroupBox("任务基础信息") - left_layout.addWidget(base_box) - base_layout = QGridLayout(base_box) - - self.task_name = QLineEdit("AGV_01_完整标定任务") - self.vehicle_id = QLineEdit("AGV_01") - self.line_id = QLineEdit("LINE_A") - self.operator = QLineEdit("张三") - self.base_link = QLineEdit("base_link") - - base_layout.addWidget(QLabel("任务名称"), 0, 0) - base_layout.addWidget(self.task_name, 0, 1) - base_layout.addWidget(QLabel("车辆 ID"), 0, 2) - base_layout.addWidget(self.vehicle_id, 0, 3) - base_layout.addWidget(QLabel("产线 / 工位"), 1, 0) - base_layout.addWidget(self.line_id, 1, 1) - base_layout.addWidget(QLabel("操作者"), 1, 2) - base_layout.addWidget(self.operator, 1, 3) - base_layout.addWidget(QLabel("base_link"), 2, 0) - base_layout.addWidget(self.base_link, 2, 1) - - # ========================================================= - # 固定步骤输入 - # 这几项会直接进入 WorkshopSessionConfig： - # - localization_source_id - # - workcell_zone_id - # - reference_target_id - # ========================================================= - fixed_box = QGroupBox("固定步骤输入 · 外部真值校核") - left_layout.addWidget(fixed_box) - fixed_layout = QFormLayout(fixed_box) - - self.localization_source = QLineEdit("truth_source_01") - self.workcell_zone = QLineEdit("zone_A") - self.reference_target = QLineEdit("board_01") - - fixed_layout.addRow("外部定位源 ID", self.localization_source) - fixed_layout.addRow("工位 / 区域 ID", self.workcell_zone) - fixed_layout.addRow("参考目标 ID", self.reference_target) - - # ========================================================= - # 运行策略 - # 这些会直接进入 WorkshopSessionConfig 的布尔开关。 - # ========================================================= - strategy_box = QGroupBox("运行策略（直接对应 WorkshopSessionConfig）") - left_layout.addWidget(strategy_box) - strategy_layout = QGridLayout(strategy_box) - - self.auto_commit = QCheckBox("自动写入参数") - self.require_approval = QCheckBox("写入前要求人工审批") - self.run_validation = QCheckBox("每阶段后自动复测") - self.stop_on_first_failure = QCheckBox("首错即停") - self.allow_optional_skip = QCheckBox("允许可选阶段跳过") - self.enable_auto_rollback = QCheckBox("验证失败自动回滚") - self.allow_rebuild_plan = QCheckBox("允许运行前重建计划") - - # 给几个更常见的默认值 - self.stop_on_first_failure.setChecked(True) - self.allow_optional_skip.setChecked(True) - - checks = [ - self.auto_commit, - self.require_approval, - self.run_validation, - self.stop_on_first_failure, - self.allow_optional_skip, - self.enable_auto_rollback, - self.allow_rebuild_plan, + def _connect_live_refresh(self) -> None: + watched_edits = [ + self.vehicle_id_edit, + self.vehicle_profile_path_edit, + self.vehicle_profile_name_edit, + self.vehicle_model_name_edit, + self.vehicle_manufacturer_edit, + self.vehicle_length_edit, + self.vehicle_width_edit, + self.vehicle_height_edit, + self.vehicle_ground_clearance_edit, + self.vehicle_wheel_base_edit, + self.vehicle_track_width_edit, + self.vehicle_wheel_radius_edit, + self.vehicle_max_steering_angle_edit, + self.vehicle_min_turning_radius_edit, + self.vehicle_max_speed_edit, + self.vehicle_max_accel_edit, + self.vehicle_control_frequency_edit, + self.vehicle_host_edit, + self.vehicle_port_edit, + self.external_topic_edit, + self.reference_source_edit, + self.workcell_zone_edit, + self.reference_target_edit, + self.data_input_edit, + self.dataset_index_edit, + self.sensor_storage_edit, + self.sensor_registry_edit, + self.session_config_output_edit, ] - for idx, cb in enumerate(checks): - strategy_layout.addWidget(cb, idx // 2, idx % 2) + for edit in watched_edits: + edit.textChanged.connect(self.refresh_command_preview) + self.skip_wifi6_check.stateChanged.connect(self.refresh_command_preview) - # ========================================================= - # 任务定义区 - # 这里的所有选择，最终都会变成 requested_tasks[] - # ========================================================= - tabs = QTabWidget() - left_layout.addWidget(tabs, 1) + def _build_left_panel(self) -> QWidget: + panel = QWidget() + layout = QVBoxLayout(panel) + layout.setContentsMargins(0, 0, 8, 0) - # -------------------- 底盘标定 -------------------- - chassis_tab = QWidget() - chassis_layout = QVBoxLayout(chassis_tab) + self.profile_box = QGroupBox("固定车间配置") + profile_grid = QGridLayout(self.profile_box) + self.profile_path_edit = QLineEdit(str(DEFAULT_SITE_PROFILE)) + browse_profile = QPushButton("选择") + browse_profile.clicked.connect(self.browse_profile) + reload_profile = QPushButton("加载") + reload_profile.clicked.connect(self.load_profile) + profile_grid.addWidget(QLabel("文件"), 0, 0) + profile_grid.addWidget(self.profile_path_edit, 0, 1) + profile_grid.addWidget(browse_profile, 0, 2) + profile_grid.addWidget(reload_profile, 0, 3) + self.profile_state_label = QLabel("未加载") + profile_grid.addWidget(QLabel("状态"), 1, 0) + profile_grid.addWidget(self.profile_state_label, 1, 1, 1, 3) + self.profile_box.hide() + layout.addWidget(self.profile_box) + vehicle_profile_box = QGroupBox("车辆画像") + vehicle_profile_grid = QGridLayout(vehicle_profile_box) + self.vehicle_profile_combo = QComboBox() + self.vehicle_profile_combo.activated.connect(self.load_selected_vehicle_profile) + self.vehicle_profile_path_edit = QLineEdit(str(DEFAULT_VEHICLE_PROFILE)) + self.vehicle_profile_name_edit = QLineEdit() + self.vehicle_model_name_edit = QLineEdit() + self.vehicle_manufacturer_edit = QLineEdit() + self.vehicle_length_edit = QLineEdit() + self.vehicle_width_edit = QLineEdit() + self.vehicle_height_edit = QLineEdit() + self.vehicle_ground_clearance_edit = QLineEdit() + self.vehicle_wheel_base_edit = QLineEdit() + self.vehicle_track_width_edit = QLineEdit() + self.vehicle_wheel_radius_edit = QLineEdit() + self.vehicle_max_steering_angle_edit = QLineEdit() + self.vehicle_min_turning_radius_edit = QLineEdit() + self.vehicle_max_speed_edit = QLineEdit() + self.vehicle_max_accel_edit = QLineEdit() + self.vehicle_control_frequency_edit = QLineEdit() + new_vehicle_profile = QPushButton("新建画像") + new_vehicle_profile.clicked.connect(self.open_new_vehicle_profile_dialog) + edit_vehicle_profile = QPushButton("编辑画像") + edit_vehicle_profile.clicked.connect(self.open_edit_vehicle_profile_dialog) + vehicle_profile_grid.addWidget(QLabel("选择画像"), 0, 0) + vehicle_profile_grid.addWidget(self.vehicle_profile_combo, 0, 1) + vehicle_profile_grid.addWidget(new_vehicle_profile, 0, 2) + vehicle_profile_grid.addWidget(edit_vehicle_profile, 0, 3) + layout.addWidget(vehicle_profile_box) + self.refresh_vehicle_profile_combo() + + site_box = QGroupBox("车辆连接") + site_grid = QGridLayout(site_box) + self.vehicle_id_edit = QLineEdit() + self.vehicle_host_edit = QLineEdit() + self.vehicle_port_edit = QLineEdit("9000") + self.skip_wifi6_check = QCheckBox("跳过网络质量检查（仅调试）") + self.external_topic_edit = QLineEdit() + self.reference_source_edit = QLineEdit() + self.workcell_zone_edit = QLineEdit() + self.reference_target_edit = QLineEdit("site_reference_target") + rows = [ + ("车辆 ID", self.vehicle_id_edit), + ("车上 Windows 程序 IP", self.vehicle_host_edit), + ("车上 Windows 程序端口", self.vehicle_port_edit), + ] + for row, (label, widget) in enumerate(rows): + site_grid.addWidget(QLabel(label), row, 0) + site_grid.addWidget(widget, row, 1, 1, 2) + site_grid.addWidget(self.skip_wifi6_check, len(rows), 0, 1, 3) + layout.addWidget(site_box) + + data_box = QGroupBox("采集数据和配置") + data_grid = QGridLayout(data_box) + self.data_input_edit = QLineEdit() + self.dataset_index_edit = QLineEdit() + self.sensor_storage_edit = QLineEdit("/tmp/agv_sensor_calibration") + self.sensor_registry_edit = QLineEdit("demo_front_camera") + self.session_config_output_edit = QLineEdit(str(DEFAULT_SESSION_CONFIG)) + self._add_path_row(data_grid, 0, "算法数据参数文件", self.data_input_edit, self.browse_data_input) + self._add_path_row(data_grid, 1, "采集数据索引文件", self.dataset_index_edit, self.browse_dataset_index) + self._add_path_row(data_grid, 2, "本轮任务文件", self.session_config_output_edit, self.browse_session_config_output) + data_grid.addWidget(QLabel("传感器数据保存目录"), 3, 0) + data_grid.addWidget(self.sensor_storage_edit, 3, 1, 1, 2) + data_grid.addWidget(QLabel("车辆传感器 ID 列表"), 4, 0) + data_grid.addWidget(self.sensor_registry_edit, 4, 1, 1, 2) + layout.addWidget(data_box) + + task_box = QGroupBox("本轮要标定的内容") + task_layout = QVBoxLayout(task_box) + self.external_check = QCheckBox("先检查车间定位是否可用") + self.external_check.setChecked(True) + self.external_check.stateChanged.connect(self.refresh_command_preview) + task_layout.addWidget(self.external_check) + + task_tabs = QTabWidget() + task_tabs.addTab(self._build_chassis_task_tab(), "底盘标定") + task_tabs.addTab(self._build_control_task_tab(), "运控参数") + task_tabs.addTab(self._build_sensor_task_tab(), "传感器标定") + task_layout.addWidget(task_tabs) + layout.addWidget(task_box) + + action_box = QGroupBox("操作") + action_layout = QGridLayout(action_box) + self.build_config_btn = QPushButton("生成本轮任务文件") + self.launch_btn = QPushButton("启动现场服务") + self.stop_launch_btn = QPushButton("停止现场服务") + self.smoke_btn = QPushButton("开始执行本轮标定") + self.stop_smoke_btn = QPushButton("停止本轮标定") + self.build_config_btn.clicked.connect(self.build_session_config) + self.launch_btn.clicked.connect(self.start_launch_stack) + self.stop_launch_btn.clicked.connect(lambda: self.stop_process(self.launch_process, "现场服务")) + self.smoke_btn.clicked.connect(self.start_smoke_test) + self.stop_smoke_btn.clicked.connect(lambda: self.stop_process(self.smoke_process, "标定流程")) + action_layout.addWidget(self.build_config_btn, 0, 0) + action_layout.addWidget(self.launch_btn, 0, 1) + action_layout.addWidget(self.stop_launch_btn, 1, 1) + action_layout.addWidget(self.smoke_btn, 1, 0) + action_layout.addWidget(self.stop_smoke_btn, 2, 0, 1, 2) + layout.addWidget(action_box) + layout.addStretch(1) + return panel + + def _build_chassis_task_tab(self) -> QWidget: + tab = QWidget() + layout = QVBoxLayout(tab) type_row = QHBoxLayout() type_row.addWidget(QLabel("底盘类型")) - self.chassis_type = QComboBox() - self.chassis_type.addItems([ - "ACKERMANN_CHASSIS", - "DIFFERENTIAL_CHASSIS", - "SINGLE_STEER_WHEEL_CHASSIS", - "MULTI_STEER_WHEEL_CHASSIS", - ]) - self.chassis_type.currentTextChanged.connect(self.rebuild_chassis_tree) - type_row.addWidget(self.chassis_type) - type_row.addWidget(QLabel("多舵轮模块 ID")) - self.multi_module_id = QLineEdit("steering_module_1") - type_row.addWidget(self.multi_module_id) + self.chassis_type_combo = QComboBox() + for value, label in CHASSIS_TYPES: + self.chassis_type_combo.addItem(label, value) + self.chassis_type_combo.currentIndexChanged.connect(self.rebuild_chassis_parameter_checks) + type_row.addWidget(self.chassis_type_combo) type_row.addStretch(1) - chassis_layout.addLayout(type_row) + layout.addLayout(type_row) - self.chassis_tree = QTreeWidget() - self.chassis_tree.setHeaderLabels(["对象", "说明"]) - self.chassis_tree.setColumnWidth(0, 320) - chassis_layout.addWidget(self.chassis_tree) - tabs.addTab(chassis_tab, "底盘标定") + self.chassis_param_area = QWidget() + self.chassis_param_layout = QVBoxLayout(self.chassis_param_area) + self.chassis_param_layout.setContentsMargins(0, 0, 0, 0) + layout.addWidget(self.chassis_param_area) + layout.addStretch(1) + self.chassis_param_checks: dict[str, QCheckBox] = {} + self.rebuild_chassis_parameter_checks() + return tab - # -------------------- 运控参数 -------------------- - control_tab = QWidget() - control_layout = QVBoxLayout(control_tab) - control_tip = QLabel("勾选项最终会变成 RequestedCalibrationTask；need_tuning / is_default 会写入 task_params。") - control_tip.setWordWrap(True) - control_layout.addWidget(control_tip) + def _build_control_task_tab(self) -> QWidget: + tab = QWidget() + layout = QVBoxLayout(tab) + self.control_param_checks: dict[str, QCheckBox] = {} + for option in CONTROL_PARAMETER_OPTIONS: + check = QCheckBox(f"{option['label']} · {option['axis']} · {option['algorithm']}") + check.setChecked(option["code"] in {"control.lateral.mpc", "control.longitudinal.pid"}) + check.stateChanged.connect(self.refresh_command_preview) + self.control_param_checks[str(option["code"])] = check + layout.addWidget(check) + layout.addStretch(1) + return tab - self.control_rows = [] - for axis, items in CONTROL_OPTIONS.items(): - box = QGroupBox(axis) - form = QGridLayout(box) - row = 0 - for item_id, name in items: - enable = QCheckBox(name) - need_tuning = QCheckBox("need_tuning") - is_default = QCheckBox("is_default") - form.addWidget(enable, row, 0) - form.addWidget(need_tuning, row, 1) - form.addWidget(is_default, row, 2) - self.control_rows.append((item_id, axis, name, enable, need_tuning, is_default)) - row += 1 - control_layout.addWidget(box) - tabs.addTab(control_tab, "运控参数标定") - - # -------------------- 传感器标定 -------------------- - sensor_tab = QWidget() - sensor_layout = QVBoxLayout(sensor_tab) - sensor_tip = QLabel("每个勾选项会映射成 RequestedCalibrationTask，device 名称会作为 target_id。") - sensor_tip.setWordWrap(True) - sensor_layout.addWidget(sensor_tip) - - self.sensor_tree = QTreeWidget() - self.sensor_tree.setHeaderLabels(["对象", "说明"]) - self.sensor_tree.setColumnWidth(0, 320) - self.sensor_checks = {} - self._build_sensor_tree() - sensor_layout.addWidget(self.sensor_tree) - tabs.addTab(sensor_tab, "传感器标定") - - # 按钮区 - btn_row = QHBoxLayout() - self.refresh_btn = QPushButton("刷新预览") - self.create_btn = QPushButton("创建任务（演示）") - self.run_btn = QPushButton("运行流程（演示）") - self.export_btn = QPushButton("导出 JSON") - self.refresh_btn.clicked.connect(self.refresh_preview) - self.create_btn.clicked.connect(self.create_session) - self.run_btn.clicked.connect(self.run_demo) - self.export_btn.clicked.connect(self.export_json) - btn_row.addWidget(self.refresh_btn) - btn_row.addWidget(self.create_btn) - btn_row.addWidget(self.run_btn) - btn_row.addWidget(self.export_btn) - btn_row.addStretch(1) - left_layout.addLayout(btn_row) - - # ========================================================= - # 右侧：概览 + JSON 预览 - # ========================================================= - overview_box = QGroupBox("当前任务概览") - right_layout.addWidget(overview_box) - overview_layout = QVBoxLayout(overview_box) - - state_row = QHBoxLayout() - self.state_label = QLabel("DRAFT") - self.selection_count_label = QLabel("当前选择：0 项") - state_row.addWidget(QLabel("当前状态：")) - state_row.addWidget(self.state_label) - state_row.addStretch(1) - state_row.addWidget(self.selection_count_label) - overview_layout.addLayout(state_row) - - self.progress = QProgressBar() - overview_layout.addWidget(self.progress) - - self.step_list = QListWidget() - overview_layout.addWidget(self.step_list, 1) - - bottom_splitter = QSplitter(Qt.Horizontal) - right_layout.addWidget(bottom_splitter, 1) - - event_box = QGroupBox("运行过程通知") - json_box = QGroupBox("WorkshopSessionConfig / RequestedCalibrationTask 预览") - bottom_splitter.addWidget(event_box) - bottom_splitter.addWidget(json_box) - bottom_splitter.setSizes([360, 520]) - - event_layout = QVBoxLayout(event_box) - self.event_log = QTextEdit() - self.event_log.setReadOnly(True) - self.event_log.setPlainText("系统初始化完成，等待创建标定任务。") - event_layout.addWidget(self.event_log) - - json_layout = QVBoxLayout(json_box) - self.json_preview = QTextEdit() - self.json_preview.setReadOnly(True) - json_layout.addWidget(self.json_preview) - - self.rebuild_chassis_tree() - self.refresh_preview() - - # ========================================================= - # 下面开始是“把 UI 选择转换成消息结构”的核心逻辑 - # ========================================================= - - def rebuild_chassis_tree(self): - # 重新构造底盘任务树： - # 1) 通用参数始终存在； - # 2) 专属参数按当前底盘类型显示。 - self.chassis_tree.clear() - self.chassis_checks = {} - - common_item = QTreeWidgetItem(["通用参数", "所有底盘形式都可能会用到"]) - self.chassis_tree.addTopLevelItem(common_item) - for task_id, name in CHASSIS_COMMON: - item = QTreeWidgetItem([name, "会映射成 RequestedCalibrationTask"]) - item.setFlags(item.flags() | Qt.ItemIsUserCheckable) - item.setCheckState(0, Qt.Unchecked) - item.setData(0, Qt.UserRole, task_id) - common_item.addChild(item) - self.chassis_checks[task_id] = item - - chassis_type = self.chassis_type.currentText() - specific_item = QTreeWidgetItem([f"{chassis_type} 专属参数", "按当前底盘类型展开"]) - self.chassis_tree.addTopLevelItem(specific_item) - for task_id, name in CHASSIS_SPECIFIC[chassis_type]: - item = QTreeWidgetItem([name, "会映射成 RequestedCalibrationTask"]) - item.setFlags(item.flags() | Qt.ItemIsUserCheckable) - item.setCheckState(0, Qt.Unchecked) - item.setData(0, Qt.UserRole, task_id) - specific_item.addChild(item) - self.chassis_checks[task_id] = item - - self.chassis_tree.expandAll() - self.chassis_tree.itemChanged.connect(lambda *_: self.refresh_preview()) - self.refresh_preview() - - def _build_sensor_tree(self): - # 构造传感器任务树： - # 分类 -> 设备 -> 具体标定任务 - self.sensor_tree.clear() - for category, devices in SENSOR_TREE.items(): - cat_item = QTreeWidgetItem([category, "传感器类型"]) - self.sensor_tree.addTopLevelItem(cat_item) - for target_id, tasks in devices.items(): - device_item = QTreeWidgetItem([target_id, "具体设备 / target_id"]) - cat_item.addChild(device_item) - for task_code, display_name, stage_type in tasks: - task_item = QTreeWidgetItem([display_name, "会映射成 RequestedCalibrationTask"]) - task_item.setFlags(task_item.flags() | Qt.ItemIsUserCheckable) - task_item.setCheckState(0, Qt.Unchecked) - task_item.setData(0, Qt.UserRole, (task_code, target_id, stage_type)) - device_item.addChild(task_item) - self.sensor_checks[(task_code, target_id)] = task_item - self.sensor_tree.expandAll() - self.sensor_tree.itemChanged.connect(lambda *_: self.refresh_preview()) - - def make_task(self, stage_type, task_code, target_id="", task_params=None, enabled=True, require_manual_approval=False, - execution_policy=POLICY_REQUIRED, reason=""): - # 这个函数的作用： - # 把当前 UI 上的一个具体选择，转换成一个 RequestedCalibrationTask 对象（这里用 dict 预览）。 - return { - "stage_type": stage_type, - "enabled": enabled, - "require_manual_approval": require_manual_approval, - "execution_policy": execution_policy, - "reason": reason, - "task_code": task_code, - "target_id": target_id, - "task_params": task_params or [], + def _build_sensor_task_tab(self) -> QWidget: + tab = QWidget() + layout = QVBoxLayout(tab) + self.sensor_id_edits: dict[str, QLineEdit] = { + "front_camera": QLineEdit("demo_front_camera"), + "down_camera": QLineEdit("demo_down_camera"), + "lidar_2d": QLineEdit("demo_lidar_2d"), + "lidar_3d": QLineEdit("demo_lidar_3d"), + "imu": QLineEdit("demo_imu"), + "arm_camera": QLineEdit("demo_arm_camera"), } + for edit in self.sensor_id_edits.values(): + edit.hide() + edit.textChanged.connect(self.refresh_command_preview) + self.sensor_profile_summary_label = QLabel("当前画像传感器：未加载") + self.sensor_profile_summary_label.setWordWrap(True) + layout.addWidget(self.sensor_profile_summary_label) - def selected_chassis_tasks(self): - tasks = [] - chassis_type = self.chassis_type.currentText() - for task_code, item in self.chassis_checks.items(): - if item.checkState(0) == Qt.Checked: - # 多舵轮场景下，target_id 直接复用模块 ID 输入； - # 其它底盘形式先用 chassis_main。 - target_id = self.multi_module_id.text().strip() if chassis_type == "MULTI_STEER_WHEEL_CHASSIS" else "chassis_main" - params = [{"key": "chassis_type", "value": chassis_type}] - tasks.append(self.make_task( - stage_type=STAGE_CHASSIS, - task_code=task_code, - target_id=target_id, - task_params=params, - )) - return tasks + self.sensor_task_checks: dict[tuple[str, str], QCheckBox] = {} + for sensor_key, sensor_label, subtype, label, stage_type in SENSOR_TASK_OPTIONS: + check = QCheckBox(f"{sensor_label} · {label}") + check.setChecked(subtype == "front_camera_intrinsic") + check.stateChanged.connect(self.refresh_command_preview) + self.sensor_task_checks[(sensor_key, subtype)] = check + layout.addWidget(check) + layout.addStretch(1) + return tab - def selected_control_tasks(self): - tasks = [] - for task_code, axis, alg_name, enable, need_tuning, is_default in self.control_rows: - if enable.isChecked(): - params = [ - {"key": "control_axis", "value": axis}, - {"key": "algorithm_name", "value": alg_name}, - {"key": "need_tuning", "value": "true" if need_tuning.isChecked() else "false"}, - {"key": "is_default", "value": "true" if is_default.isChecked() else "false"}, - ] - tasks.append(self.make_task( - stage_type=STAGE_CONTROL, - task_code=task_code, - target_id=f"{axis}_controller", - task_params=params, - )) - return tasks - - def selected_sensor_tasks(self): - tasks = [] - for (task_code, target_id), item in self.sensor_checks.items(): - if item.checkState(0) == Qt.Checked: - _, _, stage_type = item.data(0, Qt.UserRole) - tasks.append(self.make_task( - stage_type=stage_type, - task_code=task_code, - target_id=target_id, - )) - return tasks - - def build_config_payload(self): - # 这里生成的对象，就是当前这版 UI 对齐出来的 WorkshopSessionConfig 预览。 - requested_tasks = [] - requested_tasks.extend(self.selected_chassis_tasks()) - requested_tasks.extend(self.selected_control_tasks()) - requested_tasks.extend(self.selected_sensor_tasks()) - - config = { - "requested_tasks": requested_tasks, - "auto_commit_parameters": self.auto_commit.isChecked(), - "require_manual_approval_before_commit": self.require_approval.isChecked(), - "run_validation_after_each_stage": self.run_validation.isChecked(), - "stop_on_first_failure": self.stop_on_first_failure.isChecked(), - "allow_optional_stage_skip": self.allow_optional_skip.isChecked(), - "enable_auto_rollback_on_validation_failure": self.enable_auto_rollback.isChecked(), - "allow_rebuild_execution_plan": self.allow_rebuild_plan.isChecked(), - "localization_source_id": self.localization_source.text().strip(), - "workcell_zone_id": self.workcell_zone.text().strip(), - "reference_target_id": self.reference_target.text().strip(), - } - return config - - def build_steps(self): - # 这部分只是为了右侧“执行顺序预览”。 - # 它不是 ROS 消息，而是把 config 里的 requested_tasks 转成用户更容易看懂的步骤列表。 - config = self.build_config_payload() - steps = [("external_reference", "外部真值校核（固定必做）")] - for task in config["requested_tasks"]: - steps.append((task["task_code"], f'{task["stage_type"]} · {task["task_code"]} · {task["target_id"]}')) - steps.append(("report", "生成最终报告")) - return steps - - def refresh_preview(self): - # 每次界面勾选发生变化，都刷新： - # 1) 当前步骤预览 - # 2) JSON 预览 - self.steps = self.build_steps() - self.step_list.clear() - for idx, (_, name) in enumerate(self.steps, start=1): - self.step_list.addItem(QListWidgetItem(f"{idx}. {name}")) - - selected_count = max(len(self.steps) - 2, 0) - self.selection_count_label.setText(f"当前选择：{selected_count} 项") - - payload = { - "create_request_preview": { - "task_name": self.task_name.text().strip(), - "vehicle_id": self.vehicle_id.text().strip(), - "workshop_line_id": self.line_id.text().strip(), - "operator": self.operator.text().strip(), - "base_link": self.base_link.text().strip(), - "config": self.build_config_payload(), - } - } - self.json_preview.setPlainText(json.dumps(payload, ensure_ascii=False, indent=2)) - - def log(self, text): - self.event_log.setPlainText(text + "\n" + self.event_log.toPlainText()) - - def create_session(self): - # 这里只是演示“任务创建成功后的界面反应”，不是真的发 ROS service。 - self.clear_timers() - self.refresh_preview() - config = self.build_config_payload() - if len(config["requested_tasks"]) == 0: - QMessageBox.warning(self, "提示", "请至少选择一个具体标定项。") + def rebuild_chassis_parameter_checks(self) -> None: + if not hasattr(self, "chassis_param_layout"): return + while self.chassis_param_layout.count(): + item = self.chassis_param_layout.takeAt(0) + widget = item.widget() + if widget is not None: + widget.deleteLater() + self.chassis_param_checks = {} + chassis_type = self.chassis_type_combo.currentData() or "ackermann" + for option in CHASSIS_PARAMETER_OPTIONS[str(chassis_type)]: + check = QCheckBox(f"{option['label']} · {option['target']}") + check.setChecked(True) + check.stateChanged.connect(self.refresh_command_preview) + self.chassis_param_checks[str(option["code"])] = check + self.chassis_param_layout.addWidget(check) + self.chassis_param_layout.addStretch(1) + if hasattr(self, "launch_command_preview"): + self.refresh_command_preview() - self.session_state = "READY" - self.state_label.setText(self.session_state) - self.progress.setValue(0) - self.log(f"已创建标定任务：{self.task_name.text().strip()}") - self.log(f"固定步骤输入：定位源={config['localization_source_id']}，工位={config['workcell_zone_id']}，参考目标={config['reference_target_id']}") - self.log(f"本次共生成 {len(config['requested_tasks'])} 个 RequestedCalibrationTask。") + def _add_path_row(self, layout: QGridLayout, row: int, label: str, edit: QLineEdit, slot) -> None: + button = QPushButton("选择") + button.clicked.connect(slot) + layout.addWidget(QLabel(label), row, 0) + layout.addWidget(edit, row, 1) + layout.addWidget(button, row, 2) - def run_demo(self): - # 这里只做主流程演示，不接 ROS2。 - self.clear_timers() - self.refresh_preview() - config = self.build_config_payload() - if len(config["requested_tasks"]) == 0: - QMessageBox.warning(self, "提示", "请至少选择一个具体标定项。") - return + def _build_right_panel(self) -> QWidget: + panel = QWidget() + layout = QVBoxLayout(panel) + layout.setContentsMargins(0, 0, 0, 0) - self.session_state = "PRECHECK_RUNNING" - self.state_label.setText(self.session_state) - self.progress.setValue(0) - self.log("开始预检：检查固定步骤输入是否完整、以及 requested_tasks 是否非空。") + tabs = QTabWidget() + layout.addWidget(tabs, 3) - t = QTimer(self) - t.setSingleShot(True) - t.timeout.connect(self._after_precheck) - t.start(700) - self._timers.append(t) + self.check_table = QTableWidget(0, 3) + self.check_table.setHorizontalHeaderLabels(["检查项", "状态", "说明"]) + self.check_table.horizontalHeader().setSectionResizeMode(0, QHeaderView.ResizeToContents) + self.check_table.horizontalHeader().setSectionResizeMode(1, QHeaderView.ResizeToContents) + self.check_table.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch) + tabs.addTab(self.check_table, "现场检查") - def _after_precheck(self): - self.session_state = "RUNNING" - self.state_label.setText(self.session_state) - self.log("预检通过，开始正式执行。") + task_tab = QWidget() + task_layout = QVBoxLayout(task_tab) + self.task_table = QTableWidget(0, 4) + self.task_table.setHorizontalHeaderLabels(["顺序", "阶段", "标定项", "策略"]) + self.task_table.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch) + self.session_config_preview = QPlainTextEdit() + self.session_config_preview.setReadOnly(True) + task_layout.addWidget(self.task_table, 2) + tabs.addTab(task_tab, "本轮任务预览") - for index, (_, name) in enumerate(self.steps): - t = QTimer(self) - t.setSingleShot(True) - t.timeout.connect(lambda idx=index, nm=name: self._run_step(idx, nm)) - t.start(850 * (index + 1)) - self._timers.append(t) + localization_tab = QWidget() + localization_layout = QVBoxLayout(localization_tab) + self.localization_3d_view = WorkshopLocalization3DView() + self.localization_summary_label = QLabel("等待定位数据") + self.localization_table = QTableWidget(len(LOCALIZATION_DISPLAY_ROWS), 2) + self.localization_table.setHorizontalHeaderLabels(["字段", "值"]) + self.localization_table.horizontalHeader().setSectionResizeMode(0, QHeaderView.ResizeToContents) + self.localization_table.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch) + for row, name in enumerate(LOCALIZATION_DISPLAY_ROWS): + self.localization_table.setItem(row, 0, table_item(name)) + self.localization_table.setItem(row, 1, table_item("-")) + localization_layout.addWidget(self.localization_3d_view, 3) + localization_layout.addWidget(self.localization_summary_label) + localization_layout.addWidget(self.localization_table, 1) + tabs.addTab(localization_tab, "车间定位") - end_t = QTimer(self) - end_t.setSingleShot(True) - end_t.timeout.connect(self._finish_run) - end_t.start(850 * (len(self.steps) + 1)) - self._timers.append(end_t) + self.launch_command_preview = QPlainTextEdit() + self.launch_command_preview.setReadOnly(True) + self.smoke_command_preview = QPlainTextEdit() + self.smoke_command_preview.setReadOnly(True) - def _run_step(self, index, name): - # 运行过程演示：让右侧步骤列表看起来像在逐步推进。 - self.current_step_index = index - self.log(f"开始执行：{name}") - for i in range(self.step_list.count()): - original = self.steps[i][1] - prefix = "▶ " if i == index else ("✓ " if i < index else "○ ") - self.step_list.item(i).setText(f"{prefix}{original}") - self.progress.setValue(int(index / max(len(self.steps), 1) * 100)) + report_tab = QWidget() + report_layout = QVBoxLayout(report_tab) + self.report_summary_label = QLabel("暂无报告") + self.stage_table = QTableWidget(0, 5) + self.stage_table.setHorizontalHeaderLabels(["阶段", "成功", "自动验收", "状态", "摘要"]) + self.stage_table.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch) + self.metadata_table = QTableWidget(0, 2) + self.metadata_table.setHorizontalHeaderLabels(["字段", "值"]) + self.metadata_table.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch) + report_layout.addWidget(self.report_summary_label) + report_layout.addWidget(self.stage_table, 2) + report_layout.addWidget(QLabel("报告附加信息")) + report_layout.addWidget(self.metadata_table, 1) + tabs.addTab(report_tab, "报告") - def _finish_run(self): - # 演示整场任务完成后的界面变化。 - self.current_step_index = -1 - self.session_state = "SUCCEEDED" - self.state_label.setText(self.session_state) - self.progress.setValue(100) - self.log("整场标定执行完成。") - for i in range(self.step_list.count()): - self.step_list.item(i).setText(f"✓ {self.steps[i][1]}") + self.log_view = QPlainTextEdit() + self.log_view.setReadOnly(True) + self.log_view.setFont(QFont("Consolas", 10)) + self.log_view.setMinimumHeight(230) + layout.addWidget(QLabel("运行日志")) + layout.addWidget(self.log_view, 1) + return panel - def export_json(self): - # 导出当前 JSON 预览，方便你拿去和后端 / ROS 接口一起对字段。 - payload_text = self.json_preview.toPlainText().strip() - if not payload_text: - QMessageBox.warning(self, "提示", "当前没有可导出的内容。") - return - path, _ = QFileDialog.getSaveFileName(self, "导出 JSON", "workshop_session_config_preview.json", "JSON Files (*.json)") - if not path: - return - with open(path, "w", encoding="utf-8") as f: - f.write(payload_text) - QMessageBox.information(self, "完成", f"已导出到：{path}") + def closeEvent(self, event) -> None: + self.stop_process(self.smoke_process, "标定流程") + self.stop_process(self.launch_process, "现场服务") + if self.localization_spin_timer.isActive(): + self.localization_spin_timer.stop() + if self.localization_node is not None: + try: + self.localization_node.destroy_node() + except Exception: + pass + if rclpy is not None and self.localization_rclpy_initialized and rclpy.ok(): + rclpy.shutdown() + event.accept() - def clear_timers(self): - for t in self._timers: - t.stop() - t.deleteLater() - self._timers.clear() + +def main() -> int: + app = QApplication(sys.argv) + signal.signal(signal.SIGINT, lambda *_: app.quit()) + timer = QTimer() + timer.timeout.connect(lambda: None) + timer.start(200) + window = OperatorMainWindow() + window.show() + return app.exec() if __name__ == "__main__": - app = QApplication([]) - - # 让终端里的 Ctrl+C 可以退出 Qt 程序。 - signal.signal(signal.SIGINT, lambda *args: app.quit()) - - # 让 Python 有机会处理 SIGINT。 - sigint_timer = QTimer() - sigint_timer.start(200) - sigint_timer.timeout.connect(lambda: None) - - w = MainWindow() - w.show() - app.exec() + raise SystemExit(main()) diff --git a/agv_calib_brain/src/apps/operator_ui/process_report.py b/agv_calib_brain/src/apps/operator_ui/process_report.py new file mode 100644 index 0000000..3d4723d --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/process_report.py @@ -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"])) diff --git a/agv_calib_brain/src/apps/operator_ui/profile_handlers.py b/agv_calib_brain/src/apps/operator_ui/profile_handlers.py new file mode 100644 index 0000000..c6a6fac --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/profile_handlers.py @@ -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() diff --git a/agv_calib_brain/src/apps/operator_ui/profile_io.py b/agv_calib_brain/src/apps/operator_ui/profile_io.py new file mode 100644 index 0000000..219c169 --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/profile_io.py @@ -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 diff --git a/agv_calib_brain/src/apps/operator_ui/runtime_monitor.py b/agv_calib_brain/src/apps/operator_ui/runtime_monitor.py new file mode 100644 index 0000000..fef1219 --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/runtime_monitor.py @@ -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() diff --git a/agv_calib_brain/src/apps/operator_ui/session_builder.py b/agv_calib_brain/src/apps/operator_ui/session_builder.py new file mode 100644 index 0000000..4955f1f --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/session_builder.py @@ -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() diff --git a/agv_calib_brain/src/apps/operator_ui/ui_helpers.py b/agv_calib_brain/src/apps/operator_ui/ui_helpers.py new file mode 100644 index 0000000..70a6cc9 --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/ui_helpers.py @@ -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) diff --git a/agv_calib_brain/src/apps/operator_ui/vehicle_profile_dialog.py b/agv_calib_brain/src/apps/operator_ui/vehicle_profile_dialog.py new file mode 100644 index 0000000..67679cd --- /dev/null +++ b/agv_calib_brain/src/apps/operator_ui/vehicle_profile_dialog.py @@ -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() diff --git a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp index e65541c..7b0568c 100644 --- a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/plan_builder.cpp @@ -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; } diff --git a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/report_builder.cpp b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/report_builder.cpp index a77e063..0aee28a 100644 --- a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/report_builder.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/report_builder.cpp @@ -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); diff --git a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp index e6a0b95..42e5482 100644 --- a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp @@ -8,6 +8,7 @@ #include // 拼接 session_id 时要用字符串流。 #include // 文件系统查询失败时用 error_code 接住错误。 #include // 把整场执行放到后台线程时要用 std::thread。 +#include // 执行线程兜底捕获异常，避免节点直接退出。 #include // 保存从 dataset_index.yaml 读取出来的 metadata。 #include "calibration_workshop_orchestration_interfaces/msg/approval_state.hpp" @@ -978,30 +979,46 @@ void WorkshopOrchestratorV2Node::execute_session( const std::shared_ptr 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(); - const auto precheck = run_precheck_step(context.session_id); // 正式开跑前先检查：这条任务现在有没有条件开始跑。 - if (!precheck.ready_for_start) { // 如果检查结论是不允许开跑。 - finish_session_precheck_failed(goal_handle, context, precheck); // 直接按“预检失败”路径收尾并返回。 - return; - } + try { + context = prepare_session_for_run(goal->goal.session_id); // 把这条任务切到“准备开跑”状态，并记录真正开始时间。 - mark_session_running(context.session_id); // 预检通过后，把整条任务切到“正式运行中”。 - - std::string failure_reason; // 如果中途某一步失败，就把失败原因写到这里。 - if (handle_cancel_if_requested(goal_handle, context)) { // 先看外部有没有在正式执行前就要求取消。 - return; // 如果已经取消并收尾，这里直接结束。 - } - - if (!run_all_stages(goal_handle, context, failure_reason)) { // 按顺序执行每一个步骤。 - if (failure_reason.empty()) { // 失败原因为空，通常表示走的是“取消”分支。 + const auto precheck = run_precheck_step(context.session_id); // 正式开跑前先检查：这条任务现在有没有条件开始跑。 + if (!precheck.ready_for_start) { // 如果检查结论是不允许开跑。 + finish_session_precheck_failed(goal_handle, context, precheck); // 直接按“预检失败”路径收尾并返回。 return; } - finish_session_failed(goal_handle, context, failure_reason); // 某一步执行失败时，按失败路径收尾。 + + mark_session_running(context.session_id); // 预检通过后，把整条任务切到“正式运行中”。 + + std::string failure_reason; // 如果中途某一步失败，就把失败原因写到这里。 + if (handle_cancel_if_requested(goal_handle, context)) { // 先看外部有没有在正式执行前就要求取消。 + return; // 如果已经取消并收尾，这里直接结束。 + } + + if (!run_all_stages(goal_handle, context, failure_reason)) { // 按顺序执行每一个步骤。 + if (failure_reason.empty()) { // 失败原因为空，通常表示走的是“取消”分支。 + return; + } + finish_session_failed(goal_handle, context, failure_reason); // 某一步执行失败时，按失败路径收尾。 + return; + } + + 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; } - - finish_session_succeeded(goal_handle, context); // 全部步骤都成功后，按成功路径收尾。 } WorkshopOrchestratorV2Node::SessionExecutionContext WorkshopOrchestratorV2Node::prepare_session_for_run( diff --git a/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml b/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml index fd153eb..5307d09 100644 --- a/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml +++ b/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml @@ -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 diff --git a/agv_calib_brain/src/deployment/profiles/site_template.yaml b/agv_calib_brain/src/deployment/profiles/site_template.yaml index 6965d98..11cb4b2 100644 --- a/agv_calib_brain/src/deployment/profiles/site_template.yaml +++ b/agv_calib_brain/src/deployment/profiles/site_template.yaml @@ -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 diff --git a/agv_calib_brain/src/deployment/profiles/vehicle_profiles/ackermann_default.yaml b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/ackermann_default.yaml new file mode 100644 index 0000000..588d2c4 --- /dev/null +++ b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/ackermann_default.yaml @@ -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 diff --git a/agv_calib_brain/src/deployment/profiles/vehicle_profiles/ackermann_standard_v1.ymal b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/ackermann_standard_v1.ymal new file mode 100644 index 0000000..4a99087 --- /dev/null +++ b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/ackermann_standard_v1.ymal @@ -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' diff --git a/agv_calib_brain/src/deployment/profiles/vehicle_profiles/adad.ymal b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/adad.ymal new file mode 100644 index 0000000..dd44760 --- /dev/null +++ b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/adad.ymal @@ -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' diff --git a/agv_calib_brain/src/deployment/profiles/vehicle_profiles/vehicle_profile.ymal b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/vehicle_profile.ymal new file mode 100644 index 0000000..43533de --- /dev/null +++ b/agv_calib_brain/src/deployment/profiles/vehicle_profiles/vehicle_profile.ymal @@ -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' diff --git a/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py b/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py index 5c571bb..3c8843c 100644 --- a/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py +++ b/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py @@ -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"), diff --git a/agv_calib_brain/src/simulation/tools/smoke_test_workshop_orchestrator.py b/agv_calib_brain/src/simulation/tools/smoke_test_workshop_orchestrator.py index a53ac57..e72b1c6 100644 --- a/agv_calib_brain/src/simulation/tools/smoke_test_workshop_orchestrator.py +++ b/agv_calib_brain/src/simulation/tools/smoke_test_workshop_orchestrator.py @@ -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,14 +985,20 @@ 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) - session_config = make_session_config(tasks, self.args) + 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) self.verify_session_visible(session_id) @@ -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", diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml index 029147a..b6a6133 100644 --- a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml @@ -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 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py index cfadcc5..4d2e3a0 100644 --- a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py @@ -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: