diff --git a/agv_calib_brain/SIMULATION_GUIDE.md b/agv_calib_brain/SIMULATION_GUIDE.md index f41f66b..d08c1d8 100644 --- a/agv_calib_brain/SIMULATION_GUIDE.md +++ b/agv_calib_brain/SIMULATION_GUIDE.md @@ -113,7 +113,7 @@ ros2 topic hz /sensor/lidar_2d/scan ros2 topic hz /sensor/imu/data # 检查外部位姿 -ros2 topic hz /isaac/external_localization/telemetry +ros2 topic hz /isaac/external_localization/vehicle/pose ``` ## 执行端到端验收测试 diff --git a/agv_calib_brain/run_isaac_real_sim_test.sh b/agv_calib_brain/run_isaac_real_sim_test.sh index d99c304..5113f3b 100755 --- a/agv_calib_brain/run_isaac_real_sim_test.sh +++ b/agv_calib_brain/run_isaac_real_sim_test.sh @@ -529,7 +529,7 @@ start_bg "isaac" bash -lc " " if [[ "${WAIT_FOR_ISAAC_TOPICS}" -eq 1 ]]; then - wait_for_ros_topic_once "/isaac/external_localization/telemetry" "${ISAAC_WAIT_SEC}" || exit 1 + wait_for_ros_topic_once "/isaac/external_localization/vehicle/pose" "${ISAAC_WAIT_SEC}" || exit 1 wait_for_ros_topic_once "/sensor/front_camera/image_raw" "${ISAAC_WAIT_SEC}" || exit 1 wait_for_ros_topic_once "/sensor/down_camera/image_raw" "${ISAAC_WAIT_SEC}" || exit 1 wait_for_ros_topic_once "/sensor/lidar_3d/pointcloud" "${ISAAC_WAIT_SEC}" || exit 1 diff --git a/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_control_interfaces/msg/PIDParams.msg b/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_control_interfaces/msg/PIDParams.msg index 4db92be..67640bc 100644 --- a/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_control_interfaces/msg/PIDParams.msg +++ b/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_control_interfaces/msg/PIDParams.msg @@ -1,11 +1,11 @@ # ========================================================= # PID 参数 # 作用:PID 控制器参数 -# 说明:适用于横向或纵向,但需由 control_axis 配合解释 +# 说明:可用于速度、航向、角速度、舵角等回路,具体对象必须由 loop_name 和 control_axis 共同解释。 # 说明:proto optional 在 ROS2 中用 has_xxx + xxx 保真 # ========================================================= -# 回路名,例如 lateral / heading / speed +# 回路名,例如 speed / acceleration / heading / yaw_rate / steering_angle string loop_name # 比例系数 float64 kp diff --git a/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_external_localization_interfaces/proto/external_localization.proto b/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_external_localization_interfaces/proto/external_localization.proto index 069bb81..e79567b 100644 --- a/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_external_localization_interfaces/proto/external_localization.proto +++ b/agv_calib_brain/src/communication/win_ubuntu_bridge/calibration_external_localization_interfaces/proto/external_localization.proto @@ -72,7 +72,7 @@ service AgvCalibExternalLocalizationService { // ========================================================= // 服务:外部真值位姿下发服务 // 运行位置:Ubuntu 车间电脑 / 外部真值系统桥接进程 -// 调用方:Windows 车端代理 +// 调用方:Windows 车端代理可拉流,Ubuntu 车间电脑也可主动推送单帧 // 传输介质:现场通过 WiFi 6 网络承载 // 作用: // 1) 让车端在执行轨迹跟踪时获得车间坐标系下的实时位姿 @@ -263,7 +263,7 @@ message StreamExternalLocalizationTelemetryRequest { } // ========================================================= -// 外部真值位姿下发请求 +// 外部真值位姿拉流请求 // 作用:车端通过 WiFi 6 请求工控机 / 外部真值桥接进程提供实时位姿流 // ========================================================= message ExternalPoseFeedRequest { @@ -277,10 +277,10 @@ message ExternalPoseFeedRequest { } // ========================================================= -// 外部定位遥测 -// 作用:回传外部定位实时观测结果 -// 发送方:Windows 车端代理 / Linux 桥接服务 -// 接收方:Ubuntu 车间电脑 +// 外部定位车辆位姿 +// 作用:表达车间坐标系下的车辆外部定位结果和质量指标 +// ROS 侧发送方:车间外部定位节点;接收方:external_localization_service / 外部位姿桥 +// WiFi6 侧发送方:Ubuntu 外部位姿桥;接收方:Windows 车端代理 // ========================================================= message ExternalLocalizationTelemetry { int64 hardware_timestamp_us = 1; // 硬件时间戳 diff --git a/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/launch/minimal_workshop_demo.launch.py b/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/launch/minimal_workshop_demo.launch.py index 91a659b..e2a8fb0 100644 --- a/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/launch/minimal_workshop_demo.launch.py +++ b/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/launch/minimal_workshop_demo.launch.py @@ -11,6 +11,8 @@ from launch_ros.actions import Node # profile_storage_path:vehicle_profile_manager 的画像持久化文件。 # sensor_storage_root / sensor_registry / capture_pipeline_name / telemetry_topics: # 传感器 readiness 的最小真实配置入口。 +# external_telemetry_topic / expected_reference_source_name / expected_workcell_zone_id: +# 外部真值定位源的现场配置入口。 # data_input_params_file:可选 ROS 参数文件,用于把真实采集/ingest 产物路径注入算法输入。 # dataset_index_file:可选数据集索引文件;为空时主控会按 data_input_params_file 同目录自动查找。 def generate_launch_description(): @@ -72,6 +74,21 @@ def generate_launch_description(): default_value="", description="可选数据集索引文件,用于最终报告追溯本次真实采集数据", ) + external_telemetry_topic_arg = DeclareLaunchArgument( + "external_telemetry_topic", + default_value="/isaac/external_localization/vehicle/pose", + description="external_localization_service 订阅的外部定位遥测 topic", + ) + expected_reference_source_name_arg = DeclareLaunchArgument( + "expected_reference_source_name", + default_value="isaac_sim_truth_source", + description="external_localization_service 期望的外部定位源名称", + ) + expected_workcell_zone_id_arg = DeclareLaunchArgument( + "expected_workcell_zone_id", + default_value="", + description="external_localization_service 期望的工位区域 ID,留空表示不校验", + ) def make_nodes(context): use_gateway = LaunchConfiguration("use_gateway").perform(context).lower() == "true" @@ -86,6 +103,9 @@ def generate_launch_description(): arm_ready = LaunchConfiguration("arm_ready").perform(context).lower() == "true" data_input_params_file = LaunchConfiguration("data_input_params_file").perform(context).strip() dataset_index_file = LaunchConfiguration("dataset_index_file").perform(context).strip() + external_telemetry_topic = LaunchConfiguration("external_telemetry_topic").perform(context).strip() + expected_reference_source_name = LaunchConfiguration("expected_reference_source_name").perform(context).strip() + expected_workcell_zone_id = LaunchConfiguration("expected_workcell_zone_id").perform(context).strip() def with_data_input_params(params=None): merged = [] @@ -110,7 +130,11 @@ def generate_launch_description(): executable="external_localization_service_node", name="external_localization_service", output="screen", - parameters=with_data_input_params(), + parameters=with_data_input_params({ + "external_telemetry_topic": external_telemetry_topic, + "expected_reference_source_name": expected_reference_source_name, + "expected_workcell_zone_id": expected_workcell_zone_id, + }), ), Node( package="sensor_calibration_service", @@ -189,5 +213,8 @@ def generate_launch_description(): arm_ready_arg, data_input_params_file_arg, dataset_index_file_arg, + external_telemetry_topic_arg, + expected_reference_source_name_arg, + expected_workcell_zone_id_arg, OpaqueFunction(function=make_nodes), ]) diff --git a/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/proto/external_localization.proto b/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/proto/external_localization.proto index 069bb81..e79567b 100644 --- a/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/proto/external_localization.proto +++ b/agv_calib_brain/src/communication/win_ubuntu_bridge/win_ubuntu_bridge/proto/external_localization.proto @@ -72,7 +72,7 @@ service AgvCalibExternalLocalizationService { // ========================================================= // 服务:外部真值位姿下发服务 // 运行位置:Ubuntu 车间电脑 / 外部真值系统桥接进程 -// 调用方:Windows 车端代理 +// 调用方:Windows 车端代理可拉流,Ubuntu 车间电脑也可主动推送单帧 // 传输介质:现场通过 WiFi 6 网络承载 // 作用: // 1) 让车端在执行轨迹跟踪时获得车间坐标系下的实时位姿 @@ -263,7 +263,7 @@ message StreamExternalLocalizationTelemetryRequest { } // ========================================================= -// 外部真值位姿下发请求 +// 外部真值位姿拉流请求 // 作用:车端通过 WiFi 6 请求工控机 / 外部真值桥接进程提供实时位姿流 // ========================================================= message ExternalPoseFeedRequest { @@ -277,10 +277,10 @@ message ExternalPoseFeedRequest { } // ========================================================= -// 外部定位遥测 -// 作用:回传外部定位实时观测结果 -// 发送方:Windows 车端代理 / Linux 桥接服务 -// 接收方:Ubuntu 车间电脑 +// 外部定位车辆位姿 +// 作用:表达车间坐标系下的车辆外部定位结果和质量指标 +// ROS 侧发送方:车间外部定位节点;接收方:external_localization_service / 外部位姿桥 +// WiFi6 侧发送方:Ubuntu 外部位姿桥;接收方:Windows 车端代理 // ========================================================= message ExternalLocalizationTelemetry { int64 hardware_timestamp_us = 1; // 硬件时间戳 diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp index c9bf6e4..96d8d12 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp @@ -111,7 +111,7 @@ struct ChassisCalibrationInput // 底盘原始运动数据文件:例如 wheel/steering/odom/driver_state CSV、JSON 或 rosbag。 std::vector chassis_motion_data_files; - // 控制命令与执行器反馈文件:例如 cmd_vel、Ackermann 命令、驱动器反馈、制动状态。 + // 控制命令与执行器反馈文件:例如 cmd_vel、阿克曼命令、驱动器反馈、制动状态。 std::vector actuator_command_files; // 外部真值轨迹文件:例如 external pose 轨迹、地面真值、测量轨迹,用于和底盘里程计对齐。 diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_common.hpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_common.hpp index 22536b2..e6bca9c 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_common.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_common.hpp @@ -1,6 +1,7 @@ #pragma once #include "calibration_common_interfaces/msg/error_code.hpp" +#include "calibration_chassis_interfaces/msg/chassis_specific_params_type.hpp" #include "chassis_calibration_service/chassis_calibration_algorithms.hpp" namespace chassis_calibration_service @@ -27,4 +28,11 @@ inline void fill_common_result( output.response.result.estimated_params.chassis_type = input.chassis_type; } +inline void select_specific_params( + ChassisCalibrationOutput & output, + uint8_t selected_specific_params) +{ + output.response.result.estimated_params.selected_specific_params.value = selected_specific_params; +} + } // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/package.xml b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/package.xml index d8c5823..fdb0fee 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/package.xml +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/package.xml @@ -2,7 +2,7 @@ chassis_calibration_service 0.0.1 - Minimal chassis calibration service stub. + 底盘标定服务模板,提供底盘类型分发、数据输入和结果回填边界。 you Apache-2.0 diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp index f1707d7..cc91e88 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp @@ -10,45 +10,18 @@ bool AckermannChassisAlgorithm::run( { (void)failure_reason; - // 阿克曼底盘标定模板: - // 【本文件负责什么】 - // - 负责阿克曼底盘相关的标定算法实现。 - // - 后续算法工程师应主要修改本文件,不要改 node 层和模板分发层。 - // - // 【建议优先读取的输入】 - // 1. input.request - // - request_id、task_purpose、selected_primitive、straight_line / steering_sweep 等任务参数。 - // 2. input.vehicle_profile / input.chassis_type - // - 车辆静态画像、底盘类型、基础结构参数。 - // 3. input.applied_chassis_parameters - // - 当前已生效底盘参数,作为本轮优化的初始值或对照基线。 - // 4. input.latest_chassis_telemetry / input.chassis_telemetry_history - // - 轮速、舵角、角速度、里程计、电机状态、电流、温度、驱动诊断等。 - // 5. input.latest_external_localization_telemetry / input.external_localization_telemetry_history - // - 如果需要真值轨迹、真值航向、真值速度,可从这里读取。 - // 6. input.latest_control_telemetry / input.control_telemetry_history - // - 如果需要分析控制输出与底盘响应差异,可结合控制域反馈。 - // - // 【必写输出】 - // 1. output.response.result.validation_summary - // - 写横向误差、航向误差、重复性、曲率误差、模块一致性等摘要指标。 - // 2. output.response.result.estimated_params - // - 写本轮求解出的阿克曼底盘参数。 - // 3. output.response.result.suitable_for_commit - // - 明确是否建议进入提交环节。 - // - // 【可选输出】 - // - output.response.result.artifacts - // 可挂轨迹图、拟合日志、误差统计 csv、调试报告等。 - // - // 【常见失败原因】 - // - 真值源不可用、车辆未进入可移动状态、有效轨迹长度不足、舵角反馈异常、轮速反馈缺失。 - // - // 【在这里添加真实算法】 - // - 请在 fill_common_result(...) 之前或之后补充真实求解逻辑。 - // - 当前文件仅提供交付模板,不包含真实标定算法。 + // 阿克曼底盘算法填充边界: + // - 输入优先读取三类 CSV:chassis_motion_data_files、actuator_command_files、truth_trajectory_files。 + // - 输出必须填写 selected_specific_params=ACKERMANN,并写入 ackermann 专属参数和 validation_summary。 + // - 详细字段合同见 src/docs/chassis_calibration_algorithm_contract.md。 fill_common_result(input, output); - output.response.result.message = "ackermann chassis template executed."; + select_specific_params( + output, + calibration_chassis_interfaces::msg::ChassisSpecificParamsType::ACKERMANN); + output.response.result.data_quality_passed = false; + output.response.result.suitable_for_commit = false; + output.response.result.validation_summary.auto_acceptance_passed = false; + output.response.result.message = "阿克曼底盘标定模板已执行;当前仅验证接口和数据输入,真实算法待填充。"; output.response.result.recommended_parameter_version = "ackermann_template_v1"; return true; } diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp index 869fa92..05b05e4 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp @@ -1,8 +1,171 @@ #include "chassis_calibration_service/chassis_calibration_algorithm_template.hpp" +#include + namespace chassis_calibration_service { +namespace +{ + +using calibration_chassis_interfaces::msg::ChassisMotionPrimitiveType; + +bool is_positive(double value) +{ + return std::isfinite(value) && value > 0.0; +} + +bool is_non_zero(double value) +{ + return std::isfinite(value) && std::abs(value) > 0.0; +} + +bool validate_straight_line( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (!is_positive(request.goal.straight_line.target_distance_m)) { + failure_reason = "straight_line.target_distance_m 必须大于 0。"; + return false; + } + if (!is_positive(request.goal.straight_line.target_speed_ms)) { + failure_reason = "straight_line.target_speed_ms 必须大于 0。"; + return false; + } + return true; +} + +bool validate_arc( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (!is_positive(request.goal.arc.target_speed_ms)) { + failure_reason = "arc.target_speed_ms 必须大于 0。"; + return false; + } + if (!is_positive(request.goal.arc.radius_m)) { + failure_reason = "arc.radius_m 必须大于 0。"; + return false; + } + if (!is_non_zero(request.goal.arc.sweep_angle_deg)) { + failure_reason = "arc.sweep_angle_deg 不能为 0。"; + return false; + } + return true; +} + +bool validate_in_place_rotation( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (!is_non_zero(request.goal.in_place_rotation.target_yaw_deg)) { + failure_reason = "in_place_rotation.target_yaw_deg 不能为 0。"; + return false; + } + if (!is_positive(request.goal.in_place_rotation.target_angular_vel_deg_s)) { + failure_reason = "in_place_rotation.target_angular_vel_deg_s 必须大于 0。"; + return false; + } + return true; +} + +bool validate_steering_sweep( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (!std::isfinite(request.goal.steering_sweep.target_angle_deg)) { + failure_reason = "steering_sweep.target_angle_deg 必须是有效数字。"; + return false; + } + if (!is_positive(request.goal.steering_sweep.sweep_amplitude_deg)) { + failure_reason = "steering_sweep.sweep_amplitude_deg 必须大于 0。"; + return false; + } + if (!is_positive(request.goal.steering_sweep.sweep_frequency_hz)) { + failure_reason = "steering_sweep.sweep_frequency_hz 必须大于 0。"; + return false; + } + if (!is_positive(request.goal.steering_sweep.duration_sec)) { + failure_reason = "steering_sweep.duration_sec 必须大于 0。"; + return false; + } + return true; +} + +bool validate_lateral_translation( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (!is_positive(request.goal.lateral_translation.target_speed_ms)) { + failure_reason = "lateral_translation.target_speed_ms 必须大于 0。"; + return false; + } + if (!is_positive(request.goal.lateral_translation.target_distance_m)) { + failure_reason = "lateral_translation.target_distance_m 必须大于 0。"; + return false; + } + return true; +} + +bool validate_diagonal_motion( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (!is_positive(request.goal.diagonal_motion.target_speed_ms)) { + failure_reason = "diagonal_motion.target_speed_ms 必须大于 0。"; + return false; + } + if (!is_positive(request.goal.diagonal_motion.target_distance_m)) { + failure_reason = "diagonal_motion.target_distance_m 必须大于 0。"; + return false; + } + if (!std::isfinite(request.goal.diagonal_motion.heading_deg)) { + failure_reason = "diagonal_motion.heading_deg 必须是有效数字。"; + return false; + } + return true; +} + +bool validate_module_alignment( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (request.goal.module_alignment.module_ids.empty()) { + failure_reason = "module_alignment.module_ids 不能为空。"; + return false; + } + if (!std::isfinite(request.goal.module_alignment.target_zero_deg)) { + failure_reason = "module_alignment.target_zero_deg 必须是有效数字。"; + return false; + } + if (!is_positive(request.goal.module_alignment.tolerance_deg)) { + failure_reason = "module_alignment.tolerance_deg 必须大于 0。"; + return false; + } + return true; +} + +bool validate_coordinated_steering( + const ExecuteTask::Goal & request, + std::string & failure_reason) +{ + if (request.goal.coordinated_steering.module_ids.empty()) { + failure_reason = "coordinated_steering.module_ids 不能为空。"; + return false; + } + if (!std::isfinite(request.goal.coordinated_steering.target_angle_deg)) { + failure_reason = "coordinated_steering.target_angle_deg 必须是有效数字。"; + return false; + } + if (!is_positive(request.goal.coordinated_steering.hold_time_sec)) { + failure_reason = "coordinated_steering.hold_time_sec 必须大于 0。"; + return false; + } + return true; +} + +} // namespace + bool ChassisCalibrationAlgorithmTemplate::validate_input( const Input & input, std::string & failure_reason) const @@ -13,26 +176,36 @@ bool ChassisCalibrationAlgorithmTemplate::validate_input( } if (input.chassis_type.value == ChassisType::CHASSIS_TYPE_UNSPECIFIED) { - failure_reason = "底盘类型不能为空,必须明确是阿克曼、差速、单舵轮或多舵轮。"; + failure_reason = "底盘类型不能为空,必须明确是阿克曼、差速轮、单舵轮或多舵轮。"; return false; } - if (input.request.goal.selected_primitive.value != - calibration_chassis_interfaces::msg::ChassisMotionPrimitiveType::STRAIGHT_LINE) { - failure_reason = "当前模板只演示 straight_line 动作原语;如果要支持转弯、原地旋转、横移等原语,需要在这里扩展分发规则。"; + if (!is_positive(input.request.goal.timeout_sec)) { + failure_reason = "timeout_sec 必须大于 0。"; return false; } - if (input.request.goal.straight_line.target_distance_m <= 0.0) { - failure_reason = "straight_line.target_distance_m 必须大于 0。"; - return false; + switch (input.request.goal.selected_primitive.value) { + case ChassisMotionPrimitiveType::STRAIGHT_LINE: + return validate_straight_line(input.request, failure_reason); + case ChassisMotionPrimitiveType::ARC: + return validate_arc(input.request, failure_reason); + case ChassisMotionPrimitiveType::IN_PLACE_ROTATION: + return validate_in_place_rotation(input.request, failure_reason); + case ChassisMotionPrimitiveType::STEERING_SWEEP: + return validate_steering_sweep(input.request, failure_reason); + case ChassisMotionPrimitiveType::LATERAL_TRANSLATION: + return validate_lateral_translation(input.request, failure_reason); + case ChassisMotionPrimitiveType::DIAGONAL_MOTION: + return validate_diagonal_motion(input.request, failure_reason); + case ChassisMotionPrimitiveType::MODULE_ALIGNMENT: + return validate_module_alignment(input.request, failure_reason); + case ChassisMotionPrimitiveType::COORDINATED_STEERING: + return validate_coordinated_steering(input.request, failure_reason); + default: + failure_reason = "底盘动作原语类型不能为空,且必须是已支持的动作原语。"; + return false; } - if (input.request.goal.straight_line.target_speed_ms <= 0.0) { - failure_reason = "straight_line.target_speed_ms 必须大于 0。"; - return false; - } - - return true; } const ChassisCalibrationAlgorithm * ChassisCalibrationAlgorithmTemplate::resolve_algorithm( diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithms.cpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithms.cpp deleted file mode 100644 index 0d78d92..0000000 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithms.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "chassis_calibration_service/chassis_calibration_algorithms.hpp" - -#include "calibration_common_interfaces/msg/error_code.hpp" - -namespace chassis_calibration_service -{ - -namespace -{ -void fill_common_result( - const ChassisCalibrationInput & input, - ChassisCalibrationOutput & output) -{ - output.response.result.success = true; - output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK; - output.response.result.job_id = input.request.goal.header.request_id; - output.response.result.data_quality_passed = true; - output.response.result.suitable_for_commit = true; - output.response.result.estimated_straight_line_bias = 0.0; - output.response.result.validation_summary.max_lateral_error_m = 0.0; - output.response.result.validation_summary.max_yaw_error_rad = 0.0; - output.response.result.validation_summary.rms_lateral_error_m = 0.0; - output.response.result.validation_summary.rms_yaw_error_rad = 0.0; - output.response.result.validation_summary.repeatability_error_m = 0.0; - output.response.result.validation_summary.curvature_error = 0.0; - output.response.result.validation_summary.module_consistency_error = 0.0; - output.response.result.validation_summary.auto_acceptance_passed = true; - output.response.result.estimated_params.chassis_type = input.chassis_type; -} -} - -bool AckermannChassisAlgorithm::run( - const ChassisCalibrationInput & input, - ChassisCalibrationOutput & output, - std::string & failure_reason) const -{ - (void)failure_reason; - fill_common_result(input, output); - output.response.result.message = "ackermann chassis template executed."; - output.response.result.recommended_parameter_version = "ackermann_template_v1"; - return true; -} - -bool DifferentialChassisAlgorithm::run( - const ChassisCalibrationInput & input, - ChassisCalibrationOutput & output, - std::string & failure_reason) const -{ - (void)failure_reason; - fill_common_result(input, output); - output.response.result.message = "differential chassis template executed."; - output.response.result.recommended_parameter_version = "differential_template_v1"; - return true; -} - -bool SingleSteerWheelChassisAlgorithm::run( - const ChassisCalibrationInput & input, - ChassisCalibrationOutput & output, - std::string & failure_reason) const -{ - (void)failure_reason; - fill_common_result(input, output); - output.response.result.message = "single steer wheel chassis template executed."; - output.response.result.recommended_parameter_version = "single_steer_template_v1"; - return true; -} - -bool MultiSteerWheelChassisAlgorithm::run( - const ChassisCalibrationInput & input, - ChassisCalibrationOutput & output, - std::string & failure_reason) const -{ - (void)failure_reason; - fill_common_result(input, output); - output.response.result.message = "multi steer wheel chassis template executed."; - output.response.result.recommended_parameter_version = "multi_steer_template_v1"; - return true; -} - -} // namespace chassis_calibration_service diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp index e8742d8..af38361 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp @@ -1,7 +1,5 @@ #include "chassis_calibration_service/chassis_calibration_common.hpp" -#include "calibration_chassis_interfaces/msg/chassis_specific_params_type.hpp" - namespace chassis_calibration_service { @@ -10,66 +8,22 @@ bool DifferentialChassisAlgorithm::run( ChassisCalibrationOutput & output, std::string & failure_reason) const { - // 差速底盘标定逻辑(轮径与轴距标定模拟): - // 1. 检查底盘反馈数据 - if (input.chassis_telemetry_history.empty()) { - failure_reason = "没有底盘遥测数据,无法标定轮径。"; - return false; - } + (void)failure_reason; - // 2. 标定求解(模拟): - // 基于外部定位计算的真实里程与编码器累计里程的比值,计算修正后的轮径 - double encoder_distance = 0.0; - double truth_distance = 0.0; - size_t valid_points = 0; - - for (const auto & s : input.chassis_telemetry_history) { - if (s.linear_velocity_ms > 0.1) { - encoder_distance += s.linear_velocity_ms * 0.01; // 假设采样周期 10ms - valid_points++; - } - } - - if (valid_points < 100) { - failure_reason = "运动数据点过少 (当前: " + std::to_string(valid_points) + "),无法标定轮径。"; - return false; - } - - // 假设外部真值测得的距离比编码器测得的短 2% (说明标称轮径偏大) - truth_distance = encoder_distance * 0.98; - - // 3. 填充结果 + // 差速轮底盘算法填充边界: + // - 输入优先读取三类 CSV:chassis_motion_data_files、actuator_command_files、truth_trajectory_files。 + // - 已确认目标:left_wheel_radius_m、right_wheel_radius_m、axle_track_width_m。 + // - 输出必须填写 selected_specific_params=DIFFERENTIAL,并写入 validation_summary。 + // - 详细字段合同见 src/docs/chassis_calibration_algorithm_contract.md。 fill_common_result(input, output); - - auto & res = output.response.result; - res.message = "差速底盘标定成功,已计算轮径修正系数。"; - res.recommended_parameter_version = "diff_chassis_v1.1"; - - auto & params = res.estimated_params; - params.chassis_type.value = calibration_vehicle_profile_interfaces::msg::ChassisType::DIFFERENTIAL; - params.selected_specific_params.value = - calibration_chassis_interfaces::msg::ChassisSpecificParamsType::DIFFERENTIAL; - params.common.has_longitudinal_scale = true; - params.common.longitudinal_scale = truth_distance / encoder_distance; - params.common.has_effective_track_width_m = true; - params.common.effective_track_width_m = 0.65; - params.differential.has_left_wheel_radius_m = true; - params.differential.left_wheel_radius_m = 0.125 * 0.98; // 原始 125mm - params.differential.has_right_wheel_radius_m = true; - params.differential.right_wheel_radius_m = 0.125 * 0.98; - params.differential.has_axle_track_width_m = true; - params.differential.axle_track_width_m = 0.65; // 轮距保持标称值 - - // 4. 评估标定质量 - res.validation_summary.max_lateral_error_m = 0.015; - res.validation_summary.max_yaw_error_rad = 0.005; - res.validation_summary.rms_lateral_error_m = 0.005; - res.validation_summary.rms_yaw_error_rad = 0.002; - res.validation_summary.repeatability_error_m = 0.005; - res.validation_summary.curvature_error = 0.0; - res.validation_summary.module_consistency_error = 0.0; - res.validation_summary.auto_acceptance_passed = true; - + select_specific_params( + output, + calibration_chassis_interfaces::msg::ChassisSpecificParamsType::DIFFERENTIAL); + output.response.result.data_quality_passed = false; + output.response.result.suitable_for_commit = false; + output.response.result.validation_summary.auto_acceptance_passed = false; + output.response.result.message = "差速底盘标定模板已执行;当前仅验证接口和数据输入,真实算法待填充。"; + output.response.result.recommended_parameter_version = "differential_template_v1"; return true; } diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp index bfef9e8..9e6d1b6 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp @@ -10,13 +10,18 @@ bool MultiSteerWheelChassisAlgorithm::run( { (void)failure_reason; - // TODO: 在这里填写多舵轮底盘标定算法。 - // 算法工程师应从这里读取并计算: - // - 任务请求:input.request(request_id、任务目的、selected_primitive、straight_line 参数、timeout 等) - // - 底盘反馈:由上层扩展到 ChassisCalibrationInput 中的实时数据(各转向模块角度、轮速、电机反馈、IMU、定位等) - // - 输出:output.response.result(success、error_code、validation_summary、estimated_params、artifacts 等) + // 多舵轮底盘算法填充边界: + // - 输入优先读取三类 CSV:chassis_motion_data_files、actuator_command_files、truth_trajectory_files。 + // - 输出必须填写 selected_specific_params=MULTI_STEER,并写入各模块参数和 validation_summary。 + // - 详细字段合同见 src/docs/chassis_calibration_algorithm_contract.md。 fill_common_result(input, output); - output.response.result.message = "multi steer wheel chassis template executed."; + select_specific_params( + output, + calibration_chassis_interfaces::msg::ChassisSpecificParamsType::MULTI_STEER); + output.response.result.data_quality_passed = false; + output.response.result.suitable_for_commit = false; + output.response.result.validation_summary.auto_acceptance_passed = false; + output.response.result.message = "多舵轮底盘标定模板已执行;当前仅验证接口和数据输入,真实算法待填充。"; output.response.result.recommended_parameter_version = "multi_steer_template_v1"; return true; } diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp index 1f3318b..9edcee5 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp @@ -10,13 +10,18 @@ bool SingleSteerWheelChassisAlgorithm::run( { (void)failure_reason; - // TODO: 在这里填写单舵轮底盘标定算法。 - // 算法工程师应从这里读取并计算: - // - 任务请求:input.request(request_id、任务目的、selected_primitive、straight_line 参数、timeout 等) - // - 底盘反馈:由上层扩展到 ChassisCalibrationInput 中的实时数据(舵角反馈、电机反馈、轮速、IMU、定位等) - // - 输出:output.response.result(success、error_code、validation_summary、estimated_params、artifacts 等) + // 单舵轮底盘算法填充边界: + // - 输入优先读取三类 CSV:chassis_motion_data_files、actuator_command_files、truth_trajectory_files。 + // - 输出必须填写 selected_specific_params=SINGLE_STEER,并写入 single_steer 专属参数和 validation_summary。 + // - 详细字段合同见 src/docs/chassis_calibration_algorithm_contract.md。 fill_common_result(input, output); - output.response.result.message = "single steer wheel chassis template executed."; + select_specific_params( + output, + calibration_chassis_interfaces::msg::ChassisSpecificParamsType::SINGLE_STEER); + output.response.result.data_quality_passed = false; + output.response.result.suitable_for_commit = false; + output.response.result.validation_summary.auto_acceptance_passed = false; + output.response.result.message = "单舵轮底盘标定模板已执行;当前仅验证接口和数据输入,真实算法待填充。"; output.response.result.recommended_parameter_version = "single_steer_template_v1"; return true; } diff --git a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_common.hpp b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_common.hpp index 99e7dc1..33bbbe6 100644 --- a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_common.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_common.hpp @@ -7,11 +7,12 @@ namespace control_calibration_service { -// 统一填充一份最小可运行的默认结果。 +// 统一填充一份最小可运行但不可自动提交的默认结果。 // 作用: // 1. 让模板文件在尚未接入真实算法时也能稳定返回一份结构完整的结果; // 2. 让算法工程师明确最终结果要写入哪些字段; -// 3. 后续真实算法接入时,可以保留这层公共默认值,再按算法结果覆盖具体字段。 +// 3. 避免占位模板在真实现场被误判成“可写入车辆”的参数; +// 4. 后续真实算法接入时,可以保留这层公共默认值,再按算法结果覆盖具体字段。 inline void fill_common_result( const ControlCalibrationInput & input, ControlCalibrationOutput & output) @@ -20,9 +21,9 @@ inline void fill_common_result( output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK; output.response.result.message = "control calibration template executed."; output.response.result.job_id = input.request.goal.header.request_id; - output.response.result.data_quality_passed = true; - output.response.result.suitable_for_commit = true; - output.response.result.recommended_parameter_version = "demo_control_template_v1"; + output.response.result.data_quality_passed = false; + output.response.result.suitable_for_commit = false; + output.response.result.recommended_parameter_version = "template_not_commit_ready"; output.response.result.validation_summary.rms_lateral_error_m = 0.0; output.response.result.validation_summary.rms_heading_error_rad = 0.0; output.response.result.validation_summary.rms_speed_error_ms = 0.0; @@ -31,7 +32,7 @@ inline void fill_common_result( output.response.result.validation_summary.stop_position_error_m = 0.0; output.response.result.validation_summary.max_jerk = 0.0; output.response.result.validation_summary.saturation_ratio = 0.0; - output.response.result.validation_summary.auto_acceptance_passed = true; + output.response.result.validation_summary.auto_acceptance_passed = false; } } // namespace control_calibration_service diff --git a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp index f3cb5a4..5b11207 100644 --- a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp @@ -13,11 +13,12 @@ bool PidControlCalibrationAlgorithm::run( // PID 控制标定模板: // 【本文件负责什么】 // - 负责 PID 控制器相关的标定/评估算法实现。 + // - PID 可以用于速度、航向、角速度、舵角等不同回路,必须通过 loop_name 和 control_axis 明确控制对象。 // - 后续算法工程师应主要修改本文件,不要改 node 层和模板分发层。 // // 【建议优先读取的输入】 // 1. input.task_type / input.velocity_step_task / input.acceleration_deceleration_task - // - 当前任务到底是速度阶跃、加减速、轨迹跟踪还是停车精度评估。 + // - 当前任务到底是速度阶跃、加减速、航向跟踪、角速度跟踪还是舵角内环评估。 // 2. input.reference_target_velocity_ms / input.reference_hold_time_sec / input.reference_timeout_sec // - 常用参考量已被上层展开,便于直接读取。 // 3. input.active_controller_parameters / input.candidate_parameter_set @@ -42,7 +43,7 @@ bool PidControlCalibrationAlgorithm::run( // 可挂日志、波形图、调参报告、误差统计 csv 等。 // // 【常见失败原因】 - // - 参考轨迹缺失、有效控制窗口不足、速度反馈异常、底盘执行器受限、真值源不可用。 + // - 有效控制窗口不足、速度/航向/舵角反馈异常、底盘执行器受限、真值源不可用。 // // 【在这里添加真实算法】 // - 请在 fill_common_result(...) 之前或之后补充真实 PID 求解与评估逻辑。 diff --git a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp index ca429bb..6d2184e 100644 --- a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp @@ -77,7 +77,7 @@ ExternalLocalizationServiceNode::ReadinessConfig ExternalLocalizationServiceNode { ReadinessConfig config; config.telemetry_topic = declare_parameter( - "external_telemetry_topic", "/isaac/external_localization/telemetry"); + "external_telemetry_topic", "/isaac/external_localization/vehicle/pose"); config.expected_reference_source_name = declare_parameter( "expected_reference_source_name", "isaac_sim_truth_source"); config.expected_workcell_zone_id = declare_parameter( @@ -182,11 +182,11 @@ void ExternalLocalizationServiceNode::populate_readiness_from_snapshot( readiness.agent_ready = false; readiness.ready_for_reference_validation = false; readiness.error_code.code = ErrorCode::NOT_READY; - readiness.message = "尚未收到 external 仿真遥测。"; + readiness.message = "尚未收到 external 定位遥测。"; append_issue( readiness.validation_summary, "external_telemetry_missing", - "尚未收到 external 仿真遥测消息。", + "尚未收到 external 定位遥测消息。", ErrorCode::NOT_READY, true, readiness_config_.telemetry_topic); @@ -206,11 +206,11 @@ void ExternalLocalizationServiceNode::populate_readiness_from_snapshot( readiness.agent_ready = false; readiness.success = false; readiness.error_code.code = ErrorCode::TIMEOUT; - readiness.message = "external 仿真遥测超时。"; + readiness.message = "external 定位遥测超时。"; append_issue( readiness.validation_summary, "external_telemetry_timeout", - "external 仿真遥测长时间未更新。", + "external 定位遥测长时间未更新。", ErrorCode::TIMEOUT, true, readiness_config_.telemetry_topic); @@ -223,11 +223,11 @@ void ExternalLocalizationServiceNode::populate_readiness_from_snapshot( readiness.ready_for_reference_validation = false; readiness.success = false; readiness.error_code.code = ErrorCode::DATA_QUALITY_INSUFFICIENT; - readiness.message = "external 仿真遥测位姿无效。"; + readiness.message = "external 定位遥测位姿无效。"; append_issue( readiness.validation_summary, "external_pose_invalid", - "最新 external 仿真遥测位姿无效。", + "最新 external 定位遥测位姿无效。", ErrorCode::DATA_QUALITY_INSUFFICIENT, true, latest.reference_source_name); diff --git a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp index 327ca18..53f6f65 100644 --- a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/types.hpp @@ -124,6 +124,16 @@ inline constexpr const char * CONTROL_TASK_TYPE = "control.task_type"; inline constexpr const char * CONTROL_TRAJECTORY_PREFIX = "traj_pt_"; inline constexpr const char * CONTROL_STOP_AT_END = "control.stop_at_end"; inline constexpr const char * CONTROL_TIMEOUT_SEC = "control.timeout_sec"; +inline constexpr const char * CONTROL_REQUIRED_EXTERNAL_POSE_SOURCE_ID = + "trajectory_tracking.required_external_pose_source_id"; +inline constexpr const char * CONTROL_MAX_EXTERNAL_POSE_AGE_MS = + "trajectory_tracking.max_external_pose_age_ms"; +inline constexpr const char * CONTROL_MIN_EXTERNAL_POSE_QUALITY_SCORE = + "trajectory_tracking.min_external_pose_quality_score"; +inline constexpr const char * CONTROL_TRAJECTORY_ID = "trajectory_tracking.trajectory_id"; +inline constexpr const char * CONTROL_TRAJECTORY_SEGMENT_INDEX = "trajectory_tracking.segment_index"; +inline constexpr const char * CONTROL_TRAJECTORY_TOTAL_SEGMENTS = "trajectory_tracking.total_segments"; +inline constexpr const char * CONTROL_TRAJECTORY_IS_FINAL_SEGMENT = "trajectory_tracking.is_final_segment"; inline constexpr const char * CONTROL_VELOCITY_STEP_TARGET_VELOCITY_MS = "velocity_step.target_velocity_ms"; inline constexpr const char * CONTROL_VELOCITY_STEP_HOLD_TIME_SEC = "velocity_step.hold_time_sec"; inline constexpr const char * CONTROL_VELOCITY_STEP_SETTLE_BEFORE_STEP_SEC = "velocity_step.settle_before_step_sec"; diff --git a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp index f44a115..70644ff 100644 --- a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/control_gateway_client.cpp @@ -23,6 +23,74 @@ using calibration_workshop_orchestration_interfaces::msg::StagePlan; using calibration_workshop_orchestration_interfaces::msg::StageResultSummary; using calibration_workshop_orchestration_interfaces::msg::WorkshopSession; +bool parse_metadata_bool(const std::string & value, bool & parsed) +{ + if (value == "true" || value == "1" || value == "TRUE" || value == "True") { + parsed = true; + return true; + } + if (value == "false" || value == "0" || value == "FALSE" || value == "False") { + parsed = false; + return true; + } + return false; +} + +bool read_optional_double( + const std::unordered_map & metadata_map, + const std::string & key, + double & target, + std::string & failure_reason) +{ + const auto it = metadata_map.find(key); + if (it == metadata_map.end()) { + return true; + } + try { + target = std::stod(it->second); + return true; + } catch (...) { + failure_reason = key + " 格式错误。"; + return false; + } +} + +bool read_optional_uint32( + const std::unordered_map & metadata_map, + const std::string & key, + uint32_t & target, + std::string & failure_reason) +{ + const auto it = metadata_map.find(key); + if (it == metadata_map.end()) { + return true; + } + try { + target = static_cast(std::stoul(it->second)); + return true; + } catch (...) { + failure_reason = key + " 格式错误。"; + return false; + } +} + +bool read_optional_bool( + const std::unordered_map & metadata_map, + const std::string & key, + bool & target, + std::string & failure_reason) +{ + const auto it = metadata_map.find(key); + if (it == metadata_map.end()) { + return true; + } + if (parse_metadata_bool(it->second, target)) { + return true; + } + failure_reason = key + " 格式错误。"; + return false; +} + ControlGatewayClient::ControlGatewayClient(rclcpp::Node * node) : node_(node) { @@ -174,6 +242,43 @@ bool ControlGatewayClient::build_goal( } } + const auto pose_source_it = metadata_map.find(metadata_keys::CONTROL_REQUIRED_EXTERNAL_POSE_SOURCE_ID); + if (pose_source_it != metadata_map.end()) { + goal.goal.trajectory_tracking.required_external_pose_source_id = pose_source_it->second; + } + const auto trajectory_id_it = metadata_map.find(metadata_keys::CONTROL_TRAJECTORY_ID); + if (trajectory_id_it != metadata_map.end()) { + goal.goal.trajectory_tracking.trajectory_id = trajectory_id_it->second; + } + if (!read_optional_double( + metadata_map, + metadata_keys::CONTROL_MAX_EXTERNAL_POSE_AGE_MS, + goal.goal.trajectory_tracking.max_external_pose_age_ms, + failure_reason) || + !read_optional_double( + metadata_map, + metadata_keys::CONTROL_MIN_EXTERNAL_POSE_QUALITY_SCORE, + goal.goal.trajectory_tracking.min_external_pose_quality_score, + failure_reason) || + !read_optional_uint32( + metadata_map, + metadata_keys::CONTROL_TRAJECTORY_SEGMENT_INDEX, + goal.goal.trajectory_tracking.segment_index, + failure_reason) || + !read_optional_uint32( + metadata_map, + metadata_keys::CONTROL_TRAJECTORY_TOTAL_SEGMENTS, + goal.goal.trajectory_tracking.total_segments, + failure_reason) || + !read_optional_bool( + metadata_map, + metadata_keys::CONTROL_TRAJECTORY_IS_FINAL_SEGMENT, + goal.goal.trajectory_tracking.is_final_segment, + failure_reason)) + { + return false; + } + // 轨迹必须由流程配置显式提供;编排器不再隐式生成默认轨迹。 goal.goal.trajectory_tracking.path = build_trajectory_from_metadata(stage, failure_reason); if (goal.goal.trajectory_tracking.path.empty()) { 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 5e6c019..e65541c 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 @@ -512,7 +512,7 @@ StagePlan PlanBuilder::make_control_stage( stage.display_name = "运控参数调优"; stage.order_index = order_index; stage.executor_endpoint_name = "/control/execute_controller_evaluation"; - stage.description = "调用独立运控标定包执行控制评估任务。stage.metadata 需要提供 control.task_type;支持 trajectory_tracking、velocity_step、accel_decel、stop_accuracy。trajectory_tracking 需提供 traj_pt_{i}_{x_m|y_m|yaw_rad|speed_ms},其余任务需提供各自目标速度、加速度、停车目标与 timeout 等参数。"; + stage.description = "调用独立运控标定包执行控制评估任务。stage.metadata 需要提供 control.task_type;支持 trajectory_tracking、velocity_step、accel_decel、stop_accuracy。trajectory_tracking 需提供 traj_pt_{i}_{x_m|y_m|yaw_rad|speed_ms},并可提供外部真值源和质量门限;其余任务需提供各自目标速度、加速度、停车目标与 timeout 等参数。"; stage.retry_limit = 0; stage.auto_generated = true; apply_default_control_metadata(stage); 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 7f3d499..e6a0b95 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 @@ -2,10 +2,13 @@ #include // 检查报告文件是否已经存在。 #include // sleep_for 需要的时间单位。 +#include // 解析 YAML 简单值时要跳过空白字符。 #include // 规范化现场数据输入和数据集索引路径。 +#include // 读取 dataset_index.yaml 和待提交参数包。 #include // 拼接 session_id 时要用字符串流。 #include // 文件系统查询失败时用 error_code 接住错误。 #include // 把整场执行放到后台线程时要用 std::thread。 +#include // 保存从 dataset_index.yaml 读取出来的 metadata。 #include "calibration_workshop_orchestration_interfaces/msg/approval_state.hpp" #include "calibration_workshop_orchestration_interfaces/msg/precheck_item.hpp" @@ -80,6 +83,356 @@ std::string infer_dataset_index_file(const std::string & data_input_params_file) return candidate.lexically_normal().string(); } +std::string trim_copy(const std::string & value) +{ + std::size_t begin = 0; + while (begin < value.size() && std::isspace(static_cast(value[begin]))) { + ++begin; + } + std::size_t end = value.size(); + while (end > begin && std::isspace(static_cast(value[end - 1]))) { + --end; + } + return value.substr(begin, end - begin); +} + +std::size_t leading_space_count(const std::string & line) +{ + std::size_t count = 0; + while (count < line.size() && line[count] == ' ') { + ++count; + } + return count; +} + +std::string clean_yaml_scalar(const std::string & raw_value) +{ + std::string value = trim_copy(raw_value); + const auto comment_pos = value.find(" #"); + if (comment_pos != std::string::npos) { + value = trim_copy(value.substr(0, comment_pos)); + } + if (value.size() >= 2) { + const char first = value.front(); + const char last = value.back(); + if ((first == '"' && last == '"') || (first == '\'' && last == '\'')) { + value = value.substr(1, value.size() - 2); + } + } + return value; +} + +bool split_yaml_key_value( + const std::string & line, + std::string & key, + std::string & value) +{ + const auto colon_pos = line.find(':'); + if (colon_pos == std::string::npos) { + return false; + } + key = trim_copy(line.substr(0, colon_pos)); + value = clean_yaml_scalar(line.substr(colon_pos + 1)); + return !key.empty(); +} + +bool looks_like_uri(const std::string & value) +{ + const auto scheme_pos = value.find("://"); + if (scheme_pos == std::string::npos || scheme_pos == 0) { + return false; + } + for (std::size_t i = 0; i < scheme_pos; ++i) { + const char ch = value[i]; + if (!std::isalpha(static_cast(ch)) && + !std::isdigit(static_cast(ch)) && ch != '+' && ch != '-' && ch != '.') + { + return false; + } + } + return true; +} + +struct DatasetIndexTrace +{ + std::string dataset_root; + std::unordered_map metadata; +}; + +DatasetIndexTrace read_dataset_index_trace(const std::string & dataset_index_file) +{ + DatasetIndexTrace trace; + if (dataset_index_file.empty()) { + return trace; + } + + std::ifstream stream(dataset_index_file); + if (!stream.is_open()) { + return trace; + } + + std::string section; + std::string raw_line; + while (std::getline(stream, raw_line)) { + const std::string trimmed = trim_copy(raw_line); + if (trimmed.empty() || trimmed[0] == '#') { + continue; + } + + const auto indent = leading_space_count(raw_line); + std::string key; + std::string value; + if (!split_yaml_key_value(trimmed, key, value)) { + continue; + } + + if (indent == 0) { + section = value.empty() ? key : ""; + continue; + } + + if (indent == 2 && section == "session" && key == "dataset_root") { + trace.dataset_root = value; + } else if (indent == 2 && section == "metadata" && !value.empty()) { + trace.metadata[key] = value; + } + } + return trace; +} + +std::string dataset_relative_file( + const std::string & raw_file, + const std::string & dataset_index_file, + const std::string & dataset_root) +{ + if (raw_file.empty() || looks_like_uri(raw_file)) { + return raw_file; + } + + const fs::path file_path(raw_file); + if (file_path.is_absolute()) { + return file_path.lexically_normal().string(); + } + + fs::path base_dir; + if (!dataset_root.empty()) { + base_dir = fs::path(dataset_root); + } else if (!dataset_index_file.empty()) { + base_dir = fs::path(dataset_index_file).parent_path(); + } + if (base_dir.empty()) { + return file_path.lexically_normal().string(); + } + return (base_dir / file_path).lexically_normal().string(); +} + +struct PendingChassisCommitTrace +{ + std::string commit_state; + std::string parameter_version; + std::string chassis_type; + std::string approval_required; + std::string approval_approved; +}; + +struct PendingControlCommitTrace +{ + std::string commit_state; + std::string parameter_version; + std::string chassis_type; + std::string control_axis; + std::string controller_algorithm; + std::string control_role; + std::string approval_required; + std::string approval_approved; +}; + +struct PendingSensorCommitTrace +{ + std::string commit_state; + std::string parameter_version; + std::string selected_task; + std::string task_subtype; + std::string sensor_id; + std::string approval_required; + std::string approval_approved; +}; + +PendingChassisCommitTrace read_pending_chassis_commit_trace(const std::string & pending_commit_file) +{ + PendingChassisCommitTrace trace; + if (pending_commit_file.empty()) { + return trace; + } + + std::ifstream stream(pending_commit_file); + if (!stream.is_open()) { + return trace; + } + + std::string section; + std::string raw_line; + while (std::getline(stream, raw_line)) { + const std::string trimmed = trim_copy(raw_line); + if (trimmed.empty() || trimmed[0] == '#') { + continue; + } + + const auto indent = leading_space_count(raw_line); + std::string key; + std::string value; + if (!split_yaml_key_value(trimmed, key, value)) { + continue; + } + + if (indent == 0) { + if (key == "commit_state") { + trace.commit_state = value; + } + section = value.empty() ? key : ""; + continue; + } + + if (indent != 2) { + continue; + } + if (section == "commit_request") { + if (key == "parameter_version") { + trace.parameter_version = value; + } else if (key == "chassis_type") { + trace.chassis_type = value; + } + } else if (section == "approval") { + if (key == "required") { + trace.approval_required = value; + } else if (key == "approved") { + trace.approval_approved = value; + } + } + } + return trace; +} + +PendingControlCommitTrace read_pending_control_commit_trace(const std::string & pending_commit_file) +{ + PendingControlCommitTrace trace; + if (pending_commit_file.empty()) { + return trace; + } + + std::ifstream stream(pending_commit_file); + if (!stream.is_open()) { + return trace; + } + + std::string section; + std::string raw_line; + while (std::getline(stream, raw_line)) { + const std::string trimmed = trim_copy(raw_line); + if (trimmed.empty() || trimmed[0] == '#') { + continue; + } + + const auto indent = leading_space_count(raw_line); + std::string key; + std::string value; + if (!split_yaml_key_value(trimmed, key, value)) { + continue; + } + + if (indent == 0) { + if (key == "commit_state") { + trace.commit_state = value; + } + section = value.empty() ? key : ""; + continue; + } + + if (indent != 2) { + continue; + } + if (section == "commit_request") { + if (key == "parameter_version") { + trace.parameter_version = value; + } else if (key == "chassis_type") { + trace.chassis_type = value; + } else if (key == "control_axis") { + trace.control_axis = value; + } else if (key == "controller_algorithm") { + trace.controller_algorithm = value; + } else if (key == "control_role") { + trace.control_role = value; + } + } else if (section == "approval") { + if (key == "required") { + trace.approval_required = value; + } else if (key == "approved") { + trace.approval_approved = value; + } + } + } + return trace; +} + +PendingSensorCommitTrace read_pending_sensor_commit_trace(const std::string & pending_commit_file) +{ + PendingSensorCommitTrace trace; + if (pending_commit_file.empty()) { + return trace; + } + + std::ifstream stream(pending_commit_file); + if (!stream.is_open()) { + return trace; + } + + std::string section; + std::string raw_line; + while (std::getline(stream, raw_line)) { + const std::string trimmed = trim_copy(raw_line); + if (trimmed.empty() || trimmed[0] == '#') { + continue; + } + + const auto indent = leading_space_count(raw_line); + std::string key; + std::string value; + if (!split_yaml_key_value(trimmed, key, value)) { + continue; + } + + if (indent == 0) { + if (key == "commit_state") { + trace.commit_state = value; + } + section = value.empty() ? key : ""; + continue; + } + + if (indent != 2) { + continue; + } + if (section == "commit_request") { + if (key == "parameter_version") { + trace.parameter_version = value; + } else if (key == "selected_task") { + trace.selected_task = value; + } else if (key == "task_subtype") { + trace.task_subtype = value; + } else if (key == "sensor_id") { + trace.sensor_id = value; + } + } else if (section == "approval") { + if (key == "required") { + trace.approval_required = value; + } else if (key == "approved") { + trace.approval_approved = value; + } + } + } + return trace; +} + void append_metadata_value( WorkshopReport & report, const std::string & key, @@ -113,6 +466,185 @@ void append_report_file( report.report_files.push_back(ref); } } + +void append_chassis_commit_trace( + WorkshopReport & report, + const std::string & dataset_index_file) +{ + const auto dataset_trace = read_dataset_index_trace(dataset_index_file); + const auto pending_it = dataset_trace.metadata.find("chassis_pending_commit_file"); + if (pending_it == dataset_trace.metadata.end() || pending_it->second.empty()) { + return; + } + + const auto estimated_it = dataset_trace.metadata.find("chassis_estimated_params_file"); + if (estimated_it != dataset_trace.metadata.end() && !estimated_it->second.empty()) { + const std::string estimated_file = dataset_relative_file( + estimated_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto estimated_ref = make_trace_file_reference( + estimated_file, + "底盘算法输出参数文件"); + append_report_file(report, estimated_ref); + append_metadata_value(report, "chassis_estimated_params_file", estimated_ref.file_uri); + } + + const std::string pending_commit_file = dataset_relative_file( + pending_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto pending_ref = make_trace_file_reference( + pending_commit_file, + "底盘标定待审批/待提交参数包"); + append_report_file(report, pending_ref); + append_metadata_value(report, "chassis_pending_commit_file", pending_ref.file_uri); + + const auto pending_trace = read_pending_chassis_commit_trace(pending_ref.file_uri); + append_metadata_value(report, "chassis_pending_commit_state", pending_trace.commit_state); + append_metadata_value(report, "chassis_pending_parameter_version", pending_trace.parameter_version); + append_metadata_value(report, "chassis_pending_commit_chassis_type", pending_trace.chassis_type); + append_metadata_value(report, "chassis_pending_commit_approval_required", pending_trace.approval_required); + append_metadata_value(report, "chassis_pending_commit_approved", pending_trace.approval_approved); + + const auto handoff_it = dataset_trace.metadata.find("chassis_parameter_handoff_file"); + if (handoff_it != dataset_trace.metadata.end() && !handoff_it->second.empty()) { + const std::string handoff_file = dataset_relative_file( + handoff_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto handoff_ref = make_trace_file_reference( + handoff_file, + "底盘标定已审批参数交接包"); + append_report_file(report, handoff_ref); + append_metadata_value(report, "chassis_parameter_handoff_file", handoff_ref.file_uri); + } + const auto handoff_state_it = dataset_trace.metadata.find("chassis_parameter_handoff_state"); + if (handoff_state_it != dataset_trace.metadata.end()) { + append_metadata_value(report, "chassis_parameter_handoff_state", handoff_state_it->second); + } +} + +void append_control_commit_trace( + WorkshopReport & report, + const std::string & dataset_index_file) +{ + const auto dataset_trace = read_dataset_index_trace(dataset_index_file); + const auto pending_it = dataset_trace.metadata.find("control_pending_commit_file"); + if (pending_it == dataset_trace.metadata.end() || pending_it->second.empty()) { + return; + } + + const auto estimated_it = dataset_trace.metadata.find("control_estimated_params_file"); + if (estimated_it != dataset_trace.metadata.end() && !estimated_it->second.empty()) { + const std::string estimated_file = dataset_relative_file( + estimated_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto estimated_ref = make_trace_file_reference( + estimated_file, + "运控算法输出参数文件"); + append_report_file(report, estimated_ref); + append_metadata_value(report, "control_estimated_params_file", estimated_ref.file_uri); + } + + const std::string pending_commit_file = dataset_relative_file( + pending_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto pending_ref = make_trace_file_reference( + pending_commit_file, + "运控标定待审批/待提交参数包"); + append_report_file(report, pending_ref); + append_metadata_value(report, "control_pending_commit_file", pending_ref.file_uri); + + const auto pending_trace = read_pending_control_commit_trace(pending_ref.file_uri); + append_metadata_value(report, "control_pending_commit_state", pending_trace.commit_state); + append_metadata_value(report, "control_pending_parameter_version", pending_trace.parameter_version); + append_metadata_value(report, "control_pending_commit_chassis_type", pending_trace.chassis_type); + append_metadata_value(report, "control_pending_commit_control_axis", pending_trace.control_axis); + append_metadata_value(report, "control_pending_commit_controller_algorithm", pending_trace.controller_algorithm); + append_metadata_value(report, "control_pending_commit_control_role", pending_trace.control_role); + append_metadata_value(report, "control_pending_commit_approval_required", pending_trace.approval_required); + append_metadata_value(report, "control_pending_commit_approved", pending_trace.approval_approved); + + const auto handoff_it = dataset_trace.metadata.find("control_parameter_handoff_file"); + if (handoff_it != dataset_trace.metadata.end() && !handoff_it->second.empty()) { + const std::string handoff_file = dataset_relative_file( + handoff_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto handoff_ref = make_trace_file_reference( + handoff_file, + "运控标定已审批参数交接包"); + append_report_file(report, handoff_ref); + append_metadata_value(report, "control_parameter_handoff_file", handoff_ref.file_uri); + } + const auto handoff_state_it = dataset_trace.metadata.find("control_parameter_handoff_state"); + if (handoff_state_it != dataset_trace.metadata.end()) { + append_metadata_value(report, "control_parameter_handoff_state", handoff_state_it->second); + } +} + +void append_sensor_commit_trace( + WorkshopReport & report, + const std::string & dataset_index_file) +{ + const auto dataset_trace = read_dataset_index_trace(dataset_index_file); + const auto pending_it = dataset_trace.metadata.find("sensor_pending_commit_file"); + if (pending_it == dataset_trace.metadata.end() || pending_it->second.empty()) { + return; + } + + const auto estimated_it = dataset_trace.metadata.find("sensor_estimated_params_file"); + if (estimated_it != dataset_trace.metadata.end() && !estimated_it->second.empty()) { + const std::string estimated_file = dataset_relative_file( + estimated_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto estimated_ref = make_trace_file_reference( + estimated_file, + "传感器算法输出参数文件"); + append_report_file(report, estimated_ref); + append_metadata_value(report, "sensor_estimated_params_file", estimated_ref.file_uri); + } + + const std::string pending_commit_file = dataset_relative_file( + pending_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto pending_ref = make_trace_file_reference( + pending_commit_file, + "传感器标定待审批/待提交参数包"); + append_report_file(report, pending_ref); + append_metadata_value(report, "sensor_pending_commit_file", pending_ref.file_uri); + + const auto pending_trace = read_pending_sensor_commit_trace(pending_ref.file_uri); + append_metadata_value(report, "sensor_pending_commit_state", pending_trace.commit_state); + append_metadata_value(report, "sensor_pending_parameter_version", pending_trace.parameter_version); + append_metadata_value(report, "sensor_pending_commit_selected_task", pending_trace.selected_task); + append_metadata_value(report, "sensor_pending_commit_task_subtype", pending_trace.task_subtype); + append_metadata_value(report, "sensor_pending_commit_sensor_id", pending_trace.sensor_id); + append_metadata_value(report, "sensor_pending_commit_approval_required", pending_trace.approval_required); + append_metadata_value(report, "sensor_pending_commit_approved", pending_trace.approval_approved); + + const auto handoff_it = dataset_trace.metadata.find("sensor_parameter_handoff_file"); + if (handoff_it != dataset_trace.metadata.end() && !handoff_it->second.empty()) { + const std::string handoff_file = dataset_relative_file( + handoff_it->second, + dataset_index_file, + dataset_trace.dataset_root); + const auto handoff_ref = make_trace_file_reference( + handoff_file, + "传感器标定已审批参数交接包"); + append_report_file(report, handoff_ref); + append_metadata_value(report, "sensor_parameter_handoff_file", handoff_ref.file_uri); + } + const auto handoff_state_it = dataset_trace.metadata.find("sensor_parameter_handoff_state"); + if (handoff_state_it != dataset_trace.metadata.end()) { + append_metadata_value(report, "sensor_parameter_handoff_state", handoff_state_it->second); + } +} } // namespace WorkshopOrchestratorV2Node::WorkshopOrchestratorV2Node(const rclcpp::NodeOptions & options) @@ -1136,6 +1668,9 @@ void WorkshopOrchestratorV2Node::append_report_trace_files(WorkshopReport & repo "本次会话真实采集数据集索引文件"); append_report_file(report, dataset_index_ref); append_metadata_value(report, "dataset_index_file", dataset_index_ref.file_uri); + append_chassis_commit_trace(report, dataset_index_ref.file_uri); + append_control_commit_trace(report, dataset_index_ref.file_uri); + append_sensor_commit_trace(report, dataset_index_ref.file_uri); } void WorkshopOrchestratorV2Node::finish_session_precheck_failed( diff --git a/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md b/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md index 35d97fc..5afc488 100644 --- a/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md +++ b/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md @@ -7,8 +7,13 @@ - `base_link` 定义与真实车辆一致,尤其是后轴中心位置。 - 传感器 frame ID 与真实 URDF/TF 树一致。 - 标定靶尺寸已在现场实测,不能直接沿用默认值。 +- 四角 LiDAR 外部真值定位配置已填写真实安装位姿和标定球安装坐标。 +- 外部真值定位节点已发布 `/workshop/external_localization/vehicle/pose` 或现场约定 topic。 +- 底盘标定现场配置已填写真实 `chassis_type`、车辆 ID、会话目录和数据集索引路径。 +- 底盘遥测、执行器命令、外部真值轨迹三类文件已写入 `data_inputs.chassis`。 - 真实采集/ingest 已生成本次会话专用的 `dataset_index.yaml`。 - 已使用 `validate_dataset_index.py` 校验本次 `dataset_index.yaml`。 +- 已使用 `build_workshop_session_config.py` 从现场 profile 生成本次总控会话任务配置。 - 已在会话结束时调用收尾工具,从 `dataset_index.yaml` 生成 `site_data_input.yaml`。 - 已使用 `run_local_data_input_smoke.sh` 验证数据输入注入和 report 链路。 - 运行真实算法前,已通过 `data_input_params_file` 把数据输入参数文件传给现场 launch。 diff --git a/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml b/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml index 48b1be2..fd153eb 100644 --- a/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml +++ b/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml @@ -11,7 +11,7 @@ vehicle_agent: type: isaac_vehicle_agent_sim bind_host: 0.0.0.0 private_cmd_vel_topic: /vehicle/demo_agv_001/actuator/cmd_vel - external_pose_topic: /isaac/external_localization/telemetry + external_pose_topic: /isaac/external_localization/vehicle/pose external_pose_transport: wifi6_tcp chassis_telemetry_topic: /chassis/telemetry internal_ackermann_command_topic: /vehicle/demo_agv_001/internal/ackermann_cmd @@ -40,7 +40,7 @@ vehicle_sensor_agent: external_pose_bridge: type: external_pose_wifi6_bridge - source_topic: /isaac/external_localization/telemetry + source_topic: /isaac/external_localization/vehicle/pose reference_source_name: isaac_external_truth max_hz: 30.0 diff --git a/agv_calib_brain/src/deployment/profiles/site_template.yaml b/agv_calib_brain/src/deployment/profiles/site_template.yaml index 21ed8e1..6965d98 100644 --- a/agv_calib_brain/src/deployment/profiles/site_template.yaml +++ b/agv_calib_brain/src/deployment/profiles/site_template.yaml @@ -30,10 +30,99 @@ vehicle_sensor_agent: external_pose_bridge: type: external_pose_wifi6_bridge - source_topic: replace_with_external_truth_ros_topic - reference_source_name: replace_with_external_truth_source + source_topic: /workshop/external_localization/vehicle/pose + reference_source_name: workshop_four_lidar_ball_truth max_hz: 30.0 +external_localization: + # 车间电脑侧外部真值定位模板。 + # 四角 LiDAR + 车端标定球算法由这个节点发布 ExternalLocalizationTelemetry。 + type: workshop_four_lidar_ball + config_file: /data/agv_calib/replace_with_site_or_line_id/four_lidar_ball.yaml + output_topic: /workshop/external_localization/vehicle/pose + diagnostics_topic: /workshop/external_localization/diagnostics + reference_source_name: workshop_four_lidar_ball_truth + workcell_zone_id: replace_with_workcell_zone_id + recording_session_dir: /data/agv_calib/replace_with_site_or_line_id/session_xxx + recording_dataset_index_path: /data/agv_calib/replace_with_site_or_line_id/session_xxx/dataset_index.yaml + +chassis_calibration: + # 现场底盘标定数据模板。 + # 这里只定义底盘标定算法的数据输入,不直接控制车辆。 + type: workshop_chassis_data + chassis_type: ackermann + data_config_file: /data/agv_calib/replace_with_site_or_line_id/chassis_data.yaml + action_profile_file: /data/agv_calib/replace_with_site_or_line_id/chassis_action_profile.yaml + action_profile_tool: src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py + action_profile_capture_runner: src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py + capture_tool: src/site_deployment/workshop_chassis_calibration_real/capture_chassis_session.py + parameter_commit_staging_tool: src/site_deployment/workshop_chassis_calibration_real/stage_chassis_parameter_commit.py + parameter_approval_handoff_tool: src/site_deployment/workshop_chassis_calibration_real/approve_chassis_pending_parameters.py + algorithm_result_finalize_tool: src/deployment/tools/finalize_site_session.py + estimated_params_template: src/site_deployment/workshop_chassis_calibration_real/config/chassis_estimated_params_template.yaml + replay_smoke_tool: src/site_deployment/workshop_chassis_calibration_real/replay_chassis_smoke.py + recording_session_dir: /data/agv_calib/replace_with_site_or_line_id/session_xxx + recording_dataset_index_path: /data/agv_calib/replace_with_site_or_line_id/session_xxx/dataset_index.yaml + chassis_motion_data_file: chassis/chassis_motion.csv + actuator_command_file: chassis/actuator_commands.csv + truth_trajectory_file: external/truth_trajectory.csv + +control_calibration: + # 现场运控参数标定数据模板。 + # 这里只定义运控评估算法的数据输入和参数交接产物,不直接实现车端写参。 + type: workshop_control_data + chassis_type: ackermann + control_axis: combined + controller_algorithm: mpc + control_role: path_tracking_outer_loop + data_config_file: /data/agv_calib/replace_with_site_or_line_id/control_data.yaml + capture_tool: src/site_deployment/workshop_control_calibration_real/capture_control_session.py + evaluation_profile_file: src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml + evaluation_profile_tool: src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py + evaluation_profile_capture_runner: src/site_deployment/workshop_control_calibration_real/run_control_profile_capture.py + evaluation_profile_capture_smoke_tool: src/site_deployment/workshop_control_calibration_real/smoke_test_control_profile_capture.py + parameter_commit_staging_tool: src/site_deployment/workshop_control_calibration_real/stage_control_parameter_commit.py + parameter_approval_handoff_tool: src/site_deployment/workshop_control_calibration_real/approve_control_pending_parameters.py + algorithm_result_finalize_tool: src/deployment/tools/finalize_site_session.py + estimated_params_template: src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_template.yaml + role_profiles: src/site_deployment/workshop_control_calibration_real/config/control_role_profiles.yaml + estimated_params_templates_by_chassis: + ackermann: src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_ackermann_template.yaml + differential: src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_differential_template.yaml + single_steer_wheel: src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_single_steer_wheel_template.yaml + multi_steer_wheel: src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_multi_steer_wheel_template.yaml + replay_smoke_tool: src/site_deployment/workshop_control_calibration_real/replay_control_smoke.py + recording_session_dir: /data/agv_calib/replace_with_site_or_line_id/session_xxx + recording_dataset_index_path: /data/agv_calib/replace_with_site_or_line_id/session_xxx/dataset_index.yaml + control_evaluation_data_file: control/control_eval.csv + reference_signal_file: control/reference_signal.csv + chassis_response_file: control/chassis_response.csv + truth_trajectory_file: external/truth_trajectory.csv + +sensor_calibration: + # 现场传感器标定数据模板。 + # 这里只定义传感器标定算法的数据输入、任务 profile 和参数交接产物。 + type: workshop_sensor_data + profile_file: src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml + profile_tool: src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py + dataset_manifest_tool: src/site_deployment/workshop_sensor_calibration_real/sensor_dataset_manifest.py + preprocess_tool: src/site_deployment/workshop_sensor_calibration_real/sensor_preprocess_pipeline.py + data_contract: src/site_deployment/workshop_sensor_calibration_real/sensor_data_contract.md + parameter_commit_staging_tool: src/site_deployment/workshop_sensor_calibration_real/stage_sensor_parameter_commit.py + parameter_approval_handoff_tool: src/site_deployment/workshop_sensor_calibration_real/approve_sensor_pending_parameters.py + parameter_apply_handoff_tool: src/site_deployment/workshop_sensor_calibration_real/apply_sensor_parameter_handoff.py + algorithm_result_finalize_tool: src/deployment/tools/finalize_site_session.py + estimated_params_template: src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml + local_smoke_tool: src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py + recording_session_dir: /data/agv_calib/replace_with_site_or_line_id/session_xxx + recording_dataset_index_path: /data/agv_calib/replace_with_site_or_line_id/session_xxx/dataset_index.yaml + image_sample_file: sensor/front_camera/images.yaml + imu_sample_file: sensor/imu/imu_samples.mcap + pointcloud_sample_file: sensor/lidar_3d/pointclouds.mcap + laser_scan_sample_file: sensor/lidar_2d/scans.mcap + target_detection_file: sensor/front_camera/charuco_detections.json + robot_pose_sample_file: sensor/hand_eye/robot_poses.csv + workshop_sensor_ingest: type: workshop_sensor_ingest capture_session_tool: src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py @@ -53,6 +142,12 @@ algorithm_data_inputs: # 真实采集/ingest 会话结束时推荐调用这个收尾入口。 session_finalizer: src/deployment/tools/finalize_site_session.py +orchestrator_session: + # 从现场 profile、底盘动作 profile、运控评估 profile、传感器标定 profile 自动生成总控 requested_tasks。 + session_config_builder: src/deployment/tools/build_workshop_session_config.py + default_tasks: external,chassis,control,sensor_intrinsic + generated_session_config_file: /data/agv_calib/replace_with_site_or_line_id/session_xxx/workshop_session_config.yaml + frames: map: workshop base_link: rear_axle_center 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 new file mode 100644 index 0000000..5c571bb --- /dev/null +++ b/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python3 +"""从现场 profile 生成总控会话配置和 requested_tasks。""" + +from __future__ import annotations + +import argparse +import importlib.util +import sys +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +class NoAliasDumper(yaml.SafeDumper): + def ignore_aliases(self, data: Any) -> bool: + return True + + +REPO_ROOT = Path(__file__).resolve().parents[3] + +DEFAULT_TASKS = "external,chassis,control,sensor_intrinsic" + +STAGE_TYPES = { + "external": "EXTERNAL_REFERENCE_READY_CHECK_STAGE", + "chassis": "CHASSIS_CALIBRATION_STAGE", + "control": "CONTROL_CALIBRATION_STAGE", + "sensor_intrinsic": "SENSOR_INTRINSIC_CALIBRATION_STAGE", + "sensor_extrinsic": "SENSOR_EXTRINSIC_CALIBRATION_STAGE", + "hand_eye": "HAND_EYE_CALIBRATION_STAGE", +} + +SUPPORTED_CHASSIS_TYPES = { + "ackermann", + "differential", + "single_steer_wheel", + "multi_steer_wheel", +} + +PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx") + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="根据现场 site profile、底盘动作 profile、运控评估 profile 和传感器标定 profile 生成总控会话配置。" + ) + parser.add_argument( + "--site-profile", + default="src/deployment/profiles/site_template.yaml", + help="现场部署 profile YAML。", + ) + parser.add_argument( + "--tasks", + default=DEFAULT_TASKS, + help=f"逗号分隔任务列表,默认: {DEFAULT_TASKS}", + ) + parser.add_argument("--chassis-type", default="", help="覆盖底盘类型。") + parser.add_argument("--chassis-action-profile", default="", help="覆盖底盘动作 profile 路径。") + parser.add_argument("--control-evaluation-profile", default="", help="覆盖运控评估 profile 路径。") + parser.add_argument("--sensor-calibration-profile", default="", help="覆盖传感器标定 profile 路径。") + parser.add_argument( + "--profile-mode", + choices=["auto", "minimal", "required"], + default="auto", + help="profile 任务生成策略:auto 可用则展开,minimal 强制最小任务,required 要求 profile 文件存在。", + ) + 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("--sensor-task-subtype", default="front_camera_intrinsic", help="默认传感器内参任务子类型。") + parser.add_argument("--output", "-o", default="", help="输出 YAML;不填写时输出到标准输出。") + parser.add_argument( + "--format", + choices=["session_config", "requested_tasks"], + default="session_config", + help="输出完整 session_config 或只输出 requested_tasks。", + ) + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + if data is None: + return {} + if not isinstance(data, dict): + raise ValueError(f"{path} 的顶层结构必须是 YAML map。") + return data + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text( + yaml.dump(data, Dumper=NoAliasDumper, sort_keys=False, allow_unicode=True), + encoding="utf-8", + ) + + +def parse_tasks(raw: str) -> list[str]: + if raw.strip().lower() == "all": + raw = DEFAULT_TASKS + tasks: list[str] = [] + for item in raw.split(","): + task_name = item.strip() + if not task_name: + continue + if task_name not in STAGE_TYPES: + raise ValueError(f"不支持的任务名: {task_name}。") + tasks.append(task_name) + if not tasks: + raise ValueError("至少需要指定一个任务。") + return tasks + + +def has_placeholder(value: Any) -> bool: + return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS) + + +def read_string(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 resolve_path(raw_path: str, site_profile_path: Path) -> Path: + path = Path(raw_path).expanduser() + if path.is_absolute(): + return path.resolve(strict=False) + site_relative = (site_profile_path.parent / path).resolve(strict=False) + if site_relative.exists(): + return site_relative + return (REPO_ROOT / path).resolve(strict=False) + + +def resolve_configured_path( + override_path: str, + site_profile: dict[str, Any], + site_profile_path: Path, + config_path: tuple[str, ...], +) -> Path | None: + raw_path = override_path or read_string(site_profile, config_path) + if not raw_path: + return None + return resolve_path(raw_path, site_profile_path) + + +def load_module(module_name: str, path: Path) -> Any: + spec = importlib.util.spec_from_file_location(module_name, path) + if spec is None or spec.loader is None: + raise RuntimeError(f"无法加载模块: {path}") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def profile_tasks_available(profile_path: Path | None, profile_mode: str, module_name: str) -> bool: + if profile_mode == "minimal": + return False + if profile_path is not None and profile_path.exists(): + return True + if profile_mode == "required": + raise FileNotFoundError(f"{module_name} profile 文件不存在: {profile_path}") + if profile_path is not None: + print(f"[WARN] {module_name} profile 文件不存在,使用最小任务: {profile_path}", file=sys.stderr) + return False + + +def normalize_profile_task(raw_task: dict[str, Any]) -> dict[str, Any]: + task = { + "stage_type": str(raw_task["stage_type"]), + "enabled": bool(raw_task.get("enabled", True)), + "require_manual_approval": bool(raw_task.get("require_manual_approval", False)), + "execution_policy": str(raw_task.get("execution_policy", "REQUIRED")), + "reason": str(raw_task.get("reason", "现场 profile 自动生成")), + "task_code": str(raw_task.get("task_code", "")), + "target_id": str(raw_task.get("target_id", "")), + "task_params": [], + } + for item in raw_task.get("task_params", []): + task["task_params"].append({ + "key": str(item["key"]), + "value": str(item["value"]), + }) + if not task["task_code"]: + raise ValueError("profile 导出的 task_code 不能为空。") + return task + + +def export_chassis_profile_tasks(profile_path: Path, chassis_type: str) -> list[dict[str, Any]]: + tool_path = REPO_ROOT / "src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py" + tool = load_module("chassis_action_profile_tool", tool_path) + profile = tool.validate_profile(tool.load_yaml(profile_path)) + exported = tool.export_requested_tasks(profile, chassis_type) + return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]] + + +def export_control_profile_tasks(profile_path: Path, chassis_type: str) -> list[dict[str, Any]]: + tool_path = REPO_ROOT / "src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py" + tool = load_module("control_evaluation_profile_tool", tool_path) + profile = tool.validate_profile(tool.load_yaml(profile_path)) + exported = tool.export_requested_tasks(profile, chassis_type) + return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]] + + +def export_sensor_profile_tasks(profile_path: Path, task_name: str) -> list[dict[str, Any]]: + tool_path = REPO_ROOT / "src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py" + tool = load_module("sensor_calibration_profile_tool", tool_path) + profile = tool.validate_profile(tool.load_yaml(profile_path)) + exported = tool.export_requested_tasks(profile, {task_name}) + return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]] + + +def task_param(key: str, value: Any) -> dict[str, str]: + return { + "key": key, + "value": str(value), + } + + +def minimal_task( + task_name: str, + args: argparse.Namespace, + site_profile: dict[str, Any], + chassis_type: str, +) -> dict[str, Any]: + task = { + "stage_type": STAGE_TYPES[task_name], + "enabled": True, + "require_manual_approval": False, + "execution_policy": "REQUIRED", + "reason": "site profile 最小总控任务", + "task_code": task_name, + "target_id": "", + "task_params": [], + } + if task_name == "external": + task["target_id"] = args.reference_target_id + task["task_params"] = [ + task_param("external.static_sample_count", "1"), + task_param("external.dynamic_sample_count", "1"), + task_param("external.require_short_motion_segment", "false"), + task_param("external.max_position_stddev_m", "0.05"), + task_param("external.max_yaw_stddev_rad", "0.05"), + task_param("external.max_tracking_loss_ratio", "0.05"), + task_param("external.max_time_sync_offset_ms", "50.0"), + task_param("external.timeout_sec", "5.0"), + ] + elif task_name == "chassis": + task["target_id"] = chassis_type + task["task_params"] = [ + task_param("primitive_type", "straight_line"), + task_param("straight_line.target_distance_m", "0.2"), + task_param("straight_line.target_speed_ms", "0.1"), + task_param("straight_line.reverse", "false"), + task_param("brake_when_finished", "true"), + task_param("timeout_sec", "10.0"), + ] + elif task_name == "control": + task["target_id"] = chassis_type + task["task_params"] = [ + task_param("control.task_type", "velocity_step"), + task_param("velocity_step.target_velocity_ms", "0.1"), + task_param("velocity_step.hold_time_sec", "0.5"), + task_param("velocity_step.settle_before_step_sec", "0.1"), + task_param("control.timeout_sec", "10.0"), + ] + elif task_name == "sensor_intrinsic": + task["target_id"] = args.sensor_id + task["task_params"] = [ + task_param("sensor.sensor_id", args.sensor_id), + task_param("sensor.task_subtype", args.sensor_task_subtype), + task_param("camera_intrinsic.required_image_count", "1"), + task_param("camera_intrinsic.target_board_id", args.reference_target_id), + task_param("camera_intrinsic.timeout_sec", "5.0"), + ] + elif task_name == "sensor_extrinsic": + task["target_id"] = args.sensor_id + task["task_params"] = [ + task_param("sensor.sensor_id", args.sensor_id), + task_param("sensor.task_subtype", "front_camera_extrinsic"), + task_param("sensor_extrinsic.base_frame_id", read_string(site_profile, ("frames", "base_link"), "base_link")), + task_param("sensor_extrinsic.required_sample_count", "1"), + task_param("sensor_extrinsic.timeout_sec", "5.0"), + ] + elif task_name == "hand_eye": + task["target_id"] = args.sensor_id + task["task_params"] = [ + task_param("sensor.sensor_id", args.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"), + task_param("hand_eye.timeout_sec", "5.0"), + ] + return task + + +def resolve_chassis_type(args: argparse.Namespace, site_profile: dict[str, Any]) -> str: + chassis_type = ( + args.chassis_type + or read_string(site_profile, ("chassis_calibration", "chassis_type")) + or read_string(site_profile, ("control_calibration", "chassis_type")) + or "ackermann" + ) + if chassis_type not in SUPPORTED_CHASSIS_TYPES: + raise ValueError(f"chassis_type 不受支持: {chassis_type}") + return chassis_type + + +def build_requested_tasks( + tasks: list[str], + args: argparse.Namespace, + site_profile: dict[str, Any], + site_profile_path: Path, + chassis_type: str, +) -> list[dict[str, Any]]: + chassis_profile_path = resolve_configured_path( + args.chassis_action_profile, + site_profile, + site_profile_path, + ("chassis_calibration", "action_profile_file"), + ) + control_profile_path = resolve_configured_path( + args.control_evaluation_profile, + site_profile, + site_profile_path, + ("control_calibration", "evaluation_profile_file"), + ) + sensor_profile_path = resolve_configured_path( + args.sensor_calibration_profile, + site_profile, + site_profile_path, + ("sensor_calibration", "profile_file"), + ) + + requested_tasks: list[dict[str, Any]] = [] + for task_name in tasks: + if task_name == "chassis" and profile_tasks_available(chassis_profile_path, args.profile_mode, "底盘动作"): + requested_tasks.extend(export_chassis_profile_tasks(chassis_profile_path, chassis_type)) + continue + if task_name == "control" and profile_tasks_available(control_profile_path, args.profile_mode, "运控评估"): + requested_tasks.extend(export_control_profile_tasks(control_profile_path, chassis_type)) + continue + if ( + task_name in {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"} and + profile_tasks_available(sensor_profile_path, args.profile_mode, "传感器标定") + ): + requested_tasks.extend(export_sensor_profile_tasks(sensor_profile_path, task_name)) + continue + requested_tasks.append(minimal_task(task_name, args, site_profile, chassis_type)) + return requested_tasks + + +def build_session_config( + site_profile_path: Path, + site_profile: dict[str, Any], + requested_tasks: list[dict[str, Any]], + args: argparse.Namespace, +) -> dict[str, Any]: + reference_source_name = ( + read_string(site_profile, ("external_pose_bridge", "reference_source_name")) + or read_string(site_profile, ("external_localization", "reference_source_name")) + or "workshop_external_truth" + ) + workcell_zone_id = read_string(site_profile, ("external_localization", "workcell_zone_id"), "default_workcell") + return { + "schema_version": 1, + "source_site_profile": str(site_profile_path), + "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": reference_source_name, + "workcell_zone_id": workcell_zone_id, + "reference_target_id": args.reference_target_id, + "requested_tasks": requested_tasks, + }, + "requested_tasks": requested_tasks, + } + + +def main() -> int: + args = parse_args() + site_profile_path = Path(args.site_profile).expanduser().resolve(strict=False) + site_profile = load_yaml(site_profile_path) + tasks = parse_tasks(args.tasks) + chassis_type = resolve_chassis_type(args, site_profile) + requested_tasks = build_requested_tasks(tasks, args, site_profile, site_profile_path, chassis_type) + output = ( + {"schema_version": 1, "requested_tasks": requested_tasks} + if args.format == "requested_tasks" + else build_session_config(site_profile_path, site_profile, requested_tasks, args) + ) + if args.output: + write_yaml(Path(args.output).expanduser().resolve(strict=False), output) + print(f"[OK] 已写入总控会话配置: {args.output}", file=sys.stderr) + else: + print(yaml.dump(output, Dumper=NoAliasDumper, sort_keys=False, allow_unicode=True), end="") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/deployment/tools/finalize_site_session.py b/agv_calib_brain/src/deployment/tools/finalize_site_session.py index 14cde0d..5f72ffa 100644 --- a/agv_calib_brain/src/deployment/tools/finalize_site_session.py +++ b/agv_calib_brain/src/deployment/tools/finalize_site_session.py @@ -4,6 +4,7 @@ from __future__ import annotations import argparse +import importlib.util import sys from pathlib import Path from typing import Any @@ -17,6 +18,30 @@ from dataset_index_to_site_data_input import ( ) +REPO_ROOT = Path(__file__).resolve().parents[3] +CHASSIS_COMMIT_TOOL = ( + REPO_ROOT + / "src" + / "site_deployment" + / "workshop_chassis_calibration_real" + / "stage_chassis_parameter_commit.py" +) +CONTROL_COMMIT_TOOL = ( + REPO_ROOT + / "src" + / "site_deployment" + / "workshop_control_calibration_real" + / "stage_control_parameter_commit.py" +) +SENSOR_COMMIT_TOOL = ( + REPO_ROOT + / "src" + / "site_deployment" + / "workshop_sensor_calibration_real" + / "stage_sensor_parameter_commit.py" +) + + def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser( description="根据现场 profile 和本次会话数据集索引生成 site_data_input.yaml。" @@ -53,6 +78,132 @@ def parse_args() -> argparse.Namespace: action="store_true", help="不自动把 dataset_index.yaml 写入 sensor 的 synchronized_dataset_files。", ) + parser.add_argument( + "--chassis-estimated-params", + default="", + help="底盘算法输出参数 YAML;为空时自动检查会话目录 chassis/estimated_params.yaml。", + ) + parser.add_argument( + "--chassis-pending-commit-output", + default="", + help="底盘待提交参数包输出路径;为空时写到会话目录 chassis/pending_chassis_commit.yaml。", + ) + parser.add_argument( + "--skip-chassis-commit-staging", + action="store_true", + help="跳过底盘待提交参数包生成。", + ) + parser.add_argument( + "--chassis-commit-reason", + default="底盘标定算法输出待提交", + help="写入底盘待提交参数包的提交原因。", + ) + parser.add_argument( + "--chassis-operator-id", + default="", + help="生成底盘待提交参数包的操作员 ID。", + ) + parser.add_argument( + "--previous-chassis-parameter-version", + default="", + help="当前车端已生效底盘参数版本,用于回滚引用。", + ) + parser.add_argument( + "--chassis-persistent-write", + action=argparse.BooleanOptionalAction, + default=True, + help="最终提交到底盘时是否持久化。", + ) + parser.add_argument( + "--chassis-require-manual-approval", + action=argparse.BooleanOptionalAction, + default=True, + help="底盘待提交参数包是否要求人工审批。", + ) + parser.add_argument( + "--control-estimated-params", + default="", + help="运控算法输出参数 YAML;为空时自动检查会话目录 control/estimated_params.yaml。", + ) + parser.add_argument( + "--control-pending-commit-output", + default="", + help="运控待提交参数包输出路径;为空时写到会话目录 control/pending_control_commit.yaml。", + ) + parser.add_argument( + "--skip-control-commit-staging", + action="store_true", + help="跳过运控待提交参数包生成。", + ) + parser.add_argument( + "--control-commit-reason", + default="运控标定算法输出待提交", + help="写入运控待提交参数包的提交原因。", + ) + parser.add_argument( + "--control-operator-id", + default="", + help="生成运控待提交参数包的操作员 ID。", + ) + parser.add_argument( + "--previous-control-parameter-version", + default="", + help="当前车端已生效运控参数版本,用于回滚引用。", + ) + parser.add_argument( + "--control-persistent-write", + action=argparse.BooleanOptionalAction, + default=True, + help="最终提交到车端时是否持久化。", + ) + parser.add_argument( + "--control-require-manual-approval", + action=argparse.BooleanOptionalAction, + default=True, + help="运控待提交参数包是否要求人工审批。", + ) + parser.add_argument( + "--sensor-estimated-params", + default="", + help="传感器算法输出参数 YAML;为空时自动检查会话目录 sensor/estimated_params.yaml。", + ) + parser.add_argument( + "--sensor-pending-commit-output", + default="", + help="传感器待提交参数包输出路径;为空时写到会话目录 sensor/pending_sensor_commit.yaml。", + ) + parser.add_argument( + "--skip-sensor-commit-staging", + action="store_true", + help="跳过传感器待提交参数包生成。", + ) + parser.add_argument( + "--sensor-commit-reason", + default="传感器标定算法输出待提交", + help="写入传感器待提交参数包的提交原因。", + ) + parser.add_argument( + "--sensor-operator-id", + default="", + help="生成传感器待提交参数包的操作员 ID。", + ) + parser.add_argument( + "--previous-sensor-parameter-version", + default="", + help="当前车端已生效传感器参数版本,用于回滚引用。", + ) + parser.add_argument( + "--sensor-persistent-write", + action=argparse.BooleanOptionalAction, + default=True, + help="最终提交到车端时是否持久化。", + ) + parser.add_argument( + "--sensor-require-manual-approval", + action=argparse.BooleanOptionalAction, + default=True, + help="传感器待提交参数包是否要求人工审批。", + ) return parser.parse_args() @@ -135,6 +286,192 @@ def resolve_output_path( return (session_dir / "site_data_input.yaml").resolve(strict=False) +def default_chassis_estimated_params_path(session_dir: Path) -> Path: + return (session_dir / "chassis" / "estimated_params.yaml").resolve(strict=False) + + +def default_control_estimated_params_path(session_dir: Path) -> Path: + return (session_dir / "control" / "estimated_params.yaml").resolve(strict=False) + + +def default_sensor_estimated_params_path(session_dir: Path) -> Path: + return (session_dir / "sensor" / "estimated_params.yaml").resolve(strict=False) + + +def resolve_optional_path(path_value: str, default_path: Path) -> Path: + if path_value: + return resolve_relative(path_value, Path.cwd()) + return default_path + + +def load_chassis_commit_tool() -> Any: + spec = importlib.util.spec_from_file_location("stage_chassis_parameter_commit", CHASSIS_COMMIT_TOOL) + if spec is None or spec.loader is None: + raise RuntimeError(f"无法加载底盘待提交参数包工具:{CHASSIS_COMMIT_TOOL}") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def load_control_commit_tool() -> Any: + spec = importlib.util.spec_from_file_location("stage_control_parameter_commit", CONTROL_COMMIT_TOOL) + if spec is None or spec.loader is None: + raise RuntimeError(f"无法加载运控待提交参数包工具:{CONTROL_COMMIT_TOOL}") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def load_sensor_commit_tool() -> Any: + spec = importlib.util.spec_from_file_location("stage_sensor_parameter_commit", SENSOR_COMMIT_TOOL) + if spec is None or spec.loader is None: + raise RuntimeError(f"无法加载传感器待提交参数包工具:{SENSOR_COMMIT_TOOL}") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def maybe_stage_chassis_commit( + args: argparse.Namespace, + dataset_index_path: Path, + session_dir: Path, +) -> Path | None: + if args.skip_chassis_commit_staging: + return None + + default_estimated_path = default_chassis_estimated_params_path(session_dir) + estimated_params_path = resolve_optional_path(args.chassis_estimated_params, default_estimated_path) + explicit_estimated_path = bool(args.chassis_estimated_params) + if not estimated_params_path.exists(): + if explicit_estimated_path: + raise FileNotFoundError(f"底盘算法输出参数文件不存在:{estimated_params_path}") + print(f"未发现底盘算法输出参数,跳过待提交包生成:{estimated_params_path}") + return None + + chassis_commit_tool = load_chassis_commit_tool() + index = chassis_commit_tool.load_yaml(dataset_index_path) + params = chassis_commit_tool.load_yaml(estimated_params_path) + session = chassis_commit_tool.validate_dataset_index(index) + + stage_args = argparse.Namespace( + output=args.chassis_pending_commit_output, + commit_reason=args.chassis_commit_reason, + operator_id=args.chassis_operator_id, + previous_parameter_version=args.previous_chassis_parameter_version, + persistent_write=args.chassis_persistent_write, + require_manual_approval=args.chassis_require_manual_approval, + update_dataset_index=True, + ) + output_path = chassis_commit_tool.resolve_output_path(stage_args, session) + commit_package = chassis_commit_tool.build_commit_package( + dataset_index_path, + estimated_params_path, + index, + params, + stage_args, + output_path, + ) + chassis_commit_tool.write_yaml(output_path, commit_package) + chassis_commit_tool.update_dataset_index(dataset_index_path, index, output_path) + print(f"已生成底盘待提交参数包:{output_path}") + print(f"底盘待提交参数版本:{commit_package['commit_request']['parameter_version']}") + return output_path + + +def maybe_stage_control_commit( + args: argparse.Namespace, + dataset_index_path: Path, + session_dir: Path, +) -> Path | None: + if args.skip_control_commit_staging: + return None + + default_estimated_path = default_control_estimated_params_path(session_dir) + estimated_params_path = resolve_optional_path(args.control_estimated_params, default_estimated_path) + explicit_estimated_path = bool(args.control_estimated_params) + if not estimated_params_path.exists(): + if explicit_estimated_path: + raise FileNotFoundError(f"运控算法输出参数文件不存在:{estimated_params_path}") + print(f"未发现运控算法输出参数,跳过待提交包生成:{estimated_params_path}") + return None + + control_commit_tool = load_control_commit_tool() + index = control_commit_tool.load_yaml(dataset_index_path) + params = control_commit_tool.load_yaml(estimated_params_path) + session = control_commit_tool.validate_dataset_index(index) + + stage_args = argparse.Namespace( + output=args.control_pending_commit_output, + commit_reason=args.control_commit_reason, + operator_id=args.control_operator_id, + previous_parameter_version=args.previous_control_parameter_version, + persistent_write=args.control_persistent_write, + require_manual_approval=args.control_require_manual_approval, + update_dataset_index=True, + ) + output_path = control_commit_tool.resolve_output_path(stage_args, session) + commit_package = control_commit_tool.build_commit_package( + dataset_index_path, + estimated_params_path, + index, + params, + stage_args, + output_path, + ) + control_commit_tool.write_yaml(output_path, commit_package) + control_commit_tool.update_dataset_index(dataset_index_path, index, output_path) + print(f"已生成运控待提交参数包:{output_path}") + print(f"运控待提交参数版本:{commit_package['commit_request']['parameter_version']}") + return output_path + + +def maybe_stage_sensor_commit( + args: argparse.Namespace, + dataset_index_path: Path, + session_dir: Path, +) -> Path | None: + if args.skip_sensor_commit_staging: + return None + + default_estimated_path = default_sensor_estimated_params_path(session_dir) + estimated_params_path = resolve_optional_path(args.sensor_estimated_params, default_estimated_path) + explicit_estimated_path = bool(args.sensor_estimated_params) + if not estimated_params_path.exists(): + if explicit_estimated_path: + raise FileNotFoundError(f"传感器算法输出参数文件不存在:{estimated_params_path}") + print(f"未发现传感器算法输出参数,跳过待提交包生成:{estimated_params_path}") + return None + + sensor_commit_tool = load_sensor_commit_tool() + index = sensor_commit_tool.load_yaml(dataset_index_path) + params = sensor_commit_tool.load_yaml(estimated_params_path) + session = sensor_commit_tool.validate_dataset_index(index) + + stage_args = argparse.Namespace( + output=args.sensor_pending_commit_output, + commit_reason=args.sensor_commit_reason, + operator_id=args.sensor_operator_id, + previous_parameter_version=args.previous_sensor_parameter_version, + persistent_write=args.sensor_persistent_write, + require_manual_approval=args.sensor_require_manual_approval, + update_dataset_index=True, + ) + output_path = sensor_commit_tool.resolve_output_path(stage_args, session) + commit_package = sensor_commit_tool.build_commit_package( + dataset_index_path, + estimated_params_path, + index, + params, + stage_args, + output_path, + ) + sensor_commit_tool.write_yaml(output_path, commit_package) + sensor_commit_tool.update_dataset_index(dataset_index_path, index, output_path) + print(f"已生成传感器待提交参数包:{output_path}") + print(f"传感器待提交参数版本:{commit_package['commit_request']['parameter_version']}") + return output_path + + def main() -> int: args = parse_args() profile_path = Path(args.site_profile).expanduser().resolve(strict=False) @@ -158,8 +495,19 @@ def main() -> int: encoding="utf-8", ) + pending_chassis_commit_path = maybe_stage_chassis_commit(args, dataset_index_path, session_dir) + pending_control_commit_path = maybe_stage_control_commit(args, dataset_index_path, session_dir) + pending_sensor_commit_path = maybe_stage_sensor_commit(args, dataset_index_path, session_dir) + print(f"已完成会话数据输入收尾:{output_path}") print(f"启动参数:data_input_params_file:={output_path}") + print(f"启动参数:dataset_index_file:={dataset_index_path}") + if pending_chassis_commit_path is not None: + print(f"底盘待提交参数包:{pending_chassis_commit_path}") + if pending_control_commit_path is not None: + print(f"运控待提交参数包:{pending_control_commit_path}") + if pending_sensor_commit_path is not None: + print(f"传感器待提交参数包:{pending_sensor_commit_path}") return 0 diff --git a/agv_calib_brain/src/deployment/tools/validate_dataset_index.py b/agv_calib_brain/src/deployment/tools/validate_dataset_index.py index 73f946b..3f2a7ef 100755 --- a/agv_calib_brain/src/deployment/tools/validate_dataset_index.py +++ b/agv_calib_brain/src/deployment/tools/validate_dataset_index.py @@ -29,21 +29,33 @@ TASK_REQUIREMENTS = { }, "control": { "section": "control", - "all_of": ["control_evaluation_data_files"], + "all_of": [ + "control_evaluation_data_files", + "reference_signal_files", + "chassis_response_files", + "truth_trajectory_files", + ], }, "sensor_intrinsic": { "section": "sensor", - "all_of": ["image_sample_files"], + "any_of": ["image_sample_files", "imu_sample_files", "synchronized_dataset_files"], "warn_if_missing": ["target_detection_files"], }, "sensor_extrinsic": { "section": "sensor", - "all_of": ["image_sample_files", "robot_pose_sample_files"], - "warn_if_missing": ["target_detection_files"], + "any_of": [ + "image_sample_files", + "pointcloud_sample_files", + "laser_scan_sample_files", + "imu_sample_files", + "synchronized_dataset_files", + ], + "warn_if_missing": ["target_detection_files", "robot_pose_sample_files"], }, "hand_eye": { "section": "sensor", - "all_of": ["image_sample_files", "robot_pose_sample_files"], + "all_of": ["robot_pose_sample_files"], + "any_of": ["image_sample_files", "target_detection_files", "synchronized_dataset_files"], }, } diff --git a/agv_calib_brain/src/docs/algorithm_template_contract.md b/agv_calib_brain/src/docs/algorithm_template_contract.md index 31ce4a8..dda6e08 100644 --- a/agv_calib_brain/src/docs/algorithm_template_contract.md +++ b/agv_calib_brain/src/docs/algorithm_template_contract.md @@ -353,8 +353,37 @@ external、chassis、control 和 sensor 四个本地算法服务都读取了同 - 跟踪丢失率过高。 - 标靶观测数量不足。 +### 四角 LiDAR + 标定球真实外部真值定位模板 + +这部分是车间电脑侧真实外部真值定位入口,不放在车端 agent 中。模板位置: + +- `src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_localization_node.py` +- `src/site_deployment/workshop_external_lidar_localization_real/four_lidar_ball_algorithm.py` +- `src/site_deployment/workshop_external_lidar_localization_real/config/four_lidar_ball_template.yaml` + +算法实现者主要填充 `FourLidarBallLocalizationAlgorithm.solve(...)`。模板已经固定: + +- 四角 LiDAR 输入:`frames`,每帧包含 `lidar_id`、`frame_id`、`stamp_us`、`points_xyz`。 +- LiDAR 安装配置:`lidar_mounts`,每台 LiDAR 有 topic、frame 和车间坐标系安装位姿。 +- 标定球配置:`ball_targets`,每个球有半径和 `base_link` 下安装坐标。 +- 输出:`FourLidarBallLocalizationOutput`,最终会转成 `ExternalLocalizationTelemetry`。 +- 发布 topic:默认 `/workshop/external_localization/vehicle/pose`。 +- source name:默认 `workshop_four_lidar_ball_truth`。 + +算法输出必须表达车辆 `base_link` 在车间坐标系下的位姿,不能只输出某个标定球球心位置。只有一个标定球时通常无法唯一确定车辆 yaw;需要完整车辆位姿时,建议至少两个球,三球及以上更稳健。 + +真实部署时,`external_localization_service` 应覆盖这些参数: + +```text +external_telemetry_topic:=/workshop/external_localization/vehicle/pose +expected_reference_source_name:=workshop_four_lidar_ball_truth +``` + ## 底盘标定模板 +底盘算法更详细的字段、目标参数和产物要求见 +`src/docs/chassis_calibration_algorithm_contract.md`。 + 模板入口: - `src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp` @@ -584,6 +613,36 @@ external、chassis、control 和 sensor 四个本地算法服务都读取了同 当前完整 smoke 只跑 `front_camera_intrinsic` 作为 sensor intrinsic 的最小代表链路。它验证的是任务分发、结果回填和 report 汇总,不代表所有传感器子类型都已经完成真实算法验证。 +传感器现场任务、数据合同和参数交接模板已经固定在: + +- `src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml` +- `src/site_deployment/workshop_sensor_calibration_real/sensor_data_contract.md` +- `src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml` + +真实算法完成后应输出: + +```bash +/sensor/estimated_params.yaml +``` + +再由车间电脑侧生成待审批包: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/stage_sensor_parameter_commit.py \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --estimated-params /data/agv_calib/site_a/session_001/sensor/estimated_params.yaml +``` + +人工审批后生成车端写参交接包: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/approve_sensor_pending_parameters.py \ + /data/agv_calib/site_a/session_001/sensor/pending_sensor_commit.yaml \ + --operator-id operator_001 +``` + +算法文件不直接写入车辆传感器参数。参数写入车辆由后续车端写参适配器消费交接包完成。 + ## 报告和冒烟测试要求 模板维护者需要保证每个算法接入后,最终 report 能读到这些信息: diff --git a/agv_calib_brain/src/docs/chassis_calibration_algorithm_contract.md b/agv_calib_brain/src/docs/chassis_calibration_algorithm_contract.md new file mode 100644 index 0000000..ef6b2b8 --- /dev/null +++ b/agv_calib_brain/src/docs/chassis_calibration_algorithm_contract.md @@ -0,0 +1,176 @@ +# 底盘标定算法模板合同 + +本文档固定底盘标定算法实现边界。算法同事只需要填充四个底盘专属文件,不应改 node、gateway、orchestrator、report 或现场采集工具。 + +## 填充文件 + +- `src/core/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp` + +## 统一输入 + +四类算法都从 `ChassisCalibrationInput` 读取输入: + +- `input.request`:动作原语请求、`request_id`、任务目的、速度、距离、超时等。 +- `input.vehicle_profile`:车辆静态画像、底盘类型、车辆 ID、基础几何和能力配置。 +- `input.chassis_type`:算法分发后的底盘类型。 +- `input.chassis_capability`、`input.chassis_readiness`、`input.chassis_work_mode`:底盘能力和状态。 +- `input.applied_chassis_parameters`:当前已生效底盘参数,作为拟合初值或对照基线。 +- `input.latest_chassis_telemetry`、`input.chassis_telemetry_history`:轻量在线遥测。 +- `input.latest_external_localization_telemetry`、`input.external_localization_telemetry_history`:轻量外部真值状态。 +- `input.data_window_start_timestamp_us`、`input.data_window_end_timestamp_us`:本次数据窗口。 +- `input.chassis_motion_data_files`:底盘遥测 CSV。 +- `input.actuator_command_files`:执行器命令 CSV。 +- `input.truth_trajectory_files`:外部真值轨迹 CSV。 + +真实标定优先读取三类 CSV。CSV 字段协议见 `src/site_deployment/workshop_chassis_calibration_real/chassis_csv_contract.md`。 + +## 动作原语输入 + +通用模板入口已经放行并校验这些动作原语: + +- `STRAIGHT_LINE`:要求 `target_distance_m > 0`、`target_speed_ms > 0`。 +- `ARC`:要求 `target_speed_ms > 0`、`radius_m > 0`、`sweep_angle_deg != 0`。 +- `IN_PLACE_ROTATION`:要求 `target_yaw_deg != 0`、`target_angular_vel_deg_s > 0`。 +- `STEERING_SWEEP`:要求目标角为有效数字,且幅值、频率、持续时间都大于 0。 +- `LATERAL_TRANSLATION`:要求 `target_speed_ms > 0`、`target_distance_m > 0`。 +- `DIAGONAL_MOTION`:要求 `target_speed_ms > 0`、`target_distance_m > 0`,运动方向角为有效数字。 +- `MODULE_ALIGNMENT`:要求模块 ID 非空,零位角为有效数字,容差大于 0。 +- `COORDINATED_STEERING`:要求模块 ID 非空,目标角为有效数字,保持时间大于 0。 + +所有动作原语还要求 `timeout_sec > 0`。通用模板只做字段合法性检查,不在这一层强行限制“某种底盘只能跑哪些动作”;具体算法可以按底盘类型和现场能力继续做更严格的组合检查。 + +现场动作序列的默认模板见 `src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml`。该文件用于固定四类底盘各自要跑的动作组合,算法实现方可以按 `task_code` 或 `primitive_type` 区分不同数据段。 + +## 统一输出 + +四类算法都必须回填 `output.response.result`: + +- `success`:算法是否完成。 +- `error_code`:成功为 OK,失败必须写非 OK 错误码。 +- `message`:简短结果说明。 +- `job_id`:沿用 `input.request.goal.header.request_id`。 +- `data_quality_passed`:输入数据是否满足求解条件。 +- `suitable_for_commit`:是否建议进入参数提交或人工确认。 +- `recommended_parameter_version`:本轮结果版本号。 +- `estimated_straight_line_bias`:直线跑偏估计。 +- `validation_summary`:验收指标。 +- `estimated_params`:本轮估计底盘参数。 +- `artifacts`:结果 YAML、诊断 JSON、拟合残差 CSV、图表等产物引用。 + +算法完成但质量不达标时,允许 `success=true`、`data_quality_passed=true`、`suitable_for_commit=false`。采集数据不足、时间同步不可靠、真值缺失、底盘状态不安全时,应返回 `false` 并写清楚 `failure_reason`。 + +## 验收指标 + +`validation_summary` 至少要填: + +- `max_lateral_error_m` +- `max_yaw_error_rad` +- `rms_lateral_error_m` +- `rms_yaw_error_rad` +- `repeatability_error_m` +- `curvature_error` +- `module_consistency_error` +- `auto_acceptance_passed` + +这些指标用于总控 stage result 和最终 report。真实算法不能只写默认 0 值后直接声明可提交。 + +## 阿克曼底盘 + +目标参数: + +- `estimated_params.selected_specific_params.value = ACKERMANN` +- `estimated_params.ackermann.front_left_steer_zero_offset_deg` +- `estimated_params.ackermann.front_right_steer_zero_offset_deg` +- `estimated_params.ackermann.rear_left_wheel_radius_m` +- `estimated_params.ackermann.rear_right_wheel_radius_m` +- `estimated_params.ackermann.steering_ratio` +- 可选通用参数:`common.effective_wheel_base_m`、`common.longitudinal_scale`、`common.yaw_scale`、`common.straight_line_bias` + +建议输入动作:直线、圆弧、舵角扫动。 + +## 差速轮底盘 + +已确认目标参数: + +- `estimated_params.selected_specific_params.value = DIFFERENTIAL` +- `estimated_params.differential.left_wheel_radius_m` +- `estimated_params.differential.right_wheel_radius_m` +- `estimated_params.differential.axle_track_width_m` +- 可选:`left_encoder_scale`、`right_encoder_scale` +- 可选通用参数:`common.longitudinal_scale`、`common.yaw_scale`、`common.straight_line_bias` + +建议输入动作:直线、原地旋转、左右转弯。只用直线数据通常不足以稳定估计轮距。 + +## 单舵轮底盘 + +预留目标参数: + +- `estimated_params.selected_specific_params.value = SINGLE_STEER` +- `estimated_params.single_steer.drive_wheel_radius_m` +- `estimated_params.single_steer.steer_zero_offset_deg` +- `estimated_params.single_steer.steering_ratio` +- `estimated_params.single_steer.drive_encoder_scale` +- 可选通用参数:`common.effective_wheel_base_m`、`common.longitudinal_scale`、`common.yaw_scale` + +建议输入动作:直线、舵角扫动、圆弧。 + +## 多舵轮底盘 + +预留目标参数: + +- `estimated_params.selected_specific_params.value = MULTI_STEER` +- `estimated_params.multi_steer.modules[].module_id` +- `estimated_params.multi_steer.modules[].wheel_radius_m` +- `estimated_params.multi_steer.modules[].steer_zero_offset_deg` +- `estimated_params.multi_steer.modules[].module_pos_x_m` +- `estimated_params.multi_steer.modules[].module_pos_y_m` +- 可选通用参数:`common.longitudinal_scale`、`common.lateral_scale`、`common.yaw_scale` + +建议输入动作:直线、横移、斜移、模块零位检查、模块协同转向。 + +## 产物要求 + +建议每次真实算法至少输出两个产物: + +- 参数结果 YAML:记录车辆 ID、会话 ID、算法版本、输入文件、估计参数、是否建议提交。 +- 诊断 JSON:记录样本数、时间窗口、有效运动距离、真值质量、残差摘要、失败原因或质量警告。 + +产物通过 `output.response.result.artifacts` 返回,最终 report 会引用这些文件。 + +## 参数提交边界 + +算法不直接写车端参数。真实算法完成后,应先输出底盘参数 YAML,字段模板见 `src/site_deployment/workshop_chassis_calibration_real/config/chassis_estimated_params_template.yaml`。 + +现场侧再调用: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/stage_chassis_parameter_commit.py \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --estimated-params /data/agv_calib/site_a/session_001/chassis/estimated_params.yaml +``` + +该工具只生成待审批/待提交参数包,不假装写入真实车端。真正的持久化写入由后续车端参数写入适配器完成,并以待提交包中的摘要和版本作为输入。 + +现场会话推荐统一走收尾入口: + +```bash +python3 src/deployment/tools/finalize_site_session.py \ + --session-dir /data/agv_calib/site_a/session_001 \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --chassis-estimated-params /data/agv_calib/site_a/session_001/chassis/estimated_params.yaml +``` + +该入口会把底盘待提交包路径、参数版本、审批状态和算法输出参数文件写回 `dataset_index.yaml` 的 `metadata`。总控最终 report 会读取这些字段,把 `estimated_params.yaml` 和 `pending_chassis_commit.yaml` 放入 `report_files`,并在 report metadata 中给出底盘待提交状态。 + +人工审批后,车间电脑侧再调用: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/approve_chassis_pending_parameters.py \ + /data/agv_calib/site_a/session_001/chassis/pending_chassis_commit.yaml \ + --operator-id operator_001 +``` + +该工具只在车间电脑侧完成审批、摘要复核和交接包生成,不调用车端写参服务。最终链路固定为:车端执行动作,车间电脑采集数据并运行算法,车间电脑生成待提交包和已审批交接包;车端写入适配器后续再消费这个交接包。 diff --git a/agv_calib_brain/src/docs/control_calibration_algorithm_contract.md b/agv_calib_brain/src/docs/control_calibration_algorithm_contract.md new file mode 100644 index 0000000..c23976f --- /dev/null +++ b/agv_calib_brain/src/docs/control_calibration_algorithm_contract.md @@ -0,0 +1,167 @@ +# 运控参数标定算法模板合同 + +本文档固定运控参数标定算法与车间系统之间的输入、输出和交接边界。具体 PID、MPC、LQR、Pure Pursuit 求解算法由算法工程师实现;本仓库只提供模板、数据输入合同和车间电脑侧交接流程。 + +## 运行边界 + +运控标定在真实车间中的边界如下: + +- 车间电脑负责创建会话、下发运控评估任务、采集数据、运行标定算法和生成参数交接包。 +- 车端 Windows agent 只负责执行评估动作、回传执行结果和遥测。 +- 外部真值定位来自车间四角 LiDAR + 车端标定球系统,写入 `truth_trajectory.csv`。 +- 参数写入车辆这一步当前只生成交接包,不直接调用车端写参接口。 + +## 算法输入 + +运控算法应读取 `dataset_index.yaml` 转换出的 `data_input.*` 参数,或直接读取 `dataset_index.yaml` 中的文件索引。 + +必需输入: + +- `data_input.control_evaluation_data_files` +- `data_input.reference_signal_files` +- `data_input.chassis_response_files` +- `data_input.truth_trajectory_files` + +对应现场 CSV 协议见: + +```bash +src/site_deployment/workshop_control_calibration_real/control_csv_contract.md +``` + +真实采集入口: + +```bash +src/site_deployment/workshop_control_calibration_real/capture_control_session.py +``` + +合成数据 smoke 入口: + +```bash +src/site_deployment/workshop_control_calibration_real/replay_control_smoke.py +``` + +## C++ 模板入口 + +ROS 侧模板入口: + +- `src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp` +- `src/core/agv_calib_core/control_calibration_service/src/control_calibration_algorithm_template.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/mpc_control_calibration_algorithm.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/lqr_control_calibration_algorithm.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/pure_pursuit_control_calibration_algorithm.cpp` + +算法实现应主要修改具体算法文件,不应改 node 层通信代码。 + +## 算法输出 + +算法输出文件建议固定为: + +```bash +/control/estimated_params.yaml +``` + +模板见: + +```bash +src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_template.yaml +``` + +核心字段: + +- `schema_version`: 当前为 `1`。 +- `session_id`: 本次会话 ID。 +- `vehicle_id`: 车辆 ID,必须与 `dataset_index.session.vehicle_id` 一致。 +- `chassis_type`: `ackermann`、`differential`、`single_steer_wheel` 或 `multi_steer_wheel`。 +- `control_axis`: `lateral_control`、`longitudinal_control` 或 `combined`。 +- `controller_algorithm`: `pid`、`mpc`、`lqr` 或 `pure_pursuit`。 +- `control_role`: 控制器角色,例如 `path_tracking_outer_loop`、`speed_loop`、`heading_loop`、`yaw_rate_loop`、`steering_angle_inner_loop`。 +- `parameter_version`: 算法输出的参数版本号。 +- `estimated_params`: 算法估计出的参数。 +- `quality.data_quality_passed`: 数据质量是否达标。 +- `quality.suitable_for_commit`: 是否允许进入待审批包。 +- `quality.validation_summary`: 验收指标摘要。 + +只有 `data_quality_passed=true` 且 `suitable_for_commit=true` 的输出,才允许生成待审批包。 + +算法和控制轴约束: + +- `pid` 可以用于纵向速度、差速航向/角速度、单舵轮/多舵轮/阿克曼舵角内环等回路。 +- 使用 `pid` 时必须填写明确的 `control_role` 和 `loop_name`,例如 `speed_loop/speed`、`heading_loop/heading`、`yaw_rate_loop/yaw_rate`、`steering_angle_inner_loop/steering_angle`。 +- `combined` 且使用 `pid` 时,必须使用 `pid_loops` 列表分别描述每个 PID 回路,不能用一个含糊的 PID 表达横纵向联合参数。 +- 横向路径跟踪外环仍推荐按现场控制器选择 `pure_pursuit`、`lqr` 或 `mpc`;如果现场确实是 PID 外环,则用 `control_axis: lateral_control` 并明确 `control_role` 和 `loop_name`。 + +## 底盘类型默认回路 + +差速轮推荐: + +- `path_tracking_outer_loop`: `pure_pursuit` / `lqr` / `mpc`,输出线速度和角速度参考。 +- `speed_loop`: `pid`,整车线速度回路。 +- `yaw_rate_loop` 或 `heading_loop`: `pid`,角速度或航向回路。 +- `wheel_speed_inner_loop`: `pid`,左右轮速度内环。 + +单舵轮推荐: + +- `path_tracking_outer_loop`: `pure_pursuit` / `lqr` / `mpc`,输出目标速度和目标舵角。 +- `speed_loop`: `pid`,驱动轮速度或整车速度回路。 +- `steering_angle_inner_loop`: `pid`,舵角位置内环。 + +多舵轮推荐: + +- `path_tracking_outer_loop`: `pure_pursuit` / `lqr` / `mpc`,输出整车速度、角速度或模块目标。 +- `wheel_speed_inner_loop`: `pid`,各驱动轮速度内环。 +- `module_steering_inner_loop`: `pid`,各舵轮模块角度内环。 + +阿克曼推荐: + +- `path_tracking_outer_loop`: `pure_pursuit` / `lqr` / `mpc`,输出目标曲率或目标前轮转角。 +- `speed_loop`: `pid`,纵向速度回路。 +- `steering_angle_inner_loop`: `pid`,转角执行器内环。 + +## 参数交接 + +生成待审批包: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/stage_control_parameter_commit.py \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --estimated-params /data/agv_calib/site_a/session_001/control/estimated_params.yaml +``` + +输出: + +```bash +/data/agv_calib/site_a/session_001/control/pending_control_commit.yaml +``` + +审批交接: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/approve_control_pending_parameters.py \ + /data/agv_calib/site_a/session_001/control/pending_control_commit.yaml \ + --operator-id operator_001 +``` + +输出: + +```bash +/data/agv_calib/site_a/session_001/control/approved_control_parameter_handoff.yaml +``` + +`dataset_index.yaml` 的 `metadata` 会记录运控算法输出、待审批包、审批状态和交接包路径。总控 report 会把这些文件加入 `report_files`。 + +## 验收建议 + +真实算法至少应在 `quality.validation_summary` 写入: + +- 横向误差均方根。 +- 航向误差均方根。 +- 速度误差均方根。 +- 超调比例。 +- 收敛时间。 +- 停车位置误差。 +- 最大 jerk。 +- 控制饱和占比。 +- 自动验收是否通过。 + +模板不会替算法做通过判定。算法工程师需要根据现场工艺要求设置阈值,并明确写出 `suitable_for_commit`。 diff --git a/agv_calib_brain/src/docs/sensor_calibration_algorithm_contract.md b/agv_calib_brain/src/docs/sensor_calibration_algorithm_contract.md new file mode 100644 index 0000000..c192e8e --- /dev/null +++ b/agv_calib_brain/src/docs/sensor_calibration_algorithm_contract.md @@ -0,0 +1,168 @@ +# 传感器标定算法模板合同 + +本文档固定传感器内参、外参和手眼标定算法与车间系统之间的输入、输出和交接边界。具体相机、IMU、LiDAR、手眼求解算法由算法工程师实现;本仓库只提供模板、数据输入合同和车间电脑侧交接流程。 + +## 运行边界 + +- 车间电脑负责创建会话、下发传感器标定任务、采集或接入数据、运行标定算法和生成参数交接包。 +- 车端 Windows agent 只负责按现场需要执行采集动作、回传数据或回传采集结果。 +- 外部真值定位来自车间四角 LiDAR + 车端标定球系统,可作为外参和 IMU 外参的辅助输入。 +- 参数写入车辆这一步当前只生成交接包,不直接调用车端写参接口。 + +## C++ 模板入口 + +算法实现者主要修改这些文件: + +- `src/core/agv_calib_core/sensor_calibration_service/src/camera_intrinsic_calibration_algorithm.cpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/imu_intrinsic_calibration_algorithm.cpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/sensor_to_base_extrinsic_calibration_algorithm.cpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/hand_eye_calibration_algorithm.cpp` + +不要修改 node 层、gateway、orchestrator、report 或现场采集工具,除非明确需要扩展模板输入字段。 + +## 任务类型 + +当前固定的任务子类型: + +- 相机内参:`front_camera_intrinsic`、`downward_camera_intrinsic`。 +- IMU 内参:`imu_intrinsic`。 +- 传感器外参:`front_camera_extrinsic`、`downward_camera_extrinsic`、`imu_extrinsic`、`lidar_2d_extrinsic`、`lidar_3d_extrinsic`。 +- 手眼:`eye_in_hand`、`eye_to_hand`。 + +现场任务 profile: + +```bash +src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml +``` + +## 数据输入 + +算法应读取 `SensorCalibrationInput` 中的字段,或读取 `dataset_index.yaml` 转换出的 `data_input.*` 参数。 + +主要输入: + +- `input.request` +- `input.task_type` +- `input.task_subtype` +- `input.target_sensor_id` +- `input.target_sensor_type` +- `input.camera_intrinsic_task` +- `input.imu_intrinsic_task` +- `input.sensor_to_base_extrinsic_task` +- `input.hand_eye_task` +- `input.image_sample_files` +- `input.imu_sample_files` +- `input.pointcloud_sample_files` +- `input.laser_scan_sample_files` +- `input.target_detection_files` +- `input.robot_pose_sample_files` +- `input.synchronized_dataset_files` + +现场数据合同: + +```bash +src/site_deployment/workshop_sensor_calibration_real/sensor_data_contract.md +``` + +数据格式样例: + +```bash +src/site_deployment/workshop_sensor_calibration_real/config/data_examples/ +``` + +车间电脑侧数据校验和索引写入工具: + +```bash +src/site_deployment/workshop_sensor_calibration_real/sensor_dataset_manifest.py +``` + +车间电脑侧预处理和检测入口: + +```bash +src/site_deployment/workshop_sensor_calibration_real/sensor_preprocess_pipeline.py +``` + +该入口负责生成预处理质量报告,并可在安装 OpenCV 时从图像生成棋盘格或 ChArUco 检测结果。没有 OpenCV 或检测算法由其他团队提供时,只要外部程序按数据合同输出 `target_detections.json` 即可。 + +## 算法输出 + +算法输出文件建议固定为: + +```bash +/sensor/estimated_params.yaml +``` + +模板: + +```bash +src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml +``` + +核心字段: + +- `session_id`:本次会话 ID。 +- `vehicle_id`:车辆 ID,必须与 `dataset_index.session.vehicle_id` 一致。 +- `sensor_id`:目标传感器 ID。 +- `selected_task`:`camera_intrinsic`、`imu_intrinsic`、`sensor_extrinsic` 或 `hand_eye`。 +- `task_subtype`:具体子类型。 +- `parameter_version`:本轮参数版本。 +- `estimated_params`:算法估计出的参数。 +- `quality.data_quality_passed`:数据质量是否达标。 +- `quality.suitable_for_commit`:是否允许进入待审批包。 +- `quality.validation_summary.auto_acceptance_passed`:自动验收是否通过。 + +只有以上质量字段为 true 的输出,才允许生成待审批包。 + +## 参数交接 + +生成待审批包: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/stage_sensor_parameter_commit.py \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --estimated-params /data/agv_calib/site_a/session_001/sensor/estimated_params.yaml +``` + +审批交接: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/approve_sensor_pending_parameters.py \ + /data/agv_calib/site_a/session_001/sensor/pending_sensor_commit.yaml \ + --operator-id operator_001 +``` + +`dataset_index.yaml` 的 `metadata` 会记录传感器算法输出、待审批包、审批状态和交接包路径。总控 report 会把这些文件加入 `report_files`。 + +车间电脑侧导出车辆侧参数配置: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/apply_sensor_parameter_handoff.py \ + /data/agv_calib/site_a/session_001/sensor/approved_sensor_parameter_handoff.yaml \ + --operator-id operator_001 +``` + +该工具只生成可交给车端适配器消费的配置文件和回执,不直接调用 Windows 车端或厂商 SDK。 + +## 本地回放 smoke + +传感器本地回放 smoke: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py +``` + +它会用仓库内示例数据验证 `sensor_intrinsic -> sensor_extrinsic -> hand_eye` 三类传感器参数的车间电脑侧数据、交接和应用落盘链路。 + +## 验收建议 + +真实算法至少应在 `quality.validation_summary` 写入: + +- 重投影误差。 +- 平移残差。 +- 旋转残差。 +- 平面或点云配准残差。 +- 重复性误差。 +- 有效样本数量。 +- 自动验收是否通过。 + +模板不会替算法做通过判定。算法工程师需要根据现场工艺要求设置阈值,并明确写出 `suitable_for_commit`。 diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py index 5cd0cc9..3323f51 100644 --- a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py +++ b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py @@ -92,7 +92,7 @@ def parse_args(): parser.add_argument("--lidar-2d-target-bottom-z", type=float, default=0.08, help="2D LiDAR 靶标底部高度(米)") parser.add_argument("--lidar-2d-target-slope-angle-deg", type=float, default=45.0, help="2D LiDAR 斜板相对垂直基准面的夹角(度)") parser.add_argument("--disable-external-telemetry", action="store_true", help="不发布 external 仿真遥测") - parser.add_argument("--external-telemetry-topic", type=str, default="/isaac/external_localization/telemetry", help="external 仿真遥测话题") + parser.add_argument("--external-telemetry-topic", type=str, default="/isaac/external_localization/vehicle/pose", help="external 仿真遥测话题") parser.add_argument("--external-reference-source-name", type=str, default="isaac_sim_truth_source", help="external 真值源名称") parser.add_argument("--external-workcell-zone-id", type=str, default="isaac_workcell_zone_a", help="external 所属工位 ID") parser.add_argument("--external-publish-hz", type=float, default=20.0, help="external 遥测发布频率(Hz)") diff --git a/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py b/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py index e9a819f..3fec1c0 100644 --- a/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py +++ b/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py @@ -1,167 +1,17 @@ #!/usr/bin/env python3 -"""Bridge external truth pose from ROS to the simulated WiFi6 TCP vehicle link.""" +"""兼容入口:仿真端复用现场外部定位位姿桥。""" from __future__ import annotations -import argparse -import json -import socket -import struct -import time -from typing import Any - -import rclpy -from rclpy.executors import ExternalShutdownException - -try: - from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry -except ImportError: - ExternalLocalizationTelemetry = None +import sys +from pathlib import Path -EXTERNAL_POSE_PUSH_REQ = 31 -EXTERNAL_POSE_PUSH_RSP = 32 +PROJECT_ROOT = Path(__file__).resolve().parents[3] +REAL_BRIDGE_DIR = PROJECT_ROOT / "src" / "site_deployment" / "workshop_external_pose_bridge_real" +sys.path.insert(0, str(REAL_BRIDGE_DIR)) - -def now_us() -> int: - return int(time.time() * 1_000_000) - - -def read_exactly(conn: socket.socket, size: int) -> bytes: - chunks: list[bytes] = [] - remaining = size - while remaining > 0: - chunk = conn.recv(remaining) - if not chunk: - raise ConnectionError("connection closed") - chunks.append(chunk) - remaining -= len(chunk) - return b"".join(chunks) - - -def send_request( - host: str, - port: int, - msg_type: int, - payload: dict[str, Any], - timeout_sec: float, -) -> tuple[int, dict[str, Any]]: - encoded = json.dumps(payload, ensure_ascii=False, separators=(",", ":")).encode("utf-8") - with socket.create_connection((host, port), timeout=timeout_sec) as conn: - conn.settimeout(timeout_sec) - conn.sendall(struct.pack(" {args.vehicle_host}:{args.vehicle_port}" - ) - - def rate_limited(self) -> bool: - if self.args.max_hz <= 0.0: - return False - now = time.monotonic() - period = 1.0 / self.args.max_hz - if now - self.last_send_monotonic < period: - return True - self.last_send_monotonic = now - return False - - def on_pose(self, msg: Any) -> None: - if self.rate_limited(): - return - - pose = msg.workshop_pose - source_name = self.args.reference_source_name or str(msg.reference_source_name or "external_truth_wifi6") - payload = { - "hardware_timestamp_us": int(msg.hardware_timestamp_us or now_us()), - "pose_valid": bool(msg.pose_valid), - "workshop_pose": { - "x_m": float(pose.x_m), - "y_m": float(pose.y_m), - "z_m": float(pose.z_m), - "roll_rad": float(pose.roll_rad), - "pitch_rad": float(pose.pitch_rad), - "yaw_rad": float(pose.yaw_rad), - }, - "position_stddev_m": float(msg.position_stddev_m), - "yaw_stddev_rad": float(msg.yaw_stddev_rad), - "tracking_loss_ratio": float(msg.tracking_loss_ratio), - "time_sync_offset_ms": float(msg.time_sync_offset_ms), - "quality_score": float(msg.quality_score), - "observed_target_count": int(msg.observed_target_count), - "reference_source_name": source_name, - "active_job_id": str(msg.active_job_id or ""), - } - - try: - rsp_type, rsp = send_request( - self.args.vehicle_host, - self.args.vehicle_port, - EXTERNAL_POSE_PUSH_REQ, - payload, - self.args.timeout_sec, - ) - if rsp_type != EXTERNAL_POSE_PUSH_RSP or not rsp.get("success", False): - raise RuntimeError(f"unexpected response type={rsp_type}, payload={rsp}") - self.sent_count += 1 - except Exception as exc: - self.fail_count += 1 - now = time.monotonic() - if rclpy.ok() and now - self.last_error_log_monotonic >= self.args.error_log_interval_sec: - self.last_error_log_monotonic = now - self.node.get_logger().warning( - f"failed to push external pose over wifi6_sim_tcp: {exc}; fail_count={self.fail_count}" - ) - - -def parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser(description="外部真值位姿 ROS -> WiFi6/TCP 车端桥") - parser.add_argument("--source-topic", default="/isaac/external_localization/telemetry") - parser.add_argument("--vehicle-host", default="127.0.0.1") - parser.add_argument("--vehicle-port", type=int, default=9000) - parser.add_argument("--reference-source-name", default="") - parser.add_argument("--max-hz", type=float, default=30.0) - parser.add_argument("--timeout-sec", type=float, default=2.0) - parser.add_argument("--error-log-interval-sec", type=float, default=2.0) - return parser.parse_args() - - -def main() -> int: - args = parse_args() - rclpy.init(args=None) - bridge = None - try: - bridge = ExternalPoseWifi6Bridge(args) - rclpy.spin(bridge.node) - except (KeyboardInterrupt, ExternalShutdownException): - pass - finally: - if bridge is not None: - bridge.node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - return 0 +from workshop_external_pose_bridge import main # noqa: E402 if __name__ == "__main__": diff --git a/agv_calib_brain/src/simulation/tools/isaac_vehicle_unified_agent_sim.py b/agv_calib_brain/src/simulation/tools/isaac_vehicle_unified_agent_sim.py index 78682c5..ec8b366 100644 --- a/agv_calib_brain/src/simulation/tools/isaac_vehicle_unified_agent_sim.py +++ b/agv_calib_brain/src/simulation/tools/isaac_vehicle_unified_agent_sim.py @@ -152,7 +152,7 @@ def build_parser() -> argparse.ArgumentParser: parser.add_argument("--bind-host", default="0.0.0.0") parser.add_argument("--vehicle-port", type=int, default=9000) parser.add_argument("--cmd-vel-topic", default="/vehicle/demo_agv_001/actuator/cmd_vel") - parser.add_argument("--external-pose-topic", default="/isaac/external_localization/telemetry") + parser.add_argument("--external-pose-topic", default="/isaac/external_localization/vehicle/pose") parser.add_argument( "--external-pose-transport", choices=["ros_topic", "wifi6_tcp", "both"], diff --git a/agv_calib_brain/src/simulation/tools/launch_sim_stack.py b/agv_calib_brain/src/simulation/tools/launch_sim_stack.py index f71b908..482c6bb 100755 --- a/agv_calib_brain/src/simulation/tools/launch_sim_stack.py +++ b/agv_calib_brain/src/simulation/tools/launch_sim_stack.py @@ -39,7 +39,7 @@ def build_vehicle_agent_command(profile: dict, python_executable: str) -> list[s "--cmd-vel-topic", str(vehicle_agent["private_cmd_vel_topic"]), "--external-pose-topic", - str(vehicle_agent.get("external_pose_topic", "/isaac/external_localization/telemetry")), + str(vehicle_agent.get("external_pose_topic", "/isaac/external_localization/vehicle/pose")), "--external-pose-transport", str(vehicle_agent.get("external_pose_transport", "both")), "--chassis-telemetry-topic", @@ -162,9 +162,9 @@ def build_external_pose_bridge_command(profile: dict, python_executable: str) -> bridge = profile.get("external_pose_bridge", {}) command = [ python_executable, - str(PROJECT_ROOT / "src" / "simulation" / "tools" / "external_pose_wifi6_bridge.py"), + str(PROJECT_ROOT / "src" / "site_deployment" / "workshop_external_pose_bridge_real" / "workshop_external_pose_bridge.py"), "--source-topic", - str(bridge.get("source_topic", get_nested(profile, "vehicle_agent.external_pose_topic", "/isaac/external_localization/telemetry"))), + str(bridge.get("source_topic", get_nested(profile, "vehicle_agent.external_pose_topic", "/isaac/external_localization/vehicle/pose"))), "--vehicle-host", str(gateway.get("vehicle_host", "127.0.0.1")), "--vehicle-port", diff --git a/agv_calib_brain/src/simulation/tools/smoke_test_vehicle_agent.py b/agv_calib_brain/src/simulation/tools/smoke_test_vehicle_agent.py index 1eee39e..6c40d71 100755 --- a/agv_calib_brain/src/simulation/tools/smoke_test_vehicle_agent.py +++ b/agv_calib_brain/src/simulation/tools/smoke_test_vehicle_agent.py @@ -189,7 +189,7 @@ def main() -> int: vehicle_id = str(profile["vehicle_id"]) max_speed = as_float(get_nested(profile, "safety.max_speed_mps", 0.3), "safety.max_speed_mps") speed = min(abs(args.speed_mps), max_speed) - external_pose_topic = str(get_nested(profile, "vehicle_agent.external_pose_topic", "/isaac/external_localization/telemetry")) + external_pose_topic = str(get_nested(profile, "vehicle_agent.external_pose_topic", "/isaac/external_localization/vehicle/pose")) external_pose_transport = str(get_nested(profile, "vehicle_agent.external_pose_transport", "both")) external_pose_tcp_host = str(gateway.get("vehicle_host", gateway.get("control_host", "127.0.0.1"))) external_pose_tcp_port = as_int(gateway.get("vehicle_port", 9000), "workshop_pc.gateway.vehicle_port") 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 2f8ae9c..a53ac57 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 @@ -14,6 +14,7 @@ from __future__ import annotations import argparse +import importlib.util import os import sys import time @@ -72,6 +73,37 @@ TASK_STAGE_TYPES = { DEFAULT_TASKS = "external,chassis,control,sensor_intrinsic" +CHASSIS_TYPE_VALUES = { + "ackermann": ChassisType.ACKERMANN, + "differential": ChassisType.DIFFERENTIAL, + "single_steer_wheel": ChassisType.SINGLE_STEER_WHEEL, + "multi_steer_wheel": ChassisType.MULTI_STEER_WHEEL, +} + +CHASSIS_TYPE_DISPLAY_NAMES = { + "ackermann": "阿克曼", + "differential": "差速轮", + "single_steer_wheel": "单舵轮", + "multi_steer_wheel": "多舵轮", +} + +EXECUTION_POLICY_VALUES = { + "REQUIRED": StageExecutionPolicy.REQUIRED, + "OPTIONAL": StageExecutionPolicy.OPTIONAL, + "SKIP_IF_UNSUPPORTED": StageExecutionPolicy.SKIP_IF_UNSUPPORTED, +} + +PROFILE_STAGE_TYPE_VALUES = { + "EXTERNAL_REFERENCE_READY_CHECK_STAGE": WorkflowStageType.EXTERNAL_REFERENCE_READY_CHECK_STAGE, + "CHASSIS_CALIBRATION_STAGE": WorkflowStageType.CHASSIS_CALIBRATION_STAGE, + "CONTROL_CALIBRATION_STAGE": WorkflowStageType.CONTROL_CALIBRATION_STAGE, + "SENSOR_INTRINSIC_CALIBRATION_STAGE": WorkflowStageType.SENSOR_INTRINSIC_CALIBRATION_STAGE, + "SENSOR_EXTRINSIC_CALIBRATION_STAGE": WorkflowStageType.SENSOR_EXTRINSIC_CALIBRATION_STAGE, + "HAND_EYE_CALIBRATION_STAGE": WorkflowStageType.HAND_EYE_CALIBRATION_STAGE, +} + +PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx") + def now_us() -> int: return int(time.time() * 1_000_000) @@ -151,15 +183,16 @@ def make_sensor(sensor_id: str, sensor_type: int, mount_type: int, name: str) -> return sensor -def make_vehicle_profile(vehicle_id: str) -> VehicleProfile: +def make_vehicle_profile(vehicle_id: str, chassis_type_name: str) -> VehicleProfile: profile = VehicleProfile() + chassis_display_name = CHASSIS_TYPE_DISPLAY_NAMES[chassis_type_name] profile.base_info.vehicle_id = vehicle_id - profile.base_info.vehicle_name = "仿真阿克曼小车" - profile.base_info.model_name = "ackermann_sim" + profile.base_info.vehicle_name = f"仿真{chassis_display_name}底盘" + profile.base_info.model_name = f"{chassis_type_name}_smoke" profile.base_info.serial_number = f"{vehicle_id}_serial" profile.base_info.manufacturer = "AutoCalib Workshop" profile.base_info.description = "orchestrator e2e smoke profile" - profile.chassis_type.value = ChassisType.ACKERMANN + profile.chassis_type.value = CHASSIS_TYPE_VALUES[chassis_type_name] profile.base_link_frame = "base_link" profile.profile_version = "orchestrator_smoke_v1" @@ -193,6 +226,7 @@ def make_vehicle_profile(vehicle_id: str) -> VehicleProfile: WorkflowStageType.CONTROL_CALIBRATION_STAGE, WorkflowStageType.SENSOR_INTRINSIC_CALIBRATION_STAGE, WorkflowStageType.SENSOR_EXTRINSIC_CALIBRATION_STAGE, + WorkflowStageType.HAND_EYE_CALIBRATION_STAGE, ): stage = WorkflowStageType() stage.value = stage_value @@ -217,6 +251,103 @@ def parse_tasks(raw: str) -> list[str]: return tasks +def load_yaml(path: Path) -> dict: + try: + import yaml + except ImportError as exc: + raise RuntimeError("缺少 PyYAML,请先安装 yaml 模块。") from exc + 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 + + +def has_placeholder(value: object) -> bool: + return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS) + + +def read_profile_string(profile: dict, path: tuple[str, ...], default: str = "") -> str: + cursor = profile + 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 resolve_profile_path(raw_path: str, site_profile_path: Path) -> Path: + path = Path(raw_path).expanduser() + if path.is_absolute(): + return path.resolve(strict=False) + site_relative = (site_profile_path.parent / path).resolve(strict=False) + if site_relative.exists(): + return site_relative + src_relative = Path(__file__).resolve().parents[2] / path + if src_relative.exists(): + return src_relative.resolve(strict=False) + repo_relative = Path(__file__).resolve().parents[3] / path + return repo_relative.resolve(strict=False) + + +def maybe_apply_site_profile_defaults(args: argparse.Namespace) -> None: + if not args.site_profile: + if not args.chassis_profile_type: + args.chassis_profile_type = "ackermann" + return + + site_profile_path = Path(args.site_profile).expanduser().resolve(strict=False) + profile = load_yaml(site_profile_path) + + if not args.chassis_profile_type: + args.chassis_profile_type = ( + read_profile_string(profile, ("chassis_calibration", "chassis_type")) + or read_profile_string(profile, ("control_calibration", "chassis_type")) + or "ackermann" + ) + + if not args.chassis_action_profile: + raw_path = read_profile_string(profile, ("chassis_calibration", "action_profile_file")) + if raw_path: + candidate = resolve_profile_path(raw_path, site_profile_path) + if candidate.exists(): + args.chassis_action_profile = str(candidate) + + if not args.control_evaluation_profile: + raw_path = read_profile_string(profile, ("control_calibration", "evaluation_profile_file")) + if raw_path: + candidate = resolve_profile_path(raw_path, site_profile_path) + if candidate.exists(): + args.control_evaluation_profile = str(candidate) + + if not args.sensor_calibration_profile: + raw_path = read_profile_string(profile, ("sensor_calibration", "profile_file")) + if raw_path: + candidate = resolve_profile_path(raw_path, site_profile_path) + if candidate.exists(): + args.sensor_calibration_profile = str(candidate) + + reference_source_name = ( + read_profile_string(profile, ("external_pose_bridge", "reference_source_name")) + or read_profile_string(profile, ("external_localization", "reference_source_name")) + ) + if reference_source_name: + args.reference_source_name = reference_source_name + + external_topic = ( + read_profile_string(profile, ("external_pose_bridge", "source_topic")) + or read_profile_string(profile, ("external_localization", "output_topic")) + ) + if external_topic: + args.external_telemetry_topic = external_topic + + workcell_zone_id = read_profile_string(profile, ("external_localization", "workcell_zone_id")) + if workcell_zone_id: + args.workcell_zone_id = workcell_zone_id + + def task_params(task_name: str, args: argparse.Namespace) -> list[KeyValuePair]: if task_name == "external": return [ @@ -273,6 +404,97 @@ def task_params(task_name: str, args: argparse.Namespace) -> list[KeyValuePair]: return [] +def load_profile_tool(relative_path: str, module_name: str): + tool_path = Path(__file__).resolve().parents[2] / relative_path + if not tool_path.exists(): + raise FileNotFoundError(f"profile 工具不存在: {tool_path}") + spec = importlib.util.spec_from_file_location(module_name, tool_path) + if spec is None or spec.loader is None: + raise RuntimeError(f"无法加载 profile 工具: {tool_path}") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def make_requested_task_from_profile( + raw_task: dict, + default_stage_type_value: int, + default_reason: str, +) -> RequestedCalibrationTask: + task = RequestedCalibrationTask() + stage_type_name = str(raw_task.get("stage_type", "")) + task.stage_type.value = PROFILE_STAGE_TYPE_VALUES.get(stage_type_name, default_stage_type_value) + task.enabled = bool(raw_task.get("enabled", True)) + task.require_manual_approval = bool(raw_task.get("require_manual_approval", False)) + policy_name = str(raw_task.get("execution_policy", "REQUIRED")) + task.execution_policy.value = EXECUTION_POLICY_VALUES.get( + policy_name, + StageExecutionPolicy.REQUIRED, + ) + task.reason = str(raw_task.get("reason", default_reason)) + task.task_code = str(raw_task.get("task_code", "")) + task.target_id = str(raw_task.get("target_id", "")) + for item in raw_task.get("task_params", []): + task.task_params.append(kv(str(item["key"]), str(item["value"]))) + if not task.task_code: + raise ValueError("profile 导出的 task_code 不能为空。") + return task + + +def make_chassis_profile_tasks(args: argparse.Namespace) -> list[RequestedCalibrationTask]: + tool = load_profile_tool( + "site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py", + "chassis_action_profile_tool", + ) + profile_path = Path(args.chassis_action_profile).expanduser().resolve(strict=False) + profile = tool.validate_profile(tool.load_yaml(profile_path)) + exported = tool.export_requested_tasks(profile, args.chassis_profile_type) + return [ + make_requested_task_from_profile( + raw_task, + WorkflowStageType.CHASSIS_CALIBRATION_STAGE, + "现场底盘标定动作 profile", + ) + for raw_task in exported["requested_tasks"] + ] + + +def make_control_profile_tasks(args: argparse.Namespace) -> list[RequestedCalibrationTask]: + tool = load_profile_tool( + "site_deployment/workshop_control_calibration_real/control_evaluation_profile.py", + "control_evaluation_profile_tool", + ) + profile_path = Path(args.control_evaluation_profile).expanduser().resolve(strict=False) + profile = tool.validate_profile(tool.load_yaml(profile_path)) + exported = tool.export_requested_tasks(profile, args.chassis_profile_type) + return [ + make_requested_task_from_profile( + raw_task, + WorkflowStageType.CONTROL_CALIBRATION_STAGE, + "现场运控评估 profile", + ) + for raw_task in exported["requested_tasks"] + ] + + +def make_sensor_profile_tasks(task_name: str, args: argparse.Namespace) -> list[RequestedCalibrationTask]: + tool = load_profile_tool( + "site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py", + "sensor_calibration_profile_tool", + ) + profile_path = Path(args.sensor_calibration_profile).expanduser().resolve(strict=False) + profile = tool.validate_profile(tool.load_yaml(profile_path)) + exported = tool.export_requested_tasks(profile, {task_name}) + return [ + make_requested_task_from_profile( + raw_task, + TASK_STAGE_TYPES[task_name], + "现场传感器标定 profile", + ) + for raw_task in exported["requested_tasks"] + ] + + def make_requested_task(task_name: str, args: argparse.Namespace) -> RequestedCalibrationTask: task = RequestedCalibrationTask() task.stage_type.value = TASK_STAGE_TYPES[task_name] @@ -286,6 +508,16 @@ def make_requested_task(task_name: str, args: argparse.Namespace) -> RequestedCa return task +def make_requested_tasks(task_name: str, args: argparse.Namespace) -> list[RequestedCalibrationTask]: + if task_name == "chassis" and args.chassis_action_profile: + return make_chassis_profile_tasks(args) + if task_name == "control" and args.control_evaluation_profile: + return make_control_profile_tasks(args) + if task_name in {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"} and args.sensor_calibration_profile: + return make_sensor_profile_tasks(task_name, args) + return [make_requested_task(task_name, args)] + + def make_session_config(tasks: Iterable[str], args: argparse.Namespace) -> WorkshopSessionConfig: config = WorkshopSessionConfig() config.auto_commit_parameters = False @@ -299,7 +531,7 @@ def make_session_config(tasks: Iterable[str], args: argparse.Namespace) -> Works config.workcell_zone_id = args.workcell_zone_id config.reference_target_id = args.reference_target_id for task_name in tasks: - config.requested_tasks.append(make_requested_task(task_name, args)) + config.requested_tasks.extend(make_requested_tasks(task_name, args)) return config @@ -396,7 +628,7 @@ class OrchestratorSmoke: raise RuntimeError(f"注册车辆画像失败: {response.response.message}") print(f"[PASS] 已注册车辆画像: {self.args.vehicle_id}") - def create_session(self, profile: VehicleProfile, tasks: list[str]) -> str: + def create_session(self, profile: VehicleProfile, session_config: WorkshopSessionConfig) -> str: client = self.node.create_client(CreateWorkshopSession, "/workshop_v2/create_session") request = CreateWorkshopSession.Request() request.request.header = make_header(self.args.vehicle_id, "create_session", self.args.operator_id) @@ -406,7 +638,7 @@ class OrchestratorSmoke: request.request.operator_info.operator_name = "orchestrator smoke" request.request.operator_info.workstation_id = self.args.workstation_id request.request.operator_info.shift_id = "smoke" - request.request.config = make_session_config(tasks, self.args) + request.request.config = session_config request.request.workshop_line_id = self.args.workshop_line_id request.request.auto_build_execution_plan = True response = self.call_service(client, request, "/workshop_v2/create_session") @@ -503,9 +735,10 @@ class OrchestratorSmoke: if self.args.publish_fake_external_telemetry: self.spin_for(self.args.external_warmup_sec) - profile = make_vehicle_profile(self.args.vehicle_id) + profile = make_vehicle_profile(self.args.vehicle_id, self.args.chassis_profile_type) + session_config = make_session_config(tasks, self.args) self.register_vehicle_profile(profile) - session_id = self.create_session(profile, tasks) + session_id = self.create_session(profile, session_config) self.verify_session_visible(session_id) action_result = self.execute_session(session_id) report_response = action_result.result.result @@ -527,7 +760,7 @@ class OrchestratorSmoke: self.print_report(report) return 3 - expected_count = len(tasks) + expected_count = len(session_config.requested_tasks) actual_count = len(report.stage_results) if self.args.strict_stage_count and actual_count != expected_count: print( @@ -564,6 +797,8 @@ class OrchestratorSmoke: f"size={file_ref.size_bytes} " f"description={file_ref.description}" ) + for item in report.metadata: + print(f"[REPORT_METADATA] {item.key}={item.value}") for result in report.stage_results: print( "[STAGE] " @@ -577,6 +812,11 @@ class OrchestratorSmoke: def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description="workshop_orchestrator_v2 端到端验收脚本") + parser.add_argument( + "--site-profile", + default="", + help="可选现场部署 profile;填写后自动读取底盘动作、运控评估和传感器标定 profile 路径", + ) parser.add_argument("--vehicle-id", default="demo_agv_001") parser.add_argument("--tasks", default=DEFAULT_TASKS, help=f"逗号分隔任务列表,默认: {DEFAULT_TASKS}") parser.add_argument("--operator-id", default="smoke_operator") @@ -585,14 +825,35 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--workcell-zone-id", default="sim_workcell_zone") parser.add_argument("--reference-source-name", default="isaac_sim_truth_source") parser.add_argument("--reference-target-id", default="sim_reference_target") - parser.add_argument("--external-telemetry-topic", default="/isaac/external_localization/telemetry") + 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-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") parser.add_argument("--chassis-distance-m", type=float, default=0.2) parser.add_argument("--chassis-speed-mps", type=float, default=0.1) + parser.add_argument( + "--chassis-profile-type", + choices=sorted(CHASSIS_TYPE_VALUES), + default="", + help="车辆画像中的底盘类型;传入 --chassis-action-profile 时也用于选择动作序列", + ) + parser.add_argument( + "--chassis-action-profile", + default="", + help="可选底盘动作 profile;填写后会把 chassis 任务展开为多段底盘动作", + ) parser.add_argument("--control-velocity-mps", type=float, default=0.1) + parser.add_argument( + "--control-evaluation-profile", + default="", + help="可选运控评估 profile;填写后会把 control 任务展开为多段运控评估任务", + ) + parser.add_argument( + "--sensor-calibration-profile", + default="", + help="可选传感器标定 profile;填写后会把 sensor_intrinsic/sensor_extrinsic/hand_eye 展开为现场任务", + ) 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) @@ -608,6 +869,7 @@ def parse_args() -> argparse.Namespace: def main() -> int: args = parse_args() + maybe_apply_site_profile_defaults(args) rclpy.init(args=None) runner = OrchestratorSmoke(args) try: diff --git a/agv_calib_brain/src/simulation/vehicle_agent_sim/README.md b/agv_calib_brain/src/simulation/vehicle_agent_sim/README.md index a055b32..42fa6ce 100644 --- a/agv_calib_brain/src/simulation/vehicle_agent_sim/README.md +++ b/agv_calib_brain/src/simulation/vehicle_agent_sim/README.md @@ -58,7 +58,7 @@ python3 src/simulation/tools/isaac_vehicle_unified_agent_sim.py \ --vehicle-id demo_agv_001 \ --vehicle-port 9000 \ --cmd-vel-topic /vehicle/demo_agv_001/actuator/cmd_vel \ - --external-pose-topic /isaac/external_localization/telemetry \ + --external-pose-topic /isaac/external_localization/vehicle/pose \ --external-pose-transport wifi6_tcp \ --chassis-telemetry-topic /chassis/telemetry ``` diff --git a/agv_calib_brain/src/simulation/vehicle_agent_sim/scripts/isaac_vehicle_agent_sim.py b/agv_calib_brain/src/simulation/vehicle_agent_sim/scripts/isaac_vehicle_agent_sim.py index cd505f2..d5e91f6 100644 --- a/agv_calib_brain/src/simulation/vehicle_agent_sim/scripts/isaac_vehicle_agent_sim.py +++ b/agv_calib_brain/src/simulation/vehicle_agent_sim/scripts/isaac_vehicle_agent_sim.py @@ -1219,7 +1219,7 @@ def parse_args(): parser = argparse.ArgumentParser(description="Isaac vehicle-side agent simulator") parser.add_argument("--vehicle-id", default="demo_agv_001") parser.add_argument("--cmd-vel-topic", default="/vehicle/demo_agv_001/actuator/cmd_vel") - parser.add_argument("--external-pose-topic", default="/isaac/external_localization/telemetry") + parser.add_argument("--external-pose-topic", default="/isaac/external_localization/vehicle/pose") parser.add_argument( "--external-pose-transport", choices=["ros_topic", "wifi6_tcp", "both"], diff --git a/agv_calib_brain/src/site_deployment/README.md b/agv_calib_brain/src/site_deployment/README.md index f11f7f9..e070b3e 100644 --- a/agv_calib_brain/src/site_deployment/README.md +++ b/agv_calib_brain/src/site_deployment/README.md @@ -21,6 +21,19 @@ python3 src/deployment/tools/finalize_site_session.py \ --session-id session_001 ``` +生成总控会话任务示例: + +```bash +python3 src/deployment/tools/build_workshop_session_config.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --tasks external,chassis,control,sensor_intrinsic \ + --chassis-type differential \ + --chassis-action-profile src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml \ + --control-evaluation-profile src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml \ + --sensor-calibration-profile src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml \ + --output /data/agv_calib/session_001/workshop_session_config.yaml +``` + 本地回归命令: ```bash @@ -32,6 +45,11 @@ python3 src/deployment/tools/finalize_site_session.py \ 当前边界: - `vehicle_agent_real`:真实车端电脑适配器。 +- `workshop_external_lidar_localization_real`:车间电脑侧四角 LiDAR + 标定球外部真值定位模板。 +- `workshop_chassis_calibration_real`:底盘标定现场数据输入、索引写入和离线自检模板。 +- `workshop_control_calibration_real`:运控参数标定现场任务、数据输入和参数交接模板。 +- `workshop_sensor_calibration_real`:传感器内参、外参、手眼标定现场任务和参数交接模板。 +- `workshop_sensor_ingest_real`:真实采集数据集索引和数据输入参数文件生成模板。 - 现场专用 launch/config 文件应放在 `src/deployment/profiles`。 - 真实安全检查必须放在这里,或者放在真实车端控制器里,不能放进 Isaac 仿真代码。 diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/README.md b/agv_calib_brain/src/site_deployment/vehicle_agent_real/README.md index 00891a1..07209cb 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/README.md +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/README.md @@ -4,6 +4,14 @@ 它应该暴露与 `vehicle_agent_sim` 相同的 TCP 协议,但底层执行不再调用 Isaac 控制,而是对接真实车辆 SDK、PLC 接口、CAN 网关或厂商控制器接口。 +底盘执行器接入边界见: + +- `vehicle_agent_windows/include/chassis_driver_adapter.hpp` +- `vehicle_agent_windows/include/vendor_chassis_driver_template.hpp` +- `vehicle_agent_windows/docs/chassis_driver_adapter.md` + +默认 Windows 车端 agent 不会假装底盘已接入。未注入真实底盘驱动适配器时,底盘 readiness 会返回未就绪,动作原语会被拒绝。 + 现场使用前必须满足: - 拒绝 `vehicle_id` 不匹配的命令。 diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/CMakeLists.txt b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/CMakeLists.txt index fa25563..5631bd5 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/CMakeLists.txt +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/CMakeLists.txt @@ -25,6 +25,7 @@ endif() # --------------------------------------------------------------------------- add_executable(vehicle_agent_windows src/main.cpp + src/vehicle_gateway_server.cpp src/chassis_domain_server.cpp src/control_domain_server.cpp ) diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/docs/chassis_driver_adapter.md b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/docs/chassis_driver_adapter.md new file mode 100644 index 0000000..f5ae08c --- /dev/null +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/docs/chassis_driver_adapter.md @@ -0,0 +1,128 @@ +# 真实底盘驱动适配器接入说明 + +本文档定义 Windows 车端 agent 对接真实底盘 SDK、CAN、PLC 或厂商控制器的边界。 + +## 接入位置 + +协议层已经固定,真实底盘只需要实现适配器: + +- 接口文件:`include/chassis_driver_adapter.hpp` +- 厂商模板:`include/vendor_chassis_driver_template.hpp` +- 调用入口:`include/chassis_handler.hpp` + +`ChassisHandler` 默认使用安全空实现。没有注入真实适配器时: + +- readiness 返回未就绪。 +- 动作原语请求会被拒绝。 +- 紧急制动返回无法确认执行。 + +这样可以避免未接硬件时误报“底盘已就绪”。 + +## 必须实现的方法 + +真实适配器需要实现 `ChassisDriverAdapter` 的三个方法: + +```cpp +ChassisReadinessResponse get_readiness(const nlohmann::json& req); +ChassisJobResult execute_motion_primitive(const MotionPrimitiveRequest& req); +StandardResponse emergency_brake(const nlohmann::json& req); +``` + +## readiness 要检查什么 + +`get_readiness` 必须从真实底盘读取并回填: + +- 车端代理服务是否在线:`agent_ready` +- 厂商驱动、CAN、PLC 或控制器是否在线:`chassis_driver_online` +- 运动控制是否允许执行动作:`motion_control_ready` +- 急停是否释放:`estop_released` +- 当前车辆是否安全可移动:`vehicle_safe_to_move` +- 底盘遥测是否可用:`telemetry_available` +- 检查时间戳:`checked_timestamp_us` + +任一安全条件不满足时,不能返回可执行状态。 + +## 动作执行必须做什么 + +`execute_motion_primitive` 必须按这个顺序处理: + +1. 校验 `vehicle_id`、`request_id`、动作类型和动作参数。 +2. 获取任务互斥锁,同一时刻只允许一个底盘动作。 +3. 再次检查急停、使能、低速标定模式、驱动在线和运动区域安全状态。 +4. 把 `MotionPrimitiveRequest` 转成真实底盘控制器命令。 +5. 执行动作,并持续监控急停、断连、超时、驱动故障和人工中断。 +6. 动作结束后停车或刹停。 +7. 保存底盘遥测、命令日志、故障日志和执行摘要。 +8. 返回 `ChassisJobResult`。 + +动作执行失败时必须返回 `success=false`,并写明 `error_code` 和 `message`。 + +## 支持的动作原语 + +当前协议支持: + +- `STRAIGHT_LINE` +- `ARC` +- `IN_PLACE_ROTATION` +- `STEERING_SWEEP` +- `LATERAL_TRANSLATION` +- `DIAGONAL_MOTION` +- `MODULE_ALIGNMENT` +- `COORDINATED_STEERING` + +真实车辆不支持的动作必须返回 `UNSUPPORTED_CAPABILITY`,不能静默忽略。 + +## 结果字段要求 + +`ChassisJobResult` 必须填写: + +- `success` +- `error_code` +- `message` +- `job_id` +- `data_quality_passed` +- `suitable_for_commit` +- `recommended_parameter_version` +- `estimated_straight_line_bias` +- `validation_summary` +- `artifacts` + +底盘动作执行完成但算法结果不建议提交时,可以返回: + +```text +success=true +data_quality_passed=true +suitable_for_commit=false +``` + +没有真实算法结果时,不能返回 `suitable_for_commit=true`。 + +## 产物落盘 + +真实适配器或现场采集程序应至少保存: + +- 底盘遥测 CSV +- 执行器命令 CSV +- 外部真值轨迹 CSV +- 动作执行诊断 JSON + +CSV 字段协议见: + +```text +src/site_deployment/workshop_chassis_calibration_real/chassis_csv_contract.md +``` + +这些文件应通过 `artifacts` 返回,并由车间电脑侧生成或合并到 `dataset_index.yaml`。 + +## 主程序接入方式 + +现场接入时,建议复制 `VendorChassisDriverTemplate`,实现为厂商专用类,然后在 `main.cpp` 中注入: + +```cpp +#include "my_vendor_chassis_driver.hpp" + +vaw::MyVendorChassisDriver driver; +vaw::ChassisHandler chassis_handler(&driver); +``` + +不要在 TCP server、gateway server 或 frame codec 中直接调用厂商 SDK。 diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/docs/protocol.md b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/docs/protocol.md index b8b7275..e11d383 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/docs/protocol.md +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/docs/protocol.md @@ -5,22 +5,24 @@ ``` ┌─────────────────────────────────┐ TCP 长连接(每次请求建立/断开) │ Ubuntu 车间电脑 │ ──────────────────────────────────────► -│ vehicle_agent_gateway │ +│ vehicle_agent_gateway / 外部位姿桥 │ │ ├─ ChassisBridgeNode :9001 │ ◄────────────────────────────────────── -│ └─ ControlBridgeNode :9002 │ +│ ├─ ControlBridgeNode :9002 │ +│ └─ ExternalPoseBridge :9000 │ └─────────────────────────────────┘ ┌─────────────────────────────────┐ │ Windows 车端电脑 │ │ vehicle_agent_windows │ +│ ├─ VehicleGateway :9000 │ │ ├─ ChassisDomainServer :9001 │ │ └─ ControlDomainServer :9002 │ └─────────────────────────────────┘ ``` - Ubuntu 侧**主动发起**每次连接,发完请求等响应后关闭 socket。 -- Windows 侧持续监听两个 TCP 端口,每次连接处理一条请求后可关闭或保持,均可。 -- 两个端口完全独立,可以用同一进程的两个线程分别监听。 +- Windows 侧持续监听统一 gateway 端口和兼容分域端口,每次连接处理一条请求后可关闭或保持,均可。 +- 9000 是推荐的车端统一 gateway 端口,支持底盘、运控和外部定位位姿;9001/9002 是旧版分域端口或内部兼容端口。 --- @@ -64,7 +66,7 @@ def send_frame(conn, msg_type: int, payload: str): ## 3. 消息类型枚举(MsgType) -### 3.1 底盘域(端口 9001) +### 3.1 底盘域(推荐端口 9000,兼容端口 9001) | 值 | 名称 | 方向 | 说明 | |----|------|------|------| @@ -75,7 +77,7 @@ def send_frame(conn, msg_type: int, payload: str): | 5 | `CHASSIS_EMERGENCY_BRAKE_REQ` | Ubuntu → Windows | 紧急制动 | | 6 | `CHASSIS_EMERGENCY_BRAKE_RSP` | Windows → Ubuntu | 紧急制动响应 | -### 3.2 运控域(端口 9002) +### 3.2 运控域(推荐端口 9000,兼容端口 9002) | 值 | 名称 | 方向 | 说明 | |----|------|------|------| @@ -84,9 +86,19 @@ def send_frame(conn, msg_type: int, payload: str): | 13 | `CONTROL_EVALUATION_REQ` | Ubuntu → Windows | 执行控制评估任务 | | 14 | `CONTROL_EVALUATION_RSP` | Windows → Ubuntu | 控制评估结果 | +### 3.3 外部定位位姿域(推荐端口 9000) + +| 值 | 名称 | 方向 | 说明 | +|----|------|------|------| +| 31 | `EXTERNAL_POSE_PUSH_REQ` | Ubuntu → Windows | 推送车间坐标系下车辆外部定位位姿 | +| 32 | `EXTERNAL_POSE_PUSH_RSP` | Windows → Ubuntu | 外部定位位姿接收响应 | + --- -## 4. 底盘域 payload 格式(端口 9001) +## 4. 底盘域 payload 格式(推荐端口 9000,兼容端口 9001) + +真实底盘 SDK、CAN、PLC 或厂商控制器的接入边界见 +`docs/chassis_driver_adapter.md`。Windows 车端 agent 默认不会返回“底盘就绪”,必须注入真实底盘驱动适配器后才能执行动作原语。 ### 4.1 CHASSIS_GET_READINESS_REQ(type=1) @@ -113,7 +125,7 @@ Windows 返回,描述底盘当前状态。**所有布尔字段均须填写** ```json { "success": true, - "error_code": {"code": 0}, + "error_code": {"code": 1}, "message": "底盘就绪。", "agent_ready": true, "chassis_driver_online": true, @@ -128,7 +140,7 @@ Windows 返回,描述底盘当前状态。**所有布尔字段均须填写** | 字段 | 类型 | 说明 | |------|------|------| | `success` | bool | 查询本身是否成功 | -| `error_code.code` | uint32 | 错误码(0=OK,见附录 A)| +| `error_code.code` | uint32 | 错误码(1=OK,见附录 A)| | `message` | string | 人读说明 | | `agent_ready` | bool | 底盘代理服务是否就绪 | | `chassis_driver_online` | bool | 底盘驱动是否在线 | @@ -263,7 +275,7 @@ Windows 在动作完成后返回,包含质量评估结果供编排器做自动 ```json { "success": true, - "error_code": {"code": 0}, + "error_code": {"code": 1}, "message": "直线行驶完成。", "job_id": "chassis_req_1711234567100000", "data_quality_passed": true, @@ -305,7 +317,7 @@ Windows 在动作完成后返回,包含质量评估结果供编排器做自动 --- -## 5. 运控域 payload 格式(端口 9002) +## 5. 运控域 payload 格式(推荐端口 9000,兼容端口 9002) ### 5.1 CONTROL_GET_READINESS_REQ(type=11) @@ -322,7 +334,7 @@ Windows 在动作完成后返回,包含质量评估结果供编排器做自动 ```json { "success": true, - "error_code": {"code": 0}, + "error_code": {"code": 1}, "message": "运控就绪。", "agent_ready": true, "trajectory_executor_ready": true, @@ -444,7 +456,7 @@ Ubuntu 发送,让车端按当前控制参数执行一次评估任务。`select ```json { "success": true, - "error_code": {"code": 0}, + "error_code": {"code": 1}, "message": "轨迹跟踪评估完成。", "job_id": "chassis_req_1711234568000000", "data_quality_passed": true, @@ -489,9 +501,66 @@ Ubuntu 发送,让车端按当前控制参数执行一次评估任务。`select --- -## 6. 交互时序 +## 6. 外部定位位姿 payload 格式(推荐端口 9000) -### 6.1 底盘标定完整流程 +### 6.1 EXTERNAL_POSE_PUSH_REQ(type=31) + +Ubuntu 车间电脑发送,推送当前车辆在车间坐标系下的外部定位位姿。该消息对应 proto 中的 `ExternalLocalizationTelemetry`,JSON key 使用 proto 字段名。 + +```json +{ + "hardware_timestamp_us": 1711234567000000, + "pose_valid": true, + "workshop_pose": { + "x_m": 1.2, + "y_m": 0.5, + "z_m": 0.0, + "roll_rad": 0.0, + "pitch_rad": 0.0, + "yaw_rad": 1.57 + }, + "position_stddev_m": 0.01, + "yaw_stddev_rad": 0.005, + "tracking_loss_ratio": 0.0, + "time_sync_offset_ms": 2.0, + "quality_score": 0.98, + "observed_target_count": 2, + "reference_source_name": "workshop_four_lidar_ball_truth", + "active_job_id": "workshop_external_lidar_zone_a:four_lidar_ball_localization" +} +``` + +| 字段 | 类型 | 说明 | +|------|------|------| +| `hardware_timestamp_us` | int64 | 外部定位系统采样时间戳,微秒 | +| `pose_valid` | bool | 本帧位姿是否有效 | +| `workshop_pose` | object | 车辆 `base_link` 在车间坐标系下的位姿 | +| `position_stddev_m` | float64 | 位置标准差 | +| `yaw_stddev_rad` | float64 | 航向标准差 | +| `tracking_loss_ratio` | float64 | 跟踪丢失比例 | +| `time_sync_offset_ms` | float64 | 时间同步偏差 | +| `quality_score` | float64 | 外部定位质量分数,建议范围 `[0, 1]` | +| `observed_target_count` | uint32 | 当前观测到的标定球或目标数量 | +| `reference_source_name` | string | 外部定位源名称 | +| `active_job_id` | string | 外部定位节点当前任务 ID | + +### 6.2 EXTERNAL_POSE_PUSH_RSP(type=32) + +Windows 车端返回标准响应。成功接收并缓存本帧时: + +```json +{ + "success": true, + "error_code": {"code": 1}, + "message": "外部定位位姿已接收" +} +``` + +--- + +## 7. 交互时序 + +### 7.1 底盘标定完整流程 ``` Ubuntu (orchestrator) Ubuntu (gateway) Windows (vehicle_agent) @@ -511,27 +580,32 @@ Ubuntu (orchestrator) Ubuntu (gateway) Windows (vehicle_agent │◄── action result ─────────│ │ ``` -### 6.2 注意事项 +### 7.2 注意事项 1. **每次请求建立新 TCP 连接**:Ubuntu 侧 `TcpClient` 每次 `send_recv` 都会新建、使用、关闭连接。Windows 侧不需要维护长连接状态。 2. **超时控制**:Ubuntu 侧默认等待 5000ms(可通过 ROS2 参数 `chassis_timeout_ms` / `control_timeout_ms` 调整)。动作原语执行时间可能远超此值,Windows 侧应在动作完成后才回复 RSP 帧,不要提前响应。 3. **急停处理**:Ubuntu 侧 orchestrator 在收到取消请求时会关闭 socket 连接。Windows 侧检测到连接断开即应停止当前动作并触发安全停车。 -4. **并发**:底盘域和运控域在不同端口,理论上可并发,但编排器保证同一时刻只有一个域的任务在执行。 +4. **外部定位位姿**:外部位姿桥会按频率持续推送 type=31,Windows 侧应快速解析、缓存最近一帧并回复 type=32,不要在该请求里执行耗时控制逻辑。 +5. **并发**:底盘域、运控域和外部位姿输入可能同时到达;编排器保证同一时刻只有一个标定动作任务在执行,但外部位姿输入应允许持续更新。 --- -## 7. Windows 侧实现最小要求 +## 8. Windows 侧实现最小要求 -### 7.1 必须实现的接口 +### 8.1 必须实现的接口 | 端口 | REQ type | 行为 | |------|----------|------| -| 9001 | type=1 READINESS | 检查底盘状态,返回 type=2 | -| 9001 | type=3 MOTION_PRIMITIVE | 执行指定动作,完成后返回 type=4(含验收指标)| -| 9002 | type=11 READINESS | 检查运控状态,返回 type=12 | -| 9002 | type=13 EVALUATION | 执行控制评估,完成后返回 type=14(含验收指标)| +| 9000 | type=1 READINESS | 检查底盘状态,返回 type=2 | +| 9000 | type=3 MOTION_PRIMITIVE | 执行指定动作,完成后返回 type=4(含验收指标)| +| 9000 | type=5 EMERGENCY_BRAKE | 执行紧急制动,返回 type=6 | +| 9000 | type=11 READINESS | 检查运控状态,返回 type=12 | +| 9000 | type=13 EVALUATION | 执行控制评估,完成后返回 type=14(含验收指标)| +| 9000 | type=31 EXTERNAL_POSE_PUSH | 缓存外部定位位姿,返回 type=32 | +| 9001 | type=1/3/5 | 旧版底盘分域兼容入口 | +| 9002 | type=11/13 | 旧版运控分域兼容入口 | -### 7.2 错误响应格式 +### 8.2 错误响应格式 执行失败时,`success` 设为 `false`,`error_code.code` 填对应错误码,`message` 填人读错误说明: @@ -546,7 +620,7 @@ Ubuntu (orchestrator) Ubuntu (gateway) Windows (vehicle_agent } ``` -### 7.3 联调工具 +### 8.3 联调工具 Ubuntu 侧提供了 Python stub 服务器用于 Windows 侧联调前的自测: @@ -588,8 +662,10 @@ python3 src/communication/win_ubuntu_bridge/vehicle_agent_gateway/scripts/window Windows 侧自测时依次验证: -- [ ] 监听 9001 端口,收到 type=1 帧,回复 type=2 帧(`success=true`, `estop_released=true`, `vehicle_safe_to_move=true`) -- [ ] 收到 type=3 帧(`selected_primitive=1` 直线),车辆执行,完成后回复 type=4(`success=true`, `data_quality_passed=true`, `suitable_for_commit=true`) -- [ ] 监听 9002 端口,收到 type=11 帧,回复 type=12 帧 +- [ ] 监听 9000 端口,收到 type=31 外部定位位姿帧,缓存最近一帧并回复 type=32 +- [ ] 监听 9000 端口,收到 type=1 帧,回复 type=2 帧(`success=true`, `estop_released=true`, `vehicle_safe_to_move=true`) +- [ ] 监听 9001 兼容端口,收到 type=1 帧,回复 type=2 帧 +- [ ] 9000 或 9001 收到 type=3 帧(`selected_primitive=1` 直线),车辆执行,完成后回复 type=4(`success=true`, `data_quality_passed=true`, `suitable_for_commit=true`) +- [ ] 9000 或 9002 收到 type=11 帧,回复 type=12 帧 - [ ] 收到 type=13 帧(`selected_task=1` 轨迹跟踪),执行,回复 type=14 - [ ] 模拟急停:收到 type=3 帧后主动关闭连接,验证车辆停车 diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/frame_codec.py b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/frame_codec.py index 0de0274..53b9c1a 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/frame_codec.py +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/frame_codec.py @@ -19,6 +19,8 @@ from typing import Any, Dict, Tuple # 消息类型枚举(与 proto_frame.hpp MsgType 完全一致) # --------------------------------------------------------------------------- class MsgType(IntEnum): + # 未识别或无法返回到明确业务域 + UNSPECIFIED = 0 # 底盘域 CHASSIS_GET_READINESS_REQ = 1 CHASSIS_GET_READINESS_RSP = 2 @@ -31,6 +33,9 @@ class MsgType(IntEnum): CONTROL_GET_READINESS_RSP = 12 CONTROL_EVALUATION_REQ = 13 CONTROL_EVALUATION_RSP = 14 + # 外部定位位姿域 + EXTERNAL_POSE_PUSH_REQ = 31 + EXTERNAL_POSE_PUSH_RSP = 32 # --------------------------------------------------------------------------- diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/chassis_driver_adapter.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/chassis_driver_adapter.hpp new file mode 100644 index 0000000..53f7465 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/chassis_driver_adapter.hpp @@ -0,0 +1,88 @@ +#pragma once +// chassis_driver_adapter.hpp +// 真实底盘驱动适配器边界。 +// Windows 车端 agent 的 TCP 协议层只依赖这个接口,不直接调用厂商 SDK、CAN 或 PLC。 + +#include +#include + +#include "data_types.hpp" + +namespace vaw +{ + +inline int64_t unix_time_us() +{ + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); +} + +inline std::string request_job_id(const MotionPrimitiveRequest& req) +{ + if (!req.header.request_id.empty()) + return req.header.request_id; + if (!req.test_case_id.empty()) + return req.test_case_id; + return "unknown_chassis_job"; +} + +class ChassisDriverAdapter +{ +public: + virtual ~ChassisDriverAdapter() = default; + + // 查询真实底盘是否具备执行标定动作的条件。 + virtual ChassisReadinessResponse get_readiness(const nlohmann::json& req) = 0; + + // 执行动作原语。该调用应阻塞到动作完成、失败或超时,然后返回完整结果。 + virtual ChassisJobResult execute_motion_primitive(const MotionPrimitiveRequest& req) = 0; + + // 执行紧急制动。无论当前任务状态如何,真实实现都应优先停车。 + virtual StandardResponse emergency_brake(const nlohmann::json& req) = 0; +}; + +class SafeStubChassisDriverAdapter final : public ChassisDriverAdapter +{ +public: + ChassisReadinessResponse get_readiness(const nlohmann::json& req) override + { + (void)req; + ChassisReadinessResponse rsp; + rsp.success = false; + rsp.error_code = ErrorCode::NOT_READY; + rsp.message = "未接入真实底盘驱动,禁止执行底盘动作。"; + rsp.agent_ready = true; + rsp.chassis_driver_online = false; + rsp.motion_control_ready = false; + rsp.estop_released = false; + rsp.vehicle_safe_to_move = false; + rsp.telemetry_available = false; + rsp.checked_timestamp_us = unix_time_us(); + return rsp; + } + + ChassisJobResult execute_motion_primitive(const MotionPrimitiveRequest& req) override + { + ChassisJobResult result; + result.success = false; + result.error_code = ErrorCode::NOT_READY; + result.message = "未接入真实底盘驱动,动作原语已拒绝。"; + result.job_id = request_job_id(req); + result.data_quality_passed = false; + result.suitable_for_commit = false; + result.validation_summary.auto_acceptance_passed = false; + return result; + } + + StandardResponse emergency_brake(const nlohmann::json& req) override + { + (void)req; + StandardResponse rsp; + rsp.success = false; + rsp.error_code = ErrorCode::NOT_READY; + rsp.message = "未接入真实底盘驱动,无法确认紧急制动是否已执行。"; + return rsp; + } +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/chassis_handler.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/chassis_handler.hpp index c3742b8..c25f1d2 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/chassis_handler.hpp +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/chassis_handler.hpp @@ -1,9 +1,9 @@ #pragma once // chassis_handler.hpp -// 底盘处理器虚基类。 -// 子类覆盖各方法后即可接入真实底盘驱动;默认实现返回 stub 成功响应。 +// 底盘处理器。 +// TCP 协议层调用本类,本类再委托给真实底盘驱动适配器。 -#include +#include "chassis_driver_adapter.hpp" #include "data_types.hpp" namespace vaw @@ -12,54 +12,43 @@ namespace vaw class ChassisHandler { public: + explicit ChassisHandler(ChassisDriverAdapter* adapter = nullptr) + { + set_driver_adapter(adapter); + } + virtual ~ChassisHandler() = default; + void set_driver_adapter(ChassisDriverAdapter* adapter) + { + adapter_ = adapter != nullptr ? adapter : &safe_stub_; + } + // 查询底盘就绪状态 // req: CHASSIS_GET_READINESS_REQ 解析结果(AgentReadinessRequest 字段) virtual ChassisReadinessResponse get_readiness(const nlohmann::json& req) { - ChassisReadinessResponse rsp; - rsp.success = true; - rsp.error_code = ErrorCode::OK; - rsp.message = "底盘就绪(stub)"; - rsp.agent_ready = true; - rsp.chassis_driver_online = true; - rsp.motion_control_ready = true; - rsp.estop_released = true; - rsp.vehicle_safe_to_move = true; - rsp.telemetry_available = true; - rsp.checked_timestamp_us = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - return rsp; + return adapter_->get_readiness(req); } // 执行底盘动作原语 // req: 已解析的 MotionPrimitiveRequest virtual ChassisJobResult execute_motion_primitive(const MotionPrimitiveRequest& req) { - (void)req; - ChassisJobResult result; - result.success = true; - result.error_code = ErrorCode::OK; - result.message = "动作原语执行完成(stub)"; - result.job_id = req.test_case_id; - result.data_quality_passed = true; - result.suitable_for_commit = true; - result.validation_summary.auto_acceptance_passed = true; - return result; + return adapter_->execute_motion_primitive(req); } // 紧急制动 // req: CHASSIS_EMERGENCY_BRAKE_REQ payload virtual nlohmann::json emergency_brake(const nlohmann::json& req) { - (void)req; - return nlohmann::json{ - {"success", true}, - {"error_code", {"code", 0}}, - {"message", "紧急制动已执行(stub)"}, - }; + const auto rsp = adapter_->emergency_brake(req); + return nlohmann::json(rsp); } + +private: + SafeStubChassisDriverAdapter safe_stub_; + ChassisDriverAdapter* adapter_{&safe_stub_}; }; } // namespace vaw diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/control_handler.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/control_handler.hpp index 367643b..1f152af 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/control_handler.hpp +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/control_handler.hpp @@ -5,6 +5,7 @@ #include #include "data_types.hpp" +#include "external_pose_store.hpp" namespace vaw { @@ -14,6 +15,12 @@ class ControlHandler public: virtual ~ControlHandler() = default; + // 注入外部定位位姿缓存,用于运控就绪检查和后续轨迹跟踪。 + void set_external_pose_store(const ExternalPoseStore* store) + { + external_pose_store_ = store; + } + // 查询运控就绪状态 virtual ControlReadinessResponse get_readiness(const nlohmann::json& req) { @@ -28,6 +35,15 @@ public: rsp.control_output_ready = true; rsp.estop_released = true; rsp.vehicle_safe_to_move = true; + if (external_pose_store_ != nullptr) { + const auto pose_status = external_pose_store_->status(500.0); + rsp.vehicle_feedback_ready = pose_status.ready; + rsp.external_pose_feedback_ready = pose_status.ready; + rsp.external_pose_source_name = pose_status.source_name; + rsp.external_pose_age_ms = pose_status.age_ms; + rsp.external_pose_quality_score = pose_status.quality_score; + rsp.external_pose_transport = pose_status.transport; + } rsp.checked_timestamp_us = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()).count(); return rsp; @@ -48,6 +64,9 @@ public: result.validation_summary.auto_acceptance_passed = true; return result; } + +private: + const ExternalPoseStore* external_pose_store_{nullptr}; }; } // namespace vaw diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/data_types.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/data_types.hpp index 22d973a..62a1790 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/data_types.hpp +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/data_types.hpp @@ -122,6 +122,87 @@ inline void to_json(nlohmann::json& j, const FileReference& r) }; } +struct Pose3D { + double x_m{0.0}; + double y_m{0.0}; + double z_m{0.0}; + double roll_rad{0.0}; + double pitch_rad{0.0}; + double yaw_rad{0.0}; +}; + +inline void from_json(const nlohmann::json& j, Pose3D& p) +{ + p.x_m = j.value("x_m", 0.0); + p.y_m = j.value("y_m", 0.0); + p.z_m = j.value("z_m", 0.0); + p.roll_rad = j.value("roll_rad", 0.0); + p.pitch_rad = j.value("pitch_rad", 0.0); + p.yaw_rad = j.value("yaw_rad", 0.0); +} + +inline void to_json(nlohmann::json& j, const Pose3D& p) +{ + j = { + {"x_m", p.x_m}, + {"y_m", p.y_m}, + {"z_m", p.z_m}, + {"roll_rad", p.roll_rad}, + {"pitch_rad", p.pitch_rad}, + {"yaw_rad", p.yaw_rad}, + }; +} + +struct StandardResponse { + bool success{false}; + ErrorCode error_code{ErrorCode::OK}; + std::string message; +}; + +inline void to_json(nlohmann::json& j, const StandardResponse& r) +{ + j = { + {"success", r.success}, + {"error_code", nlohmann::json{{"code", static_cast(r.error_code)}}}, + {"message", r.message}, + }; +} + +// =========================================================================== +// 外部定位位姿输入 +// =========================================================================== + +struct ExternalLocalizationTelemetry { + int64_t hardware_timestamp_us{0}; + bool pose_valid{false}; + Pose3D workshop_pose; + double position_stddev_m{0.0}; + double yaw_stddev_rad{0.0}; + double tracking_loss_ratio{1.0}; + double time_sync_offset_ms{0.0}; + double quality_score{0.0}; + uint32_t observed_target_count{0}; + std::string reference_source_name; + std::string active_job_id; +}; + +inline void from_json(const nlohmann::json& j, ExternalLocalizationTelemetry& t) +{ + t.hardware_timestamp_us = j.value("hardware_timestamp_us", int64_t{0}); + t.pose_valid = j.value("pose_valid", false); + if (j.contains("workshop_pose")) { + t.workshop_pose = j["workshop_pose"].get(); + } + t.position_stddev_m = j.value("position_stddev_m", 0.0); + t.yaw_stddev_rad = j.value("yaw_stddev_rad", 0.0); + t.tracking_loss_ratio = j.value("tracking_loss_ratio", 1.0); + t.time_sync_offset_ms = j.value("time_sync_offset_ms", 0.0); + t.quality_score = j.value("quality_score", 0.0); + t.observed_target_count = j.value("observed_target_count", uint32_t{0}); + t.reference_source_name = j.value("reference_source_name", std::string{}); + t.active_job_id = j.value("active_job_id", std::string{}); +} + // =========================================================================== // 底盘命令原语 // =========================================================================== @@ -358,7 +439,7 @@ inline void to_json(nlohmann::json& j, const ChassisReadinessResponse& r) { j = { {"success", r.success}, - {"error_code", {"code", static_cast(r.error_code)}}, + {"error_code", nlohmann::json{{"code", static_cast(r.error_code)}}}, {"message", r.message}, {"agent_ready", r.agent_ready}, {"chassis_driver_online", r.chassis_driver_online}, @@ -371,10 +452,10 @@ inline void to_json(nlohmann::json& j, const ChassisReadinessResponse& r) } struct ChassisValidationSummary { - double straight_line_error_m{0.0}; - double heading_error_deg{0.0}; - double arc_radius_error_m{0.0}; - double rotation_error_deg{0.0}; + double max_lateral_error_m{0.0}; + double max_yaw_error_rad{0.0}; + double rms_lateral_error_m{0.0}; + double rms_yaw_error_rad{0.0}; double repeatability_error_m{0.0}; double curvature_error{0.0}; double module_consistency_error{0.0}; @@ -384,10 +465,10 @@ struct ChassisValidationSummary { inline void to_json(nlohmann::json& j, const ChassisValidationSummary& s) { j = { - {"straight_line_error_m", s.straight_line_error_m}, - {"heading_error_deg", s.heading_error_deg}, - {"arc_radius_error_m", s.arc_radius_error_m}, - {"rotation_error_deg", s.rotation_error_deg}, + {"max_lateral_error_m", s.max_lateral_error_m}, + {"max_yaw_error_rad", s.max_yaw_error_rad}, + {"rms_lateral_error_m", s.rms_lateral_error_m}, + {"rms_yaw_error_rad", s.rms_yaw_error_rad}, {"repeatability_error_m", s.repeatability_error_m}, {"curvature_error", s.curvature_error}, {"module_consistency_error", s.module_consistency_error}, @@ -412,7 +493,7 @@ inline void to_json(nlohmann::json& j, const ChassisJobResult& r) { j = { {"success", r.success}, - {"error_code", {"code", static_cast(r.error_code)}}, + {"error_code", nlohmann::json{{"code", static_cast(r.error_code)}}}, {"message", r.message}, {"job_id", r.job_id}, {"data_quality_passed", r.data_quality_passed}, @@ -580,7 +661,7 @@ inline void to_json(nlohmann::json& j, const ControlReadinessResponse& r) { j = { {"success", r.success}, - {"error_code", {"code", static_cast(r.error_code)}}, + {"error_code", nlohmann::json{{"code", static_cast(r.error_code)}}}, {"message", r.message}, {"agent_ready", r.agent_ready}, {"trajectory_executor_ready", r.trajectory_executor_ready}, @@ -640,7 +721,7 @@ inline void to_json(nlohmann::json& j, const ControlJobResult& r) { j = { {"success", r.success}, - {"error_code", {"code", static_cast(r.error_code)}}, + {"error_code", nlohmann::json{{"code", static_cast(r.error_code)}}}, {"message", r.message}, {"job_id", r.job_id}, {"data_quality_passed", r.data_quality_passed}, diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/external_pose_store.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/external_pose_store.hpp new file mode 100644 index 0000000..10f983c --- /dev/null +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/external_pose_store.hpp @@ -0,0 +1,78 @@ +#pragma once +// external_pose_store.hpp +// 车端缓存车间电脑推送来的外部定位车辆位姿。 + +#include +#include +#include + +#include "data_types.hpp" + +namespace vaw +{ + +struct ExternalPoseStatus +{ + bool ready{false}; + std::string source_name; + double age_ms{0.0}; + double quality_score{0.0}; + std::string transport{"wifi6_tcp"}; +}; + +class ExternalPoseStore +{ +public: + StandardResponse update(const ExternalLocalizationTelemetry& telemetry) + { + std::lock_guard lock(mutex_); + latest_ = telemetry; + receive_wall_time_us_ = now_us(); + has_latest_ = true; + + StandardResponse rsp; + rsp.success = true; + rsp.error_code = ErrorCode::OK; + rsp.message = telemetry.pose_valid ? "外部定位位姿已接收" : "外部定位位姿已接收,但 pose_valid=false"; + return rsp; + } + + bool latest(ExternalLocalizationTelemetry& telemetry) const + { + std::lock_guard lock(mutex_); + if (!has_latest_) { + return false; + } + telemetry = latest_; + return true; + } + + ExternalPoseStatus status(double max_age_ms) const + { + std::lock_guard lock(mutex_); + ExternalPoseStatus status; + if (!has_latest_) { + return status; + } + + status.source_name = latest_.reference_source_name; + status.quality_score = latest_.quality_score; + status.age_ms = static_cast(now_us() - receive_wall_time_us_) / 1000.0; + status.ready = latest_.pose_valid && status.age_ms <= max_age_ms && latest_.quality_score > 0.0; + return status; + } + +private: + static int64_t now_us() + { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + } + + mutable std::mutex mutex_; + ExternalLocalizationTelemetry latest_; + int64_t receive_wall_time_us_{0}; + bool has_latest_{false}; +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/frame_codec.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/frame_codec.hpp index d1fe9ad..d104eb5 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/frame_codec.hpp +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/frame_codec.hpp @@ -14,6 +14,7 @@ using sock_t = SOCKET; # define SOCK_INVALID INVALID_SOCKET # define SOCK_CLOSE(s) closesocket(s) +# define SOCK_SHUTDOWN_BOTH SD_BOTH #else # include # include @@ -23,6 +24,7 @@ using sock_t = int; # define SOCK_INVALID (-1) # define SOCK_CLOSE(s) ::close(s) +# define SOCK_SHUTDOWN_BOTH SHUT_RDWR #endif namespace vaw // vehicle_agent_windows @@ -32,6 +34,8 @@ namespace vaw // vehicle_agent_windows // 枚举:与 Python frame_codec.py / proto_frame.hpp MsgType 完全一致 // --------------------------------------------------------------------------- enum class MsgType : uint32_t { + // 未识别或无法返回到明确业务域 + UNSPECIFIED = 0, // 底盘域 CHASSIS_GET_READINESS_REQ = 1, CHASSIS_GET_READINESS_RSP = 2, @@ -44,6 +48,9 @@ enum class MsgType : uint32_t { CONTROL_GET_READINESS_RSP = 12, CONTROL_EVALUATION_REQ = 13, CONTROL_EVALUATION_RSP = 14, + // 外部定位位姿域 + EXTERNAL_POSE_PUSH_REQ = 31, + EXTERNAL_POSE_PUSH_RSP = 32, }; // --------------------------------------------------------------------------- diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/vehicle_gateway_server.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/vehicle_gateway_server.hpp new file mode 100644 index 0000000..c084bd9 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/vehicle_gateway_server.hpp @@ -0,0 +1,43 @@ +#pragma once +// vehicle_gateway_server.hpp +// 车端统一 WiFi6/TCP 入口。现场车间电脑优先连接这个端口。 + +#include +#include + +#include "chassis_handler.hpp" +#include "control_handler.hpp" +#include "external_pose_store.hpp" +#include "frame_codec.hpp" + +namespace vaw +{ + +class VehicleGatewayServer +{ +public: + VehicleGatewayServer( + uint16_t port, + ChassisHandler* chassis_handler, + ControlHandler* control_handler, + ExternalPoseStore* external_pose_store); + ~VehicleGatewayServer(); + + // 阻塞运行,直到 stop() 被调用。 + void run(); + void stop(); + +private: + void handle_connection(sock_t conn); + MsgType response_type_for(MsgType request_type) const; + void send_error_response(sock_t conn, MsgType response_type, const std::string& message) const; + + uint16_t port_; + ChassisHandler* chassis_handler_; + ControlHandler* control_handler_; + ExternalPoseStore* external_pose_store_; + std::atomic running_; + sock_t listen_sock_; +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/vendor_chassis_driver_template.hpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/vendor_chassis_driver_template.hpp new file mode 100644 index 0000000..a116340 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/include/vendor_chassis_driver_template.hpp @@ -0,0 +1,70 @@ +#pragma once +// vendor_chassis_driver_template.hpp +// 厂商底盘驱动适配模板。 +// 现场对接 SDK、CAN、PLC 或运动控制器时,复制本文件并实现各个私有钩子。 + +#include "chassis_driver_adapter.hpp" + +namespace vaw +{ + +class VendorChassisDriverTemplate : public ChassisDriverAdapter +{ +public: + ChassisReadinessResponse get_readiness(const nlohmann::json& req) override + { + (void)req; + + // 真实实现应在这里读取: + // - SDK / CAN / PLC 连接状态 + // - 急停状态 + // - 车辆使能状态 + // - 当前是否低速标定模式 + // - 遥测链路是否可用 + ChassisReadinessResponse rsp; + rsp.success = false; + rsp.error_code = ErrorCode::NOT_READY; + rsp.message = "厂商底盘适配器模板尚未实现。"; + rsp.agent_ready = true; + rsp.chassis_driver_online = false; + rsp.motion_control_ready = false; + rsp.estop_released = false; + rsp.vehicle_safe_to_move = false; + rsp.telemetry_available = false; + rsp.checked_timestamp_us = unix_time_us(); + return rsp; + } + + ChassisJobResult execute_motion_primitive(const MotionPrimitiveRequest& req) override + { + // 真实实现必须完成这些步骤: + // 1. 再次检查 vehicle_id、急停、使能、低速模式和互斥任务锁。 + // 2. 把动作原语转换成真实底盘控制器可执行的命令。 + // 3. 执行过程中持续监控超时、断连、急停和驱动故障。 + // 4. 动作结束后刹停,并落盘底盘遥测和命令日志。 + // 5. 返回真实执行结果,不能用模板默认值冒充成功。 + ChassisJobResult result; + result.success = false; + result.error_code = ErrorCode::UNSUPPORTED_CAPABILITY; + result.message = "厂商底盘动作执行模板尚未实现。"; + result.job_id = request_job_id(req); + result.data_quality_passed = false; + result.suitable_for_commit = false; + result.validation_summary.auto_acceptance_passed = false; + return result; + } + + StandardResponse emergency_brake(const nlohmann::json& req) override + { + (void)req; + + // 真实实现应直接调用底盘控制器的最高优先级停车或断使能接口。 + StandardResponse rsp; + rsp.success = false; + rsp.error_code = ErrorCode::NOT_READY; + rsp.message = "厂商底盘紧急制动模板尚未实现。"; + return rsp; + } +}; + +} // namespace vaw diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/chassis_domain_server.cpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/chassis_domain_server.cpp index 890c900..72e6e89 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/chassis_domain_server.cpp +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/chassis_domain_server.cpp @@ -71,6 +71,7 @@ void ChassisDomainServer::stop() { running_ = false; if (listen_sock_ != SOCK_INVALID) { + ::shutdown(listen_sock_, SOCK_SHUTDOWN_BOTH); SOCK_CLOSE(listen_sock_); listen_sock_ = SOCK_INVALID; } diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/control_domain_server.cpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/control_domain_server.cpp index ec9584b..cbf776c 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/control_domain_server.cpp +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/control_domain_server.cpp @@ -68,6 +68,7 @@ void ControlDomainServer::stop() { running_ = false; if (listen_sock_ != SOCK_INVALID) { + ::shutdown(listen_sock_, SOCK_SHUTDOWN_BOTH); SOCK_CLOSE(listen_sock_); listen_sock_ = SOCK_INVALID; } diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/main.cpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/main.cpp index 76e4f86..f90dc9f 100644 --- a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/main.cpp +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/main.cpp @@ -1,9 +1,9 @@ // main.cpp // vehicle_agent_windows 入口。 -// 启动底盘域(默认 9001)和运控域(默认 9002)两个 TCP 服务线程。 +// 启动统一 gateway(默认 9000)、底盘域(默认 9001)和运控域(默认 9002)服务线程。 // // 用法: -// vehicle_agent_windows [--chassis-port ] [--control-port ] +// vehicle_agent_windows [--gateway-port ] [--chassis-port ] [--control-port ] #include #include @@ -16,6 +16,8 @@ #include "control_domain_server.hpp" #include "chassis_handler.hpp" #include "control_handler.hpp" +#include "external_pose_store.hpp" +#include "vehicle_gateway_server.hpp" static std::atomic g_shutdown{false}; @@ -23,12 +25,15 @@ static void sig_handler(int) { g_shutdown = true; } int main(int argc, char* argv[]) { + uint16_t gateway_port = 9000; uint16_t chassis_port = 9001; uint16_t control_port = 9002; // 简单命令行解析 for (int i = 1; i < argc; ++i) { - if (std::strcmp(argv[i], "--chassis-port") == 0 && i + 1 < argc) + if (std::strcmp(argv[i], "--gateway-port") == 0 && i + 1 < argc) + gateway_port = static_cast(std::atoi(argv[++i])); + else if (std::strcmp(argv[i], "--chassis-port") == 0 && i + 1 < argc) chassis_port = static_cast(std::atoi(argv[++i])); else if (std::strcmp(argv[i], "--control-port") == 0 && i + 1 < argc) control_port = static_cast(std::atoi(argv[++i])); @@ -37,13 +42,26 @@ int main(int argc, char* argv[]) std::signal(SIGINT, sig_handler); std::signal(SIGTERM, sig_handler); - // 默认 stub handler,子类化后可接入真实硬件 + // 默认安全 handler:未注入真实底盘驱动适配器时,readiness 返回未就绪,动作请求会被拒绝。 vaw::ChassisHandler chassis_handler; vaw::ControlHandler control_handler; + vaw::ExternalPoseStore external_pose_store; + control_handler.set_external_pose_store(&external_pose_store); + vaw::VehicleGatewayServer gateway_server( + gateway_port, + &chassis_handler, + &control_handler, + &external_pose_store); vaw::ChassisDomainServer chassis_server(chassis_port, &chassis_handler); vaw::ControlDomainServer control_server(control_port, &control_handler); + std::thread t_gateway([&]() { + try { gateway_server.run(); } + catch (const std::exception& e) + { std::fprintf(stderr, "[gateway] 启动失败: %s\n", e.what()); } + }); + std::thread t_chassis([&]() { try { chassis_server.run(); } catch (const std::exception& e) @@ -56,16 +74,22 @@ int main(int argc, char* argv[]) { std::fprintf(stderr, "[control] 启动失败: %s\n", e.what()); } }); - std::printf("vehicle_agent_windows 已启动,按 Ctrl+C 退出。\n"); + std::printf( + "vehicle_agent_windows 已启动,gateway=%u, chassis=%u, control=%u,按 Ctrl+C 退出。\n", + gateway_port, + chassis_port, + control_port); // 等待退出信号 while (!g_shutdown) std::this_thread::sleep_for(std::chrono::milliseconds(200)); std::printf("\n正在关闭...\n"); + gateway_server.stop(); chassis_server.stop(); control_server.stop(); + t_gateway.join(); t_chassis.join(); t_control.join(); diff --git a/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/vehicle_gateway_server.cpp b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/vehicle_gateway_server.cpp new file mode 100644 index 0000000..be089a5 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/vehicle_agent_real/vehicle_agent_windows/src/vehicle_gateway_server.cpp @@ -0,0 +1,194 @@ +// vehicle_gateway_server.cpp +// 车端统一 WiFi6/TCP 入口,按消息类型分发到底盘、运控和外部定位位姿缓存。 + +#include "vehicle_gateway_server.hpp" + +#include +#include +#include +#include + +#include "data_types.hpp" +#include "frame_codec.hpp" + +namespace vaw +{ + +VehicleGatewayServer::VehicleGatewayServer( + uint16_t port, + ChassisHandler* chassis_handler, + ControlHandler* control_handler, + ExternalPoseStore* external_pose_store) + : port_(port), + chassis_handler_(chassis_handler), + control_handler_(control_handler), + external_pose_store_(external_pose_store), + running_(false), + listen_sock_(SOCK_INVALID) +{} + +VehicleGatewayServer::~VehicleGatewayServer() +{ + stop(); +} + +void VehicleGatewayServer::run() +{ +#ifdef _WIN32 + WSADATA wsa; + WSAStartup(MAKEWORD(2, 2), &wsa); +#endif + + listen_sock_ = ::socket(AF_INET, SOCK_STREAM, 0); + if (listen_sock_ == SOCK_INVALID) + throw std::runtime_error("统一入口:创建 socket 失败"); + + int opt = 1; + ::setsockopt(listen_sock_, SOL_SOCKET, SO_REUSEADDR, + reinterpret_cast(&opt), sizeof(opt)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + addr.sin_addr.s_addr = INADDR_ANY; + + if (::bind(listen_sock_, reinterpret_cast(&addr), sizeof(addr)) < 0) + throw std::runtime_error("统一入口:bind 失败"); + + ::listen(listen_sock_, 32); + running_ = true; + std::printf("[gateway] 监听端口 %d\n", port_); + + while (running_) { + sockaddr_in client_addr{}; +#ifdef _WIN32 + int addr_len = sizeof(client_addr); +#else + socklen_t addr_len = sizeof(client_addr); +#endif + sock_t conn = ::accept(listen_sock_, + reinterpret_cast(&client_addr), &addr_len); + if (conn == SOCK_INVALID) break; + + std::thread([this, conn]() { handle_connection(conn); }).detach(); + } +} + +void VehicleGatewayServer::stop() +{ + running_ = false; + if (listen_sock_ != SOCK_INVALID) { + ::shutdown(listen_sock_, SOCK_SHUTDOWN_BOTH); + SOCK_CLOSE(listen_sock_); + listen_sock_ = SOCK_INVALID; + } +} + +MsgType VehicleGatewayServer::response_type_for(MsgType request_type) const +{ + switch (request_type) { + case MsgType::CHASSIS_GET_READINESS_REQ: + return MsgType::CHASSIS_GET_READINESS_RSP; + case MsgType::CHASSIS_MOTION_PRIMITIVE_REQ: + return MsgType::CHASSIS_MOTION_PRIMITIVE_RSP; + case MsgType::CHASSIS_EMERGENCY_BRAKE_REQ: + return MsgType::CHASSIS_EMERGENCY_BRAKE_RSP; + case MsgType::CONTROL_GET_READINESS_REQ: + return MsgType::CONTROL_GET_READINESS_RSP; + case MsgType::CONTROL_EVALUATION_REQ: + return MsgType::CONTROL_EVALUATION_RSP; + case MsgType::EXTERNAL_POSE_PUSH_REQ: + return MsgType::EXTERNAL_POSE_PUSH_RSP; + default: + return MsgType::UNSPECIFIED; + } +} + +void VehicleGatewayServer::send_error_response(sock_t conn, MsgType response_type, const std::string& message) const +{ + StandardResponse rsp; + rsp.success = false; + rsp.error_code = ErrorCode::INVALID_ARGUMENT; + rsp.message = message; + nlohmann::json j_rsp = rsp; + send_frame(conn, response_type, j_rsp.dump()); +} + +void VehicleGatewayServer::handle_connection(sock_t conn) +{ + std::printf("[gateway] 新连接\n"); + MsgType msg_type = MsgType::UNSPECIFIED; + try { + std::string payload; + recv_frame(conn, msg_type, payload); + std::printf("[gateway] 收到帧 type=%u\n", static_cast(msg_type)); + + nlohmann::json j_payload; + if (!payload.empty()) { + j_payload = nlohmann::json::parse(payload); + } + + switch (msg_type) { + case MsgType::CHASSIS_GET_READINESS_REQ: { + auto rsp = chassis_handler_->get_readiness(j_payload); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CHASSIS_GET_READINESS_RSP, j_rsp.dump()); + break; + } + + case MsgType::CHASSIS_MOTION_PRIMITIVE_REQ: { + MotionPrimitiveRequest req; + from_json(j_payload, req); + auto rsp = chassis_handler_->execute_motion_primitive(req); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CHASSIS_MOTION_PRIMITIVE_RSP, j_rsp.dump()); + break; + } + + case MsgType::CHASSIS_EMERGENCY_BRAKE_REQ: { + auto j_rsp = chassis_handler_->emergency_brake(j_payload); + send_frame(conn, MsgType::CHASSIS_EMERGENCY_BRAKE_RSP, j_rsp.dump()); + break; + } + + case MsgType::CONTROL_GET_READINESS_REQ: { + auto rsp = control_handler_->get_readiness(j_payload); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CONTROL_GET_READINESS_RSP, j_rsp.dump()); + break; + } + + case MsgType::CONTROL_EVALUATION_REQ: { + ControllerEvaluationRequest req; + from_json(j_payload, req); + auto rsp = control_handler_->execute_evaluation(req); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::CONTROL_EVALUATION_RSP, j_rsp.dump()); + break; + } + + case MsgType::EXTERNAL_POSE_PUSH_REQ: { + ExternalLocalizationTelemetry req; + from_json(j_payload, req); + auto rsp = external_pose_store_->update(req); + nlohmann::json j_rsp = rsp; + send_frame(conn, MsgType::EXTERNAL_POSE_PUSH_RSP, j_rsp.dump()); + break; + } + + default: + std::printf("[gateway] 未知帧类型 %u,忽略\n", static_cast(msg_type)); + send_error_response( + conn, + MsgType::UNSPECIFIED, + "不支持的统一入口帧类型: " + std::to_string(static_cast(msg_type))); + break; + } + } catch (const std::exception& e) { + std::printf("[gateway] 处理帧异常: %s\n", e.what()); + send_error_response(conn, response_type_for(msg_type), e.what()); + } + SOCK_CLOSE(conn); +} + +} // namespace vaw diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/README.md b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/README.md new file mode 100644 index 0000000..81eed79 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/README.md @@ -0,0 +1,275 @@ +# 底盘标定现场数据模板 + +这个目录只处理底盘标定的现场数据输入边界,不直接控制车辆。真实车辆运动仍由 Windows 车端 agent 执行动作原语,车间电脑侧负责采集底盘遥测、执行器命令和外部真值轨迹,并把这些文件写入 `dataset_index.yaml`。 + +## 数据流 + +```text +Windows 车端 agent 执行动作原语 + -> 底盘遥测 / 执行器命令 / 外部真值轨迹落盘 + -> replay_chassis_smoke.py 或真实采集程序写 dataset_index.yaml + -> dataset_index_to_site_data_input.py 生成 site_data_input.yaml + -> chassis_calibration_service 通过 data_input.* 读取算法输入 +``` + +## 数据输入字段 + +`chassis_calibration_service` 当前读取: + +- `data_input.chassis_motion_data_files`:底盘遥测,例如里程计、速度、角速度、故障码。 +- `data_input.actuator_command_files`:执行器命令和反馈,例如目标速度、目标转角、制动命令。 +- `data_input.truth_trajectory_files`:外部真值轨迹,用于和车端里程计对齐。 + +三类 CSV 的固定字段、单位、时间基和坐标系见 `chassis_csv_contract.md`。现场采集和离线自检脚本都会按这个协议校验列名和基础类型。 + +## 底盘类型 + +当前底盘标定保留四类入口: + +- 阿克曼底盘标定:模板入口保留,方便继续复用当前 Isaac 小车和现场兼容配置。 +- 差速轮底盘标定:已确认标定目标为左轮半径、右轮半径、驱动轮轮距。 +- 单舵轮底盘标定:算法模板已预留,具体标定目标由算法实现方补充。 +- 多舵轮底盘标定:算法模板已预留,具体标定目标由算法实现方补充。 + +## 动作原语 + +`chassis_calibration_service` 的通用入口已经支持这些动作原语的字段合法性校验: + +- `straight_line` +- `arc` +- `in_place_rotation` +- `steering_sweep` +- `lateral_translation` +- `diagonal_motion` +- `module_alignment` +- `coordinated_steering` + +通用入口只检查字段是否可用,具体“某类底盘需要哪些动作组合”和“数据是否足够求解”由四个底盘专属算法模板继续判断。 + +## 现场动作 profile + +动作序列模板见: + +```text +src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml +``` + +这个 profile 固定了四类底盘在真实标定车间中建议执行的动作序列、速度、距离、超时和采集要求。现场部署时需要先按工位尺寸、安全速度、真实模块 ID 修改它,再用工具校验: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py \ + --profile /data/agv_calib/site_a/chassis_action_profile.yaml +``` + +导出某类底盘的总控任务片段: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py \ + --profile /data/agv_calib/site_a/chassis_action_profile.yaml \ + --chassis-type differential \ + --format requested_tasks \ + --output /data/agv_calib/site_a/chassis_requested_tasks.yaml +``` + +导出的 `requested_tasks` 字段和总控里的 `RequestedCalibrationTask` 对齐。后续接入总控时,可以直接把这些动作拆成多个底盘阶段,而不是只跑一个默认直线阶段。 + +总控 smoke 已支持直接读取这个 profile: + +```bash +python3 src/simulation/tools/smoke_test_workshop_orchestrator.py \ + --tasks external,chassis,control,sensor_intrinsic \ + --chassis-profile-type ackermann \ + --chassis-action-profile src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml +``` + +不传 `--chassis-action-profile` 时,smoke 仍保持旧行为,只生成一个最小直线底盘阶段。 + +现场 profile 也可以统一生成总控会话配置: + +```bash +python3 src/deployment/tools/build_workshop_session_config.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --tasks external,chassis,control,sensor_intrinsic \ + --chassis-type differential \ + --chassis-action-profile src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml \ + --control-evaluation-profile src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml \ + --output /data/agv_calib/site_a/session_001/workshop_session_config.yaml +``` + +## 按动作 profile 执行并采集 + +现场需要边执行底盘动作边采集数据时,使用: + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash + +python3 src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py \ + --config /data/agv_calib/site_a/chassis_data.yaml \ + --action-profile /data/agv_calib/site_a/chassis_action_profile.yaml \ + --chassis-type differential \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id AGV-001 \ + --session-dir /data/agv_calib/site_a/session_001 \ + --dataset-index-path /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --chassis-telemetry-topic /chassis/telemetry \ + --external-pose-topic /workshop/external_localization/vehicle/pose +``` + +这个入口会: + +- 打开三类 CSV 采集文件。 +- 按动作 profile 顺序向 `/chassis/execute_motion_primitive` 发送动作。 +- 动作前后按 profile 配置保留采集缓冲时间。 +- 命令话题不可用时,`command_source=auto` 会按动作 profile 写计划命令行。 +- 全部动作结束后统一写 `dataset_index.yaml`。 + +本地最小 smoke: + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash + +python3 src/site_deployment/workshop_chassis_calibration_real/smoke_test_chassis_profile_capture.py \ + --chassis-type ackermann +``` + +这个 smoke 会启动假的底盘 action server、底盘遥测和外部真值发布器,然后调用 `run_chassis_profile_capture.py` 验证 CSV 和 `dataset_index.yaml` 写入链路。 + +## 待提交参数包 + +真实算法输出参数后,先生成待审批/待提交参数包,不在这里假装已经写入车端: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/stage_chassis_parameter_commit.py \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --estimated-params /data/agv_calib/site_a/session_001/chassis/estimated_params.yaml \ + --operator-id operator_001 \ + --previous-parameter-version current_vehicle_chassis_v1 +``` + +输出默认写到: + +```text +/data/agv_calib/site_a/session_001/chassis/pending_chassis_commit.yaml +``` + +这个文件记录参数版本、参数摘要、数据集索引、人工审批状态、持久化写入意图和回滚引用。真正写入车端仍由后续真实车端提交实现负责。 + +会话收尾时也可以直接走统一入口。该入口会先生成 `site_data_input.yaml`,如果发现 `chassis/estimated_params.yaml` 或显式传入 `--chassis-estimated-params`,会自动生成 `pending_chassis_commit.yaml` 并把底盘待提交信息写回 `dataset_index.yaml`: + +```bash +python3 src/deployment/tools/finalize_site_session.py \ + --site-profile /data/agv_calib/site_a/site.yaml \ + --session-dir /data/agv_calib/site_a/session_001 \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --chassis-estimated-params /data/agv_calib/site_a/session_001/chassis/estimated_params.yaml \ + --chassis-operator-id operator_001 \ + --previous-chassis-parameter-version current_vehicle_chassis_v1 +``` + +总控 report 会从 `dataset_index.yaml` 的底盘待提交元数据中自动加入: + +- `chassis_estimated_params_file` +- `chassis_pending_commit_file` +- `chassis_pending_commit_state` +- `chassis_pending_parameter_version` +- `chassis_pending_commit_approval_required` +- `chassis_pending_commit_approved` + +人工确认后,车间电脑侧先生成“已审批参数交接包”。这一步仍然不调用车端写参服务,只把后续车端适配器需要消费的内容固定下来: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/approve_chassis_pending_parameters.py \ + /data/agv_calib/site_a/session_001/chassis/pending_chassis_commit.yaml \ + --operator-id operator_001 +``` + +这条命令会校验 `estimated_params.yaml` 的 sha256 摘要,标记人工审批通过,生成: + +```text +/data/agv_calib/site_a/session_001/chassis/approved_chassis_parameter_handoff.yaml +``` + +该交接包是车间电脑侧产物,记录参数版本、车辆 ID、审批人、sha256 摘要、待写入参数和车端后续需要实现的消费合同。真实车端写入、持久化和失败回滚先不在这里实现。 + +## 离线自检 + +复制模板后填写真实 `session_id/site_id/vehicle_id/session_dir/dataset_index_path`,或用命令行覆盖: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/replay_chassis_smoke.py \ + --config src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_template.yaml \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id AGV-001 \ + --session-dir /tmp/agv_calib_chassis/session_001 \ + --dataset-index-path /tmp/agv_calib_chassis/session_001/dataset_index.yaml +``` + +不传真实 CSV 时,脚本会生成一段最小合成数据,只用于验证索引和参数注入链路。现场使用真实数据时: + +```bash +python3 src/site_deployment/workshop_chassis_calibration_real/replay_chassis_smoke.py \ + --config /data/agv_calib/site_a/chassis_data.yaml \ + --chassis-motion-csv /data/raw/chassis_motion.csv \ + --actuator-command-csv /data/raw/actuator_commands.csv \ + --truth-trajectory-csv /data/raw/truth_trajectory.csv \ + --no-synthetic +``` + +随后可运行: + +```bash +python3 src/deployment/tools/validate_dataset_index.py \ + /tmp/agv_calib_chassis/session_001/dataset_index.yaml \ + --tasks chassis \ + --require-existing-files + +python3 src/deployment/tools/dataset_index_to_site_data_input.py \ + /tmp/agv_calib_chassis/session_001/dataset_index.yaml \ + --strict +``` + +## 现场采集落盘 + +现场运行时使用 `capture_chassis_session.py` 采集三类数据: + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash + +python3 src/site_deployment/workshop_chassis_calibration_real/capture_chassis_session.py \ + --config /data/agv_calib/site_a/chassis_data.yaml \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id AGV-001 \ + --session-dir /data/agv_calib/site_a/session_001 \ + --dataset-index-path /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --chassis-telemetry-topic /chassis/telemetry \ + --external-pose-topic /workshop/external_localization/vehicle/pose \ + --ackermann-command-topic /vehicle/AGV-001/internal/ackermann_cmd \ + --duration-sec 60 +``` + +输出文件: + +- `chassis/chassis_motion.csv`:底盘遥测。 +- `chassis/actuator_commands.csv`:执行器命令。命令话题没有数据时,`command_source=auto` 会写入一条计划命令,避免数据索引缺失。 +- `external/truth_trajectory.csv`:外部真值轨迹。 +- `chassis/chassis_diagnostics.json`:采集摘要和检查结果。 +- `dataset_index.yaml`:给后续 `site_data_input.yaml` 转换使用的数据索引。 + +## 算法填充边界 + +详细算法合同见 `src/docs/chassis_calibration_algorithm_contract.md`。 + +底盘算法同事主要填这些 C++ 文件: + +- `ackermann_chassis_algorithm.cpp` +- `differential_chassis_algorithm.cpp` +- `single_steer_wheel_chassis_algorithm.cpp` +- `multi_steer_wheel_chassis_algorithm.cpp` + +这些文件的输入已经包含 `chassis_motion_data_files`、`actuator_command_files`、`truth_trajectory_files`,后续可以在算法里按车型读取对应文件并写回 `estimated_params` 和 `validation_summary`。 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/approve_chassis_pending_parameters.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/approve_chassis_pending_parameters.py new file mode 100644 index 0000000..cfb15cd --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/approve_chassis_pending_parameters.py @@ -0,0 +1,257 @@ +#!/usr/bin/env python3 +"""审批底盘待提交参数包,并生成车间电脑侧交接包。""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="审批底盘待提交参数包,并生成车间电脑侧参数交接包。") + parser.add_argument("pending_commit", help="底盘待提交参数包 pending_chassis_commit.yaml。") + parser.add_argument("--operator-id", required=True, help="审批操作员 ID。") + parser.add_argument("--approval-note", default="", help="审批备注。") + parser.add_argument("--output", default="", help="交接包输出路径;为空时写到会话目录 chassis/approved_chassis_parameter_handoff.yaml。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否把交接包路径写回 dataset_index.yaml。") + parser.add_argument("--dry-run", action="store_true", help="只校验并打印结果,不修改文件。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def bool_value(value: Any) -> bool: + if isinstance(value, bool): + return value + if isinstance(value, str): + return value.strip().lower() in ("1", "true", "yes", "y") + return bool(value) + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def resolve_dataset_file(path_value: str, dataset_root: Path) -> Path: + path = Path(path_value).expanduser() + if path.is_absolute(): + return path + return (dataset_root / path).resolve(strict=False) + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def validate_pending_commit(package: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]: + if int(package.get("schema_version", 0) or 0) != 1: + raise ValueError("pending_chassis_commit.schema_version 必须为 1。") + session = require_map(package.get("session"), "pending_chassis_commit.session") + source = require_map(package.get("source"), "pending_chassis_commit.source") + approval = require_map(package.get("approval"), "pending_chassis_commit.approval") + commit_request = require_map(package.get("commit_request"), "pending_chassis_commit.commit_request") + require_string(session.get("session_id"), "session.session_id") + require_string(session.get("vehicle_id"), "session.vehicle_id") + require_string(session.get("dataset_root"), "session.dataset_root") + require_string(commit_request.get("parameter_version"), "commit_request.parameter_version") + require_string(commit_request.get("chassis_type"), "commit_request.chassis_type") + require_map(commit_request.get("estimated_params"), "commit_request.estimated_params") + return session, source, approval, commit_request + + +def validate_estimated_params_digest(session: dict[str, Any], source: dict[str, Any]) -> dict[str, str]: + digest = require_map(source.get("estimated_params_digest"), "source.estimated_params_digest") + checksum_type = require_string(digest.get("checksum_type"), "estimated_params_digest.checksum_type") + checksum_value = require_string(digest.get("checksum_value"), "estimated_params_digest.checksum_value") + if checksum_type.lower() != "sha256": + raise ValueError("当前只支持 sha256 参数摘要。") + + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + estimated_params_file = require_string(source.get("estimated_params_file"), "source.estimated_params_file") + estimated_params_path = resolve_dataset_file(estimated_params_file, dataset_root) + if not estimated_params_path.exists(): + raise FileNotFoundError(f"底盘算法输出参数文件不存在,无法校验摘要:{estimated_params_path}") + actual_checksum = sha256_file(estimated_params_path) + if actual_checksum != checksum_value: + raise ValueError("底盘算法输出参数摘要不一致,禁止审批交接。") + return { + "checksum_type": checksum_type, + "checksum_value": checksum_value, + "estimated_params_file": relative_to_root(estimated_params_path, dataset_root), + } + + +def resolve_output_path(args: argparse.Namespace, session: dict[str, Any]) -> Path: + if args.output: + return Path(args.output).expanduser().resolve(strict=False) + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + return (dataset_root / "chassis" / "approved_chassis_parameter_handoff.yaml").resolve(strict=False) + + +def build_handoff_package( + pending_path: Path, + output_path: Path, + package: dict[str, Any], + args: argparse.Namespace, +) -> dict[str, Any]: + session, source, approval, commit_request = validate_pending_commit(package) + digest = validate_estimated_params_digest(session, source) + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + approved_us = now_us() + + approval["required"] = bool_value(approval.get("required", True)) + approval["approved"] = True + approval["operator_id"] = args.operator_id + approval["approved_timestamp_us"] = approved_us + approval["approval_note"] = args.approval_note + package["commit_state"] = "approved_waiting_vehicle_handoff" + package["vehicle_handoff"] = { + "handoff_file": relative_to_root(output_path, dataset_root), + "handoff_state": "ready_for_vehicle_adapter", + "generated_timestamp_us": approved_us, + "generated_by": "approve_chassis_pending_parameters.py", + } + + return { + "schema_version": 1, + "handoff_state": "ready_for_vehicle_adapter", + "created_timestamp_us": approved_us, + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "source": { + "pending_commit_file": relative_to_root(pending_path, dataset_root), + "dataset_index_file": source.get("dataset_index_file", ""), + "estimated_params_file": digest["estimated_params_file"], + "estimated_params_digest": { + "checksum_type": digest["checksum_type"], + "checksum_value": digest["checksum_value"], + }, + }, + "approval": { + "approved": True, + "operator_id": args.operator_id, + "approved_timestamp_us": approved_us, + "approval_note": args.approval_note, + }, + "vehicle_commit_request": { + "parameter_version": commit_request["parameter_version"], + "chassis_type": commit_request["chassis_type"], + "commit_reason": commit_request.get("commit_reason", ""), + "persistent_write": bool_value(commit_request.get("persistent_write", True)), + "estimated_params": commit_request["estimated_params"], + }, + "vehicle_adapter_contract": { + "status": "waiting_vehicle_side_adapter", + "required_request": "vehicle_side_parameter_write_adapter", + "required_digest_check": "sha256", + "rollback_required_on_vehicle_commit_failure": bool_value( + package.get("rollback", {}).get("rollback_required_on_vehicle_commit_failure", True) + ), + }, + } + + +def update_dataset_index( + pending_package: dict[str, Any], + handoff_path: Path, + handoff_package: dict[str, Any], +) -> None: + session = require_map(pending_package.get("session"), "pending_chassis_commit.session") + source = require_map(pending_package.get("source"), "pending_chassis_commit.source") + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + dataset_index_file = source.get("dataset_index_file", "") + if not isinstance(dataset_index_file, str) or not dataset_index_file: + return + dataset_index_path = resolve_dataset_file(dataset_index_file, dataset_root) + if not dataset_index_path.exists(): + return + + index = load_yaml(dataset_index_path) + metadata = index.setdefault("metadata", {}) + metadata["chassis_parameter_handoff_file"] = relative_to_root(handoff_path, dataset_root) + metadata["chassis_parameter_handoff_state"] = handoff_package["handoff_state"] + metadata["chassis_pending_commit_state"] = pending_package.get("commit_state", "") + metadata["chassis_pending_commit_approved"] = True + metadata["chassis_pending_commit_approved_operator_id"] = handoff_package["approval"]["operator_id"] + metadata["chassis_pending_commit_approved_timestamp_us"] = handoff_package["approval"]["approved_timestamp_us"] + write_yaml(dataset_index_path, index) + + +def main() -> int: + args = parse_args() + pending_path = Path(args.pending_commit).expanduser().resolve(strict=False) + package = load_yaml(pending_path) + session, _, _, commit_request = validate_pending_commit(package) + output_path = resolve_output_path(args, session) + handoff_package = build_handoff_package(pending_path, output_path, package, args) + + print(f"[OK] 底盘待提交参数包校验通过:{pending_path}") + print(f"[OK] 参数版本:{commit_request['parameter_version']}") + print(f"[OK] 目标车辆:{session['vehicle_id']}") + print(f"[OK] 交接包状态:{handoff_package['handoff_state']}") + + if args.dry_run: + print("[OK] dry-run:未修改文件。") + return 0 + + write_yaml(output_path, handoff_package) + write_yaml(pending_path, package) + if args.update_dataset_index: + update_dataset_index(package, output_path, handoff_package) + print(f"[OK] 已生成底盘参数交接包:{output_path}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/capture_chassis_session.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/capture_chassis_session.py new file mode 100644 index 0000000..ad92e43 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/capture_chassis_session.py @@ -0,0 +1,384 @@ +#!/usr/bin/env python3 +"""现场底盘标定数据采集落盘入口。""" + +from __future__ import annotations + +import argparse +import csv +import json +import sys +import time +from pathlib import Path +from typing import Any + +from chassis_data_common import ( + ACTUATOR_COMMAND_FIELDS, + CHASSIS_MOTION_FIELDS, + TRUTH_TRAJECTORY_FIELDS, + apply_cli_overrides, + build_dataset_index, + build_summary, + has_placeholder, + load_config, + read_csv_rows, + resolve_path, + normalize_chassis_type_name, + validate_chassis_dataset_files, + validate_config, + validate_summary, + write_yaml, +) + + +def float_value(value: Any, default: float = 0.0) -> float: + if value in (None, ""): + return default + return float(value) + + +def int_value(value: Any, default: int = 0) -> int: + if value in (None, ""): + return default + return int(value) + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="从现场 ROS 话题采集底盘遥测、执行器命令和外部真值轨迹,并写入 dataset_index.yaml。" + ) + parser.add_argument("--config", required=True, help="底盘标定现场数据配置文件。") + parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type。") + parser.add_argument("--session-id", default="", help="覆盖 session_id。") + parser.add_argument("--site-id", default="", help="覆盖 site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 vehicle_id。") + parser.add_argument("--session-dir", default="", help="覆盖 session_dir。") + parser.add_argument("--dataset-index-path", default="", help="覆盖 dataset_index_path。") + parser.add_argument("--chassis-telemetry-topic", default="", help="底盘遥测话题。") + parser.add_argument("--external-pose-topic", default="", help="外部真值位姿话题。") + parser.add_argument("--ackermann-command-topic", default="", help="阿克曼内部命令话题。") + parser.add_argument( + "--command-source", + choices=["auto", "topic", "planned", "none"], + default="", + help="执行器命令 CSV 来源:auto 表示优先话题,缺失时写入计划命令。", + ) + parser.add_argument("--duration-sec", type=float, default=0.0, help="采集时长;0 表示按 Ctrl+C 结束。") + parser.add_argument("--max-chassis-samples", type=int, default=0, help="达到该底盘样本数后自动结束;0 表示不限。") + parser.add_argument("--target-speed-ms", type=float, default=0.0, help="计划命令目标速度。") + parser.add_argument("--target-distance-m", type=float, default=0.0, help="计划命令目标距离。") + parser.add_argument("--target-steering-angle-rad", type=float, default=0.0, help="计划命令目标转角。") + parser.add_argument("--brake-command", type=float, default=0.0, help="计划命令制动值。") + parser.add_argument("--command-id", default="", help="计划命令 ID。") + parser.add_argument("--allow-incomplete", action="store_true", help="样本不足时仍写入索引和诊断文件。") + return parser.parse_args() + + +def config_topic(config: dict[str, Any], key: str, default: str) -> str: + capture = config.get("capture", {}) or {} + if not isinstance(capture, dict): + return default + value = str(capture.get(key, "") or "").strip() + if not value or has_placeholder(value): + return default + return value + + +def synthetic_value(config: dict[str, Any], key: str, default: float) -> float: + synthetic = config.get("synthetic", {}) or {} + if not isinstance(synthetic, dict): + return default + return float_value(synthetic.get(key), default) + + +def config_command_source(config: dict[str, Any]) -> str: + capture = config.get("capture", {}) or {} + if not isinstance(capture, dict): + return "auto" + value = str(capture.get("command_source", "auto") or "auto").strip() + return value if value in {"auto", "topic", "planned", "none"} else "auto" + + +def module_rows(modules: Any) -> list[dict[str, Any]]: + result: list[dict[str, Any]] = [] + for module in modules: + result.append({ + "module_id": str(getattr(module, "module_id", "")), + "encoder_ticks": int_value(getattr(module, "encoder_ticks", 0)), + "wheel_speed_rpm": float_value(getattr(module, "wheel_speed_rpm", 0.0)), + "steer_angle_deg": float_value(getattr(module, "steer_angle_deg", 0.0)), + "motor_current_amp": float_value(getattr(module, "motor_current_amp", 0.0)), + }) + return result + + +class CsvAppender: + def __init__(self, path: Path, fieldnames: list[str]) -> None: + self.path = path + self.path.parent.mkdir(parents=True, exist_ok=True) + self.stream = self.path.open("w", newline="", encoding="utf-8") + self.writer = csv.DictWriter(self.stream, fieldnames=fieldnames) + self.writer.writeheader() + self.count = 0 + + def write_row(self, row: dict[str, Any]) -> None: + self.writer.writerow(row) + self.stream.flush() + self.count += 1 + + def close(self) -> None: + self.stream.close() + + +class ChassisSessionCapture: + def __init__( + self, + node: Any, + config: dict[str, Any], + paths: dict[str, Path], + args: argparse.Namespace, + ) -> None: + self.node = node + self.config = config + self.paths = paths + self.args = args + self.started_us = now_us() + self.chassis_csv = CsvAppender(paths["chassis_motion_data_file"], CHASSIS_MOTION_FIELDS) + self.command_csv = CsvAppender(paths["actuator_command_file"], ACTUATOR_COMMAND_FIELDS) + self.truth_csv = CsvAppender(paths["truth_trajectory_file"], TRUTH_TRAJECTORY_FIELDS) + + def close(self) -> None: + self.chassis_csv.close() + self.command_csv.close() + self.truth_csv.close() + + def write_planned_command_if_needed(self) -> None: + if self.args.command_source not in ("auto", "planned"): + return + if self.command_csv.count > 0: + return + self.command_csv.write_row({ + "command_timestamp_us": self.started_us, + "command_id": self.args.command_id or f"{self.config.get('session_id', 'session')}_planned_command", + "source": "planned_command", + "target_speed_ms": self.args.target_speed_ms or synthetic_value(self.config, "target_speed_ms", 0.1), + "target_distance_m": self.args.target_distance_m or synthetic_value(self.config, "target_distance_m", 0.2), + "target_accel_ms2": 0.0, + "target_steering_angle_rad": self.args.target_steering_angle_rad, + "target_steering_rate_rads": 0.0, + "brake_command": self.args.brake_command, + "throttle_command": 0.0, + "command_timeout_sec": 0.0, + }) + + def on_chassis_telemetry(self, msg: Any) -> None: + chassis_type = normalize_chassis_type_name(getattr(getattr(msg, "chassis_type", None), "value", "")) + self.chassis_csv.write_row({ + "hardware_timestamp_us": int_value(getattr(msg, "hardware_timestamp_us", 0), now_us()), + "chassis_type": chassis_type, + "odom_x_m": float_value(getattr(msg, "odom_x_m", 0.0)), + "odom_y_m": float_value(getattr(msg, "odom_y_m", 0.0)), + "odom_yaw_rad": float_value(getattr(msg, "odom_yaw_rad", 0.0)), + "linear_velocity_ms": float_value(getattr(msg, "linear_velocity_ms", 0.0)), + "angular_velocity_rads": float_value(getattr(msg, "angular_velocity_rads", 0.0)), + "estop_engaged": int(bool(getattr(msg, "estop_engaged", False))), + "driver_error_code": int_value(getattr(msg, "driver_error_code", 0)), + "active_job_id": str(getattr(msg, "active_job_id", "")), + "lateral_slip_estimate": float_value(getattr(msg, "lateral_slip_estimate", 0.0)), + "curvature_estimate": float_value(getattr(msg, "curvature_estimate", 0.0)), + "module_states_json": json.dumps(module_rows(getattr(msg, "modules", [])), ensure_ascii=False), + }) + + def on_external_pose(self, msg: Any) -> None: + pose = getattr(msg, "workshop_pose", None) + self.truth_csv.write_row({ + "hardware_timestamp_us": int_value(getattr(msg, "hardware_timestamp_us", 0), now_us()), + "pose_valid": int(bool(getattr(msg, "pose_valid", False))), + "x_m": float_value(getattr(pose, "x_m", 0.0)), + "y_m": float_value(getattr(pose, "y_m", 0.0)), + "z_m": float_value(getattr(pose, "z_m", 0.0)), + "roll_rad": float_value(getattr(pose, "roll_rad", 0.0)), + "pitch_rad": float_value(getattr(pose, "pitch_rad", 0.0)), + "yaw_rad": float_value(getattr(pose, "yaw_rad", 0.0)), + "position_stddev_m": float_value(getattr(msg, "position_stddev_m", 0.0)), + "yaw_stddev_rad": float_value(getattr(msg, "yaw_stddev_rad", 0.0)), + "tracking_loss_ratio": float_value(getattr(msg, "tracking_loss_ratio", 0.0)), + "time_sync_offset_ms": float_value(getattr(msg, "time_sync_offset_ms", 0.0)), + "quality_score": float_value(getattr(msg, "quality_score", 0.0)), + "observed_target_count": int_value(getattr(msg, "observed_target_count", 0)), + "reference_source_name": str(getattr(msg, "reference_source_name", "")), + "active_job_id": str(getattr(msg, "active_job_id", "")), + }) + + def on_ackermann_command(self, msg: Any) -> None: + if self.args.command_source == "planned": + return + self.command_csv.write_row({ + "command_timestamp_us": int_value(getattr(msg, "command_timestamp_us", 0), now_us()), + "command_id": str(getattr(msg, "command_id", "")), + "source": str(getattr(msg, "source", "")), + "target_speed_ms": float_value(getattr(msg, "target_speed_ms", 0.0)), + "target_distance_m": self.args.target_distance_m, + "target_accel_ms2": float_value(getattr(msg, "target_accel_ms2", 0.0)), + "target_steering_angle_rad": float_value(getattr(msg, "target_steering_angle_rad", 0.0)), + "target_steering_rate_rads": float_value(getattr(msg, "target_steering_rate_rads", 0.0)), + "brake_command": float_value(getattr(msg, "brake_command", 0.0)), + "throttle_command": float_value(getattr(msg, "throttle_command", 0.0)), + "command_timeout_sec": float_value(getattr(msg, "command_timeout_sec", 0.0)), + }) + + +def make_target_paths(config: dict[str, Any]) -> dict[str, Path]: + session_dir = Path(str(config["session_dir"])).expanduser().resolve(strict=False) + files = config["files"] + return { + "chassis_motion_data_file": resolve_path(session_dir, str(files["chassis_motion_data_file"])), + "actuator_command_file": resolve_path(session_dir, str(files["actuator_command_file"])), + "truth_trajectory_file": resolve_path(session_dir, str(files["truth_trajectory_file"])), + "diagnostics_file": resolve_path(session_dir, str(files["diagnostics_file"])), + } + + +def finalize_capture( + config: dict[str, Any], + target_paths: dict[str, Path], + config_path: Path, + args: argparse.Namespace, +) -> int: + contract_validation = validate_chassis_dataset_files(target_paths) + if not contract_validation.ok: + summary = { + "session_id": config.get("session_id", ""), + "site_id": config.get("site_id", ""), + "vehicle_id": config.get("vehicle_id", ""), + "chassis_type": config.get("chassis_type", ""), + "contract_errors": contract_validation.errors, + "contract_warnings": contract_validation.warnings, + } + target_paths["diagnostics_file"].parent.mkdir(parents=True, exist_ok=True) + target_paths["diagnostics_file"].write_text( + json.dumps(summary, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + for error in contract_validation.errors: + print(f"[错误] {error}", file=sys.stderr) + print(f"[错误] 已写入诊断文件: {target_paths['diagnostics_file']}", file=sys.stderr) + return 1 + + chassis_rows = read_csv_rows(target_paths["chassis_motion_data_file"]) + command_rows = read_csv_rows(target_paths["actuator_command_file"]) + truth_rows = read_csv_rows(target_paths["truth_trajectory_file"]) + summary = build_summary(chassis_rows, command_rows, truth_rows, config) + + summary_validation = validate_summary(summary, config) + summary["contract_errors"] = contract_validation.errors + summary["contract_warnings"] = contract_validation.warnings + summary["validation_errors"] = summary_validation.errors + summary["validation_warnings"] = summary_validation.warnings + target_paths["diagnostics_file"].parent.mkdir(parents=True, exist_ok=True) + target_paths["diagnostics_file"].write_text( + json.dumps(summary, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + + if summary_validation.errors and not args.allow_incomplete: + for error in summary_validation.errors: + print(f"[错误] {error}", file=sys.stderr) + print(f"[错误] 已写入诊断文件: {target_paths['diagnostics_file']}", file=sys.stderr) + return 1 + + for warning in contract_validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + for warning in summary_validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + + dataset_index = build_dataset_index(config, summary, target_paths, config_path) + dataset_index_path = Path(str(config["dataset_index_path"])).expanduser().resolve(strict=False) + write_yaml(dataset_index_path, dataset_index) + print(json.dumps(summary, ensure_ascii=False, separators=(",", ":"))) + print(f"[OK] 已写入底盘采集数据集索引: {dataset_index_path}", file=sys.stderr) + return 0 + + +def main() -> int: + args = parse_args() + config_path = Path(args.config).expanduser().resolve(strict=False) + config = load_config(config_path) + apply_cli_overrides(config, args) + if not args.command_source: + args.command_source = config_command_source(config) + validation = validate_config(config, config_path) + if not validation.ok: + for error in validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + try: + import rclpy + from calibration_chassis_interfaces.msg import ChassisTelemetry + from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry + except ImportError as exc: + print(f"[错误] 缺少 ROS 2 Python 依赖或标定消息包,无法现场采集: {exc}", file=sys.stderr) + return 1 + + try: + from vehicle_internal_interfaces.msg import AckermannDriveCommand + except ImportError: + AckermannDriveCommand = None + + target_paths = make_target_paths(config) + chassis_topic = args.chassis_telemetry_topic or config_topic(config, "chassis_telemetry_topic", "/chassis/telemetry") + external_topic = args.external_pose_topic or config_topic( + config, + "external_pose_topic", + "/workshop/external_localization/vehicle/pose", + ) + command_topic = args.ackermann_command_topic or config_topic(config, "ackermann_command_topic", "") + + rclpy.init() + node = rclpy.create_node("workshop_chassis_session_capture") + capture = ChassisSessionCapture(node, config, target_paths, args) + node.create_subscription(ChassisTelemetry, chassis_topic, capture.on_chassis_telemetry, 50) + node.create_subscription(ExternalLocalizationTelemetry, external_topic, capture.on_external_pose, 50) + if AckermannDriveCommand is not None and command_topic and args.command_source in ("auto", "topic"): + node.create_subscription(AckermannDriveCommand, command_topic, capture.on_ackermann_command, 50) + elif args.command_source == "topic": + print("[错误] 已要求从命令话题采集,但命令消息包或话题配置不可用。", file=sys.stderr) + capture.close() + node.destroy_node() + rclpy.shutdown() + return 1 + + print(f"[*] 底盘遥测采集: {chassis_topic}", file=sys.stderr) + print(f"[*] 外部真值采集: {external_topic}", file=sys.stderr) + if command_topic: + print(f"[*] 执行器命令采集: {command_topic}", file=sys.stderr) + + deadline = time.monotonic() + args.duration_sec if args.duration_sec > 0.0 else None + try: + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.1) + if deadline is not None and time.monotonic() >= deadline: + break + if args.max_chassis_samples > 0 and capture.chassis_csv.count >= args.max_chassis_samples: + break + except KeyboardInterrupt: + pass + finally: + capture.write_planned_command_if_needed() + capture.close() + node.destroy_node() + rclpy.shutdown() + + return finalize_capture(config, target_paths, config_path, args) + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py new file mode 100644 index 0000000..3452a23 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py @@ -0,0 +1,364 @@ +#!/usr/bin/env python3 +"""校验底盘标定现场动作配置,并导出总控任务片段。""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +SUPPORTED_CHASSIS_TYPES = { + "ackermann", + "differential", + "single_steer_wheel", + "multi_steer_wheel", +} + +PRIMITIVE_REQUIRED_KEYS = { + "straight_line": [ + "straight_line.target_distance_m", + "straight_line.target_speed_ms", + ], + "arc": [ + "arc.target_speed_ms", + "arc.radius_m", + "arc.sweep_angle_deg", + ], + "in_place_rotation": [ + "in_place_rotation.target_yaw_deg", + "in_place_rotation.target_angular_vel_deg_s", + ], + "steering_sweep": [ + "steering_sweep.target_angle_deg", + "steering_sweep.sweep_amplitude_deg", + "steering_sweep.sweep_frequency_hz", + "steering_sweep.duration_sec", + ], + "lateral_translation": [ + "lateral_translation.target_speed_ms", + "lateral_translation.target_distance_m", + ], + "diagonal_motion": [ + "diagonal_motion.target_speed_ms", + "diagonal_motion.target_distance_m", + "diagonal_motion.heading_deg", + ], + "module_alignment": [ + "module_alignment.module_ids", + "module_alignment.target_zero_deg", + "module_alignment.tolerance_deg", + ], + "coordinated_steering": [ + "coordinated_steering.module_ids", + "coordinated_steering.target_angle_deg", + "coordinated_steering.hold_time_sec", + ], +} + +COMMON_REQUIRED_KEYS = [ + "brake_when_finished", + "timeout_sec", +] + +POSITIVE_VALUE_KEYS = { + "straight_line.target_distance_m", + "straight_line.target_speed_ms", + "arc.target_speed_ms", + "arc.radius_m", + "in_place_rotation.target_angular_vel_deg_s", + "steering_sweep.sweep_amplitude_deg", + "steering_sweep.sweep_frequency_hz", + "steering_sweep.duration_sec", + "lateral_translation.target_speed_ms", + "lateral_translation.target_distance_m", + "diagonal_motion.target_speed_ms", + "diagonal_motion.target_distance_m", + "module_alignment.tolerance_deg", + "coordinated_steering.hold_time_sec", + "timeout_sec", +} + +NON_ZERO_VALUE_KEYS = { + "arc.sweep_angle_deg", + "in_place_rotation.target_yaw_deg", +} + +LINEAR_SPEED_KEYS = { + "straight_line.target_speed_ms", + "arc.target_speed_ms", + "lateral_translation.target_speed_ms", + "diagonal_motion.target_speed_ms", +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="校验并导出底盘标定现场动作 profile。") + parser.add_argument( + "--profile", + default="src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml", + help="底盘动作 profile YAML 路径。", + ) + parser.add_argument( + "--chassis-type", + choices=sorted(SUPPORTED_CHASSIS_TYPES), + help="只导出指定底盘类型;不填写时只做整表校验和摘要输出。", + ) + parser.add_argument( + "--format", + choices=["summary", "requested_tasks"], + default="summary", + help="输出格式:summary 为摘要,requested_tasks 为总控任务片段。", + ) + parser.add_argument( + "-o", + "--output", + help="输出文件路径;不填写时输出到标准输出。", + ) + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + if data is None: + return {} + if not isinstance(data, dict): + raise ValueError(f"{path} 的顶层结构必须是 YAML map。") + return data + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_list(value: Any, field_name: str) -> list[Any]: + if not isinstance(value, list): + raise ValueError(f"{field_name} 必须是 YAML list。") + return value + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def as_float(value: Any, field_name: str) -> float: + try: + return float(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是数字,当前值为 {value!r}。") from exc + + +def stringify_value(value: Any) -> str: + if isinstance(value, bool): + return "true" if value else "false" + if isinstance(value, list): + return ",".join(stringify_value(item) for item in value) + return str(value) + + +def read_max_linear_speed(profile: dict[str, Any]) -> float: + safety = profile.get("safety", {}) + if safety is None: + return 0.0 + safety_map = require_map(safety, "safety") + raw_value = safety_map.get("max_linear_speed_ms", 0.0) + if raw_value in (None, ""): + return 0.0 + value = as_float(raw_value, "safety.max_linear_speed_ms") + if value < 0.0: + raise ValueError("safety.max_linear_speed_ms 不能小于 0。") + return value + + +def validate_metadata_value( + key: str, + value: Any, + action_name: str, + max_linear_speed_ms: float, +) -> None: + if key in POSITIVE_VALUE_KEYS and as_float(value, f"{action_name}.{key}") <= 0.0: + raise ValueError(f"{action_name}.{key} 必须大于 0。") + if key in NON_ZERO_VALUE_KEYS and as_float(value, f"{action_name}.{key}") == 0.0: + raise ValueError(f"{action_name}.{key} 不能为 0。") + if key in LINEAR_SPEED_KEYS and max_linear_speed_ms > 0.0: + speed = as_float(value, f"{action_name}.{key}") + if speed > max_linear_speed_ms: + raise ValueError( + f"{action_name}.{key}={speed} 超过安全上限 {max_linear_speed_ms}。" + ) + if key.endswith("module_ids"): + if isinstance(value, list): + if not value: + raise ValueError(f"{action_name}.{key} 不能为空。") + for item in value: + require_string(item, f"{action_name}.{key}[]") + return + require_string(value, f"{action_name}.{key}") + + +def validate_action( + action: dict[str, Any], + action_index: int, + profile_name: str, + max_linear_speed_ms: float, +) -> None: + action_name = f"{profile_name}.actions[{action_index}]" + task_code = require_string(action.get("task_code"), f"{action_name}.task_code") + primitive_type = require_string(action.get("primitive_type"), f"{action_name}.primitive_type") + if primitive_type not in PRIMITIVE_REQUIRED_KEYS: + raise ValueError(f"{action_name}.primitive_type 不受支持:{primitive_type}") + + metadata = require_map(action.get("metadata"), f"{action_name}.metadata") + missing_keys = [ + key + for key in ["primitive_type", *PRIMITIVE_REQUIRED_KEYS[primitive_type], *COMMON_REQUIRED_KEYS] + if key not in metadata and key != "primitive_type" + ] + if missing_keys: + raise ValueError(f"{task_code} 缺少 metadata 字段:{', '.join(missing_keys)}") + + for key, value in metadata.items(): + validate_metadata_value(key, value, task_code, max_linear_speed_ms) + + +def validate_profile(profile: dict[str, Any]) -> dict[str, Any]: + schema_version = int(profile.get("schema_version", 0)) + if schema_version != 1: + raise ValueError("schema_version 必须为 1。") + + read_max_linear_speed(profile) + data_capture = require_map(profile.get("data_capture"), "data_capture") + required_inputs = require_list( + data_capture.get("required_data_inputs"), + "data_capture.required_data_inputs", + ) + for item in required_inputs: + require_string(item, "data_capture.required_data_inputs[]") + + chassis_profiles = require_map(profile.get("chassis_profiles"), "chassis_profiles") + missing_profiles = sorted(SUPPORTED_CHASSIS_TYPES - set(chassis_profiles)) + if missing_profiles: + raise ValueError(f"缺少底盘类型配置:{', '.join(missing_profiles)}") + + max_linear_speed_ms = read_max_linear_speed(profile) + for chassis_type, section_value in chassis_profiles.items(): + if chassis_type not in SUPPORTED_CHASSIS_TYPES: + raise ValueError(f"未知底盘类型:{chassis_type}") + section = require_map(section_value, f"chassis_profiles.{chassis_type}") + require_list(section.get("calibration_targets"), f"chassis_profiles.{chassis_type}.calibration_targets") + actions = require_list(section.get("actions"), f"chassis_profiles.{chassis_type}.actions") + if not actions: + raise ValueError(f"chassis_profiles.{chassis_type}.actions 不能为空。") + seen_task_codes: set[str] = set() + for index, action_value in enumerate(actions): + action = require_map(action_value, f"chassis_profiles.{chassis_type}.actions[{index}]") + task_code = require_string( + action.get("task_code"), + f"chassis_profiles.{chassis_type}.actions[{index}].task_code", + ) + if task_code in seen_task_codes: + raise ValueError(f"重复的 task_code:{task_code}") + seen_task_codes.add(task_code) + validate_action(action, index, f"chassis_profiles.{chassis_type}", max_linear_speed_ms) + return profile + + +def make_task_param(key: str, value: Any) -> dict[str, str]: + return { + "key": key, + "value": stringify_value(value), + } + + +def export_requested_tasks(profile: dict[str, Any], chassis_type: str) -> dict[str, Any]: + section = profile["chassis_profiles"][chassis_type] + tasks: list[dict[str, Any]] = [] + for action in section["actions"]: + metadata = dict(action["metadata"]) + metadata["primitive_type"] = action["primitive_type"] + task_params = [ + make_task_param(key, metadata[key]) + for key in sorted(metadata) + ] + tasks.append({ + "stage_type": "CHASSIS_CALIBRATION_STAGE", + "enabled": True, + "require_manual_approval": False, + "execution_policy": "REQUIRED", + "reason": "现场底盘标定动作 profile", + "task_code": action["task_code"], + "target_id": chassis_type, + "task_params": task_params, + }) + return { + "schema_version": 1, + "chassis_type": chassis_type, + "requested_tasks": tasks, + } + + +def build_summary(profile: dict[str, Any]) -> dict[str, Any]: + summary: dict[str, Any] = { + "schema_version": profile["schema_version"], + "profile_name": profile.get("profile_name", ""), + "chassis_profiles": {}, + } + for chassis_type, section in profile["chassis_profiles"].items(): + summary["chassis_profiles"][chassis_type] = { + "action_count": len(section["actions"]), + "actions": [ + { + "task_code": action["task_code"], + "primitive_type": action["primitive_type"], + "display_name": action.get("display_name", ""), + } + for action in section["actions"] + ], + } + return summary + + +def render_yaml(data: dict[str, Any]) -> str: + return yaml.safe_dump(data, sort_keys=False, allow_unicode=True) + + +def main() -> int: + args = parse_args() + profile_path = Path(args.profile).expanduser().resolve(strict=False) + profile = validate_profile(load_yaml(profile_path)) + + if args.format == "requested_tasks": + if not args.chassis_type: + raise ValueError("--format requested_tasks 必须指定 --chassis-type。") + output = export_requested_tasks(profile, args.chassis_type) + else: + output = build_summary(profile) + + rendered = render_yaml(output) + if args.output: + output_path = Path(args.output).expanduser().resolve(strict=False) + output_path.parent.mkdir(parents=True, exist_ok=True) + output_path.write_text(rendered, encoding="utf-8") + print(f"[OK] 已写入底盘动作 profile 输出:{output_path}", file=sys.stderr) + else: + print(rendered, end="") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_csv_contract.md b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_csv_contract.md new file mode 100644 index 0000000..4e1f9b6 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_csv_contract.md @@ -0,0 +1,75 @@ +# 底盘标定 CSV 字段协议 + +本协议是现场底盘标定数据落盘的固定格式。`capture_chassis_session.py` 和 `replay_chassis_smoke.py` 会在写入 `dataset_index.yaml` 前校验这些列。 + +## 通用约定 + +- 文件编码:UTF-8。 +- 分隔符:英文逗号。 +- 表头:必须包含本协议列名;允许增加额外列,但算法模板只承诺读取本协议列。 +- 时间基:`hardware_timestamp_us` 和 `command_timestamp_us` 均为 Unix 微秒时间戳,必须来自同一套车间时钟或已完成时间同步换算。 +- 坐标系:`truth_trajectory.csv` 中的位姿表示 `base_link` 在 `workshop` 坐标系下的位姿。 +- 角度单位:字段名带 `_rad` 的单位为弧度,字段名带 `_deg` 的单位为角度。 +- 布尔值:使用 `0/1`,也允许 `true/false`。 +- 空值:数值字段不允许为空;没有对应实测值时写 `0`,并在诊断文件中说明。 + +## chassis_motion.csv + +路径约定:`chassis/chassis_motion.csv` + +| 字段 | 类型 | 单位 | 说明 | +| --- | --- | --- | --- | +| `hardware_timestamp_us` | int64 | us | 底盘遥测硬件时间戳。 | +| `chassis_type` | string | 无 | `ackermann`、`differential`、`single_steer_wheel`、`multi_steer_wheel`。 | +| `odom_x_m` | float64 | m | 车端里程计 X。 | +| `odom_y_m` | float64 | m | 车端里程计 Y。 | +| `odom_yaw_rad` | float64 | rad | 车端里程计偏航角。 | +| `linear_velocity_ms` | float64 | m/s | 实际线速度。 | +| `angular_velocity_rads` | float64 | rad/s | 实际角速度。 | +| `estop_engaged` | bool | 无 | 急停是否触发。 | +| `driver_error_code` | uint32 | 无 | 底盘驱动错误码。 | +| `active_job_id` | string | 无 | 当前底盘动作任务 ID。 | +| `lateral_slip_estimate` | float64 | 无 | 估计横向滑移。 | +| `curvature_estimate` | float64 | 1/m | 估计曲率。 | +| `module_states_json` | JSON array | 无 | 轮/舵模块数组,每个元素包含 `module_id`、`encoder_ticks`、`wheel_speed_rpm`、`steer_angle_deg`、`motor_current_amp`。 | + +## actuator_commands.csv + +路径约定:`chassis/actuator_commands.csv` + +| 字段 | 类型 | 单位 | 说明 | +| --- | --- | --- | --- | +| `command_timestamp_us` | int64 | us | 命令下发时间戳。 | +| `command_id` | string | 无 | 命令 ID。 | +| `source` | string | 无 | 命令来源,例如 `vehicle_agent`、`planned_command`。 | +| `target_speed_ms` | float64 | m/s | 目标速度。 | +| `target_distance_m` | float64 | m | 目标距离;实时底层命令没有该值时写本次动作原语计划距离或 `0`。 | +| `target_accel_ms2` | float64 | m/s^2 | 目标加速度;没有该值时写 `0`。 | +| `target_steering_angle_rad` | float64 | rad | 阿克曼或舵轮目标转角;差速轮无转角时写 `0`。 | +| `target_steering_rate_rads` | float64 | rad/s | 目标转角速度;没有该值时写 `0`。 | +| `brake_command` | float64 | 无 | 制动命令,范围建议为 `[0, 1]`。 | +| `throttle_command` | float64 | 无 | 驱动命令,范围建议为 `[0, 1]`。 | +| `command_timeout_sec` | float64 | s | 命令超时时间;没有该值时写 `0`。 | + +## truth_trajectory.csv + +路径约定:`external/truth_trajectory.csv` + +| 字段 | 类型 | 单位 | 说明 | +| --- | --- | --- | --- | +| `hardware_timestamp_us` | int64 | us | 外部真值观测时间戳。 | +| `pose_valid` | bool | 无 | 位姿是否有效。 | +| `x_m` | float64 | m | `base_link` 在 `workshop` 下的 X。 | +| `y_m` | float64 | m | `base_link` 在 `workshop` 下的 Y。 | +| `z_m` | float64 | m | `base_link` 在 `workshop` 下的 Z。 | +| `roll_rad` | float64 | rad | 横滚角。 | +| `pitch_rad` | float64 | rad | 俯仰角。 | +| `yaw_rad` | float64 | rad | 偏航角。 | +| `position_stddev_m` | float64 | m | 位置标准差。 | +| `yaw_stddev_rad` | float64 | rad | 航向标准差。 | +| `tracking_loss_ratio` | float64 | 无 | 跟踪丢失比例。 | +| `time_sync_offset_ms` | float64 | ms | 外部真值与车端时钟偏差。 | +| `quality_score` | float64 | 无 | 外部真值质量分。 | +| `observed_target_count` | uint32 | 个 | 当前观测到的标定目标数量。 | +| `reference_source_name` | string | 无 | 外部真值来源名称。 | +| `active_job_id` | string | 无 | 当前底盘动作任务 ID。 | diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_data_common.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_data_common.py new file mode 100644 index 0000000..4d07205 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_data_common.py @@ -0,0 +1,556 @@ +#!/usr/bin/env python3 +"""底盘标定现场数据配置、检查和数据集索引工具。""" + +from __future__ import annotations + +import csv +import json +import math +import shutil +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import yaml + + +CHASSIS_TYPES = {"ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"} +CHASSIS_CSV_CONTRACT_VERSION = 1 +CHASSIS_TYPE_VALUE_TO_NAME = { + 1: "ackermann", + 2: "differential", + 3: "single_steer_wheel", + 4: "multi_steer_wheel", +} +PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx") + +CHASSIS_MOTION_FIELDS = [ + "hardware_timestamp_us", + "chassis_type", + "odom_x_m", + "odom_y_m", + "odom_yaw_rad", + "linear_velocity_ms", + "angular_velocity_rads", + "estop_engaged", + "driver_error_code", + "active_job_id", + "lateral_slip_estimate", + "curvature_estimate", + "module_states_json", +] + +ACTUATOR_COMMAND_FIELDS = [ + "command_timestamp_us", + "command_id", + "source", + "target_speed_ms", + "target_distance_m", + "target_accel_ms2", + "target_steering_angle_rad", + "target_steering_rate_rads", + "brake_command", + "throttle_command", + "command_timeout_sec", +] + +TRUTH_TRAJECTORY_FIELDS = [ + "hardware_timestamp_us", + "pose_valid", + "x_m", + "y_m", + "z_m", + "roll_rad", + "pitch_rad", + "yaw_rad", + "position_stddev_m", + "yaw_stddev_rad", + "tracking_loss_ratio", + "time_sync_offset_ms", + "quality_score", + "observed_target_count", + "reference_source_name", + "active_job_id", +] + +INTEGER_FIELDS_BY_TABLE = { + "chassis_motion": { + "hardware_timestamp_us", + "driver_error_code", + }, + "actuator_command": { + "command_timestamp_us", + }, + "truth_trajectory": { + "hardware_timestamp_us", + "observed_target_count", + }, +} + +FLOAT_FIELDS_BY_TABLE = { + "chassis_motion": { + "odom_x_m", + "odom_y_m", + "odom_yaw_rad", + "linear_velocity_ms", + "angular_velocity_rads", + "lateral_slip_estimate", + "curvature_estimate", + }, + "actuator_command": { + "target_speed_ms", + "target_distance_m", + "target_accel_ms2", + "target_steering_angle_rad", + "target_steering_rate_rads", + "brake_command", + "throttle_command", + "command_timeout_sec", + }, + "truth_trajectory": { + "x_m", + "y_m", + "z_m", + "roll_rad", + "pitch_rad", + "yaw_rad", + "position_stddev_m", + "yaw_stddev_rad", + "tracking_loss_ratio", + "time_sync_offset_ms", + "quality_score", + }, +} + +BOOLEAN_FIELDS_BY_TABLE = { + "chassis_motion": {"estop_engaged"}, + "truth_trajectory": {"pose_valid"}, +} + +CSV_CONTRACTS = { + "chassis_motion": { + "fields": CHASSIS_MOTION_FIELDS, + "json_fields": {"module_states_json"}, + }, + "actuator_command": { + "fields": ACTUATOR_COMMAND_FIELDS, + "json_fields": set(), + }, + "truth_trajectory": { + "fields": TRUTH_TRAJECTORY_FIELDS, + "json_fields": set(), + }, +} + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def load_config(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) or {} + if not isinstance(data, dict): + raise ValueError(f"配置文件必须是 YAML 字典: {path}") + return data + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def has_placeholder(value: Any) -> bool: + return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS) + + +def resolve_path(root: Path, raw_value: str) -> Path: + path = Path(raw_value).expanduser() + if path.is_absolute(): + return path + return root / path + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +@dataclass +class ValidationResult: + errors: list[str] = field(default_factory=list) + warnings: list[str] = field(default_factory=list) + + @property + def ok(self) -> bool: + return not self.errors + + def extend(self, other: "ValidationResult") -> None: + self.errors.extend(other.errors) + self.warnings.extend(other.warnings) + + +def normalize_chassis_type_name(value: Any) -> str: + if value in (None, ""): + return "" + if isinstance(value, int): + return CHASSIS_TYPE_VALUE_TO_NAME.get(value, "") + text = str(value).strip() + if not text: + return "" + if text.isdigit(): + return CHASSIS_TYPE_VALUE_TO_NAME.get(int(text), "") + return text + + +def is_bool_token(value: Any) -> bool: + return str(value).strip().lower() in {"0", "1", "true", "false"} + + +def validate_csv_contract(path: Path, table_name: str) -> ValidationResult: + result = ValidationResult() + contract = CSV_CONTRACTS.get(table_name) + if contract is None: + result.errors.append(f"未知 CSV 协议名称: {table_name}") + return result + if not path.exists(): + result.errors.append(f"{table_name} 文件不存在: {path}") + return result + + required_fields = list(contract["fields"]) + integer_fields = INTEGER_FIELDS_BY_TABLE.get(table_name, set()) + float_fields = FLOAT_FIELDS_BY_TABLE.get(table_name, set()) + boolean_fields = BOOLEAN_FIELDS_BY_TABLE.get(table_name, set()) + json_fields = contract.get("json_fields", set()) + + with path.open("r", newline="", encoding="utf-8") as stream: + reader = csv.DictReader(stream) + fieldnames = reader.fieldnames or [] + for field_name in required_fields: + if field_name not in fieldnames: + result.errors.append(f"{table_name} 缺少必需列: {field_name}") + if result.errors: + return result + + row_count = 0 + for row_number, row in enumerate(reader, start=2): + row_count += 1 + for field_name in integer_fields: + try: + int(str(row.get(field_name, "")).strip()) + except (TypeError, ValueError): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是整数。") + for field_name in float_fields: + try: + float(str(row.get(field_name, "")).strip()) + except (TypeError, ValueError): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是浮点数。") + for field_name in boolean_fields: + if not is_bool_token(row.get(field_name, "")): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是 0/1 或 true/false。") + for field_name in json_fields: + try: + parsed = json.loads(str(row.get(field_name, "")).strip()) + except json.JSONDecodeError: + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是合法 JSON。") + continue + if field_name == "module_states_json" and not isinstance(parsed, list): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是 JSON 数组。") + if table_name == "chassis_motion": + if normalize_chassis_type_name(row.get("chassis_type")) not in CHASSIS_TYPES: + result.errors.append(f"{table_name} 第 {row_number} 行 chassis_type 必须是已支持的底盘类型。") + if row_count == 0: + result.warnings.append(f"{table_name} 只有表头,没有数据行。") + return result + + +def validate_chassis_dataset_files(paths: dict[str, Path]) -> ValidationResult: + result = ValidationResult() + checks = [ + ("chassis_motion", "chassis_motion_data_file"), + ("actuator_command", "actuator_command_file"), + ("truth_trajectory", "truth_trajectory_file"), + ] + for table_name, path_key in checks: + result.extend(validate_csv_contract(paths[path_key], table_name)) + return result + + +def validate_config(config: dict[str, Any], config_path: Path | None = None) -> ValidationResult: + result = ValidationResult() + + csv_contract_version = int(config.get("csv_contract_version", CHASSIS_CSV_CONTRACT_VERSION) or 0) + if csv_contract_version != CHASSIS_CSV_CONTRACT_VERSION: + result.errors.append( + f"csv_contract_version 必须为 {CHASSIS_CSV_CONTRACT_VERSION}。" + ) + + chassis_type = str(config.get("chassis_type", "")).strip() + if chassis_type not in CHASSIS_TYPES: + result.errors.append(f"chassis_type 必须是 {sorted(CHASSIS_TYPES)} 之一。") + + for field_name in ("session_id", "site_id", "vehicle_id", "session_dir", "dataset_index_path"): + value = str(config.get(field_name, "")).strip() + if not value: + result.errors.append(f"{field_name} 不能为空。") + elif has_placeholder(value): + result.errors.append(f"{field_name} 不能保留模板占位符。") + + files = config.get("files", {}) + if not isinstance(files, dict): + result.errors.append("files 必须是 YAML 字典。") + files = {} + for field_name in ( + "chassis_motion_data_file", + "actuator_command_file", + "truth_trajectory_file", + "diagnostics_file", + ): + value = str(files.get(field_name, "")).strip() + if not value: + result.errors.append(f"files.{field_name} 不能为空。") + elif has_placeholder(value): + result.errors.append(f"files.{field_name} 不能保留模板占位符。") + + validation = config.get("validation", {}) or {} + if not isinstance(validation, dict): + result.errors.append("validation 必须是 YAML 字典。") + validation = {} + for field_name in ( + "min_chassis_motion_samples", + "min_actuator_command_samples", + "min_truth_trajectory_samples", + ): + if int(validation.get(field_name, 0) or 0) <= 0: + result.errors.append(f"validation.{field_name} 必须大于 0。") + if float(validation.get("min_motion_distance_m", 0.0) or 0.0) < 0.0: + result.errors.append("validation.min_motion_distance_m 不能小于 0。") + if float(validation.get("max_time_gap_ms", 0.0) or 0.0) <= 0.0: + result.errors.append("validation.max_time_gap_ms 必须大于 0。") + + if config_path is not None and not config_path.exists(): + result.errors.append(f"配置文件不存在: {config_path}") + return result + + +def apply_cli_overrides(config: dict[str, Any], args: Any) -> None: + for attr_name in ("session_id", "site_id", "vehicle_id", "session_dir", "dataset_index_path"): + value = getattr(args, attr_name, "") + if value: + config[attr_name] = value + if getattr(args, "chassis_type", ""): + config["chassis_type"] = args.chassis_type + + +def read_csv_rows(path: Path) -> list[dict[str, str]]: + with path.open("r", newline="", encoding="utf-8") as stream: + return list(csv.DictReader(stream)) + + +def timestamp_us(row: dict[str, Any]) -> int: + for key in ("hardware_timestamp_us", "timestamp_us", "command_timestamp_us"): + value = row.get(key) + if value not in (None, ""): + return int(float(value)) + return 0 + + +def numeric(row: dict[str, Any], key: str, default: float = 0.0) -> float: + value = row.get(key) + if value in (None, ""): + return default + return float(value) + + +def trajectory_distance(rows: list[dict[str, str]], x_key: str, y_key: str) -> float: + distance = 0.0 + previous: tuple[float, float] | None = None + for row in rows: + current = (numeric(row, x_key), numeric(row, y_key)) + if previous is not None: + distance += math.hypot(current[0] - previous[0], current[1] - previous[1]) + previous = current + return distance + + +def max_timestamp_gap_ms(rows: list[dict[str, str]]) -> float: + stamps = sorted(timestamp_us(row) for row in rows if timestamp_us(row) > 0) + if len(stamps) < 2: + return 0.0 + return max((b - a) / 1000.0 for a, b in zip(stamps, stamps[1:])) + + +def write_csv(path: Path, fieldnames: list[str], rows: list[dict[str, Any]]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", newline="", encoding="utf-8") as stream: + writer = csv.DictWriter(stream, fieldnames=fieldnames) + writer.writeheader() + for row in rows: + writer.writerow(row) + + +def copy_or_generate_file(source: str, target: Path, fieldnames: list[str], rows: list[dict[str, Any]]) -> Path: + target.parent.mkdir(parents=True, exist_ok=True) + if source: + shutil.copyfile(Path(source).expanduser().resolve(strict=False), target) + else: + write_csv(target, fieldnames, rows) + return target + + +def synthetic_rows(config: dict[str, Any]) -> tuple[list[dict[str, Any]], list[dict[str, Any]], list[dict[str, Any]]]: + synthetic = config.get("synthetic", {}) or {} + start_us = now_us() + period_us = int(float(synthetic.get("sample_period_ms", 100.0) or 100.0) * 1000.0) + speed = float(synthetic.get("target_speed_ms", 0.1) or 0.1) + distance = float(synthetic.get("target_distance_m", 0.2) or 0.2) + sample_count = max(3, int(distance / max(speed, 1e-6) / max(period_us / 1e6, 1e-6)) + 1) + + chassis_rows: list[dict[str, Any]] = [] + truth_rows: list[dict[str, Any]] = [] + for index in range(sample_count): + stamp = start_us + index * period_us + x_m = min(distance, index * speed * period_us / 1e6) + chassis_rows.append({ + "hardware_timestamp_us": stamp, + "chassis_type": config.get("chassis_type", ""), + "odom_x_m": x_m, + "odom_y_m": 0.0, + "odom_yaw_rad": 0.0, + "linear_velocity_ms": speed, + "angular_velocity_rads": 0.0, + "estop_engaged": 0, + "driver_error_code": 0, + "active_job_id": config.get("session_id", ""), + "lateral_slip_estimate": 0.0, + "curvature_estimate": 0.0, + "module_states_json": "[]", + }) + truth_rows.append({ + "hardware_timestamp_us": stamp, + "pose_valid": 1, + "x_m": x_m, + "y_m": 0.0, + "z_m": 0.0, + "roll_rad": 0.0, + "pitch_rad": 0.0, + "yaw_rad": 0.0, + "position_stddev_m": 0.0, + "yaw_stddev_rad": 0.0, + "tracking_loss_ratio": 0.0, + "time_sync_offset_ms": 0.0, + "quality_score": 1.0, + "observed_target_count": 0, + "reference_source_name": "synthetic_chassis_smoke", + "active_job_id": config.get("session_id", ""), + }) + + command_rows = [ + { + "command_timestamp_us": start_us, + "command_id": f"{config.get('session_id', 'session')}_straight_line", + "source": "synthetic_chassis_smoke", + "target_speed_ms": speed, + "target_distance_m": distance, + "target_accel_ms2": 0.0, + "target_steering_angle_rad": 0.0, + "target_steering_rate_rads": 0.0, + "brake_command": 0.0, + "throttle_command": 0.0, + "command_timeout_sec": 0.0, + } + ] + return chassis_rows, command_rows, truth_rows + + +def build_summary( + chassis_rows: list[dict[str, str]], + command_rows: list[dict[str, str]], + truth_rows: list[dict[str, str]], + config: dict[str, Any], +) -> dict[str, Any]: + chassis_distance = trajectory_distance(chassis_rows, "odom_x_m", "odom_y_m") + truth_distance = trajectory_distance(truth_rows, "x_m", "y_m") + stamps = [timestamp_us(row) for row in chassis_rows + truth_rows + command_rows if timestamp_us(row) > 0] + start_us = min(stamps) if stamps else now_us() + end_us = max(stamps) if stamps else start_us + 1 + return { + "chassis_csv_contract_version": CHASSIS_CSV_CONTRACT_VERSION, + "session_id": config.get("session_id", ""), + "site_id": config.get("site_id", ""), + "vehicle_id": config.get("vehicle_id", ""), + "chassis_type": config.get("chassis_type", ""), + "data_window_start_timestamp_us": int(start_us), + "data_window_end_timestamp_us": int(max(end_us, start_us + 1)), + "chassis_motion_sample_count": len(chassis_rows), + "actuator_command_sample_count": len(command_rows), + "truth_trajectory_sample_count": len(truth_rows), + "chassis_odom_distance_m": chassis_distance, + "truth_distance_m": truth_distance, + "distance_scale_hint": (truth_distance / chassis_distance) if chassis_distance > 1e-9 else 0.0, + "max_chassis_timestamp_gap_ms": max_timestamp_gap_ms(chassis_rows), + "max_truth_timestamp_gap_ms": max_timestamp_gap_ms(truth_rows), + } + + +def validate_summary(summary: dict[str, Any], config: dict[str, Any]) -> ValidationResult: + result = ValidationResult() + validation = config.get("validation", {}) or {} + checks = [ + ("chassis_motion_sample_count", "min_chassis_motion_samples", "底盘运动样本数不足"), + ("actuator_command_sample_count", "min_actuator_command_samples", "执行器命令样本数不足"), + ("truth_trajectory_sample_count", "min_truth_trajectory_samples", "真值轨迹样本数不足"), + ] + for summary_key, config_key, message in checks: + if int(summary.get(summary_key, 0)) < int(validation.get(config_key, 0) or 0): + result.errors.append(f"{message}: {summary.get(summary_key, 0)}") + if float(summary.get("chassis_odom_distance_m", 0.0)) < float(validation.get("min_motion_distance_m", 0.0) or 0.0): + result.errors.append("底盘运动距离不足。") + max_gap = float(validation.get("max_time_gap_ms", 200.0) or 200.0) + if float(summary.get("max_chassis_timestamp_gap_ms", 0.0)) > max_gap: + result.warnings.append("底盘遥测时间间隔超过配置阈值。") + if float(summary.get("max_truth_timestamp_gap_ms", 0.0)) > max_gap: + result.warnings.append("真值轨迹时间间隔超过配置阈值。") + return result + + +def build_dataset_index( + config: dict[str, Any], + summary: dict[str, Any], + paths: dict[str, Path], + config_path: Path, +) -> dict[str, Any]: + session_dir = Path(str(config["session_dir"])).expanduser().resolve(strict=False) + return { + "schema_version": 1, + "session": { + "session_id": str(config["session_id"]), + "site_id": str(config["site_id"]), + "vehicle_id": str(config["vehicle_id"]), + "dataset_root": str(session_dir), + "data_window_start_timestamp_us": int(summary["data_window_start_timestamp_us"]), + "data_window_end_timestamp_us": int(summary["data_window_end_timestamp_us"]), + }, + "data_inputs": { + "chassis": { + "chassis_motion_data_files": [ + relative_to_root(paths["chassis_motion_data_file"], session_dir) + ], + "actuator_command_files": [ + relative_to_root(paths["actuator_command_file"], session_dir) + ], + "truth_trajectory_files": [ + relative_to_root(paths["truth_trajectory_file"], session_dir) + ], + } + }, + "metadata": { + "chassis_csv_contract_version": CHASSIS_CSV_CONTRACT_VERSION, + "chassis_data_config_file": str(config_path), + "chassis_diagnostics_file": relative_to_root(paths["diagnostics_file"], session_dir), + "chassis_replay_summary": summary, + }, + } diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml new file mode 100644 index 0000000..9631ccf --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml @@ -0,0 +1,270 @@ +# 底盘标定现场动作配置模板。 +# 本文件固定四类底盘在真实标定车间中建议执行的动作序列。 +# 真实车速、距离、角速度和模块 ID 必须按现场安全评估后再替换。 + +schema_version: 1 +profile_name: workshop_chassis_calibration_action_profile + +safety: + max_linear_speed_ms: 0.1 + default_brake_when_finished: true + default_timeout_sec: 60.0 + +data_capture: + required_data_inputs: + - chassis_motion_data_files + - actuator_command_files + - truth_trajectory_files + start_before_motion_sec: 1.0 + stop_after_motion_sec: 1.0 + min_truth_hz: 20.0 + min_chassis_telemetry_hz: 20.0 + +chassis_profiles: + ackermann: + description: 阿克曼底盘标定动作序列 + calibration_targets: + - ackermann.front_left_steer_zero_offset_deg + - ackermann.front_right_steer_zero_offset_deg + - ackermann.rear_left_wheel_radius_m + - ackermann.rear_right_wheel_radius_m + - ackermann.steering_ratio + actions: + - task_code: chassis.ackermann.straight_forward + display_name: 阿克曼直线前进 + primitive_type: straight_line + metadata: + straight_line.target_distance_m: 1.0 + straight_line.target_speed_ms: 0.1 + straight_line.reverse: false + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.ackermann.straight_reverse + display_name: 阿克曼直线倒车 + primitive_type: straight_line + metadata: + straight_line.target_distance_m: 0.6 + straight_line.target_speed_ms: 0.08 + straight_line.reverse: true + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.ackermann.arc_left + display_name: 阿克曼左圆弧 + primitive_type: arc + metadata: + arc.target_speed_ms: 0.08 + arc.radius_m: 1.0 + arc.sweep_angle_deg: 45.0 + arc.clockwise: false + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.ackermann.arc_right + display_name: 阿克曼右圆弧 + primitive_type: arc + metadata: + arc.target_speed_ms: 0.08 + arc.radius_m: 1.0 + arc.sweep_angle_deg: 45.0 + arc.clockwise: true + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.ackermann.steering_sweep + display_name: 阿克曼舵角扫动 + primitive_type: steering_sweep + metadata: + steering_sweep.target_angle_deg: 0.0 + steering_sweep.sweep_amplitude_deg: 8.0 + steering_sweep.sweep_frequency_hz: 0.2 + steering_sweep.duration_sec: 12.0 + brake_when_finished: true + timeout_sec: 30.0 + + differential: + description: 差速轮底盘标定动作序列 + calibration_targets: + - differential.left_wheel_radius_m + - differential.right_wheel_radius_m + - differential.axle_track_width_m + actions: + - task_code: chassis.differential.straight_forward + display_name: 差速直线前进 + primitive_type: straight_line + metadata: + straight_line.target_distance_m: 1.0 + straight_line.target_speed_ms: 0.1 + straight_line.reverse: false + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.differential.straight_reverse + display_name: 差速直线倒车 + primitive_type: straight_line + metadata: + straight_line.target_distance_m: 0.6 + straight_line.target_speed_ms: 0.08 + straight_line.reverse: true + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.differential.rotate_left + display_name: 差速左原地旋转 + primitive_type: in_place_rotation + metadata: + in_place_rotation.target_yaw_deg: 90.0 + in_place_rotation.target_angular_vel_deg_s: 10.0 + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.differential.rotate_right + display_name: 差速右原地旋转 + primitive_type: in_place_rotation + metadata: + in_place_rotation.target_yaw_deg: -90.0 + in_place_rotation.target_angular_vel_deg_s: 10.0 + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.differential.arc_left + display_name: 差速左转圆弧 + primitive_type: arc + metadata: + arc.target_speed_ms: 0.08 + arc.radius_m: 1.0 + arc.sweep_angle_deg: 45.0 + arc.clockwise: false + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.differential.arc_right + display_name: 差速右转圆弧 + primitive_type: arc + metadata: + arc.target_speed_ms: 0.08 + arc.radius_m: 1.0 + arc.sweep_angle_deg: 45.0 + arc.clockwise: true + brake_when_finished: true + timeout_sec: 45.0 + + single_steer_wheel: + description: 单舵轮底盘标定动作序列 + calibration_targets: + - single_steer.drive_wheel_radius_m + - single_steer.steer_zero_offset_deg + - single_steer.steering_ratio + - single_steer.drive_encoder_scale + actions: + - task_code: chassis.single_steer.straight_forward + display_name: 单舵轮直线前进 + primitive_type: straight_line + metadata: + straight_line.target_distance_m: 1.0 + straight_line.target_speed_ms: 0.1 + straight_line.reverse: false + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.single_steer.steering_sweep + display_name: 单舵轮舵角扫动 + primitive_type: steering_sweep + metadata: + steering_sweep.target_angle_deg: 0.0 + steering_sweep.sweep_amplitude_deg: 10.0 + steering_sweep.sweep_frequency_hz: 0.2 + steering_sweep.duration_sec: 12.0 + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.single_steer.arc_left + display_name: 单舵轮左圆弧 + primitive_type: arc + metadata: + arc.target_speed_ms: 0.08 + arc.radius_m: 1.0 + arc.sweep_angle_deg: 45.0 + arc.clockwise: false + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.single_steer.arc_right + display_name: 单舵轮右圆弧 + primitive_type: arc + metadata: + arc.target_speed_ms: 0.08 + arc.radius_m: 1.0 + arc.sweep_angle_deg: 45.0 + arc.clockwise: true + brake_when_finished: true + timeout_sec: 45.0 + + multi_steer_wheel: + description: 多舵轮底盘标定动作序列 + calibration_targets: + - multi_steer.modules[].wheel_radius_m + - multi_steer.modules[].steer_zero_offset_deg + - multi_steer.modules[].module_pos_x_m + - multi_steer.modules[].module_pos_y_m + actions: + - task_code: chassis.multi_steer.straight_forward + display_name: 多舵轮直线前进 + primitive_type: straight_line + metadata: + straight_line.target_distance_m: 1.0 + straight_line.target_speed_ms: 0.1 + straight_line.reverse: false + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.multi_steer.lateral_left + display_name: 多舵轮左横移 + primitive_type: lateral_translation + metadata: + lateral_translation.target_speed_ms: 0.06 + lateral_translation.target_distance_m: 0.5 + lateral_translation.move_left: true + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.multi_steer.lateral_right + display_name: 多舵轮右横移 + primitive_type: lateral_translation + metadata: + lateral_translation.target_speed_ms: 0.06 + lateral_translation.target_distance_m: 0.5 + lateral_translation.move_left: false + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.multi_steer.diagonal_forward_left + display_name: 多舵轮左前斜移 + primitive_type: diagonal_motion + metadata: + diagonal_motion.target_speed_ms: 0.06 + diagonal_motion.target_distance_m: 0.5 + diagonal_motion.heading_deg: 45.0 + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.multi_steer.diagonal_forward_right + display_name: 多舵轮右前斜移 + primitive_type: diagonal_motion + metadata: + diagonal_motion.target_speed_ms: 0.06 + diagonal_motion.target_distance_m: 0.5 + diagonal_motion.heading_deg: -45.0 + brake_when_finished: true + timeout_sec: 45.0 + - task_code: chassis.multi_steer.module_alignment + display_name: 多舵轮模块零位检查 + primitive_type: module_alignment + metadata: + module_alignment.module_ids: + - front_left + - front_right + - rear_left + - rear_right + module_alignment.target_zero_deg: 0.0 + module_alignment.tolerance_deg: 1.0 + brake_when_finished: true + timeout_sec: 30.0 + - task_code: chassis.multi_steer.coordinated_steering + display_name: 多舵轮协同转向 + primitive_type: coordinated_steering + metadata: + coordinated_steering.module_ids: + - front_left + - front_right + - rear_left + - rear_right + coordinated_steering.target_angle_deg: 20.0 + coordinated_steering.hold_time_sec: 5.0 + brake_when_finished: true + timeout_sec: 30.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_template.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_template.yaml new file mode 100644 index 0000000..ba20052 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_template.yaml @@ -0,0 +1,64 @@ +# 底盘标定现场数据配置模板。 +# 该文件只定义底盘标定数据输入和落盘边界,真实底盘控制由车端 agent 执行。 + +csv_contract_version: 1 +chassis_type: ackermann +action_profile_file: src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml +session_id: replace_with_session_id +site_id: replace_with_site_or_line_id +vehicle_id: replace_with_real_vehicle_id +session_dir: /data/agv_calib/replace_with_site_or_line_id/session_xxx +dataset_index_path: /data/agv_calib/replace_with_site_or_line_id/session_xxx/dataset_index.yaml + +# 这些文件会写入 dataset_index.yaml 的 data_inputs.chassis。 +files: + chassis_motion_data_file: chassis/chassis_motion.csv + actuator_command_file: chassis/actuator_commands.csv + truth_trajectory_file: external/truth_trajectory.csv + diagnostics_file: chassis/chassis_diagnostics.json + +# 现场采集话题配置。 +capture: + chassis_telemetry_topic: /chassis/telemetry + external_pose_topic: /workshop/external_localization/vehicle/pose + ackermann_command_topic: /vehicle/replace_with_real_vehicle_id/internal/ackermann_cmd + command_source: auto + +# 当前底盘标定保留阿克曼、差速轮、单舵轮、多舵轮四类入口。 +# 已确认差速轮底盘标定目标:左轮半径、右轮半径、驱动轮轮距。 +calibration_targets: + ackermann: + - ackermann.front_left_steer_zero_offset_deg + - ackermann.front_right_steer_zero_offset_deg + - ackermann.rear_left_wheel_radius_m + - ackermann.rear_right_wheel_radius_m + - ackermann.steering_ratio + differential: + - differential.left_wheel_radius_m + - differential.right_wheel_radius_m + - differential.axle_track_width_m + single_steer_wheel: + - single_steer.drive_wheel_radius_m + - single_steer.steer_zero_offset_deg + - single_steer.steering_ratio + - single_steer.drive_encoder_scale + multi_steer_wheel: + - multi_steer.modules[].wheel_radius_m + - multi_steer.modules[].steer_zero_offset_deg + - multi_steer.modules[].module_pos_x_m + - multi_steer.modules[].module_pos_y_m + +# 离线自检和现场采集收尾时使用的最小检查阈值。 +validation: + min_chassis_motion_samples: 2 + min_actuator_command_samples: 1 + min_truth_trajectory_samples: 2 + min_motion_distance_m: 0.01 + max_time_gap_ms: 200.0 + +# 没有传入真实 CSV 时,replay_chassis_smoke.py 会生成一段最小合成数据,只用于链路自检。 +synthetic: + enabled: true + sample_period_ms: 100 + target_speed_ms: 0.1 + target_distance_m: 0.2 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_estimated_params_template.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_estimated_params_template.yaml new file mode 100644 index 0000000..d6b5e02 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_estimated_params_template.yaml @@ -0,0 +1,32 @@ +# 底盘算法输出参数模板。 +# 真实算法填充完成后,应输出同类 YAML,再交给 stage_chassis_parameter_commit.py 生成待提交参数包。 + +schema_version: 1 +session_id: replace_with_session_id +vehicle_id: replace_with_real_vehicle_id +chassis_type: differential +parameter_version: replace_with_algorithm_parameter_version + +quality: + data_quality_passed: true + suitable_for_commit: true + auto_acceptance_passed: true + +validation_summary: + max_lateral_error_m: 0.0 + max_yaw_error_rad: 0.0 + rms_lateral_error_m: 0.0 + rms_yaw_error_rad: 0.0 + repeatability_error_m: 0.0 + curvature_error: 0.0 + module_consistency_error: 0.0 + +estimated_params: + common: + longitudinal_scale: 1.0 + yaw_scale: 1.0 + straight_line_bias: 0.0 + differential: + left_wheel_radius_m: replace_with_algorithm_output + right_wheel_radius_m: replace_with_algorithm_output + axle_track_width_m: replace_with_algorithm_output diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/replay_chassis_smoke.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/replay_chassis_smoke.py new file mode 100644 index 0000000..3dc1b7a --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/replay_chassis_smoke.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +"""底盘标定现场数据离线回放和数据输入自检入口。""" + +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path + +from chassis_data_common import ( + ACTUATOR_COMMAND_FIELDS, + CHASSIS_MOTION_FIELDS, + TRUTH_TRAJECTORY_FIELDS, + apply_cli_overrides, + build_dataset_index, + build_summary, + copy_or_generate_file, + load_config, + read_csv_rows, + resolve_path, + synthetic_rows, + validate_chassis_dataset_files, + validate_config, + validate_summary, + write_yaml, +) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="验证底盘标定现场数据文件、dataset_index 写入和 data_input 转换链路。" + ) + parser.add_argument("--config", required=True, help="底盘标定现场数据配置文件。") + parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type。") + parser.add_argument("--session-id", default="", help="覆盖 session_id。") + parser.add_argument("--site-id", default="", help="覆盖 site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 vehicle_id。") + parser.add_argument("--session-dir", default="", help="覆盖 session_dir。") + parser.add_argument("--dataset-index-path", default="", help="覆盖 dataset_index_path。") + parser.add_argument("--chassis-motion-csv", default="", help="真实底盘遥测 CSV。") + parser.add_argument("--actuator-command-csv", default="", help="真实执行器命令 CSV。") + parser.add_argument("--truth-trajectory-csv", default="", help="真实外部真值轨迹 CSV。") + parser.add_argument( + "--no-synthetic", + action="store_true", + help="缺少任一输入 CSV 时直接报错,不生成合成数据。", + ) + return parser.parse_args() + + +def require_inputs(args: argparse.Namespace) -> None: + if args.no_synthetic and ( + not args.chassis_motion_csv or not args.actuator_command_csv or not args.truth_trajectory_csv + ): + raise ValueError("--no-synthetic 要求同时提供三类 CSV 输入。") + + +def main() -> int: + args = parse_args() + require_inputs(args) + config_path = Path(args.config).expanduser().resolve(strict=False) + config = load_config(config_path) + apply_cli_overrides(config, args) + + validation = validate_config(config, config_path) + for warning in validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + if not validation.ok: + for error in validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + session_dir = Path(str(config["session_dir"])).expanduser().resolve(strict=False) + files = config["files"] + target_paths = { + "chassis_motion_data_file": resolve_path(session_dir, str(files["chassis_motion_data_file"])), + "actuator_command_file": resolve_path(session_dir, str(files["actuator_command_file"])), + "truth_trajectory_file": resolve_path(session_dir, str(files["truth_trajectory_file"])), + "diagnostics_file": resolve_path(session_dir, str(files["diagnostics_file"])), + } + + chassis_rows, command_rows, truth_rows = synthetic_rows(config) + copy_or_generate_file( + args.chassis_motion_csv, + target_paths["chassis_motion_data_file"], + CHASSIS_MOTION_FIELDS, + chassis_rows, + ) + copy_or_generate_file( + args.actuator_command_csv, + target_paths["actuator_command_file"], + ACTUATOR_COMMAND_FIELDS, + command_rows, + ) + copy_or_generate_file( + args.truth_trajectory_csv, + target_paths["truth_trajectory_file"], + TRUTH_TRAJECTORY_FIELDS, + truth_rows, + ) + + contract_validation = validate_chassis_dataset_files(target_paths) + for warning in contract_validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + if not contract_validation.ok: + for error in contract_validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + actual_chassis_rows = read_csv_rows(target_paths["chassis_motion_data_file"]) + actual_command_rows = read_csv_rows(target_paths["actuator_command_file"]) + actual_truth_rows = read_csv_rows(target_paths["truth_trajectory_file"]) + summary = build_summary(actual_chassis_rows, actual_command_rows, actual_truth_rows, config) + + summary_validation = validate_summary(summary, config) + for warning in summary_validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + if not summary_validation.ok: + for error in summary_validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + target_paths["diagnostics_file"].parent.mkdir(parents=True, exist_ok=True) + target_paths["diagnostics_file"].write_text( + json.dumps(summary, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + + dataset_index = build_dataset_index(config, summary, target_paths, config_path) + dataset_index_path = Path(str(config["dataset_index_path"])).expanduser().resolve(strict=False) + write_yaml(dataset_index_path, dataset_index) + print(json.dumps(summary, ensure_ascii=False, separators=(",", ":"))) + print(f"[OK] 已写入底盘数据集索引: {dataset_index_path}", file=sys.stderr) + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py new file mode 100644 index 0000000..7cc6bce --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py @@ -0,0 +1,447 @@ +#!/usr/bin/env python3 +"""按底盘动作 profile 执行动作,并同步采集现场数据。""" + +from __future__ import annotations + +import argparse +import math +import sys +import time +from pathlib import Path +from typing import Any + +SCRIPT_DIR = Path(__file__).resolve().parent +if str(SCRIPT_DIR) not in sys.path: + sys.path.insert(0, str(SCRIPT_DIR)) + +from chassis_action_profile import load_yaml as load_action_profile_yaml +from chassis_action_profile import validate_profile as validate_action_profile +from chassis_data_common import apply_cli_overrides, load_config, validate_config +from capture_chassis_session import ( + ChassisSessionCapture, + config_command_source, + config_topic, + finalize_capture, + make_target_paths, + now_us, +) + + +PRIMITIVE_TYPE_VALUES = { + "straight_line": "STRAIGHT_LINE", + "arc": "ARC", + "in_place_rotation": "IN_PLACE_ROTATION", + "steering_sweep": "STEERING_SWEEP", + "lateral_translation": "LATERAL_TRANSLATION", + "diagonal_motion": "DIAGONAL_MOTION", + "module_alignment": "MODULE_ALIGNMENT", + "coordinated_steering": "COORDINATED_STEERING", +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="按现场底盘动作 profile 顺序发送动作原语,同时采集底盘、命令和外部真值数据。" + ) + parser.add_argument("--config", required=True, help="底盘标定现场数据配置文件。") + parser.add_argument( + "--action-profile", + default="src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml", + help="底盘动作 profile YAML 路径。", + ) + parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type,并选择对应动作序列。") + parser.add_argument("--task-code", default="", help="只执行指定 task_code;为空时执行该底盘类型全部动作。") + parser.add_argument("--session-id", default="", help="覆盖 session_id。") + parser.add_argument("--site-id", default="", help="覆盖 site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 vehicle_id。") + parser.add_argument("--operator-id", default="workshop_operator", help="写入请求头的操作员 ID。") + parser.add_argument("--workshop-host", default="workshop_chassis_profile_capture", help="写入请求头的车间主机名。") + parser.add_argument("--session-dir", default="", help="覆盖 session_dir。") + parser.add_argument("--dataset-index-path", default="", help="覆盖 dataset_index_path。") + parser.add_argument("--chassis-telemetry-topic", default="", help="底盘遥测话题。") + parser.add_argument("--external-pose-topic", default="", help="外部真值位姿话题。") + parser.add_argument("--ackermann-command-topic", default="", help="阿克曼内部命令话题。") + parser.add_argument( + "--command-source", + choices=["auto", "topic", "planned", "none"], + default="", + help="执行器命令 CSV 来源:auto 表示优先话题,缺失时按动作 profile 写计划命令。", + ) + parser.add_argument("--action-server", default="/chassis/execute_motion_primitive", help="底盘动作 action 名称。") + parser.add_argument("--service-timeout-sec", type=float, default=10.0, help="等待 action server 和 goal 响应的超时。") + parser.add_argument("--action-timeout-sec", type=float, default=0.0, help="覆盖每段动作等待结果超时;0 表示使用 profile timeout。") + parser.add_argument("--action-result-grace-sec", type=float, default=5.0, help="profile timeout 之外额外等待时间。") + parser.add_argument("--start-before-motion-sec", type=float, default=-1.0, help="动作前提前采集时间;负数表示读取 profile。") + parser.add_argument("--stop-after-motion-sec", type=float, default=-1.0, help="动作后延迟采集时间;负数表示读取 profile。") + parser.add_argument("--stop-on-action-failure", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--allow-incomplete", action="store_true", help="样本不足时仍写入索引和诊断文件。") + return parser.parse_args() + + +def as_float(value: Any, default: float = 0.0) -> float: + if value in (None, ""): + return default + return float(value) + + +def as_bool(value: Any, default: bool = False) -> bool: + if value in (None, ""): + return default + if isinstance(value, bool): + return value + return str(value).strip().lower() in {"1", "true", "yes", "on"} + + +def as_string_list(value: Any) -> list[str]: + if isinstance(value, list): + return [str(item) for item in value if str(item)] + result: list[str] = [] + token = "" + for ch in str(value): + if ch in ",; \t\n": + if token: + result.append(token) + token = "" + continue + token += ch + if token: + result.append(token) + return result + + +def spin_for(rclpy_module: Any, node: Any, seconds: float) -> None: + deadline = time.monotonic() + max(0.0, seconds) + while rclpy_module.ok() and time.monotonic() < deadline: + rclpy_module.spin_once(node, timeout_sec=0.05) + + +def spin_until_future( + rclpy_module: Any, + node: Any, + future: Any, + timeout_sec: float, + wait_name: str, +) -> Any: + deadline = time.monotonic() + timeout_sec + while rclpy_module.ok() and not future.done() and time.monotonic() < deadline: + rclpy_module.spin_once(node, timeout_sec=0.05) + if not future.done(): + raise TimeoutError(f"等待 {wait_name} 超时。") + return future.result() + + +def select_actions(profile: dict[str, Any], chassis_type: str, task_code: str) -> list[dict[str, Any]]: + section = profile["chassis_profiles"][chassis_type] + actions = list(section["actions"]) + if not task_code: + return actions + selected = [action for action in actions if action.get("task_code") == task_code] + if not selected: + raise ValueError(f"动作 profile 中没有 task_code={task_code!r}。") + return selected + + +def fill_common_goal(goal: Any, config: dict[str, Any], action: dict[str, Any], args: argparse.Namespace) -> None: + task_code = str(action["task_code"]) + metadata = action["metadata"] + goal.goal.header.session_id = str(config["session_id"]) + goal.goal.header.task_id = task_code + goal.goal.header.vehicle_id = str(config["vehicle_id"]) + goal.goal.header.request_id = f"{task_code}_{now_us()}" + goal.goal.header.client_send_timestamp_us = now_us() + goal.goal.header.operator_id = args.operator_id + goal.goal.header.workshop_host = args.workshop_host + goal.goal.test_case_id = task_code + goal.goal.task_purpose.value = goal.goal.task_purpose.DATA_COLLECTION + goal.goal.brake_when_finished = as_bool(metadata.get("brake_when_finished"), True) + goal.goal.timeout_sec = as_float(metadata.get("timeout_sec"), 60.0) + goal.goal.source_iteration_id = str(config["session_id"]) + + +def fill_primitive_goal(goal: Any, action: dict[str, Any]) -> None: + metadata = action["metadata"] + primitive_type = str(action["primitive_type"]) + goal.goal.selected_primitive.value = getattr(goal.goal.selected_primitive, PRIMITIVE_TYPE_VALUES[primitive_type]) + + if primitive_type == "straight_line": + goal.goal.straight_line.target_distance_m = as_float(metadata["straight_line.target_distance_m"]) + goal.goal.straight_line.target_speed_ms = as_float(metadata["straight_line.target_speed_ms"]) + goal.goal.straight_line.reverse = as_bool(metadata.get("straight_line.reverse"), False) + elif primitive_type == "arc": + goal.goal.arc.target_speed_ms = as_float(metadata["arc.target_speed_ms"]) + goal.goal.arc.radius_m = as_float(metadata["arc.radius_m"]) + goal.goal.arc.sweep_angle_deg = as_float(metadata["arc.sweep_angle_deg"]) + goal.goal.arc.clockwise = as_bool(metadata.get("arc.clockwise"), False) + elif primitive_type == "in_place_rotation": + goal.goal.in_place_rotation.target_yaw_deg = as_float(metadata["in_place_rotation.target_yaw_deg"]) + goal.goal.in_place_rotation.target_angular_vel_deg_s = as_float( + metadata["in_place_rotation.target_angular_vel_deg_s"] + ) + elif primitive_type == "steering_sweep": + goal.goal.steering_sweep.target_angle_deg = as_float(metadata["steering_sweep.target_angle_deg"]) + goal.goal.steering_sweep.sweep_amplitude_deg = as_float(metadata["steering_sweep.sweep_amplitude_deg"]) + goal.goal.steering_sweep.sweep_frequency_hz = as_float(metadata["steering_sweep.sweep_frequency_hz"]) + goal.goal.steering_sweep.duration_sec = as_float(metadata["steering_sweep.duration_sec"]) + elif primitive_type == "lateral_translation": + goal.goal.lateral_translation.target_speed_ms = as_float(metadata["lateral_translation.target_speed_ms"]) + goal.goal.lateral_translation.target_distance_m = as_float(metadata["lateral_translation.target_distance_m"]) + goal.goal.lateral_translation.move_left = as_bool(metadata.get("lateral_translation.move_left"), False) + elif primitive_type == "diagonal_motion": + goal.goal.diagonal_motion.target_speed_ms = as_float(metadata["diagonal_motion.target_speed_ms"]) + goal.goal.diagonal_motion.target_distance_m = as_float(metadata["diagonal_motion.target_distance_m"]) + goal.goal.diagonal_motion.heading_deg = as_float(metadata["diagonal_motion.heading_deg"]) + elif primitive_type == "module_alignment": + goal.goal.module_alignment.module_ids = as_string_list(metadata["module_alignment.module_ids"]) + goal.goal.module_alignment.target_zero_deg = as_float(metadata["module_alignment.target_zero_deg"]) + goal.goal.module_alignment.tolerance_deg = as_float(metadata["module_alignment.tolerance_deg"]) + elif primitive_type == "coordinated_steering": + goal.goal.coordinated_steering.module_ids = as_string_list(metadata["coordinated_steering.module_ids"]) + goal.goal.coordinated_steering.target_angle_deg = as_float(metadata["coordinated_steering.target_angle_deg"]) + goal.goal.coordinated_steering.hold_time_sec = as_float(metadata["coordinated_steering.hold_time_sec"]) + else: + raise ValueError(f"不支持的动作原语:{primitive_type}") + + +def build_goal(execute_task_type: Any, config: dict[str, Any], action: dict[str, Any], args: argparse.Namespace) -> Any: + goal = execute_task_type.Goal() + fill_common_goal(goal, config, action, args) + fill_primitive_goal(goal, action) + return goal + + +def planned_command_values(action: dict[str, Any]) -> dict[str, float]: + metadata = action["metadata"] + primitive_type = str(action["primitive_type"]) + speed = 0.0 + distance = 0.0 + steering_angle_rad = 0.0 + + if primitive_type == "straight_line": + speed = as_float(metadata["straight_line.target_speed_ms"]) + distance = as_float(metadata["straight_line.target_distance_m"]) + elif primitive_type == "arc": + speed = as_float(metadata["arc.target_speed_ms"]) + distance = as_float(metadata["arc.radius_m"]) * abs(math.radians(as_float(metadata["arc.sweep_angle_deg"]))) + elif primitive_type == "lateral_translation": + speed = as_float(metadata["lateral_translation.target_speed_ms"]) + distance = as_float(metadata["lateral_translation.target_distance_m"]) + elif primitive_type == "diagonal_motion": + speed = as_float(metadata["diagonal_motion.target_speed_ms"]) + distance = as_float(metadata["diagonal_motion.target_distance_m"]) + elif primitive_type == "steering_sweep": + steering_angle_rad = math.radians(as_float(metadata["steering_sweep.target_angle_deg"])) + elif primitive_type == "module_alignment": + steering_angle_rad = math.radians(as_float(metadata["module_alignment.target_zero_deg"])) + elif primitive_type == "coordinated_steering": + steering_angle_rad = math.radians(as_float(metadata["coordinated_steering.target_angle_deg"])) + + return { + "target_speed_ms": speed, + "target_distance_m": distance, + "target_steering_angle_rad": steering_angle_rad, + } + + +def write_planned_command_row(capture: ChassisSessionCapture, action: dict[str, Any], command_timestamp_us: int) -> None: + values = planned_command_values(action) + capture.command_csv.write_row({ + "command_timestamp_us": command_timestamp_us, + "command_id": str(action["task_code"]), + "source": "chassis_action_profile", + "target_speed_ms": values["target_speed_ms"], + "target_distance_m": values["target_distance_m"], + "target_accel_ms2": 0.0, + "target_steering_angle_rad": values["target_steering_angle_rad"], + "target_steering_rate_rads": 0.0, + "brake_command": 0.0, + "throttle_command": 0.0, + "command_timeout_sec": as_float(action["metadata"].get("timeout_sec"), 0.0), + }) + + +def update_capture_args_for_action(capture: ChassisSessionCapture, action: dict[str, Any]) -> None: + values = planned_command_values(action) + capture.args.command_id = str(action["task_code"]) + capture.args.target_speed_ms = values["target_speed_ms"] + capture.args.target_distance_m = values["target_distance_m"] + capture.args.target_steering_angle_rad = values["target_steering_angle_rad"] + capture.args.brake_command = 0.0 + + +def execute_action( + rclpy_module: Any, + node: Any, + action_client: Any, + execute_task_type: Any, + config: dict[str, Any], + action: dict[str, Any], + args: argparse.Namespace, +) -> Any: + if not action_client.wait_for_server(timeout_sec=args.service_timeout_sec): + raise TimeoutError(f"底盘动作 action server 不可用: {args.action_server}") + + goal = build_goal(execute_task_type, config, action, args) + send_future = action_client.send_goal_async(goal) + goal_handle = spin_until_future( + rclpy_module, + node, + send_future, + args.service_timeout_sec, + "发送底盘动作 goal", + ) + if not goal_handle.accepted: + raise RuntimeError(f"底盘动作被拒绝: {action['task_code']}") + + result_timeout = args.action_timeout_sec + if result_timeout <= 0.0: + result_timeout = goal.goal.timeout_sec + max(0.0, args.action_result_grace_sec) + result_future = goal_handle.get_result_async() + return spin_until_future( + rclpy_module, + node, + result_future, + result_timeout, + f"底盘动作结果 {action['task_code']}", + ) + + +def import_ros_dependencies() -> dict[str, Any]: + try: + import rclpy + from rclpy.action import ActionClient + from calibration_chassis_interfaces.action import ExecuteMotionPrimitive + from calibration_chassis_interfaces.msg import ChassisTelemetry + from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry + except ImportError as exc: + raise RuntimeError(f"缺少 ROS 2 Python 依赖或标定消息包: {exc}") from exc + + try: + from vehicle_internal_interfaces.msg import AckermannDriveCommand + except ImportError: + AckermannDriveCommand = None + + return { + "rclpy": rclpy, + "ActionClient": ActionClient, + "ExecuteMotionPrimitive": ExecuteMotionPrimitive, + "ChassisTelemetry": ChassisTelemetry, + "ExternalLocalizationTelemetry": ExternalLocalizationTelemetry, + "AckermannDriveCommand": AckermannDriveCommand, + } + + +def main() -> int: + args = parse_args() + config_path = Path(args.config).expanduser().resolve(strict=False) + config = load_config(config_path) + apply_cli_overrides(config, args) + if args.chassis_type: + config["chassis_type"] = args.chassis_type + if not args.command_source: + args.command_source = config_command_source(config) + + validation = validate_config(config, config_path) + if not validation.ok: + for error in validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + profile_path = Path(args.action_profile).expanduser().resolve(strict=False) + action_profile = validate_action_profile(load_action_profile_yaml(profile_path)) + chassis_type = str(config["chassis_type"]) + actions = select_actions(action_profile, chassis_type, args.task_code) + data_capture = action_profile.get("data_capture", {}) or {} + start_before_sec = ( + as_float(data_capture.get("start_before_motion_sec"), 1.0) + if args.start_before_motion_sec < 0.0 else args.start_before_motion_sec + ) + stop_after_sec = ( + as_float(data_capture.get("stop_after_motion_sec"), 1.0) + if args.stop_after_motion_sec < 0.0 else args.stop_after_motion_sec + ) + + try: + deps = import_ros_dependencies() + except RuntimeError as exc: + print(f"[错误] {exc}", file=sys.stderr) + return 1 + + rclpy_module = deps["rclpy"] + target_paths = make_target_paths(config) + chassis_topic = args.chassis_telemetry_topic or config_topic(config, "chassis_telemetry_topic", "/chassis/telemetry") + external_topic = args.external_pose_topic or config_topic( + config, + "external_pose_topic", + "/workshop/external_localization/vehicle/pose", + ) + command_topic = args.ackermann_command_topic or config_topic(config, "ackermann_command_topic", "") + + rclpy_module.init() + node = rclpy_module.create_node("workshop_chassis_profile_capture") + capture = ChassisSessionCapture(node, config, target_paths, args) + action_failed = False + finalize_code = 1 + + try: + node.create_subscription(deps["ChassisTelemetry"], chassis_topic, capture.on_chassis_telemetry, 50) + node.create_subscription(deps["ExternalLocalizationTelemetry"], external_topic, capture.on_external_pose, 50) + if deps["AckermannDriveCommand"] is not None and command_topic and args.command_source in ("auto", "topic"): + node.create_subscription(deps["AckermannDriveCommand"], command_topic, capture.on_ackermann_command, 50) + elif args.command_source == "topic": + raise RuntimeError("已要求从命令话题采集,但命令消息包或话题配置不可用。") + + action_client = deps["ActionClient"](node, deps["ExecuteMotionPrimitive"], args.action_server) + print(f"[*] 底盘动作 profile: {profile_path}", file=sys.stderr) + print(f"[*] 底盘类型: {chassis_type}", file=sys.stderr) + print(f"[*] 动作数量: {len(actions)}", file=sys.stderr) + print(f"[*] 底盘遥测采集: {chassis_topic}", file=sys.stderr) + print(f"[*] 外部真值采集: {external_topic}", file=sys.stderr) + if command_topic: + print(f"[*] 执行器命令采集: {command_topic}", file=sys.stderr) + + for index, action in enumerate(actions, start=1): + update_capture_args_for_action(capture, action) + before_command_count = capture.command_csv.count + command_timestamp_us = now_us() + print(f"[*] 执行动作 {index}/{len(actions)}: {action['task_code']}", file=sys.stderr) + spin_for(rclpy_module, node, start_before_sec) + if args.command_source == "planned": + write_planned_command_row(capture, action, command_timestamp_us) + wrapped_result = execute_action( + rclpy_module, + node, + action_client, + deps["ExecuteMotionPrimitive"], + config, + action, + args, + ) + spin_for(rclpy_module, node, stop_after_sec) + if args.command_source == "auto" and capture.command_csv.count == before_command_count: + write_planned_command_row(capture, action, command_timestamp_us) + + job_result = wrapped_result.result.result + if not job_result.success: + action_failed = True + print(f"[错误] 动作失败 {action['task_code']}: {job_result.message}", file=sys.stderr) + if args.stop_on_action_failure: + break + else: + message = job_result.message or "动作完成。" + print(f"[*] 动作完成 {action['task_code']}: {message}", file=sys.stderr) + except Exception as exc: + action_failed = True + print(f"[错误] {exc}", file=sys.stderr) + finally: + capture.close() + node.destroy_node() + rclpy_module.shutdown() + finalize_code = finalize_capture(config, target_paths, config_path, args) + + if action_failed: + return 2 if finalize_code == 0 else finalize_code + return finalize_code + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/smoke_test_chassis_profile_capture.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/smoke_test_chassis_profile_capture.py new file mode 100644 index 0000000..6ea820c --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/smoke_test_chassis_profile_capture.py @@ -0,0 +1,408 @@ +#!/usr/bin/env python3 +"""本地验证底盘动作 profile 执行和采集落盘链路。""" + +from __future__ import annotations + +import argparse +import math +import os +import subprocess +import sys +import tempfile +import threading +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + +SCRIPT_DIR = Path(__file__).resolve().parent +if str(SCRIPT_DIR) not in sys.path: + sys.path.insert(0, str(SCRIPT_DIR)) + +from chassis_data_common import read_csv_rows + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="启动假的底盘 action server 和遥测话题,验证 run_chassis_profile_capture.py。" + ) + parser.add_argument( + "--action-profile", + default="src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml", + help="底盘动作 profile YAML 路径。", + ) + parser.add_argument( + "--chassis-type", + default="ackermann", + choices=["ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"], + help="本次验证使用的底盘类型。", + ) + parser.add_argument( + "--task-code", + default="", + help="只验证指定动作;为空时默认取该底盘类型的第一个动作。", + ) + parser.add_argument("--session-dir", default="", help="输出目录;为空时使用临时目录。") + parser.add_argument("--keep-output", action="store_true", help="保留临时输出目录。") + parser.add_argument("--verbose", action="store_true", help="打印 profile runner 的完整输出。") + parser.add_argument("--timeout-sec", type=float, default=30.0, help="等待 profile runner 结束的超时。") + parser.add_argument("--rmw-implementation", default="rmw_fastrtps_cpp", help="本地 smoke 使用的 RMW 实现。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def first_task_code(action_profile_path: Path, chassis_type: str) -> str: + profile = load_yaml(action_profile_path) + section = profile["chassis_profiles"][chassis_type] + actions = section["actions"] + if not actions: + raise ValueError(f"{chassis_type} 没有配置动作。") + return str(actions[0]["task_code"]) + + +def write_smoke_config(path: Path, session_dir: Path, chassis_type: str) -> None: + config = { + "csv_contract_version": 1, + "chassis_type": chassis_type, + "session_id": "chassis_profile_capture_smoke", + "site_id": "local_smoke_site", + "vehicle_id": "smoke_agv_001", + "session_dir": str(session_dir), + "dataset_index_path": str(session_dir / "dataset_index.yaml"), + "files": { + "chassis_motion_data_file": "chassis/chassis_motion.csv", + "actuator_command_file": "chassis/actuator_commands.csv", + "truth_trajectory_file": "external/truth_trajectory.csv", + "diagnostics_file": "chassis/chassis_diagnostics.json", + }, + "capture": { + "chassis_telemetry_topic": "/smoke/chassis/telemetry", + "external_pose_topic": "/smoke/external_localization/vehicle/pose", + "command_source": "planned", + }, + "validation": { + "min_chassis_motion_samples": 2, + "min_actuator_command_samples": 1, + "min_truth_trajectory_samples": 2, + "min_motion_distance_m": 0.001, + "max_time_gap_ms": 500.0, + }, + "synthetic": { + "enabled": False, + }, + } + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(config, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +class FakeChassisFixture: + def __init__(self, node: Any, action_name: str, chassis_type_name: str) -> None: + from rclpy.action import ActionServer + from calibration_chassis_interfaces.action import ExecuteMotionPrimitive + from calibration_chassis_interfaces.msg import ChassisMotionPrimitiveType + from calibration_chassis_interfaces.msg import ChassisTelemetry + from calibration_common_interfaces.msg import ErrorCode + from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry + from calibration_vehicle_profile_interfaces.msg import ChassisType + + self.node = node + self.ExecuteMotionPrimitive = ExecuteMotionPrimitive + self.ChassisMotionPrimitiveType = ChassisMotionPrimitiveType + self.ChassisTelemetry = ChassisTelemetry + self.ExternalLocalizationTelemetry = ExternalLocalizationTelemetry + self.ErrorCode = ErrorCode + self.ChassisType = ChassisType + self.chassis_type_name = chassis_type_name + self.chassis_type_value = { + "ackermann": ChassisType.ACKERMANN, + "differential": ChassisType.DIFFERENTIAL, + "single_steer_wheel": ChassisType.SINGLE_STEER_WHEEL, + "multi_steer_wheel": ChassisType.MULTI_STEER_WHEEL, + }[chassis_type_name] + self.lock = threading.Lock() + self.x_m = 0.0 + self.y_m = 0.0 + self.yaw_rad = 0.0 + self.active_job_id = "" + self.chassis_pub = node.create_publisher(ChassisTelemetry, "/smoke/chassis/telemetry", 20) + self.truth_pub = node.create_publisher( + ExternalLocalizationTelemetry, + "/smoke/external_localization/vehicle/pose", + 20, + ) + self.timer = node.create_timer(0.05, self.publish_telemetry) + self.action_server = ActionServer( + node, + ExecuteMotionPrimitive, + action_name, + execute_callback=self.execute_motion, + ) + + def publish_telemetry(self) -> None: + stamp = now_us() + with self.lock: + x_m = self.x_m + y_m = self.y_m + yaw_rad = self.yaw_rad + active_job_id = self.active_job_id + + chassis_msg = self.ChassisTelemetry() + chassis_msg.hardware_timestamp_us = stamp + chassis_msg.chassis_type.value = self.chassis_type_value + chassis_msg.odom_x_m = x_m + chassis_msg.odom_y_m = y_m + chassis_msg.odom_yaw_rad = yaw_rad + chassis_msg.linear_velocity_ms = 0.05 + chassis_msg.angular_velocity_rads = 0.0 + chassis_msg.estop_engaged = False + chassis_msg.driver_error_code = 0 + chassis_msg.active_job_id = active_job_id + chassis_msg.lateral_slip_estimate = 0.0 + chassis_msg.curvature_estimate = 0.0 + self.chassis_pub.publish(chassis_msg) + + truth_msg = self.ExternalLocalizationTelemetry() + truth_msg.hardware_timestamp_us = stamp + truth_msg.pose_valid = True + truth_msg.workshop_pose.x_m = x_m + truth_msg.workshop_pose.y_m = y_m + truth_msg.workshop_pose.z_m = 0.0 + truth_msg.workshop_pose.roll_rad = 0.0 + truth_msg.workshop_pose.pitch_rad = 0.0 + truth_msg.workshop_pose.yaw_rad = yaw_rad + truth_msg.position_stddev_m = 0.001 + truth_msg.yaw_stddev_rad = 0.001 + truth_msg.tracking_loss_ratio = 0.0 + truth_msg.time_sync_offset_ms = 1.0 + truth_msg.quality_score = 1.0 + truth_msg.observed_target_count = 4 + truth_msg.reference_source_name = "smoke_fake_truth" + truth_msg.active_job_id = active_job_id + self.truth_pub.publish(truth_msg) + + def execute_motion(self, goal_handle: Any) -> Any: + request = goal_handle.request.goal + with self.lock: + self.active_job_id = request.header.request_id + + primitive = request.selected_primitive.value + steps = 8 + for _ in range(steps): + self.advance_pose(primitive, request, steps) + self.publish_telemetry() + time.sleep(0.05) + + result = self.ExecuteMotionPrimitive.Result() + result.result.success = True + result.result.error_code.code = self.ErrorCode.OK + result.result.message = "本地 smoke 假底盘动作完成。" + result.result.job_id = request.header.request_id + result.result.data_quality_passed = True + result.result.suitable_for_commit = True + result.result.recommended_parameter_version = "smoke_fake_chassis" + goal_handle.succeed() + with self.lock: + self.active_job_id = "" + return result + + def advance_pose(self, primitive: int, request: Any, steps: int) -> None: + with self.lock: + if primitive == self.ChassisMotionPrimitiveType.STRAIGHT_LINE: + sign = -1.0 if request.straight_line.reverse else 1.0 + self.x_m += sign * request.straight_line.target_distance_m / steps + elif primitive == self.ChassisMotionPrimitiveType.ARC: + sign = -1.0 if request.arc.clockwise else 1.0 + delta_yaw = sign * request.arc.sweep_angle_deg * 3.141592653589793 / 180.0 / steps + self.yaw_rad += delta_yaw + self.x_m += request.arc.radius_m * abs(delta_yaw) + self.y_m += sign * 0.01 + elif primitive == self.ChassisMotionPrimitiveType.IN_PLACE_ROTATION: + self.yaw_rad += request.in_place_rotation.target_yaw_deg * 3.141592653589793 / 180.0 / steps + elif primitive == self.ChassisMotionPrimitiveType.LATERAL_TRANSLATION: + sign = 1.0 if request.lateral_translation.move_left else -1.0 + self.y_m += sign * request.lateral_translation.target_distance_m / steps + elif primitive == self.ChassisMotionPrimitiveType.DIAGONAL_MOTION: + distance = request.diagonal_motion.target_distance_m / steps + heading = request.diagonal_motion.heading_deg * 3.141592653589793 / 180.0 + self.x_m += distance * math.cos(heading) + self.y_m += distance * math.sin(heading) + elif primitive in ( + self.ChassisMotionPrimitiveType.STEERING_SWEEP, + self.ChassisMotionPrimitiveType.MODULE_ALIGNMENT, + self.ChassisMotionPrimitiveType.COORDINATED_STEERING, + ): + self.x_m += 0.001 + + +def run_profile_capture( + repo_root: Path, + config_path: Path, + action_profile_path: Path, + session_dir: Path, + args: argparse.Namespace, + action_name: str, + task_code: str, +) -> subprocess.CompletedProcess[str]: + command = [ + sys.executable, + str(SCRIPT_DIR / "run_chassis_profile_capture.py"), + "--config", + str(config_path), + "--action-profile", + str(action_profile_path), + "--chassis-type", + args.chassis_type, + "--task-code", + task_code, + "--session-id", + "chassis_profile_capture_smoke", + "--site-id", + "local_smoke_site", + "--vehicle-id", + "smoke_agv_001", + "--session-dir", + str(session_dir), + "--dataset-index-path", + str(session_dir / "dataset_index.yaml"), + "--chassis-telemetry-topic", + "/smoke/chassis/telemetry", + "--external-pose-topic", + "/smoke/external_localization/vehicle/pose", + "--command-source", + "planned", + "--action-server", + action_name, + "--start-before-motion-sec", + "0.2", + "--stop-after-motion-sec", + "0.2", + "--service-timeout-sec", + "5.0", + "--action-result-grace-sec", + "5.0", + ] + return subprocess.run( + command, + cwd=repo_root, + env=os.environ.copy(), + text=True, + capture_output=True, + timeout=args.timeout_sec, + check=False, + ) + + +def validate_outputs(session_dir: Path) -> None: + dataset_index_path = session_dir / "dataset_index.yaml" + if not dataset_index_path.exists(): + raise RuntimeError(f"未生成 dataset_index.yaml: {dataset_index_path}") + index = load_yaml(dataset_index_path) + chassis_inputs = index.get("data_inputs", {}).get("chassis", {}) + for field_name in ( + "chassis_motion_data_files", + "actuator_command_files", + "truth_trajectory_files", + ): + values = chassis_inputs.get(field_name) + if not values: + raise RuntimeError(f"dataset_index 缺少 data_inputs.chassis.{field_name}") + + chassis_rows = read_csv_rows(session_dir / "chassis/chassis_motion.csv") + command_rows = read_csv_rows(session_dir / "chassis/actuator_commands.csv") + truth_rows = read_csv_rows(session_dir / "external/truth_trajectory.csv") + if len(chassis_rows) < 2 or len(command_rows) < 1 or len(truth_rows) < 2: + raise RuntimeError( + "采集样本数不足: " + f"chassis={len(chassis_rows)}, command={len(command_rows)}, truth={len(truth_rows)}" + ) + + +def main() -> int: + args = parse_args() + repo_root = Path.cwd() + action_profile_path = Path(args.action_profile).expanduser().resolve(strict=False) + task_code = args.task_code or first_task_code(action_profile_path, args.chassis_type) + action_name = f"/smoke/chassis/execute_motion_primitive_{now_us()}" + + temp_dir: Any = None + if args.session_dir: + session_dir = Path(args.session_dir).expanduser().resolve(strict=False) + session_dir.mkdir(parents=True, exist_ok=True) + else: + temp_dir = tempfile.TemporaryDirectory(prefix="agv_chassis_profile_capture_smoke_") + session_dir = Path(temp_dir.name) + + if args.rmw_implementation: + os.environ["RMW_IMPLEMENTATION"] = args.rmw_implementation + + try: + import rclpy + from rclpy.executors import MultiThreadedExecutor + except ImportError as exc: + print(f"[FAIL] 缺少 ROS2 Python 环境,请先 source install/setup.bash: {exc}", file=sys.stderr) + if temp_dir is not None: + temp_dir.cleanup() + return 1 + + config_path = session_dir / "chassis_data.yaml" + write_smoke_config(config_path, session_dir, args.chassis_type) + ros_log_dir = session_dir / "ros_log" + ros_log_dir.mkdir(parents=True, exist_ok=True) + os.environ.setdefault("ROS_LOG_DIR", str(ros_log_dir)) + + rclpy.init(args=None) + node = rclpy.create_node("smoke_chassis_profile_capture_fixture") + fixture = FakeChassisFixture(node, action_name, args.chassis_type) + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + thread = threading.Thread(target=executor.spin, daemon=True) + thread.start() + + try: + process = run_profile_capture( + repo_root, + config_path, + action_profile_path, + session_dir, + args, + action_name, + task_code, + ) + if args.verbose or process.returncode != 0: + print(process.stdout, end="") + print(process.stderr, end="", file=sys.stderr) + if process.returncode != 0: + print(f"[FAIL] run_chassis_profile_capture.py 返回 {process.returncode}", file=sys.stderr) + return process.returncode + validate_outputs(session_dir) + print(f"[PASS] 底盘 profile 采集 smoke 通过: {session_dir}") + print(f"[PASS] task_code={task_code}") + return 0 + finally: + executor.shutdown() + fixture.action_server.destroy() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + thread.join(timeout=2.0) + if temp_dir is not None and not args.keep_output: + temp_dir.cleanup() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/stage_chassis_parameter_commit.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/stage_chassis_parameter_commit.py new file mode 100644 index 0000000..ab71230 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/stage_chassis_parameter_commit.py @@ -0,0 +1,304 @@ +#!/usr/bin/env python3 +"""生成底盘标定待提交参数包。""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +CHASSIS_TYPES = {"ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"} + +REQUIRED_PARAM_FIELDS = { + "ackermann": [ + "ackermann.front_left_steer_zero_offset_deg", + "ackermann.front_right_steer_zero_offset_deg", + "ackermann.rear_left_wheel_radius_m", + "ackermann.rear_right_wheel_radius_m", + "ackermann.steering_ratio", + ], + "differential": [ + "differential.left_wheel_radius_m", + "differential.right_wheel_radius_m", + "differential.axle_track_width_m", + ], + "single_steer_wheel": [ + "single_steer.drive_wheel_radius_m", + "single_steer.steer_zero_offset_deg", + "single_steer.steering_ratio", + "single_steer.drive_encoder_scale", + ], + "multi_steer_wheel": [ + "multi_steer.modules", + ], +} + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="把底盘算法输出整理成现场待审批/待提交参数包。") + parser.add_argument("--dataset-index", required=True, help="本次会话 dataset_index.yaml。") + parser.add_argument("--estimated-params", required=True, help="算法输出的底盘参数 YAML。") + parser.add_argument("--output", default="", help="输出待提交参数包;为空时写到会话目录 chassis/pending_chassis_commit.yaml。") + parser.add_argument("--commit-reason", default="底盘标定算法输出待提交", help="写入参数包的提交原因。") + parser.add_argument("--operator-id", default="", help="生成待提交包的操作员 ID。") + parser.add_argument("--previous-parameter-version", default="", help="当前车端已生效参数版本,用于回滚引用。") + parser.add_argument("--persistent-write", action=argparse.BooleanOptionalAction, default=True, help="最终提交到车端时是否持久化。") + parser.add_argument("--require-manual-approval", action=argparse.BooleanOptionalAction, default=True, help="是否要求人工审批。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否把待提交包路径写回 dataset_index.yaml。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def dotted_get(data: dict[str, Any], dotted_key: str) -> Any: + current: Any = data + for part in dotted_key.split("."): + if not isinstance(current, dict) or part not in current: + return None + current = current[part] + return current + + +def has_value(value: Any) -> bool: + if value in (None, ""): + return False + if isinstance(value, list): + return bool(value) + return True + + +def require_number(value: Any, field_name: str) -> float: + if not has_value(value): + raise ValueError(f"{field_name} 不能为空。") + try: + return float(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是数字,当前值为 {value!r}。") from exc + + +def dataset_chassis_type(index: dict[str, Any]) -> str: + metadata = index.get("metadata", {}) + if isinstance(metadata, dict): + summary = metadata.get("chassis_replay_summary", {}) + if isinstance(summary, dict): + value = str(summary.get("chassis_type", "") or "").strip() + if value: + return value + return "" + + +def validate_dataset_index(index: dict[str, Any]) -> dict[str, Any]: + if int(index.get("schema_version", 0) or 0) != 1: + raise ValueError("dataset_index.schema_version 必须为 1。") + session = require_map(index.get("session"), "dataset_index.session") + for field_name in ("session_id", "vehicle_id", "dataset_root"): + require_string(session.get(field_name), f"dataset_index.session.{field_name}") + chassis_inputs = require_map( + require_map(index.get("data_inputs"), "dataset_index.data_inputs").get("chassis"), + "dataset_index.data_inputs.chassis", + ) + for field_name in ("chassis_motion_data_files", "actuator_command_files", "truth_trajectory_files"): + values = chassis_inputs.get(field_name) + if not isinstance(values, list) or not values: + raise ValueError(f"dataset_index.data_inputs.chassis.{field_name} 不能为空。") + return session + + +def validate_estimated_params( + params: dict[str, Any], + session: dict[str, Any], + index: dict[str, Any], +) -> tuple[str, str, dict[str, Any]]: + if int(params.get("schema_version", 0) or 0) != 1: + raise ValueError("estimated_params.schema_version 必须为 1。") + parameter_version = require_string(params.get("parameter_version"), "estimated_params.parameter_version") + chassis_type = require_string(params.get("chassis_type"), "estimated_params.chassis_type") + if chassis_type not in CHASSIS_TYPES: + raise ValueError(f"estimated_params.chassis_type 必须是 {sorted(CHASSIS_TYPES)} 之一。") + index_chassis_type = dataset_chassis_type(index) + if index_chassis_type and index_chassis_type != chassis_type: + raise ValueError("estimated_params.chassis_type 与 dataset_index 中的底盘类型不一致。") + vehicle_id = str(params.get("vehicle_id", "") or "").strip() + if vehicle_id and vehicle_id != session.get("vehicle_id"): + raise ValueError("estimated_params.vehicle_id 与 dataset_index.session.vehicle_id 不一致。") + estimated = require_map(params.get("estimated_params"), "estimated_params.estimated_params") + for field_name in REQUIRED_PARAM_FIELDS[chassis_type]: + value = dotted_get(estimated, field_name) + if field_name == "multi_steer.modules": + if not isinstance(value, list) or not value: + raise ValueError("estimated_params.estimated_params.multi_steer.modules 不能为空。") + continue + require_number(value, f"estimated_params.estimated_params.{field_name}") + quality = require_map(params.get("quality"), "estimated_params.quality") + if not bool(quality.get("data_quality_passed", False)): + raise ValueError("estimated_params.quality.data_quality_passed 必须为 true。") + if not bool(quality.get("suitable_for_commit", False)): + raise ValueError("estimated_params.quality.suitable_for_commit 必须为 true。") + return parameter_version, chassis_type, estimated + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def resolve_output_path(args: argparse.Namespace, session: dict[str, Any]) -> Path: + if args.output: + return Path(args.output).expanduser().resolve(strict=False) + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + return dataset_root / "chassis" / "pending_chassis_commit.yaml" + + +def build_commit_package( + dataset_index_path: Path, + estimated_params_path: Path, + index: dict[str, Any], + params: dict[str, Any], + args: argparse.Namespace, + output_path: Path, +) -> dict[str, Any]: + session = index["session"] + parameter_version, chassis_type, estimated = validate_estimated_params(params, session, index) + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + created_us = now_us() + return { + "schema_version": 1, + "commit_state": "pending_manual_approval" if args.require_manual_approval else "pending_vehicle_commit", + "created_timestamp_us": created_us, + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "source": { + "dataset_index_file": relative_to_root(dataset_index_path, dataset_root), + "estimated_params_file": relative_to_root(estimated_params_path, dataset_root), + "estimated_params_digest": { + "checksum_type": "sha256", + "checksum_value": sha256_file(estimated_params_path), + }, + }, + "approval": { + "required": bool(args.require_manual_approval), + "approved": False, + "operator_id": args.operator_id, + "approved_timestamp_us": 0, + }, + "commit_request": { + "parameter_version": parameter_version, + "chassis_type": chassis_type, + "commit_reason": args.commit_reason, + "persistent_write": bool(args.persistent_write), + "estimated_params": estimated, + }, + "rollback": { + "previous_parameter_version": args.previous_parameter_version, + "rollback_required_on_vehicle_commit_failure": True, + "rollback_verified": False, + }, + "trace": { + "pending_commit_file": relative_to_root(output_path, dataset_root), + "generated_by": "stage_chassis_parameter_commit.py", + }, + } + + +def update_dataset_index(index_path: Path, index: dict[str, Any], output_path: Path) -> None: + session = index["session"] + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + metadata = index.setdefault("metadata", {}) + commit_package = load_yaml(output_path) + source = require_map(commit_package.get("source"), "pending_commit.source") + digest = require_map(source.get("estimated_params_digest"), "pending_commit.source.estimated_params_digest") + approval = require_map(commit_package.get("approval"), "pending_commit.approval") + commit_request = require_map(commit_package.get("commit_request"), "pending_commit.commit_request") + metadata["chassis_estimated_params_file"] = source.get("estimated_params_file", "") + metadata["chassis_estimated_params_checksum_type"] = digest.get("checksum_type", "") + metadata["chassis_estimated_params_checksum_value"] = digest.get("checksum_value", "") + metadata["chassis_pending_commit_file"] = relative_to_root(output_path, dataset_root) + metadata["chassis_pending_commit_state"] = commit_package.get("commit_state", "") + metadata["chassis_pending_parameter_version"] = commit_request.get("parameter_version", "") + metadata["chassis_pending_commit_chassis_type"] = commit_request.get("chassis_type", "") + metadata["chassis_pending_commit_approval_required"] = bool(approval.get("required", False)) + metadata["chassis_pending_commit_approved"] = bool(approval.get("approved", False)) + write_yaml(index_path, index) + + +def main() -> int: + args = parse_args() + dataset_index_path = Path(args.dataset_index).expanduser().resolve(strict=False) + estimated_params_path = Path(args.estimated_params).expanduser().resolve(strict=False) + index = load_yaml(dataset_index_path) + params = load_yaml(estimated_params_path) + session = validate_dataset_index(index) + output_path = resolve_output_path(args, session) + commit_package = build_commit_package( + dataset_index_path, + estimated_params_path, + index, + params, + args, + output_path, + ) + write_yaml(output_path, commit_package) + if args.update_dataset_index: + update_dataset_index(dataset_index_path, index, output_path) + print(f"[OK] 已生成底盘待提交参数包: {output_path}") + print(f"[OK] commit_state={commit_package['commit_state']}") + print(f"[OK] parameter_version={commit_package['commit_request']['parameter_version']}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/README.md b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/README.md new file mode 100644 index 0000000..16a2d1f --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/README.md @@ -0,0 +1,207 @@ +# 真实车间运控参数标定 + +这个目录只处理运控参数标定的车间电脑侧边界。真实车辆动作仍由 Windows 车端 agent 执行,车间电脑负责下发评估任务、采集运控评估数据、底盘响应和外部真值轨迹,然后把这些文件写入 `dataset_index.yaml`。 + +## 数据流 + +```text +车间电脑总控 + -> 车端 agent 执行运控评估任务 + -> 车间电脑采集 control_eval / reference_signal / chassis_response / truth_trajectory + -> dataset_index_to_site_data_input.py 生成 site_data_input.yaml + -> control_calibration_service 或离线算法读取 data_input.* + -> 算法输出 control/estimated_params.yaml + -> stage_control_parameter_commit.py 生成 pending_control_commit.yaml + -> approve_control_pending_parameters.py 生成 approved_control_parameter_handoff.yaml +``` + +## 数据文件 + +固定 CSV 协议见: + +```bash +src/site_deployment/workshop_control_calibration_real/control_csv_contract.md +``` + +最小数据输入包括: + +- `control/control_eval.csv` +- `control/reference_signal.csv` +- `control/chassis_response.csv` +- `external/truth_trajectory.csv` + +这些文件会写到 `dataset_index.yaml` 的 `data_inputs.control.*`,再自动转换为 `control_calibration_service` 的 `data_input.*` 参数。 + +控制器约束: + +- PID 可以用于速度、航向、角速度、舵角等回路。 +- `controller_algorithm: pid` 时,必须在算法输出中填写明确的 `control_role` 和 `loop_name`。 +- `control_axis: combined` 且使用 PID 时,必须使用 `pid_loops` 分别列出每个 PID 回路,不能用一个含糊的 PID 表达横纵向联合参数。 + +## Smoke + +生成一套合成运控数据并写入数据集索引: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/replay_control_smoke.py \ + --session-id control_smoke_001 \ + --site-id site_a \ + --vehicle-id agv_001 \ + --session-dir /tmp/agv_control_smoke \ + --dataset-index-path /tmp/agv_control_smoke/dataset_index.yaml \ + --control-axis combined \ + --controller-algorithm mpc +``` + +生成 `site_data_input.yaml`: + +```bash +python3 src/deployment/tools/finalize_site_session.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --session-dir /tmp/agv_control_smoke \ + --dataset-index /tmp/agv_control_smoke/dataset_index.yaml \ + --output /tmp/agv_control_smoke/site_data_input.yaml \ + --no-strict +``` + +## 真实采集 + +车间电脑侧可以直接订阅现场 ROS 话题并落盘: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/capture_control_session.py \ + --config /data/agv_calib/site_a/control_data.yaml \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id agv_001 \ + --session-dir /data/agv_calib/site_a/session_001 \ + --dataset-index-path /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --duration-sec 60.0 +``` + +默认订阅: + +- `/control/telemetry` +- `/chassis/telemetry` +- `/workshop/external_localization/vehicle/pose` + +如果现场话题不同,可以通过 `--control-telemetry-topic`、`--chassis-telemetry-topic`、`--external-pose-topic` 覆盖。 + +## 评估任务 profile + +现场推荐评估动作已经固化在: + +```bash +src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml +``` + +该文件按 `ackermann`、`differential`、`single_steer_wheel`、`multi_steer_wheel` 拆分,固定每类底盘建议跑的轨迹跟踪、速度阶跃、加减速和停车精度任务。每个任务同时声明 `control_axis`、`controller_algorithm`、`control_role` 和必要的外部真值质量门限。 + +校验并查看摘要: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py \ + --profile src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml +``` + +导出总控可接收的 `requested_tasks` 片段: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py \ + --profile src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml \ + --chassis-type differential \ + --format requested_tasks \ + --output /data/agv_calib/site_a/control_requested_tasks.yaml +``` + +导出的任务片段和 `RequestedCalibrationTask` 对齐。后续接入总控时,可以把这些运控评估拆成多个阶段,而不是只跑一个默认轨迹。 + +按 profile 下发运控评估任务,并同步采集现场数据: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/run_control_profile_capture.py \ + --config /data/agv_calib/site_a/control_data.yaml \ + --evaluation-profile src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml \ + --chassis-type differential \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id agv_001 \ + --session-dir /data/agv_calib/site_a/session_001 \ + --dataset-index-path /data/agv_calib/site_a/session_001/dataset_index.yaml +``` + +如果只想跑某一个评估任务,可以增加 `--task-code control.differential.speed_step_low`。 + +本地 smoke 验证 profile runner: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/smoke_test_control_profile_capture.py \ + --chassis-type differential \ + --task-code control.differential.speed_step_low +``` + +这个 smoke 会启动假的运控 action server、运控遥测、底盘遥测和外部真值话题,只验证车间电脑侧的“下发任务、采集落盘、写入 dataset_index”链路。 + +总控 smoke 已支持直接读取运控评估 profile: + +```bash +python3 src/simulation/tools/smoke_test_workshop_orchestrator.py \ + --tasks external,chassis,control,sensor_intrinsic \ + --chassis-profile-type differential \ + --control-evaluation-profile src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml +``` + +也可以从现场 profile 自动读取: + +```bash +python3 src/simulation/tools/smoke_test_workshop_orchestrator.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --tasks external,chassis,control,sensor_intrinsic +``` + +## 参数结果交接 + +算法工程师应输出: + +```bash +/tmp/agv_control_smoke/control/estimated_params.yaml +``` + +格式模板: + +```bash +src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_template.yaml +``` + +按底盘类型拆好的默认模板: + +```bash +src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_differential_template.yaml +src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_single_steer_wheel_template.yaml +src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_multi_steer_wheel_template.yaml +src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_ackermann_template.yaml +``` + +每类底盘推荐标定哪些控制回路见: + +```bash +src/site_deployment/workshop_control_calibration_real/config/control_role_profiles.yaml +``` + +生成待审批包: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/stage_control_parameter_commit.py \ + --dataset-index /tmp/agv_control_smoke/dataset_index.yaml \ + --estimated-params /tmp/agv_control_smoke/control/estimated_params.yaml +``` + +审批并生成车间电脑侧交接包: + +```bash +python3 src/site_deployment/workshop_control_calibration_real/approve_control_pending_parameters.py \ + /tmp/agv_control_smoke/control/pending_control_commit.yaml \ + --operator-id operator_001 +``` + +当前交接包只表示“车间电脑侧已经审批通过,等待车端写参适配器处理”。车端写参实现暂不在本目录内处理。 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/approve_control_pending_parameters.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/approve_control_pending_parameters.py new file mode 100644 index 0000000..57f6c73 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/approve_control_pending_parameters.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python3 +"""审批运控待提交参数包,并生成车间电脑侧交接包。""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="审批运控待提交参数包,并生成车间电脑侧参数交接包。") + parser.add_argument("pending_commit", help="运控待提交参数包 pending_control_commit.yaml。") + parser.add_argument("--operator-id", required=True, help="审批操作员 ID。") + parser.add_argument("--approval-note", default="", help="审批备注。") + parser.add_argument("--output", default="", help="交接包输出路径;为空时写到会话目录 control/approved_control_parameter_handoff.yaml。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否把交接包路径写回 dataset_index.yaml。") + parser.add_argument("--dry-run", action="store_true", help="只校验并打印结果,不修改文件。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def bool_value(value: Any) -> bool: + if isinstance(value, bool): + return value + if isinstance(value, str): + return value.strip().lower() in ("1", "true", "yes", "y") + return bool(value) + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def resolve_dataset_file(path_value: str, dataset_root: Path) -> Path: + path = Path(path_value).expanduser() + if path.is_absolute(): + return path + return (dataset_root / path).resolve(strict=False) + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def validate_pending_commit(package: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]: + if int(package.get("schema_version", 0) or 0) != 1: + raise ValueError("pending_control_commit.schema_version 必须为 1。") + session = require_map(package.get("session"), "pending_control_commit.session") + source = require_map(package.get("source"), "pending_control_commit.source") + approval = require_map(package.get("approval"), "pending_control_commit.approval") + commit_request = require_map(package.get("commit_request"), "pending_control_commit.commit_request") + require_string(session.get("session_id"), "session.session_id") + require_string(session.get("vehicle_id"), "session.vehicle_id") + require_string(session.get("dataset_root"), "session.dataset_root") + require_string(commit_request.get("parameter_version"), "commit_request.parameter_version") + require_string(commit_request.get("chassis_type"), "commit_request.chassis_type") + require_string(commit_request.get("control_axis"), "commit_request.control_axis") + require_string(commit_request.get("controller_algorithm"), "commit_request.controller_algorithm") + require_string(commit_request.get("control_role"), "commit_request.control_role") + require_map(commit_request.get("estimated_params"), "commit_request.estimated_params") + return session, source, approval, commit_request + + +def validate_estimated_params_digest(session: dict[str, Any], source: dict[str, Any]) -> dict[str, str]: + digest = require_map(source.get("estimated_params_digest"), "source.estimated_params_digest") + checksum_type = require_string(digest.get("checksum_type"), "estimated_params_digest.checksum_type") + checksum_value = require_string(digest.get("checksum_value"), "estimated_params_digest.checksum_value") + if checksum_type.lower() != "sha256": + raise ValueError("当前只支持 sha256 参数摘要。") + + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + estimated_params_file = require_string(source.get("estimated_params_file"), "source.estimated_params_file") + estimated_params_path = resolve_dataset_file(estimated_params_file, dataset_root) + if not estimated_params_path.exists(): + raise FileNotFoundError(f"运控算法输出参数文件不存在,无法校验摘要:{estimated_params_path}") + actual_checksum = sha256_file(estimated_params_path) + if actual_checksum != checksum_value: + raise ValueError("运控算法输出参数摘要不一致,禁止审批交接。") + return { + "checksum_type": checksum_type, + "checksum_value": checksum_value, + "estimated_params_file": relative_to_root(estimated_params_path, dataset_root), + } + + +def resolve_output_path(args: argparse.Namespace, session: dict[str, Any]) -> Path: + if args.output: + return Path(args.output).expanduser().resolve(strict=False) + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + return (dataset_root / "control" / "approved_control_parameter_handoff.yaml").resolve(strict=False) + + +def build_handoff_package( + pending_path: Path, + output_path: Path, + package: dict[str, Any], + args: argparse.Namespace, +) -> dict[str, Any]: + session, source, approval, commit_request = validate_pending_commit(package) + digest = validate_estimated_params_digest(session, source) + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + approved_us = now_us() + + approval["required"] = bool_value(approval.get("required", True)) + approval["approved"] = True + approval["operator_id"] = args.operator_id + approval["approved_timestamp_us"] = approved_us + approval["approval_note"] = args.approval_note + package["commit_state"] = "approved_waiting_vehicle_handoff" + package["vehicle_handoff"] = { + "handoff_file": relative_to_root(output_path, dataset_root), + "handoff_state": "ready_for_vehicle_adapter", + "generated_timestamp_us": approved_us, + "generated_by": "approve_control_pending_parameters.py", + } + + return { + "schema_version": 1, + "handoff_state": "ready_for_vehicle_adapter", + "created_timestamp_us": approved_us, + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "source": { + "pending_commit_file": relative_to_root(pending_path, dataset_root), + "dataset_index_file": source.get("dataset_index_file", ""), + "estimated_params_file": digest["estimated_params_file"], + "estimated_params_digest": { + "checksum_type": digest["checksum_type"], + "checksum_value": digest["checksum_value"], + }, + }, + "approval": { + "approved": True, + "operator_id": args.operator_id, + "approved_timestamp_us": approved_us, + "approval_note": args.approval_note, + }, + "vehicle_commit_request": { + "parameter_version": commit_request["parameter_version"], + "chassis_type": commit_request["chassis_type"], + "control_axis": commit_request["control_axis"], + "controller_algorithm": commit_request["controller_algorithm"], + "control_role": commit_request["control_role"], + "commit_reason": commit_request.get("commit_reason", ""), + "persistent_write": bool_value(commit_request.get("persistent_write", True)), + "estimated_params": commit_request["estimated_params"], + }, + "vehicle_adapter_contract": { + "status": "waiting_vehicle_side_adapter", + "required_request": "vehicle_side_control_parameter_write_adapter", + "required_digest_check": "sha256", + "rollback_required_on_vehicle_commit_failure": bool_value( + package.get("rollback", {}).get("rollback_required_on_vehicle_commit_failure", True) + ), + }, + } + + +def update_dataset_index( + pending_package: dict[str, Any], + handoff_path: Path, + handoff_package: dict[str, Any], +) -> None: + session = require_map(pending_package.get("session"), "pending_control_commit.session") + source = require_map(pending_package.get("source"), "pending_control_commit.source") + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + dataset_index_file = source.get("dataset_index_file", "") + if not isinstance(dataset_index_file, str) or not dataset_index_file: + return + dataset_index_path = resolve_dataset_file(dataset_index_file, dataset_root) + if not dataset_index_path.exists(): + return + + index = load_yaml(dataset_index_path) + metadata = index.setdefault("metadata", {}) + metadata["control_parameter_handoff_file"] = relative_to_root(handoff_path, dataset_root) + metadata["control_parameter_handoff_state"] = handoff_package["handoff_state"] + metadata["control_pending_commit_state"] = pending_package.get("commit_state", "") + metadata["control_pending_commit_approved"] = True + metadata["control_pending_commit_approved_operator_id"] = handoff_package["approval"]["operator_id"] + metadata["control_pending_commit_approved_timestamp_us"] = handoff_package["approval"]["approved_timestamp_us"] + write_yaml(dataset_index_path, index) + + +def main() -> int: + args = parse_args() + pending_path = Path(args.pending_commit).expanduser().resolve(strict=False) + package = load_yaml(pending_path) + session, _, _, commit_request = validate_pending_commit(package) + output_path = resolve_output_path(args, session) + handoff_package = build_handoff_package(pending_path, output_path, package, args) + + print(f"[OK] 运控待提交参数包校验通过:{pending_path}") + print(f"[OK] 参数版本:{commit_request['parameter_version']}") + print(f"[OK] 目标车辆:{session['vehicle_id']}") + print(f"[OK] 交接包状态:{handoff_package['handoff_state']}") + + if args.dry_run: + print("[OK] dry-run:未修改文件。") + return 0 + + write_yaml(output_path, handoff_package) + write_yaml(pending_path, package) + if args.update_dataset_index: + update_dataset_index(package, output_path, handoff_package) + print(f"[OK] 已生成运控参数交接包:{output_path}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/capture_control_session.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/capture_control_session.py new file mode 100644 index 0000000..255acee --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/capture_control_session.py @@ -0,0 +1,391 @@ +#!/usr/bin/env python3 +"""现场运控标定数据采集落盘入口。""" + +from __future__ import annotations + +import argparse +import csv +import json +import math +import sys +import time +from pathlib import Path +from typing import Any + +from control_data_common import ( + CHASSIS_RESPONSE_FIELDS, + CONTROL_EVALUATION_FIELDS, + REFERENCE_SIGNAL_FIELDS, + TRUTH_TRAJECTORY_FIELDS, + apply_cli_overrides, + build_dataset_index, + build_summary, + has_placeholder, + load_config, + read_csv_rows, + resolve_path, + validate_config, + validate_control_dataset_files, + validate_summary, + write_yaml, +) + + +def float_value(value: Any, default: float = 0.0) -> float: + if value in (None, ""): + return default + return float(value) + + +def int_value(value: Any, default: int = 0) -> int: + if value in (None, ""): + return default + return int(value) + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="从现场 ROS 话题采集运控评估、底盘响应和外部真值轨迹,并写入 dataset_index.yaml。" + ) + parser.add_argument("--config", required=True, help="运控标定现场数据配置文件。") + parser.add_argument("--control-axis", default="", help="覆盖 control_axis。") + parser.add_argument("--controller-algorithm", default="", help="覆盖 controller_algorithm。") + parser.add_argument("--control-role", default="", help="覆盖 control_role。") + parser.add_argument("--session-id", default="", help="覆盖 session_id。") + parser.add_argument("--site-id", default="", help="覆盖 site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 vehicle_id。") + parser.add_argument("--session-dir", default="", help="覆盖 session_dir。") + parser.add_argument("--dataset-index-path", default="", help="覆盖 dataset_index_path。") + parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type。") + parser.add_argument("--control-telemetry-topic", default="", help="运控遥测话题。") + parser.add_argument("--chassis-telemetry-topic", default="", help="底盘遥测话题。") + parser.add_argument("--external-pose-topic", default="", help="外部真值位姿话题。") + parser.add_argument("--task-type", default="", help="本次采集对应的运控评估任务类型。") + parser.add_argument("--duration-sec", type=float, default=0.0, help="采集时长;0 表示按 Ctrl+C 结束。") + parser.add_argument("--max-control-samples", type=int, default=0, help="达到该运控样本数后自动结束;0 表示不限。") + parser.add_argument("--allow-incomplete", action="store_true", help="样本不足时仍写入索引和诊断文件。") + return parser.parse_args() + + +def config_topic(config: dict[str, Any], key: str, default: str) -> str: + capture = config.get("capture", {}) or {} + if not isinstance(capture, dict): + return default + value = str(capture.get(key, "") or "").strip() + if not value or has_placeholder(value): + return default + return value + + +def synthetic_value(config: dict[str, Any], key: str, default: float) -> float: + synthetic = config.get("synthetic", {}) or {} + if not isinstance(synthetic, dict): + return default + return float_value(synthetic.get(key), default) + + +def task_type_from_config(config: dict[str, Any], args: argparse.Namespace) -> str: + if args.task_type: + return args.task_type + synthetic = config.get("synthetic", {}) or {} + if isinstance(synthetic, dict): + value = str(synthetic.get("task_type", "") or "").strip() + if value: + return value + return "trajectory_tracking" + + +def average_module_steer_rad(modules: Any) -> float: + values: list[float] = [] + for module in modules: + values.append(math.radians(float_value(getattr(module, "steer_angle_deg", 0.0)))) + if not values: + return 0.0 + return sum(values) / len(values) + + +class CsvAppender: + def __init__(self, path: Path, fieldnames: list[str]) -> None: + self.path = path + self.path.parent.mkdir(parents=True, exist_ok=True) + self.stream = self.path.open("w", newline="", encoding="utf-8") + self.writer = csv.DictWriter(self.stream, fieldnames=fieldnames) + self.writer.writeheader() + self.count = 0 + + def write_row(self, row: dict[str, Any]) -> None: + self.writer.writerow(row) + self.stream.flush() + self.count += 1 + + def close(self) -> None: + self.stream.close() + + +class ControlSessionCapture: + def __init__( + self, + config: dict[str, Any], + paths: dict[str, Path], + args: argparse.Namespace, + ) -> None: + self.config = config + self.paths = paths + self.args = args + self.task_type = task_type_from_config(config, args) + self.chassis_type = str(config.get("chassis_type", "ackermann") or "ackermann") + self.control_axis = str(config.get("control_axis", "combined") or "combined") + self.controller_algorithm = str(config.get("controller_algorithm", "mpc") or "mpc") + self.control_role = str(config.get("control_role", "path_tracking_outer_loop") or "path_tracking_outer_loop") + self.reference_index = 0 + self.control_csv = CsvAppender(paths["control_evaluation_data_file"], CONTROL_EVALUATION_FIELDS) + self.reference_csv = CsvAppender(paths["reference_signal_file"], REFERENCE_SIGNAL_FIELDS) + self.chassis_csv = CsvAppender(paths["chassis_response_file"], CHASSIS_RESPONSE_FIELDS) + self.truth_csv = CsvAppender(paths["truth_trajectory_file"], TRUTH_TRAJECTORY_FIELDS) + + def close(self) -> None: + self.control_csv.close() + self.reference_csv.close() + self.chassis_csv.close() + self.truth_csv.close() + + def on_control_telemetry(self, msg: Any) -> None: + stamp = int_value(getattr(msg, "hardware_timestamp_us", 0), now_us()) + measured_x = float_value(getattr(msg, "odom_x_m", 0.0)) + measured_y = float_value(getattr(msg, "odom_y_m", 0.0)) + measured_yaw = float_value(getattr(msg, "odom_yaw_rad", 0.0)) + measured_speed = float_value(getattr(msg, "linear_velocity_ms", 0.0)) + lateral_error = float_value(getattr(msg, "lateral_error_m", 0.0)) + heading_error = float_value(getattr(msg, "heading_error_rad", 0.0)) + speed_error = float_value(getattr(msg, "speed_error_ms", 0.0)) + target_velocity = measured_speed + speed_error + target_y = measured_y - lateral_error + target_yaw = measured_yaw - heading_error + active_job_id = str(getattr(msg, "active_job_id", "")) + + self.control_csv.write_row({ + "hardware_timestamp_us": stamp, + "chassis_type": self.chassis_type, + "task_type": self.task_type, + "control_axis": self.control_axis, + "controller_algorithm": self.controller_algorithm, + "control_role": self.control_role, + "target_x_m": measured_x, + "target_y_m": target_y, + "target_yaw_rad": target_yaw, + "target_velocity_ms": target_velocity, + "measured_x_m": measured_x, + "measured_y_m": measured_y, + "measured_yaw_rad": measured_yaw, + "measured_velocity_ms": measured_speed, + "lateral_error_m": lateral_error, + "heading_error_rad": heading_error, + "speed_error_ms": speed_error, + "control_output_steer_rad": float_value(getattr(msg, "steering_output", 0.0)), + "control_output_accel_ms2": float_value(getattr(msg, "throttle_output", 0.0)), + "control_output_brake": float_value(getattr(msg, "brake_output", 0.0)), + "saturation_flag": int(bool(getattr(msg, "saturation_flag", False))), + "active_job_id": active_job_id, + "diagnostics_json": json.dumps({ + "parameter_version": str(getattr(msg, "parameter_version", "")), + "pose_source_name": str(getattr(msg, "pose_source_name", "")), + "pose_from_external_truth": bool(getattr(msg, "pose_from_external_truth", False)), + }, ensure_ascii=False), + }) + self.reference_csv.write_row({ + "reference_timestamp_us": stamp, + "task_type": self.task_type, + "sequence_index": self.reference_index, + "target_x_m": measured_x, + "target_y_m": target_y, + "target_yaw_rad": target_yaw, + "target_velocity_ms": target_velocity, + "target_accel_ms2": synthetic_value(self.config, "target_accel_ms2", 0.0), + "curvature_1pm": 0.0, + "stop_required": 1 if self.task_type == "stop_accuracy" else 0, + "active_job_id": active_job_id, + }) + self.reference_index += 1 + + def on_chassis_telemetry(self, msg: Any) -> None: + self.chassis_csv.write_row({ + "hardware_timestamp_us": int_value(getattr(msg, "hardware_timestamp_us", 0), now_us()), + "odom_x_m": float_value(getattr(msg, "odom_x_m", 0.0)), + "odom_y_m": float_value(getattr(msg, "odom_y_m", 0.0)), + "odom_yaw_rad": float_value(getattr(msg, "odom_yaw_rad", 0.0)), + "linear_velocity_ms": float_value(getattr(msg, "linear_velocity_ms", 0.0)), + "angular_velocity_rads": float_value(getattr(msg, "angular_velocity_rads", 0.0)), + "steering_angle_rad": average_module_steer_rad(getattr(msg, "modules", [])), + "accel_ms2": float_value(getattr(msg, "accel_ms2", 0.0)), + "brake_state": int(bool(getattr(msg, "brake_state", False))), + "driver_error_code": int_value(getattr(msg, "driver_error_code", 0)), + "active_job_id": str(getattr(msg, "active_job_id", "")), + }) + + def on_external_pose(self, msg: Any) -> None: + pose = getattr(msg, "workshop_pose", None) + self.truth_csv.write_row({ + "hardware_timestamp_us": int_value(getattr(msg, "hardware_timestamp_us", 0), now_us()), + "pose_valid": int(bool(getattr(msg, "pose_valid", False))), + "x_m": float_value(getattr(pose, "x_m", 0.0)), + "y_m": float_value(getattr(pose, "y_m", 0.0)), + "z_m": float_value(getattr(pose, "z_m", 0.0)), + "roll_rad": float_value(getattr(pose, "roll_rad", 0.0)), + "pitch_rad": float_value(getattr(pose, "pitch_rad", 0.0)), + "yaw_rad": float_value(getattr(pose, "yaw_rad", 0.0)), + "position_stddev_m": float_value(getattr(msg, "position_stddev_m", 0.0)), + "yaw_stddev_rad": float_value(getattr(msg, "yaw_stddev_rad", 0.0)), + "tracking_loss_ratio": float_value(getattr(msg, "tracking_loss_ratio", 0.0)), + "time_sync_offset_ms": float_value(getattr(msg, "time_sync_offset_ms", 0.0)), + "quality_score": float_value(getattr(msg, "quality_score", 0.0)), + "observed_target_count": int_value(getattr(msg, "observed_target_count", 0)), + "reference_source_name": str(getattr(msg, "reference_source_name", "")), + "active_job_id": str(getattr(msg, "active_job_id", "")), + }) + + +def make_target_paths(config: dict[str, Any]) -> dict[str, Path]: + session_dir = Path(str(config["session_dir"])).expanduser().resolve(strict=False) + files = config["files"] + return { + "control_evaluation_data_file": resolve_path(session_dir, str(files["control_evaluation_data_file"])), + "reference_signal_file": resolve_path(session_dir, str(files["reference_signal_file"])), + "chassis_response_file": resolve_path(session_dir, str(files["chassis_response_file"])), + "truth_trajectory_file": resolve_path(session_dir, str(files["truth_trajectory_file"])), + "diagnostics_file": resolve_path(session_dir, str(files["diagnostics_file"])), + } + + +def finalize_capture( + config: dict[str, Any], + target_paths: dict[str, Path], + config_path: Path, + args: argparse.Namespace, +) -> int: + contract_validation = validate_control_dataset_files(target_paths) + if not contract_validation.ok: + summary = { + "session_id": config.get("session_id", ""), + "site_id": config.get("site_id", ""), + "vehicle_id": config.get("vehicle_id", ""), + "chassis_type": config.get("chassis_type", ""), + "control_axis": config.get("control_axis", ""), + "controller_algorithm": config.get("controller_algorithm", ""), + "control_role": config.get("control_role", ""), + "contract_errors": contract_validation.errors, + "contract_warnings": contract_validation.warnings, + } + target_paths["diagnostics_file"].parent.mkdir(parents=True, exist_ok=True) + target_paths["diagnostics_file"].write_text( + json.dumps(summary, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + for error in contract_validation.errors: + print(f"[错误] {error}", file=sys.stderr) + print(f"[错误] 已写入诊断文件: {target_paths['diagnostics_file']}", file=sys.stderr) + return 1 + + control_rows = read_csv_rows(target_paths["control_evaluation_data_file"]) + reference_rows = read_csv_rows(target_paths["reference_signal_file"]) + chassis_rows = read_csv_rows(target_paths["chassis_response_file"]) + truth_rows = read_csv_rows(target_paths["truth_trajectory_file"]) + summary = build_summary(control_rows, reference_rows, chassis_rows, truth_rows, config) + + summary_validation = validate_summary(summary, config) + summary["contract_errors"] = contract_validation.errors + summary["contract_warnings"] = contract_validation.warnings + summary["validation_errors"] = summary_validation.errors + summary["validation_warnings"] = summary_validation.warnings + target_paths["diagnostics_file"].parent.mkdir(parents=True, exist_ok=True) + target_paths["diagnostics_file"].write_text( + json.dumps(summary, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + + if summary_validation.errors and not args.allow_incomplete: + for error in summary_validation.errors: + print(f"[错误] {error}", file=sys.stderr) + print(f"[错误] 已写入诊断文件: {target_paths['diagnostics_file']}", file=sys.stderr) + return 1 + + for warning in contract_validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + for warning in summary_validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + + dataset_index = build_dataset_index(config, summary, target_paths, config_path) + dataset_index_path = Path(str(config["dataset_index_path"])).expanduser().resolve(strict=False) + write_yaml(dataset_index_path, dataset_index) + print(json.dumps(summary, ensure_ascii=False, separators=(",", ":"))) + print(f"[OK] 已写入运控采集数据集索引: {dataset_index_path}", file=sys.stderr) + return 0 + + +def main() -> int: + args = parse_args() + config_path = Path(args.config).expanduser().resolve(strict=False) + config = load_config(config_path) + apply_cli_overrides(config, args) + validation = validate_config(config, config_path) + if not validation.ok: + for error in validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + try: + import rclpy + from calibration_chassis_interfaces.msg import ChassisTelemetry + from calibration_control_interfaces.msg import ControlTelemetry + from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry + except ImportError as exc: + print(f"[错误] 缺少 ROS 2 Python 依赖或标定消息包,无法现场采集: {exc}", file=sys.stderr) + return 1 + + target_paths = make_target_paths(config) + control_topic = args.control_telemetry_topic or config_topic(config, "control_telemetry_topic", "/control/telemetry") + chassis_topic = args.chassis_telemetry_topic or config_topic(config, "chassis_telemetry_topic", "/chassis/telemetry") + external_topic = args.external_pose_topic or config_topic( + config, + "external_pose_topic", + "/workshop/external_localization/vehicle/pose", + ) + + rclpy.init() + node = rclpy.create_node("workshop_control_session_capture") + capture = ControlSessionCapture(config, target_paths, args) + node.create_subscription(ControlTelemetry, control_topic, capture.on_control_telemetry, 50) + node.create_subscription(ChassisTelemetry, chassis_topic, capture.on_chassis_telemetry, 50) + node.create_subscription(ExternalLocalizationTelemetry, external_topic, capture.on_external_pose, 50) + + print(f"[*] 运控遥测采集: {control_topic}", file=sys.stderr) + print(f"[*] 底盘响应采集: {chassis_topic}", file=sys.stderr) + print(f"[*] 外部真值采集: {external_topic}", file=sys.stderr) + + deadline = time.monotonic() + args.duration_sec if args.duration_sec > 0.0 else None + try: + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.1) + if deadline is not None and time.monotonic() >= deadline: + break + if args.max_control_samples > 0 and capture.control_csv.count >= args.max_control_samples: + break + except KeyboardInterrupt: + pass + finally: + capture.close() + node.destroy_node() + rclpy.shutdown() + + return finalize_capture(config, target_paths, config_path, args) + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_data_template.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_data_template.yaml new file mode 100644 index 0000000..c6bdb32 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_data_template.yaml @@ -0,0 +1,43 @@ +# 运控标定现场数据模板。 +# 该文件只定义运控评估算法的数据输入和落盘边界,真实动作执行由车端 agent 完成。 + +csv_contract_version: 1 +session_id: replace_with_session_id +site_id: replace_with_site_or_line_id +vehicle_id: replace_with_real_vehicle_id +session_dir: /data/agv_calib/replace_with_site_or_line_id/session_xxx +dataset_index_path: /data/agv_calib/replace_with_site_or_line_id/session_xxx/dataset_index.yaml + +chassis_type: ackermann +control_axis: combined +controller_algorithm: mpc +control_role: path_tracking_outer_loop + +# 这些文件会写入 dataset_index.yaml 的 data_inputs.control。 +files: + control_evaluation_data_file: control/control_eval.csv + reference_signal_file: control/reference_signal.csv + chassis_response_file: control/chassis_response.csv + truth_trajectory_file: external/truth_trajectory.csv + diagnostics_file: control/control_diagnostics.json + +capture: + control_telemetry_topic: /control/telemetry + chassis_telemetry_topic: /chassis/telemetry + external_pose_topic: /workshop/external_localization/vehicle/pose + +validation: + min_control_evaluation_samples: 50 + min_reference_signal_samples: 50 + min_chassis_response_samples: 50 + min_truth_trajectory_samples: 50 + max_time_gap_ms: 200.0 + +synthetic: + task_type: trajectory_tracking + duration_sec: 6.0 + sample_period_ms: 50.0 + target_speed_ms: 0.2 + lateral_error_m: 0.015 + heading_error_rad: 0.01 + speed_error_ms: 0.02 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_ackermann_template.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_ackermann_template.yaml new file mode 100644 index 0000000..09ec631 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_ackermann_template.yaml @@ -0,0 +1,39 @@ +# 阿克曼运控算法输出参数模板。 + +schema_version: 1 +session_id: replace_with_session_id +vehicle_id: replace_with_real_vehicle_id +chassis_type: ackermann +control_axis: combined +controller_algorithm: pid +control_role: multi_loop_pid +parameter_version: replace_with_control_parameter_version + +estimated_params: + pid_loops: + - control_axis: longitudinal_control + control_role: speed_loop + loop_name: speed + kp: 0.0 + ki: 0.0 + kd: 0.0 + - control_axis: lateral_control + control_role: steering_angle_inner_loop + loop_name: steering_angle + kp: 0.0 + ki: 0.0 + kd: 0.0 + +quality: + data_quality_passed: false + suitable_for_commit: false + auto_acceptance_passed: false + validation_summary: + rms_lateral_error_m: 0.0 + rms_heading_error_rad: 0.0 + rms_speed_error_ms: 0.0 + overshoot_ratio: 0.0 + settle_time_sec: 0.0 + stop_position_error_m: 0.0 + max_jerk: 0.0 + saturation_ratio: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_differential_template.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_differential_template.yaml new file mode 100644 index 0000000..8048eb7 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_differential_template.yaml @@ -0,0 +1,45 @@ +# 差速轮运控算法输出参数模板。 + +schema_version: 1 +session_id: replace_with_session_id +vehicle_id: replace_with_real_vehicle_id +chassis_type: differential +control_axis: combined +controller_algorithm: pid +control_role: multi_loop_pid +parameter_version: replace_with_control_parameter_version + +estimated_params: + pid_loops: + - control_axis: longitudinal_control + control_role: speed_loop + loop_name: speed + kp: 0.0 + ki: 0.0 + kd: 0.0 + - control_axis: lateral_control + control_role: yaw_rate_loop + loop_name: yaw_rate + kp: 0.0 + ki: 0.0 + kd: 0.0 + - control_axis: longitudinal_control + control_role: wheel_speed_inner_loop + loop_name: left_right_wheel_speed + kp: 0.0 + ki: 0.0 + kd: 0.0 + +quality: + data_quality_passed: false + suitable_for_commit: false + auto_acceptance_passed: false + validation_summary: + rms_lateral_error_m: 0.0 + rms_heading_error_rad: 0.0 + rms_speed_error_ms: 0.0 + overshoot_ratio: 0.0 + settle_time_sec: 0.0 + stop_position_error_m: 0.0 + max_jerk: 0.0 + saturation_ratio: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_multi_steer_wheel_template.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_multi_steer_wheel_template.yaml new file mode 100644 index 0000000..e7cc688 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_multi_steer_wheel_template.yaml @@ -0,0 +1,39 @@ +# 多舵轮运控算法输出参数模板。 + +schema_version: 1 +session_id: replace_with_session_id +vehicle_id: replace_with_real_vehicle_id +chassis_type: multi_steer_wheel +control_axis: combined +controller_algorithm: pid +control_role: multi_loop_pid +parameter_version: replace_with_control_parameter_version + +estimated_params: + pid_loops: + - control_axis: longitudinal_control + control_role: wheel_speed_inner_loop + loop_name: module_wheel_speed + kp: 0.0 + ki: 0.0 + kd: 0.0 + - control_axis: lateral_control + control_role: module_steering_inner_loop + loop_name: module_steering_angle + kp: 0.0 + ki: 0.0 + kd: 0.0 + +quality: + data_quality_passed: false + suitable_for_commit: false + auto_acceptance_passed: false + validation_summary: + rms_lateral_error_m: 0.0 + rms_heading_error_rad: 0.0 + rms_speed_error_ms: 0.0 + overshoot_ratio: 0.0 + settle_time_sec: 0.0 + stop_position_error_m: 0.0 + max_jerk: 0.0 + saturation_ratio: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_single_steer_wheel_template.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_single_steer_wheel_template.yaml new file mode 100644 index 0000000..2abb807 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_single_steer_wheel_template.yaml @@ -0,0 +1,39 @@ +# 单舵轮运控算法输出参数模板。 + +schema_version: 1 +session_id: replace_with_session_id +vehicle_id: replace_with_real_vehicle_id +chassis_type: single_steer_wheel +control_axis: combined +controller_algorithm: pid +control_role: multi_loop_pid +parameter_version: replace_with_control_parameter_version + +estimated_params: + pid_loops: + - control_axis: longitudinal_control + control_role: speed_loop + loop_name: speed + kp: 0.0 + ki: 0.0 + kd: 0.0 + - control_axis: lateral_control + control_role: steering_angle_inner_loop + loop_name: steering_angle + kp: 0.0 + ki: 0.0 + kd: 0.0 + +quality: + data_quality_passed: false + suitable_for_commit: false + auto_acceptance_passed: false + validation_summary: + rms_lateral_error_m: 0.0 + rms_heading_error_rad: 0.0 + rms_speed_error_ms: 0.0 + overshoot_ratio: 0.0 + settle_time_sec: 0.0 + stop_position_error_m: 0.0 + max_jerk: 0.0 + saturation_ratio: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_template.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_template.yaml new file mode 100644 index 0000000..dbb1fee --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_estimated_params_template.yaml @@ -0,0 +1,96 @@ +# 运控算法输出参数模板。 +# 算法工程师在完成求解后复制本文件,填写真实参数和质量结论。 + +schema_version: 1 +session_id: replace_with_session_id +vehicle_id: replace_with_real_vehicle_id +chassis_type: ackermann +control_axis: combined +controller_algorithm: mpc +control_role: path_tracking_outer_loop +parameter_version: replace_with_control_parameter_version + +estimated_params: + # 单个 PID 回路。使用 PID 时必须明确 loop_name 和 control_axis。 + # loop_name 示例:speed / acceleration / heading / yaw_rate / steering_angle。 + pid: + control_axis: longitudinal_control + control_role: speed_loop + loop_name: speed + kp: 0.0 + ki: 0.0 + kd: 0.0 + has_integral_limit: false + integral_limit: 0.0 + has_output_limit: false + output_limit: 0.0 + # 如果 controller_algorithm=pid 且 control_axis=combined,应使用这个列表分别描述每个 PID 回路。 + pid_loops: + - control_axis: longitudinal_control + control_role: speed_loop + loop_name: speed + kp: 0.0 + ki: 0.0 + kd: 0.0 + - control_axis: lateral_control + control_role: heading_loop + loop_name: heading + kp: 0.0 + ki: 0.0 + kd: 0.0 + lateral_mpc: + prediction_horizon: 10 + control_horizon: 3 + model_dt_s: 0.05 + q_lateral: 1.0 + q_heading: 1.0 + r_steering: 1.0 + r_steering_rate: 1.0 + has_steering_limit_deg: false + steering_limit_deg: 0.0 + longitudinal_mpc: + prediction_horizon: 10 + control_horizon: 3 + model_dt_s: 0.05 + q_speed: 1.0 + q_accel: 1.0 + r_throttle: 1.0 + r_brake: 1.0 + r_jerk: 1.0 + has_throttle_limit: false + throttle_limit: 0.0 + has_brake_limit: false + brake_limit: 0.0 + lqr: + q_state_weights: + - 1.0 + - 1.0 + r_input_weights: + - 1.0 + has_preview_time_s: false + preview_time_s: 0.0 + pure_pursuit: + lookahead_m: 1.0 + has_min_lookahead_m: false + min_lookahead_m: 0.0 + has_max_lookahead_m: false + max_lookahead_m: 0.0 + has_curvature_gain: false + curvature_gain: 0.0 + has_steering_limit_deg: false + steering_limit_deg: 0.0 + +quality: + data_quality_passed: false + suitable_for_commit: false + auto_acceptance_passed: false + validation_summary: + rms_lateral_error_m: 0.0 + rms_heading_error_rad: 0.0 + rms_speed_error_ms: 0.0 + overshoot_ratio: 0.0 + settle_time_sec: 0.0 + stop_position_error_m: 0.0 + max_jerk: 0.0 + saturation_ratio: 0.0 + notes: replace_with_algorithm_notes diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml new file mode 100644 index 0000000..c5c3dc4 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml @@ -0,0 +1,383 @@ +# 运控评估现场任务配置模板。 +# 本文件固定四类底盘在真实标定车间中建议执行的运控评估序列。 +# 真实速度、轨迹长度和停车点必须按现场安全评估后再替换。 + +schema_version: 1 +profile_name: workshop_control_evaluation_profile + +safety: + max_linear_speed_ms: 0.2 + max_accel_ms2: 0.15 + default_external_pose_source_id: workshop_external_localization + +data_capture: + required_data_inputs: + - control_evaluation_data_files + - reference_signal_files + - chassis_response_files + - truth_trajectory_files + start_before_task_sec: 1.0 + stop_after_task_sec: 1.0 + min_control_telemetry_hz: 20.0 + min_chassis_telemetry_hz: 20.0 + min_truth_hz: 20.0 + +chassis_profiles: + ackermann: + description: 阿克曼底盘运控评估序列 + calibration_targets: + - ackermann.path_tracking_outer_loop + - ackermann.speed_loop + - ackermann.steering_angle_inner_loop + - ackermann.stop_accuracy + actions: + - task_code: control.ackermann.path_tracking_s_curve + display_name: 阿克曼低速 S 形轨迹跟踪 + selected_task: trajectory_tracking + control_axis: combined + controller_algorithm: pure_pursuit + control_role: path_tracking_outer_loop + trajectory_tracking: + trajectory_id: ackermann_s_curve_low_speed + stop_at_end: true + timeout_sec: 60.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + segment_index: 0 + total_segments: 1 + is_final_segment: true + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.10 + - x_m: 0.6 + y_m: 0.10 + yaw_rad: 0.12 + target_speed_ms: 0.12 + - x_m: 1.2 + y_m: -0.10 + yaw_rad: -0.12 + target_speed_ms: 0.12 + - x_m: 1.8 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.10 + - task_code: control.ackermann.speed_step_low + display_name: 阿克曼低速速度阶跃 + selected_task: velocity_step + control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + velocity_step: + target_velocity_ms: 0.15 + hold_time_sec: 8.0 + settle_before_step_sec: 2.0 + - task_code: control.ackermann.steering_response_arc + display_name: 阿克曼转角响应轨迹 + selected_task: trajectory_tracking + control_axis: lateral_control + controller_algorithm: pid + control_role: steering_angle_inner_loop + loop_name: steering_angle + trajectory_tracking: + trajectory_id: ackermann_steering_response_arc + stop_at_end: true + timeout_sec: 45.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 0.5 + y_m: 0.08 + yaw_rad: 0.15 + target_speed_ms: 0.08 + - x_m: 1.0 + y_m: 0.28 + yaw_rad: 0.30 + target_speed_ms: 0.08 + - task_code: control.ackermann.stop_accuracy + display_name: 阿克曼停车精度 + selected_task: stop_accuracy + control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + stop_accuracy: + target_stop_x_m: 1.2 + target_stop_y_m: 0.0 + target_stop_yaw_rad: 0.0 + timeout_sec: 45.0 + + differential: + description: 差速轮底盘运控评估序列 + calibration_targets: + - differential.path_tracking_outer_loop + - differential.speed_loop + - differential.yaw_rate_loop + - differential.acceleration_loop + actions: + - task_code: control.differential.path_tracking_line + display_name: 差速低速直线轨迹跟踪 + selected_task: trajectory_tracking + control_axis: combined + controller_algorithm: pure_pursuit + control_role: path_tracking_outer_loop + trajectory_tracking: + trajectory_id: differential_line_low_speed + stop_at_end: true + timeout_sec: 45.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.10 + - x_m: 0.8 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.12 + - x_m: 1.6 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.10 + - task_code: control.differential.speed_step_low + display_name: 差速低速速度阶跃 + selected_task: velocity_step + control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + velocity_step: + target_velocity_ms: 0.15 + hold_time_sec: 8.0 + settle_before_step_sec: 2.0 + - task_code: control.differential.yaw_rate_arc + display_name: 差速角速度响应圆弧 + selected_task: trajectory_tracking + control_axis: lateral_control + controller_algorithm: pid + control_role: yaw_rate_loop + loop_name: yaw_rate + trajectory_tracking: + trajectory_id: differential_yaw_rate_arc + stop_at_end: true + timeout_sec: 45.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 0.45 + y_m: 0.08 + yaw_rad: 0.18 + target_speed_ms: 0.08 + - x_m: 0.85 + y_m: 0.30 + yaw_rad: 0.36 + target_speed_ms: 0.08 + - task_code: control.differential.accel_decel_low + display_name: 差速低速加减速响应 + selected_task: acceleration_deceleration + control_axis: longitudinal_control + controller_algorithm: pid + control_role: acceleration_loop + loop_name: acceleration + accel_decel: + start_velocity_ms: 0.05 + target_velocity_ms: 0.16 + target_accel_ms2: 0.08 + hold_time_sec: 5.0 + + single_steer_wheel: + description: 单舵轮底盘运控评估序列 + calibration_targets: + - single_steer.path_tracking_outer_loop + - single_steer.speed_loop + - single_steer.steering_angle_inner_loop + - single_steer.stop_accuracy + actions: + - task_code: control.single_steer.path_tracking_arc + display_name: 单舵轮低速圆弧轨迹跟踪 + selected_task: trajectory_tracking + control_axis: combined + controller_algorithm: pure_pursuit + control_role: path_tracking_outer_loop + trajectory_tracking: + trajectory_id: single_steer_arc_low_speed + stop_at_end: true + timeout_sec: 45.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.09 + - x_m: 0.55 + y_m: 0.08 + yaw_rad: 0.12 + target_speed_ms: 0.10 + - x_m: 1.10 + y_m: 0.28 + yaw_rad: 0.24 + target_speed_ms: 0.09 + - task_code: control.single_steer.speed_step_low + display_name: 单舵轮低速速度阶跃 + selected_task: velocity_step + control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + velocity_step: + target_velocity_ms: 0.14 + hold_time_sec: 8.0 + settle_before_step_sec: 2.0 + - task_code: control.single_steer.steering_response_s_curve + display_name: 单舵轮舵角响应 S 形轨迹 + selected_task: trajectory_tracking + control_axis: lateral_control + controller_algorithm: pid + control_role: steering_angle_inner_loop + loop_name: steering_angle + trajectory_tracking: + trajectory_id: single_steer_steering_response_s_curve + stop_at_end: true + timeout_sec: 60.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 0.5 + y_m: 0.10 + yaw_rad: 0.12 + target_speed_ms: 0.08 + - x_m: 1.0 + y_m: -0.10 + yaw_rad: -0.12 + target_speed_ms: 0.08 + - x_m: 1.5 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - task_code: control.single_steer.stop_accuracy + display_name: 单舵轮停车精度 + selected_task: stop_accuracy + control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + stop_accuracy: + target_stop_x_m: 1.0 + target_stop_y_m: 0.0 + target_stop_yaw_rad: 0.0 + timeout_sec: 45.0 + + multi_steer_wheel: + description: 多舵轮底盘运控评估序列 + calibration_targets: + - multi_steer.path_tracking_outer_loop + - multi_steer.wheel_speed_inner_loop + - multi_steer.module_steering_inner_loop + - multi_steer.stop_accuracy + actions: + - task_code: control.multi_steer.path_tracking_lateral_offset + display_name: 多舵轮横向偏移轨迹跟踪 + selected_task: trajectory_tracking + control_axis: combined + controller_algorithm: mpc + control_role: path_tracking_outer_loop + trajectory_tracking: + trajectory_id: multi_steer_lateral_offset_low_speed + stop_at_end: true + timeout_sec: 60.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 0.4 + y_m: 0.2 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 0.8 + y_m: 0.2 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 1.2 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - task_code: control.multi_steer.wheel_speed_step_low + display_name: 多舵轮轮速阶跃 + selected_task: velocity_step + control_axis: longitudinal_control + controller_algorithm: pid + control_role: wheel_speed_inner_loop + loop_name: module_wheel_speed + velocity_step: + target_velocity_ms: 0.12 + hold_time_sec: 8.0 + settle_before_step_sec: 2.0 + - task_code: control.multi_steer.module_steering_response + display_name: 多舵轮模块转角响应轨迹 + selected_task: trajectory_tracking + control_axis: lateral_control + controller_algorithm: pid + control_role: module_steering_inner_loop + loop_name: module_steering_angle + trajectory_tracking: + trajectory_id: multi_steer_module_steering_response + stop_at_end: true + timeout_sec: 60.0 + required_external_pose_source_id: workshop_external_localization + max_external_pose_age_ms: 100.0 + min_external_pose_quality_score: 0.7 + path: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - x_m: 0.3 + y_m: 0.18 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - x_m: 0.6 + y_m: -0.18 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - x_m: 0.9 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - task_code: control.multi_steer.stop_accuracy + display_name: 多舵轮停车精度 + selected_task: stop_accuracy + control_axis: longitudinal_control + controller_algorithm: pid + control_role: wheel_speed_inner_loop + loop_name: module_wheel_speed + stop_accuracy: + target_stop_x_m: 0.8 + target_stop_y_m: 0.0 + target_stop_yaw_rad: 0.0 + timeout_sec: 45.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_role_profiles.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_role_profiles.yaml new file mode 100644 index 0000000..2dd0b88 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/config/control_role_profiles.yaml @@ -0,0 +1,82 @@ +# 运控参数标定默认回路配置。 +# 该文件只定义每类底盘推荐标定哪些控制回路,具体算法仍由算法工程师实现。 + +schema_version: 1 + +profiles: + differential: + description: 差速轮底盘 + recommended_roles: + - control_axis: combined + controller_algorithm: pure_pursuit + control_role: path_tracking_outer_loop + purpose: 路径跟踪外环,生成线速度和角速度参考。 + - control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + purpose: 整车线速度回路。 + - control_axis: lateral_control + controller_algorithm: pid + control_role: yaw_rate_loop + loop_name: yaw_rate + purpose: 角速度回路。 + - control_axis: longitudinal_control + controller_algorithm: pid + control_role: wheel_speed_inner_loop + loop_name: left_right_wheel_speed + purpose: 左右轮速度内环。 + + single_steer_wheel: + description: 单舵轮底盘 + recommended_roles: + - control_axis: combined + controller_algorithm: pure_pursuit + control_role: path_tracking_outer_loop + purpose: 路径跟踪外环,生成目标速度和目标舵角。 + - control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + purpose: 驱动轮速度或整车速度回路。 + - control_axis: lateral_control + controller_algorithm: pid + control_role: steering_angle_inner_loop + loop_name: steering_angle + purpose: 舵角位置内环。 + + multi_steer_wheel: + description: 多舵轮底盘 + recommended_roles: + - control_axis: combined + controller_algorithm: mpc + control_role: path_tracking_outer_loop + purpose: 整车路径跟踪外环,生成整车速度、角速度或模块目标。 + - control_axis: longitudinal_control + controller_algorithm: pid + control_role: wheel_speed_inner_loop + loop_name: module_wheel_speed + purpose: 各驱动轮速度内环。 + - control_axis: lateral_control + controller_algorithm: pid + control_role: module_steering_inner_loop + loop_name: module_steering_angle + purpose: 各舵轮模块角度内环。 + + ackermann: + description: 阿克曼底盘 + recommended_roles: + - control_axis: combined + controller_algorithm: pure_pursuit + control_role: path_tracking_outer_loop + purpose: 路径跟踪外环,生成目标曲率或前轮转角。 + - control_axis: longitudinal_control + controller_algorithm: pid + control_role: speed_loop + loop_name: speed + purpose: 纵向速度回路。 + - control_axis: lateral_control + controller_algorithm: pid + control_role: steering_angle_inner_loop + loop_name: steering_angle + purpose: 转角执行器内环。 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_csv_contract.md b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_csv_contract.md new file mode 100644 index 0000000..f0548c3 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_csv_contract.md @@ -0,0 +1,123 @@ +# 运控标定现场 CSV 协议 + +本协议是现场运控参数标定数据落盘的固定格式。`replay_control_smoke.py` 会在写入 `dataset_index.yaml` 前校验这些列,真实采集程序也应输出相同格式。 + +## control/control_eval.csv + +用途:记录控制器执行评估任务时的目标、实测、控制输出和误差。 + +`chassis_type` 取值:`ackermann`、`differential`、`single_steer_wheel`、`multi_steer_wheel`。 + +`task_type` 取值:`trajectory_tracking`、`velocity_step`、`acceleration_deceleration`、`stop_accuracy`。 + +`control_axis` 取值:`lateral_control`、`longitudinal_control`、`combined`。 + +`controller_algorithm` 取值:`pid`、`mpc`、`lqr`、`pure_pursuit`。 + +`control_role` 取值:`path_tracking_outer_loop`、`speed_loop`、`acceleration_loop`、`heading_loop`、`yaw_rate_loop`、`steering_angle_inner_loop`、`module_steering_inner_loop`、`wheel_speed_inner_loop`、`multi_loop_pid`。 + +约束:`pid` 可以配合 `lateral_control` 或 `longitudinal_control`,但参数输出中必须通过 `control_role` 和 `loop_name` 明确具体回路,例如 `speed_loop/speed`、`heading_loop/heading`、`yaw_rate_loop/yaw_rate` 或 `steering_angle_inner_loop/steering_angle`。 + +必需列: + +- `hardware_timestamp_us` +- `chassis_type` +- `task_type` +- `control_axis` +- `controller_algorithm` +- `control_role` +- `target_x_m` +- `target_y_m` +- `target_yaw_rad` +- `target_velocity_ms` +- `measured_x_m` +- `measured_y_m` +- `measured_yaw_rad` +- `measured_velocity_ms` +- `lateral_error_m` +- `heading_error_rad` +- `speed_error_ms` +- `control_output_steer_rad` +- `control_output_accel_ms2` +- `control_output_brake` +- `saturation_flag` +- `active_job_id` +- `diagnostics_json` + +## control/reference_signal.csv + +用途:记录本次评估任务的参考轨迹、参考速度、参考加速度和停车要求。 + +必需列: + +- `reference_timestamp_us` +- `task_type` +- `sequence_index` +- `target_x_m` +- `target_y_m` +- `target_yaw_rad` +- `target_velocity_ms` +- `target_accel_ms2` +- `curvature_1pm` +- `stop_required` +- `active_job_id` + +## control/chassis_response.csv + +用途:记录执行运控评估时的底盘实际响应,算法用它区分控制器问题和底盘执行问题。 + +必需列: + +- `hardware_timestamp_us` +- `odom_x_m` +- `odom_y_m` +- `odom_yaw_rad` +- `linear_velocity_ms` +- `angular_velocity_rads` +- `steering_angle_rad` +- `accel_ms2` +- `brake_state` +- `driver_error_code` +- `active_job_id` + +## external/truth_trajectory.csv + +用途:记录车间外部真值定位输出的车辆轨迹,用于控制误差验收和时序对齐。 + +必需列: + +- `hardware_timestamp_us` +- `pose_valid` +- `x_m` +- `y_m` +- `z_m` +- `roll_rad` +- `pitch_rad` +- `yaw_rad` +- `position_stddev_m` +- `yaw_stddev_rad` +- `tracking_loss_ratio` +- `time_sync_offset_ms` +- `quality_score` +- `observed_target_count` +- `reference_source_name` +- `active_job_id` + +## dataset_index.yaml 写入字段 + +运控数据应写入: + +```yaml +data_inputs: + control: + control_evaluation_data_files: + - control/control_eval.csv + reference_signal_files: + - control/reference_signal.csv + chassis_response_files: + - control/chassis_response.csv + truth_trajectory_files: + - external/truth_trajectory.csv +``` + +会话收尾工具会把这些字段转换成 `control_calibration_service` 的 `data_input.*` ROS 参数。 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_data_common.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_data_common.py new file mode 100644 index 0000000..54914f0 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_data_common.py @@ -0,0 +1,666 @@ +#!/usr/bin/env python3 +"""运控标定现场数据配置、检查和数据集索引工具。""" + +from __future__ import annotations + +import csv +import json +import math +import shutil +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import yaml + + +CONTROL_CSV_CONTRACT_VERSION = 1 +CHASSIS_TYPES = {"ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"} +CONTROL_AXES = {"lateral_control", "longitudinal_control", "combined"} +CONTROLLER_ALGORITHMS = {"pid", "mpc", "lqr", "pure_pursuit"} +CONTROL_ROLES = { + "path_tracking_outer_loop", + "speed_loop", + "acceleration_loop", + "heading_loop", + "yaw_rate_loop", + "steering_angle_inner_loop", + "module_steering_inner_loop", + "wheel_speed_inner_loop", + "multi_loop_pid", +} +TASK_TYPES = { + "trajectory_tracking", + "velocity_step", + "acceleration_deceleration", + "stop_accuracy", +} +PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx") + +CONTROL_EVALUATION_FIELDS = [ + "hardware_timestamp_us", + "chassis_type", + "task_type", + "control_axis", + "controller_algorithm", + "control_role", + "target_x_m", + "target_y_m", + "target_yaw_rad", + "target_velocity_ms", + "measured_x_m", + "measured_y_m", + "measured_yaw_rad", + "measured_velocity_ms", + "lateral_error_m", + "heading_error_rad", + "speed_error_ms", + "control_output_steer_rad", + "control_output_accel_ms2", + "control_output_brake", + "saturation_flag", + "active_job_id", + "diagnostics_json", +] + +REFERENCE_SIGNAL_FIELDS = [ + "reference_timestamp_us", + "task_type", + "sequence_index", + "target_x_m", + "target_y_m", + "target_yaw_rad", + "target_velocity_ms", + "target_accel_ms2", + "curvature_1pm", + "stop_required", + "active_job_id", +] + +CHASSIS_RESPONSE_FIELDS = [ + "hardware_timestamp_us", + "odom_x_m", + "odom_y_m", + "odom_yaw_rad", + "linear_velocity_ms", + "angular_velocity_rads", + "steering_angle_rad", + "accel_ms2", + "brake_state", + "driver_error_code", + "active_job_id", +] + +TRUTH_TRAJECTORY_FIELDS = [ + "hardware_timestamp_us", + "pose_valid", + "x_m", + "y_m", + "z_m", + "roll_rad", + "pitch_rad", + "yaw_rad", + "position_stddev_m", + "yaw_stddev_rad", + "tracking_loss_ratio", + "time_sync_offset_ms", + "quality_score", + "observed_target_count", + "reference_source_name", + "active_job_id", +] + +INTEGER_FIELDS_BY_TABLE = { + "control_evaluation": {"hardware_timestamp_us"}, + "reference_signal": {"reference_timestamp_us", "sequence_index"}, + "chassis_response": {"hardware_timestamp_us", "driver_error_code"}, + "truth_trajectory": {"hardware_timestamp_us", "observed_target_count"}, +} + +FLOAT_FIELDS_BY_TABLE = { + "control_evaluation": { + "target_x_m", + "target_y_m", + "target_yaw_rad", + "target_velocity_ms", + "measured_x_m", + "measured_y_m", + "measured_yaw_rad", + "measured_velocity_ms", + "lateral_error_m", + "heading_error_rad", + "speed_error_ms", + "control_output_steer_rad", + "control_output_accel_ms2", + "control_output_brake", + }, + "reference_signal": { + "target_x_m", + "target_y_m", + "target_yaw_rad", + "target_velocity_ms", + "target_accel_ms2", + "curvature_1pm", + }, + "chassis_response": { + "odom_x_m", + "odom_y_m", + "odom_yaw_rad", + "linear_velocity_ms", + "angular_velocity_rads", + "steering_angle_rad", + "accel_ms2", + }, + "truth_trajectory": { + "x_m", + "y_m", + "z_m", + "roll_rad", + "pitch_rad", + "yaw_rad", + "position_stddev_m", + "yaw_stddev_rad", + "tracking_loss_ratio", + "time_sync_offset_ms", + "quality_score", + }, +} + +BOOLEAN_FIELDS_BY_TABLE = { + "control_evaluation": {"saturation_flag"}, + "reference_signal": {"stop_required"}, + "truth_trajectory": {"pose_valid"}, +} + +CSV_CONTRACTS = { + "control_evaluation": { + "fields": CONTROL_EVALUATION_FIELDS, + "json_fields": {"diagnostics_json"}, + }, + "reference_signal": { + "fields": REFERENCE_SIGNAL_FIELDS, + "json_fields": set(), + }, + "chassis_response": { + "fields": CHASSIS_RESPONSE_FIELDS, + "json_fields": set(), + }, + "truth_trajectory": { + "fields": TRUTH_TRAJECTORY_FIELDS, + "json_fields": set(), + }, +} + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def load_config(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) or {} + if not isinstance(data, dict): + raise ValueError(f"配置文件必须是 YAML 字典: {path}") + return data + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def has_placeholder(value: Any) -> bool: + return isinstance(value, str) and any(marker in value for marker in PLACEHOLDER_MARKERS) + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def resolve_path(root: Path, raw_value: str) -> Path: + path = Path(raw_value).expanduser() + if path.is_absolute(): + return path + return root / path + + +@dataclass +class ValidationResult: + errors: list[str] = field(default_factory=list) + warnings: list[str] = field(default_factory=list) + + @property + def ok(self) -> bool: + return not self.errors + + def extend(self, other: "ValidationResult") -> None: + self.errors.extend(other.errors) + self.warnings.extend(other.warnings) + + +def is_bool_token(value: Any) -> bool: + return str(value).strip().lower() in {"0", "1", "true", "false"} + + +def validate_csv_contract(path: Path, table_name: str) -> ValidationResult: + result = ValidationResult() + contract = CSV_CONTRACTS.get(table_name) + if contract is None: + result.errors.append(f"未知 CSV 协议名称: {table_name}") + return result + if not path.exists(): + result.errors.append(f"{table_name} 文件不存在: {path}") + return result + + required_fields = list(contract["fields"]) + integer_fields = INTEGER_FIELDS_BY_TABLE.get(table_name, set()) + float_fields = FLOAT_FIELDS_BY_TABLE.get(table_name, set()) + boolean_fields = BOOLEAN_FIELDS_BY_TABLE.get(table_name, set()) + json_fields = contract.get("json_fields", set()) + + with path.open("r", newline="", encoding="utf-8") as stream: + reader = csv.DictReader(stream) + fieldnames = reader.fieldnames or [] + for field_name in required_fields: + if field_name not in fieldnames: + result.errors.append(f"{table_name} 缺少必需列: {field_name}") + if result.errors: + return result + + row_count = 0 + for row_number, row in enumerate(reader, start=2): + row_count += 1 + for field_name in integer_fields: + try: + int(str(row.get(field_name, "")).strip()) + except (TypeError, ValueError): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是整数。") + for field_name in float_fields: + try: + float(str(row.get(field_name, "")).strip()) + except (TypeError, ValueError): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是浮点数。") + for field_name in boolean_fields: + if not is_bool_token(row.get(field_name, "")): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是 0/1 或 true/false。") + for field_name in json_fields: + try: + parsed = json.loads(str(row.get(field_name, "")).strip()) + except json.JSONDecodeError: + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是合法 JSON。") + continue + if field_name == "diagnostics_json" and not isinstance(parsed, dict): + result.errors.append(f"{table_name} 第 {row_number} 行 {field_name} 必须是 JSON 对象。") + if table_name == "control_evaluation": + if str(row.get("chassis_type", "")).strip() not in CHASSIS_TYPES: + result.errors.append(f"{table_name} 第 {row_number} 行 chassis_type 必须是已支持的底盘类型。") + row_control_axis = str(row.get("control_axis", "")).strip() + row_controller_algorithm = str(row.get("controller_algorithm", "")).strip() + if row_control_axis not in CONTROL_AXES: + result.errors.append(f"{table_name} 第 {row_number} 行 control_axis 必须是已支持的控制轴。") + if row_controller_algorithm not in CONTROLLER_ALGORITHMS: + result.errors.append(f"{table_name} 第 {row_number} 行 controller_algorithm 必须是已支持的控制算法。") + if str(row.get("control_role", "")).strip() not in CONTROL_ROLES: + result.errors.append(f"{table_name} 第 {row_number} 行 control_role 必须是已支持的控制回路角色。") + if table_name in {"control_evaluation", "reference_signal"}: + if str(row.get("task_type", "")).strip() not in TASK_TYPES: + result.errors.append(f"{table_name} 第 {row_number} 行 task_type 必须是已支持的运控评估任务。") + if row_count == 0: + result.warnings.append(f"{table_name} 只有表头,没有数据行。") + return result + + +def validate_control_dataset_files(paths: dict[str, Path]) -> ValidationResult: + result = ValidationResult() + checks = [ + ("control_evaluation", "control_evaluation_data_file"), + ("reference_signal", "reference_signal_file"), + ("chassis_response", "chassis_response_file"), + ("truth_trajectory", "truth_trajectory_file"), + ] + for table_name, path_key in checks: + result.extend(validate_csv_contract(paths[path_key], table_name)) + return result + + +def validate_config(config: dict[str, Any], config_path: Path | None = None) -> ValidationResult: + result = ValidationResult() + + csv_contract_version = int(config.get("csv_contract_version", CONTROL_CSV_CONTRACT_VERSION) or 0) + if csv_contract_version != CONTROL_CSV_CONTRACT_VERSION: + result.errors.append(f"csv_contract_version 必须为 {CONTROL_CSV_CONTRACT_VERSION}。") + + if str(config.get("chassis_type", "")).strip() not in CHASSIS_TYPES: + result.errors.append(f"chassis_type 必须是 {sorted(CHASSIS_TYPES)} 之一。") + if str(config.get("control_axis", "")).strip() not in CONTROL_AXES: + result.errors.append(f"control_axis 必须是 {sorted(CONTROL_AXES)} 之一。") + if str(config.get("controller_algorithm", "")).strip() not in CONTROLLER_ALGORITHMS: + result.errors.append(f"controller_algorithm 必须是 {sorted(CONTROLLER_ALGORITHMS)} 之一。") + if str(config.get("control_role", "")).strip() not in CONTROL_ROLES: + result.errors.append(f"control_role 必须是 {sorted(CONTROL_ROLES)} 之一。") + + for field_name in ("session_id", "site_id", "vehicle_id", "session_dir", "dataset_index_path"): + value = str(config.get(field_name, "")).strip() + if not value: + result.errors.append(f"{field_name} 不能为空。") + elif has_placeholder(value): + result.errors.append(f"{field_name} 不能保留模板占位符。") + + files = config.get("files", {}) + if not isinstance(files, dict): + result.errors.append("files 必须是 YAML 字典。") + files = {} + for field_name in ( + "control_evaluation_data_file", + "reference_signal_file", + "chassis_response_file", + "truth_trajectory_file", + "diagnostics_file", + ): + value = str(files.get(field_name, "")).strip() + if not value: + result.errors.append(f"files.{field_name} 不能为空。") + elif has_placeholder(value): + result.errors.append(f"files.{field_name} 不能保留模板占位符。") + + validation = config.get("validation", {}) or {} + if not isinstance(validation, dict): + result.errors.append("validation 必须是 YAML 字典。") + validation = {} + for field_name in ( + "min_control_evaluation_samples", + "min_reference_signal_samples", + "min_chassis_response_samples", + "min_truth_trajectory_samples", + ): + if int(validation.get(field_name, 0) or 0) <= 0: + result.errors.append(f"validation.{field_name} 必须大于 0。") + if float(validation.get("max_time_gap_ms", 0.0) or 0.0) <= 0.0: + result.errors.append("validation.max_time_gap_ms 必须大于 0。") + + if config_path is not None and not config_path.exists(): + result.errors.append(f"配置文件不存在: {config_path}") + return result + + +def apply_cli_overrides(config: dict[str, Any], args: Any) -> None: + for attr_name in ("session_id", "site_id", "vehicle_id", "session_dir", "dataset_index_path"): + value = getattr(args, attr_name, "") + if value: + config[attr_name] = value + if getattr(args, "control_axis", ""): + config["control_axis"] = args.control_axis + if getattr(args, "chassis_type", ""): + config["chassis_type"] = args.chassis_type + if getattr(args, "controller_algorithm", ""): + config["controller_algorithm"] = args.controller_algorithm + if getattr(args, "control_role", ""): + config["control_role"] = args.control_role + + +def read_csv_rows(path: Path) -> list[dict[str, str]]: + with path.open("r", newline="", encoding="utf-8") as stream: + return list(csv.DictReader(stream)) + + +def timestamp_us(row: dict[str, Any]) -> int: + for key in ("hardware_timestamp_us", "reference_timestamp_us", "timestamp_us"): + value = row.get(key) + if value not in (None, ""): + return int(float(value)) + return 0 + + +def numeric(row: dict[str, Any], key: str, default: float = 0.0) -> float: + value = row.get(key) + if value in (None, ""): + return default + return float(value) + + +def rms(rows: list[dict[str, str]], key: str) -> float: + if not rows: + return 0.0 + return math.sqrt(sum(numeric(row, key) ** 2 for row in rows) / len(rows)) + + +def max_timestamp_gap_ms(rows: list[dict[str, str]]) -> float: + stamps = sorted(timestamp_us(row) for row in rows if timestamp_us(row) > 0) + if len(stamps) < 2: + return 0.0 + return max((b - a) / 1000.0 for a, b in zip(stamps, stamps[1:])) + + +def boolish(value: Any) -> bool: + return str(value).strip().lower() in {"1", "true"} + + +def write_csv(path: Path, fieldnames: list[str], rows: list[dict[str, Any]]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", newline="", encoding="utf-8") as stream: + writer = csv.DictWriter(stream, fieldnames=fieldnames) + writer.writeheader() + for row in rows: + writer.writerow(row) + + +def copy_or_generate_file(source: str, target: Path, fieldnames: list[str], rows: list[dict[str, Any]]) -> Path: + target.parent.mkdir(parents=True, exist_ok=True) + if source: + shutil.copyfile(Path(source).expanduser().resolve(strict=False), target) + else: + write_csv(target, fieldnames, rows) + return target + + +def synthetic_rows( + config: dict[str, Any], +) -> tuple[list[dict[str, Any]], list[dict[str, Any]], list[dict[str, Any]], list[dict[str, Any]]]: + synthetic = config.get("synthetic", {}) or {} + start_us = now_us() + period_us = int(float(synthetic.get("sample_period_ms", 50.0) or 50.0) * 1000.0) + duration_sec = float(synthetic.get("duration_sec", 6.0) or 6.0) + target_speed = float(synthetic.get("target_speed_ms", 0.2) or 0.2) + lateral_error = float(synthetic.get("lateral_error_m", 0.015) or 0.015) + heading_error = float(synthetic.get("heading_error_rad", 0.01) or 0.01) + speed_error = float(synthetic.get("speed_error_ms", 0.02) or 0.02) + sample_count = max(3, int(duration_sec / max(period_us / 1e6, 1e-6)) + 1) + + task_type = str(synthetic.get("task_type", "trajectory_tracking") or "trajectory_tracking") + chassis_type = str(config.get("chassis_type", "ackermann") or "ackermann") + control_axis = str(config.get("control_axis", "combined") or "combined") + algorithm = str(config.get("controller_algorithm", "mpc") or "mpc") + control_role = str(config.get("control_role", "path_tracking_outer_loop") or "path_tracking_outer_loop") + active_job_id = str(config.get("session_id", "session") or "session") + + control_rows: list[dict[str, Any]] = [] + reference_rows: list[dict[str, Any]] = [] + chassis_rows: list[dict[str, Any]] = [] + truth_rows: list[dict[str, Any]] = [] + + for index in range(sample_count): + stamp = start_us + index * period_us + t_sec = index * period_us / 1e6 + target_x = target_speed * t_sec + target_y = 0.0 + measured_x = target_x - speed_error * t_sec + measured_y = lateral_error + measured_yaw = heading_error + measured_speed = max(0.0, target_speed - speed_error) + + control_rows.append({ + "hardware_timestamp_us": stamp, + "chassis_type": chassis_type, + "task_type": task_type, + "control_axis": control_axis, + "controller_algorithm": algorithm, + "control_role": control_role, + "target_x_m": target_x, + "target_y_m": target_y, + "target_yaw_rad": 0.0, + "target_velocity_ms": target_speed, + "measured_x_m": measured_x, + "measured_y_m": measured_y, + "measured_yaw_rad": measured_yaw, + "measured_velocity_ms": measured_speed, + "lateral_error_m": lateral_error, + "heading_error_rad": heading_error, + "speed_error_ms": speed_error, + "control_output_steer_rad": 0.02, + "control_output_accel_ms2": 0.0, + "control_output_brake": 0.0, + "saturation_flag": 0, + "active_job_id": active_job_id, + "diagnostics_json": "{}", + }) + reference_rows.append({ + "reference_timestamp_us": stamp, + "task_type": task_type, + "sequence_index": index, + "target_x_m": target_x, + "target_y_m": target_y, + "target_yaw_rad": 0.0, + "target_velocity_ms": target_speed, + "target_accel_ms2": 0.0, + "curvature_1pm": 0.0, + "stop_required": 0, + "active_job_id": active_job_id, + }) + chassis_rows.append({ + "hardware_timestamp_us": stamp, + "odom_x_m": measured_x, + "odom_y_m": measured_y, + "odom_yaw_rad": measured_yaw, + "linear_velocity_ms": measured_speed, + "angular_velocity_rads": 0.0, + "steering_angle_rad": 0.02, + "accel_ms2": 0.0, + "brake_state": 0, + "driver_error_code": 0, + "active_job_id": active_job_id, + }) + truth_rows.append({ + "hardware_timestamp_us": stamp, + "pose_valid": 1, + "x_m": measured_x, + "y_m": measured_y, + "z_m": 0.0, + "roll_rad": 0.0, + "pitch_rad": 0.0, + "yaw_rad": measured_yaw, + "position_stddev_m": 0.005, + "yaw_stddev_rad": 0.002, + "tracking_loss_ratio": 0.0, + "time_sync_offset_ms": 0.0, + "quality_score": 1.0, + "observed_target_count": 4, + "reference_source_name": "synthetic_control_smoke", + "active_job_id": active_job_id, + }) + + return control_rows, reference_rows, chassis_rows, truth_rows + + +def build_summary( + control_rows: list[dict[str, str]], + reference_rows: list[dict[str, str]], + chassis_rows: list[dict[str, str]], + truth_rows: list[dict[str, str]], + config: dict[str, Any], +) -> dict[str, Any]: + stamps = [ + timestamp_us(row) + for row in control_rows + reference_rows + chassis_rows + truth_rows + if timestamp_us(row) > 0 + ] + start_us = min(stamps) if stamps else now_us() + end_us = max(stamps) if stamps else start_us + 1 + saturation_count = sum(1 for row in control_rows if boolish(row.get("saturation_flag", "0"))) + return { + "control_csv_contract_version": CONTROL_CSV_CONTRACT_VERSION, + "session_id": config.get("session_id", ""), + "site_id": config.get("site_id", ""), + "vehicle_id": config.get("vehicle_id", ""), + "chassis_type": config.get("chassis_type", ""), + "control_axis": config.get("control_axis", ""), + "controller_algorithm": config.get("controller_algorithm", ""), + "control_role": config.get("control_role", ""), + "data_window_start_timestamp_us": int(start_us), + "data_window_end_timestamp_us": int(max(end_us, start_us + 1)), + "control_evaluation_sample_count": len(control_rows), + "reference_signal_sample_count": len(reference_rows), + "chassis_response_sample_count": len(chassis_rows), + "truth_trajectory_sample_count": len(truth_rows), + "rms_lateral_error_m": rms(control_rows, "lateral_error_m"), + "rms_heading_error_rad": rms(control_rows, "heading_error_rad"), + "rms_speed_error_ms": rms(control_rows, "speed_error_ms"), + "saturation_ratio": saturation_count / len(control_rows) if control_rows else 0.0, + "max_control_timestamp_gap_ms": max_timestamp_gap_ms(control_rows), + "max_truth_timestamp_gap_ms": max_timestamp_gap_ms(truth_rows), + } + + +def validate_summary(summary: dict[str, Any], config: dict[str, Any]) -> ValidationResult: + result = ValidationResult() + validation = config.get("validation", {}) or {} + checks = [ + ("control_evaluation_sample_count", "min_control_evaluation_samples", "运控评估样本数不足"), + ("reference_signal_sample_count", "min_reference_signal_samples", "参考信号样本数不足"), + ("chassis_response_sample_count", "min_chassis_response_samples", "底盘响应样本数不足"), + ("truth_trajectory_sample_count", "min_truth_trajectory_samples", "真值轨迹样本数不足"), + ] + for summary_key, config_key, message in checks: + if int(summary.get(summary_key, 0)) < int(validation.get(config_key, 0) or 0): + result.errors.append(f"{message}: {summary.get(summary_key, 0)}") + max_gap = float(validation.get("max_time_gap_ms", 200.0) or 200.0) + if float(summary.get("max_control_timestamp_gap_ms", 0.0)) > max_gap: + result.warnings.append("运控评估数据时间间隔超过配置阈值。") + if float(summary.get("max_truth_timestamp_gap_ms", 0.0)) > max_gap: + result.warnings.append("真值轨迹时间间隔超过配置阈值。") + return result + + +def build_dataset_index( + config: dict[str, Any], + summary: dict[str, Any], + paths: dict[str, Path], + config_path: Path, +) -> dict[str, Any]: + session_dir = Path(str(config["session_dir"])).expanduser().resolve(strict=False) + return { + "schema_version": 1, + "session": { + "session_id": str(config["session_id"]), + "site_id": str(config["site_id"]), + "vehicle_id": str(config["vehicle_id"]), + "dataset_root": str(session_dir), + "data_window_start_timestamp_us": int(summary["data_window_start_timestamp_us"]), + "data_window_end_timestamp_us": int(summary["data_window_end_timestamp_us"]), + }, + "data_inputs": { + "control": { + "control_evaluation_data_files": [ + relative_to_root(paths["control_evaluation_data_file"], session_dir) + ], + "reference_signal_files": [ + relative_to_root(paths["reference_signal_file"], session_dir) + ], + "chassis_response_files": [ + relative_to_root(paths["chassis_response_file"], session_dir) + ], + "truth_trajectory_files": [ + relative_to_root(paths["truth_trajectory_file"], session_dir) + ], + } + }, + "metadata": { + "control_csv_contract_version": CONTROL_CSV_CONTRACT_VERSION, + "control_data_config_file": str(config_path), + "control_diagnostics_file": relative_to_root(paths["diagnostics_file"], session_dir), + "control_replay_summary": summary, + }, + } diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py new file mode 100644 index 0000000..36c9b2a --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py @@ -0,0 +1,481 @@ +#!/usr/bin/env python3 +"""校验运控评估现场任务配置,并导出总控任务片段。""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + +SCRIPT_DIR = Path(__file__).resolve().parent +if str(SCRIPT_DIR) not in sys.path: + sys.path.insert(0, str(SCRIPT_DIR)) + +from stage_control_parameter_commit import ( + CHASSIS_TYPES, + CONTROL_AXES, + CONTROLLER_ALGORITHMS, + CONTROL_ROLES, + validate_control_role, +) + + +TASK_TYPES = { + "trajectory_tracking", + "velocity_step", + "acceleration_deceleration", + "accel_decel", + "stop_accuracy", +} + +TASK_TYPE_TO_METADATA = { + "trajectory_tracking": "trajectory_tracking", + "velocity_step": "velocity_step", + "acceleration_deceleration": "accel_decel", + "accel_decel": "accel_decel", + "stop_accuracy": "stop_accuracy", +} + +TASK_PAYLOAD_KEYS = { + "trajectory_tracking": "trajectory_tracking", + "velocity_step": "velocity_step", + "acceleration_deceleration": "accel_decel", + "accel_decel": "accel_decel", + "stop_accuracy": "stop_accuracy", +} + +TRAJECTORY_POINT_KEYS = ["x_m", "y_m", "yaw_rad", "target_speed_ms"] + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="校验并导出运控评估现场 profile。") + parser.add_argument( + "--profile", + default="src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml", + help="运控评估 profile YAML 路径。", + ) + parser.add_argument( + "--chassis-type", + choices=sorted(CHASSIS_TYPES), + help="只导出指定底盘类型;不填写时只做整表校验和摘要输出。", + ) + parser.add_argument( + "--format", + choices=["summary", "requested_tasks"], + default="summary", + help="输出格式:summary 为摘要,requested_tasks 为总控任务片段。", + ) + parser.add_argument("-o", "--output", help="输出文件路径;不填写时输出到标准输出。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + if data is None: + return {} + if not isinstance(data, dict): + raise ValueError(f"{path} 的顶层结构必须是 YAML map。") + return data + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_list(value: Any, field_name: str) -> list[Any]: + if not isinstance(value, list): + raise ValueError(f"{field_name} 必须是 YAML list。") + return value + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def as_float(value: Any, field_name: str) -> float: + try: + return float(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是数字,当前值为 {value!r}。") from exc + + +def as_int(value: Any, field_name: str) -> int: + try: + return int(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是整数,当前值为 {value!r}。") from exc + + +def stringify_value(value: Any) -> str: + if isinstance(value, bool): + return "true" if value else "false" + if isinstance(value, list): + return ",".join(stringify_value(item) for item in value) + return str(value) + + +def read_safety(profile: dict[str, Any]) -> dict[str, float]: + safety = require_map(profile.get("safety", {}), "safety") + max_linear_speed_ms = as_float(safety.get("max_linear_speed_ms", 0.0), "safety.max_linear_speed_ms") + max_accel_ms2 = as_float(safety.get("max_accel_ms2", 0.0), "safety.max_accel_ms2") + if max_linear_speed_ms < 0.0: + raise ValueError("safety.max_linear_speed_ms 不能小于 0。") + if max_accel_ms2 < 0.0: + raise ValueError("safety.max_accel_ms2 不能小于 0。") + return { + "max_linear_speed_ms": max_linear_speed_ms, + "max_accel_ms2": max_accel_ms2, + } + + +def validate_non_negative(value: Any, field_name: str) -> float: + number = as_float(value, field_name) + if number < 0.0: + raise ValueError(f"{field_name} 不能小于 0。") + return number + + +def validate_positive(value: Any, field_name: str) -> float: + number = as_float(value, field_name) + if number <= 0.0: + raise ValueError(f"{field_name} 必须大于 0。") + return number + + +def validate_speed(value: Any, field_name: str, safety: dict[str, float]) -> float: + speed = validate_non_negative(value, field_name) + max_linear_speed_ms = safety["max_linear_speed_ms"] + if max_linear_speed_ms > 0.0 and speed > max_linear_speed_ms: + raise ValueError(f"{field_name}={speed} 超过安全上限 {max_linear_speed_ms}。") + return speed + + +def validate_accel(value: Any, field_name: str, safety: dict[str, float]) -> float: + accel = as_float(value, field_name) + max_accel_ms2 = safety["max_accel_ms2"] + if max_accel_ms2 > 0.0 and abs(accel) > max_accel_ms2: + raise ValueError(f"{field_name}={accel} 超过安全上限 {max_accel_ms2}。") + if accel == 0.0: + raise ValueError(f"{field_name} 不能为 0。") + return accel + + +def validate_task_identity( + action: dict[str, Any], + action_name: str, + chassis_type: str, +) -> tuple[str, str, str, str]: + selected_task = require_string(action.get("selected_task"), f"{action_name}.selected_task") + if selected_task not in TASK_TYPES: + raise ValueError(f"{action_name}.selected_task 不受支持:{selected_task}") + + control_axis = require_string(action.get("control_axis"), f"{action_name}.control_axis") + if control_axis not in CONTROL_AXES: + raise ValueError(f"{action_name}.control_axis 必须是 {sorted(CONTROL_AXES)} 之一。") + + controller_algorithm = require_string( + action.get("controller_algorithm"), + f"{action_name}.controller_algorithm", + ) + if controller_algorithm not in CONTROLLER_ALGORITHMS: + raise ValueError( + f"{action_name}.controller_algorithm 必须是 {sorted(CONTROLLER_ALGORITHMS)} 之一。" + ) + + control_role = require_string(action.get("control_role"), f"{action_name}.control_role") + if control_role not in CONTROL_ROLES: + raise ValueError(f"{action_name}.control_role 必须是 {sorted(CONTROL_ROLES)} 之一。") + validate_control_role(controller_algorithm, control_axis, control_role, action_name, chassis_type) + return selected_task, control_axis, controller_algorithm, control_role + + +def validate_trajectory_payload( + payload: dict[str, Any], + action_name: str, + safety: dict[str, float], +) -> None: + validate_positive(payload.get("timeout_sec"), f"{action_name}.trajectory_tracking.timeout_sec") + path = require_list(payload.get("path"), f"{action_name}.trajectory_tracking.path") + if len(path) < 2: + raise ValueError(f"{action_name}.trajectory_tracking.path 至少需要 2 个轨迹点。") + + for index, point_value in enumerate(path): + point = require_map(point_value, f"{action_name}.trajectory_tracking.path[{index}]") + for key in TRAJECTORY_POINT_KEYS: + if key not in point: + raise ValueError(f"{action_name}.trajectory_tracking.path[{index}] 缺少 {key}。") + as_float(point["x_m"], f"{action_name}.trajectory_tracking.path[{index}].x_m") + as_float(point["y_m"], f"{action_name}.trajectory_tracking.path[{index}].y_m") + as_float(point["yaw_rad"], f"{action_name}.trajectory_tracking.path[{index}].yaw_rad") + validate_speed( + point["target_speed_ms"], + f"{action_name}.trajectory_tracking.path[{index}].target_speed_ms", + safety, + ) + + if "max_external_pose_age_ms" in payload: + validate_positive( + payload["max_external_pose_age_ms"], + f"{action_name}.trajectory_tracking.max_external_pose_age_ms", + ) + if "min_external_pose_quality_score" in payload: + quality = as_float( + payload["min_external_pose_quality_score"], + f"{action_name}.trajectory_tracking.min_external_pose_quality_score", + ) + if quality < 0.0 or quality > 1.0: + raise ValueError(f"{action_name}.trajectory_tracking.min_external_pose_quality_score 必须在 0 到 1 之间。") + if "segment_index" in payload: + segment_index = as_int(payload["segment_index"], f"{action_name}.trajectory_tracking.segment_index") + if segment_index < 0: + raise ValueError(f"{action_name}.trajectory_tracking.segment_index 不能小于 0。") + if "total_segments" in payload: + total_segments = as_int(payload["total_segments"], f"{action_name}.trajectory_tracking.total_segments") + if total_segments < 0: + raise ValueError(f"{action_name}.trajectory_tracking.total_segments 不能小于 0。") + + +def validate_velocity_step_payload( + payload: dict[str, Any], + action_name: str, + safety: dict[str, float], +) -> None: + validate_speed(payload.get("target_velocity_ms"), f"{action_name}.velocity_step.target_velocity_ms", safety) + validate_positive(payload.get("hold_time_sec"), f"{action_name}.velocity_step.hold_time_sec") + validate_non_negative( + payload.get("settle_before_step_sec"), + f"{action_name}.velocity_step.settle_before_step_sec", + ) + + +def validate_accel_decel_payload( + payload: dict[str, Any], + action_name: str, + safety: dict[str, float], +) -> None: + validate_speed(payload.get("start_velocity_ms"), f"{action_name}.accel_decel.start_velocity_ms", safety) + validate_speed(payload.get("target_velocity_ms"), f"{action_name}.accel_decel.target_velocity_ms", safety) + validate_accel(payload.get("target_accel_ms2"), f"{action_name}.accel_decel.target_accel_ms2", safety) + validate_positive(payload.get("hold_time_sec"), f"{action_name}.accel_decel.hold_time_sec") + + +def validate_stop_accuracy_payload(payload: dict[str, Any], action_name: str) -> None: + as_float(payload.get("target_stop_x_m"), f"{action_name}.stop_accuracy.target_stop_x_m") + as_float(payload.get("target_stop_y_m"), f"{action_name}.stop_accuracy.target_stop_y_m") + as_float(payload.get("target_stop_yaw_rad"), f"{action_name}.stop_accuracy.target_stop_yaw_rad") + validate_positive(payload.get("timeout_sec"), f"{action_name}.stop_accuracy.timeout_sec") + + +def validate_action( + action: dict[str, Any], + action_index: int, + chassis_type: str, + safety: dict[str, float], +) -> None: + action_name = f"chassis_profiles.{chassis_type}.actions[{action_index}]" + require_string(action.get("task_code"), f"{action_name}.task_code") + selected_task, _, _, _ = validate_task_identity(action, action_name, chassis_type) + + payload_key = TASK_PAYLOAD_KEYS[selected_task] + payload = require_map(action.get(payload_key), f"{action_name}.{payload_key}") + if selected_task == "trajectory_tracking": + validate_trajectory_payload(payload, action_name, safety) + elif selected_task == "velocity_step": + validate_velocity_step_payload(payload, action_name, safety) + elif selected_task in {"acceleration_deceleration", "accel_decel"}: + validate_accel_decel_payload(payload, action_name, safety) + elif selected_task == "stop_accuracy": + validate_stop_accuracy_payload(payload, action_name) + + +def validate_profile(profile: dict[str, Any]) -> dict[str, Any]: + schema_version = int(profile.get("schema_version", 0)) + if schema_version != 1: + raise ValueError("schema_version 必须为 1。") + + safety = read_safety(profile) + data_capture = require_map(profile.get("data_capture"), "data_capture") + required_inputs = require_list(data_capture.get("required_data_inputs"), "data_capture.required_data_inputs") + for item in required_inputs: + require_string(item, "data_capture.required_data_inputs[]") + + chassis_profiles = require_map(profile.get("chassis_profiles"), "chassis_profiles") + missing_profiles = sorted(CHASSIS_TYPES - set(chassis_profiles)) + if missing_profiles: + raise ValueError(f"缺少底盘类型配置:{', '.join(missing_profiles)}") + + for chassis_type, section_value in chassis_profiles.items(): + if chassis_type not in CHASSIS_TYPES: + raise ValueError(f"未知底盘类型:{chassis_type}") + section = require_map(section_value, f"chassis_profiles.{chassis_type}") + require_list(section.get("calibration_targets"), f"chassis_profiles.{chassis_type}.calibration_targets") + actions = require_list(section.get("actions"), f"chassis_profiles.{chassis_type}.actions") + if not actions: + raise ValueError(f"chassis_profiles.{chassis_type}.actions 不能为空。") + seen_task_codes: set[str] = set() + for index, action_value in enumerate(actions): + action = require_map(action_value, f"chassis_profiles.{chassis_type}.actions[{index}]") + task_code = require_string( + action.get("task_code"), + f"chassis_profiles.{chassis_type}.actions[{index}].task_code", + ) + if task_code in seen_task_codes: + raise ValueError(f"重复的 task_code:{task_code}") + seen_task_codes.add(task_code) + validate_action(action, index, chassis_type, safety) + return profile + + +def make_task_param(key: str, value: Any) -> dict[str, str]: + return { + "key": key, + "value": stringify_value(value), + } + + +def add_if_present(metadata: dict[str, Any], key: str, payload: dict[str, Any], source_key: str) -> None: + if source_key in payload and payload[source_key] not in (None, ""): + metadata[key] = payload[source_key] + + +def flatten_trajectory_payload(metadata: dict[str, Any], payload: dict[str, Any]) -> None: + metadata["control.stop_at_end"] = payload.get("stop_at_end", True) + metadata["control.timeout_sec"] = payload["timeout_sec"] + add_if_present(metadata, "trajectory_tracking.required_external_pose_source_id", payload, "required_external_pose_source_id") + add_if_present(metadata, "trajectory_tracking.max_external_pose_age_ms", payload, "max_external_pose_age_ms") + add_if_present(metadata, "trajectory_tracking.min_external_pose_quality_score", payload, "min_external_pose_quality_score") + add_if_present(metadata, "trajectory_tracking.trajectory_id", payload, "trajectory_id") + add_if_present(metadata, "trajectory_tracking.segment_index", payload, "segment_index") + add_if_present(metadata, "trajectory_tracking.total_segments", payload, "total_segments") + add_if_present(metadata, "trajectory_tracking.is_final_segment", payload, "is_final_segment") + + for index, point in enumerate(payload["path"]): + metadata[f"traj_pt_{index}_x_m"] = point["x_m"] + metadata[f"traj_pt_{index}_y_m"] = point["y_m"] + metadata[f"traj_pt_{index}_yaw_rad"] = point["yaw_rad"] + metadata[f"traj_pt_{index}_speed_ms"] = point["target_speed_ms"] + + +def flatten_action_metadata(action: dict[str, Any], chassis_type: str) -> dict[str, Any]: + selected_task = action["selected_task"] + payload_key = TASK_PAYLOAD_KEYS[selected_task] + payload = action[payload_key] + metadata: dict[str, Any] = { + "control.task_type": TASK_TYPE_TO_METADATA[selected_task], + "control.chassis_type": chassis_type, + "control.axis": action["control_axis"], + "control.algorithm": action["controller_algorithm"], + "control.role": action["control_role"], + } + add_if_present(metadata, "control.loop_name", action, "loop_name") + + if selected_task == "trajectory_tracking": + flatten_trajectory_payload(metadata, payload) + elif selected_task == "velocity_step": + metadata["velocity_step.target_velocity_ms"] = payload["target_velocity_ms"] + metadata["velocity_step.hold_time_sec"] = payload["hold_time_sec"] + metadata["velocity_step.settle_before_step_sec"] = payload["settle_before_step_sec"] + elif selected_task in {"acceleration_deceleration", "accel_decel"}: + metadata["accel_decel.start_velocity_ms"] = payload["start_velocity_ms"] + metadata["accel_decel.target_velocity_ms"] = payload["target_velocity_ms"] + metadata["accel_decel.target_accel_ms2"] = payload["target_accel_ms2"] + metadata["accel_decel.hold_time_sec"] = payload["hold_time_sec"] + elif selected_task == "stop_accuracy": + metadata["stop_accuracy.target_stop_x_m"] = payload["target_stop_x_m"] + metadata["stop_accuracy.target_stop_y_m"] = payload["target_stop_y_m"] + metadata["stop_accuracy.target_stop_yaw_rad"] = payload["target_stop_yaw_rad"] + metadata["control.timeout_sec"] = payload["timeout_sec"] + return metadata + + +def export_requested_tasks(profile: dict[str, Any], chassis_type: str) -> dict[str, Any]: + section = profile["chassis_profiles"][chassis_type] + tasks: list[dict[str, Any]] = [] + for action in section["actions"]: + metadata = flatten_action_metadata(action, chassis_type) + task_params = [make_task_param(key, metadata[key]) for key in sorted(metadata)] + tasks.append({ + "stage_type": "CONTROL_CALIBRATION_STAGE", + "enabled": True, + "require_manual_approval": False, + "execution_policy": "REQUIRED", + "reason": "现场运控评估 profile", + "task_code": action["task_code"], + "target_id": chassis_type, + "task_params": task_params, + }) + return { + "schema_version": 1, + "chassis_type": chassis_type, + "requested_tasks": tasks, + } + + +def build_summary(profile: dict[str, Any]) -> dict[str, Any]: + summary: dict[str, Any] = { + "schema_version": profile["schema_version"], + "profile_name": profile.get("profile_name", ""), + "chassis_profiles": {}, + } + for chassis_type, section in profile["chassis_profiles"].items(): + summary["chassis_profiles"][chassis_type] = { + "action_count": len(section["actions"]), + "actions": [ + { + "task_code": action["task_code"], + "selected_task": action["selected_task"], + "control_axis": action["control_axis"], + "controller_algorithm": action["controller_algorithm"], + "control_role": action["control_role"], + "display_name": action.get("display_name", ""), + } + for action in section["actions"] + ], + } + return summary + + +def render_yaml(data: dict[str, Any]) -> str: + return yaml.safe_dump(data, sort_keys=False, allow_unicode=True) + + +def main() -> int: + args = parse_args() + profile_path = Path(args.profile).expanduser().resolve(strict=False) + profile = validate_profile(load_yaml(profile_path)) + + if args.format == "requested_tasks": + if not args.chassis_type: + raise ValueError("--format requested_tasks 必须指定 --chassis-type。") + output = export_requested_tasks(profile, args.chassis_type) + else: + output = build_summary(profile) + + rendered = render_yaml(output) + if args.output: + output_path = Path(args.output).expanduser().resolve(strict=False) + output_path.parent.mkdir(parents=True, exist_ok=True) + output_path.write_text(rendered, encoding="utf-8") + print(f"[OK] 已写入运控评估 profile 输出:{output_path}", file=sys.stderr) + else: + print(rendered, end="") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/replay_control_smoke.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/replay_control_smoke.py new file mode 100644 index 0000000..6e82c70 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/replay_control_smoke.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +"""验证运控标定现场数据文件、dataset_index 写入和 data_input 转换链路。""" + +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path + +from control_data_common import ( + CHASSIS_RESPONSE_FIELDS, + CONTROL_EVALUATION_FIELDS, + REFERENCE_SIGNAL_FIELDS, + TRUTH_TRAJECTORY_FIELDS, + apply_cli_overrides, + build_dataset_index, + build_summary, + copy_or_generate_file, + load_config, + read_csv_rows, + resolve_path, + synthetic_rows, + validate_config, + validate_control_dataset_files, + validate_summary, + write_yaml, +) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="生成或校验一组运控标定现场数据,并写入 dataset_index.yaml。" + ) + parser.add_argument( + "--config", + default="src/site_deployment/workshop_control_calibration_real/config/control_data_template.yaml", + help="运控现场数据配置 YAML。", + ) + parser.add_argument("--session-id", default="", help="覆盖 session_id。") + parser.add_argument("--site-id", default="", help="覆盖 site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 vehicle_id。") + parser.add_argument("--session-dir", default="", help="覆盖 session_dir。") + parser.add_argument("--dataset-index-path", default="", help="覆盖 dataset_index_path。") + parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type。") + parser.add_argument("--control-axis", default="", help="覆盖 control_axis。") + parser.add_argument("--controller-algorithm", default="", help="覆盖 controller_algorithm。") + parser.add_argument("--control-role", default="", help="覆盖 control_role。") + parser.add_argument("--source-control-evaluation", default="", help="已有运控评估 CSV;为空则生成合成数据。") + parser.add_argument("--source-reference-signal", default="", help="已有参考信号 CSV;为空则生成合成数据。") + parser.add_argument("--source-chassis-response", default="", help="已有底盘响应 CSV;为空则生成合成数据。") + parser.add_argument("--source-truth-trajectory", default="", help="已有真值轨迹 CSV;为空则生成合成数据。") + return parser.parse_args() + + +def print_validation_result(prefix: str, errors: list[str], warnings: list[str]) -> None: + for warning in warnings: + print(f"[WARN] {prefix}: {warning}", file=sys.stderr) + for error in errors: + print(f"[ERROR] {prefix}: {error}", file=sys.stderr) + + +def main() -> int: + args = parse_args() + config_path = Path(args.config).expanduser().resolve(strict=False) + config = load_config(config_path) + apply_cli_overrides(config, args) + + config_result = validate_config(config, config_path) + print_validation_result("配置检查", config_result.errors, config_result.warnings) + if not config_result.ok: + return 2 + + session_dir = Path(str(config["session_dir"])).expanduser().resolve(strict=False) + files = config["files"] + target_paths = { + "control_evaluation_data_file": resolve_path(session_dir, str(files["control_evaluation_data_file"])), + "reference_signal_file": resolve_path(session_dir, str(files["reference_signal_file"])), + "chassis_response_file": resolve_path(session_dir, str(files["chassis_response_file"])), + "truth_trajectory_file": resolve_path(session_dir, str(files["truth_trajectory_file"])), + "diagnostics_file": resolve_path(session_dir, str(files["diagnostics_file"])), + } + + control_rows, reference_rows, chassis_rows, truth_rows = synthetic_rows(config) + copy_or_generate_file( + args.source_control_evaluation, + target_paths["control_evaluation_data_file"], + CONTROL_EVALUATION_FIELDS, + control_rows, + ) + copy_or_generate_file( + args.source_reference_signal, + target_paths["reference_signal_file"], + REFERENCE_SIGNAL_FIELDS, + reference_rows, + ) + copy_or_generate_file( + args.source_chassis_response, + target_paths["chassis_response_file"], + CHASSIS_RESPONSE_FIELDS, + chassis_rows, + ) + copy_or_generate_file( + args.source_truth_trajectory, + target_paths["truth_trajectory_file"], + TRUTH_TRAJECTORY_FIELDS, + truth_rows, + ) + + file_result = validate_control_dataset_files(target_paths) + print_validation_result("CSV 检查", file_result.errors, file_result.warnings) + if not file_result.ok: + return 3 + + control_rows = read_csv_rows(target_paths["control_evaluation_data_file"]) + reference_rows = read_csv_rows(target_paths["reference_signal_file"]) + chassis_rows = read_csv_rows(target_paths["chassis_response_file"]) + truth_rows = read_csv_rows(target_paths["truth_trajectory_file"]) + summary = build_summary(control_rows, reference_rows, chassis_rows, truth_rows, config) + summary_result = validate_summary(summary, config) + print_validation_result("摘要检查", summary_result.errors, summary_result.warnings) + if not summary_result.ok: + return 4 + + target_paths["diagnostics_file"].parent.mkdir(parents=True, exist_ok=True) + target_paths["diagnostics_file"].write_text( + json.dumps(summary, ensure_ascii=False, indent=2), + encoding="utf-8", + ) + + dataset_index = build_dataset_index(config, summary, target_paths, config_path) + dataset_index_path = Path(str(config["dataset_index_path"])).expanduser().resolve(strict=False) + write_yaml(dataset_index_path, dataset_index) + + print(f"[OK] 已写入运控评估数据: {target_paths['control_evaluation_data_file']}", file=sys.stderr) + print(f"[OK] 已写入运控数据集索引: {dataset_index_path}", file=sys.stderr) + print(f"[OK] 运控 RMS 横向误差: {summary['rms_lateral_error_m']:.6f} m", file=sys.stderr) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/run_control_profile_capture.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/run_control_profile_capture.py new file mode 100644 index 0000000..b3ddce7 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/run_control_profile_capture.py @@ -0,0 +1,413 @@ +#!/usr/bin/env python3 +"""按运控评估 profile 执行任务,并同步采集现场数据。""" + +from __future__ import annotations + +import argparse +import sys +import time +from pathlib import Path +from typing import Any + +SCRIPT_DIR = Path(__file__).resolve().parent +if str(SCRIPT_DIR) not in sys.path: + sys.path.insert(0, str(SCRIPT_DIR)) + +from capture_control_session import ( + ControlSessionCapture, + config_topic, + finalize_capture, + make_target_paths, + now_us, +) +from control_data_common import apply_cli_overrides, load_config, validate_config +from control_evaluation_profile import ( + TASK_PAYLOAD_KEYS, + TASK_TYPE_TO_METADATA, + load_yaml as load_evaluation_profile_yaml, + validate_profile as validate_evaluation_profile, +) + + +TASK_TYPE_VALUES = { + "trajectory_tracking": "TRAJECTORY_TRACKING", + "velocity_step": "VELOCITY_STEP", + "acceleration_deceleration": "ACCELERATION_DECELERATION", + "accel_decel": "ACCELERATION_DECELERATION", + "stop_accuracy": "STOP_ACCURACY", +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="按现场运控评估 profile 顺序发送控制评估任务,同时采集运控、底盘和外部真值数据。" + ) + parser.add_argument("--config", required=True, help="运控标定现场数据配置文件。") + parser.add_argument( + "--evaluation-profile", + default="src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml", + help="运控评估 profile YAML 路径。", + ) + parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type,并选择对应运控评估序列。") + parser.add_argument("--task-code", default="", help="只执行指定 task_code;为空时执行该底盘类型全部任务。") + parser.add_argument("--session-id", default="", help="覆盖 session_id。") + parser.add_argument("--site-id", default="", help="覆盖 site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 vehicle_id。") + parser.add_argument("--operator-id", default="workshop_operator", help="写入请求头的操作员 ID。") + parser.add_argument("--workshop-host", default="workshop_control_profile_capture", help="写入请求头的车间主机名。") + parser.add_argument("--session-dir", default="", help="覆盖 session_dir。") + parser.add_argument("--dataset-index-path", default="", help="覆盖 dataset_index_path。") + parser.add_argument("--control-telemetry-topic", default="", help="运控遥测话题。") + parser.add_argument("--chassis-telemetry-topic", default="", help="底盘遥测话题。") + parser.add_argument("--external-pose-topic", default="", help="外部真值位姿话题。") + parser.add_argument("--task-type", default="", help=argparse.SUPPRESS) + parser.add_argument("--action-server", default="/control/execute_controller_evaluation", help="运控评估 action 名称。") + parser.add_argument("--service-timeout-sec", type=float, default=10.0, help="等待 action server 和 goal 响应的超时。") + parser.add_argument("--action-timeout-sec", type=float, default=0.0, help="覆盖每段任务等待结果超时;0 表示按 profile 推导。") + parser.add_argument("--action-result-grace-sec", type=float, default=8.0, help="profile 推导时长之外额外等待时间。") + parser.add_argument("--start-before-task-sec", type=float, default=-1.0, help="任务前提前采集时间;负数表示读取 profile。") + parser.add_argument("--stop-after-task-sec", type=float, default=-1.0, help="任务后延迟采集时间;负数表示读取 profile。") + parser.add_argument("--stop-on-action-failure", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--allow-incomplete", action="store_true", help="样本不足时仍写入索引和诊断文件。") + return parser.parse_args() + + +def as_float(value: Any, default: float = 0.0) -> float: + if value in (None, ""): + return default + return float(value) + + +def as_bool(value: Any, default: bool = False) -> bool: + if value in (None, ""): + return default + if isinstance(value, bool): + return value + return str(value).strip().lower() in {"1", "true", "yes", "on"} + + +def as_uint(value: Any, default: int = 0) -> int: + if value in (None, ""): + return default + return max(0, int(value)) + + +def spin_for(rclpy_module: Any, node: Any, seconds: float) -> None: + deadline = time.monotonic() + max(0.0, seconds) + while rclpy_module.ok() and time.monotonic() < deadline: + rclpy_module.spin_once(node, timeout_sec=0.05) + + +def spin_until_future( + rclpy_module: Any, + node: Any, + future: Any, + timeout_sec: float, + wait_name: str, +) -> Any: + deadline = time.monotonic() + timeout_sec + while rclpy_module.ok() and not future.done() and time.monotonic() < deadline: + rclpy_module.spin_once(node, timeout_sec=0.05) + if not future.done(): + raise TimeoutError(f"等待 {wait_name} 超时。") + return future.result() + + +def select_actions(profile: dict[str, Any], chassis_type: str, task_code: str) -> list[dict[str, Any]]: + section = profile["chassis_profiles"][chassis_type] + actions = list(section["actions"]) + if not task_code: + return actions + selected = [action for action in actions if action.get("task_code") == task_code] + if not selected: + raise ValueError(f"运控评估 profile 中没有 task_code={task_code!r}。") + return selected + + +def normalized_task_type(action: dict[str, Any]) -> str: + return TASK_TYPE_TO_METADATA[str(action["selected_task"])] + + +def action_payload(action: dict[str, Any]) -> dict[str, Any]: + payload_key = TASK_PAYLOAD_KEYS[str(action["selected_task"])] + return action[payload_key] + + +def task_timeout_sec(action: dict[str, Any], args: argparse.Namespace) -> float: + if args.action_timeout_sec > 0.0: + return args.action_timeout_sec + + selected_task = str(action["selected_task"]) + payload = action_payload(action) + if selected_task == "trajectory_tracking": + base = as_float(payload.get("timeout_sec"), 60.0) + elif selected_task == "stop_accuracy": + base = as_float(payload.get("timeout_sec"), 45.0) + elif selected_task == "velocity_step": + base = as_float(payload.get("settle_before_step_sec"), 0.0) + as_float(payload.get("hold_time_sec"), 0.0) + else: + start_speed = as_float(payload.get("start_velocity_ms"), 0.0) + target_speed = as_float(payload.get("target_velocity_ms"), 0.0) + target_accel = abs(as_float(payload.get("target_accel_ms2"), 0.1)) + ramp_sec = abs(target_speed - start_speed) / target_accel if target_accel > 0.0 else 0.0 + base = ramp_sec + as_float(payload.get("hold_time_sec"), 0.0) + return base + max(0.0, args.action_result_grace_sec) + + +def fill_common_goal(goal: Any, config: dict[str, Any], action: dict[str, Any], args: argparse.Namespace) -> None: + task_code = str(action["task_code"]) + goal.goal.header.session_id = str(config["session_id"]) + goal.goal.header.task_id = task_code + goal.goal.header.vehicle_id = str(config["vehicle_id"]) + goal.goal.header.request_id = f"{task_code}_{now_us()}" + goal.goal.header.client_send_timestamp_us = now_us() + goal.goal.header.operator_id = args.operator_id + goal.goal.header.workshop_host = args.workshop_host + goal.goal.test_case_id = task_code + goal.goal.task_purpose.value = goal.goal.task_purpose.DATA_COLLECTION + goal.goal.selected_task.value = getattr(goal.goal.selected_task, TASK_TYPE_VALUES[str(action["selected_task"])]) + goal.goal.source_iteration_id = str(config["session_id"]) + + +def fill_trajectory_goal(goal: Any, action: dict[str, Any], trajectory_point_type: Any) -> None: + payload = action_payload(action) + goal.goal.trajectory_tracking.stop_at_end = as_bool(payload.get("stop_at_end"), True) + goal.goal.trajectory_tracking.timeout_sec = as_float(payload.get("timeout_sec"), 60.0) + goal.goal.trajectory_tracking.required_external_pose_source_id = str( + payload.get("required_external_pose_source_id", "") or "" + ) + goal.goal.trajectory_tracking.max_external_pose_age_ms = as_float(payload.get("max_external_pose_age_ms"), 0.0) + goal.goal.trajectory_tracking.min_external_pose_quality_score = as_float( + payload.get("min_external_pose_quality_score"), + 0.0, + ) + goal.goal.trajectory_tracking.trajectory_id = str(payload.get("trajectory_id", "") or "") + goal.goal.trajectory_tracking.segment_index = as_uint(payload.get("segment_index"), 0) + goal.goal.trajectory_tracking.total_segments = as_uint(payload.get("total_segments"), 0) + goal.goal.trajectory_tracking.is_final_segment = as_bool(payload.get("is_final_segment"), False) + + for point in payload["path"]: + point_msg = trajectory_point_type() + point_msg.x_m = as_float(point["x_m"]) + point_msg.y_m = as_float(point["y_m"]) + point_msg.yaw_rad = as_float(point["yaw_rad"]) + point_msg.target_speed_ms = as_float(point["target_speed_ms"]) + goal.goal.trajectory_tracking.path.append(point_msg) + + +def fill_velocity_step_goal(goal: Any, action: dict[str, Any]) -> None: + payload = action_payload(action) + goal.goal.velocity_step.target_velocity_ms = as_float(payload["target_velocity_ms"]) + goal.goal.velocity_step.hold_time_sec = as_float(payload["hold_time_sec"]) + goal.goal.velocity_step.settle_before_step_sec = as_float(payload["settle_before_step_sec"]) + + +def fill_accel_decel_goal(goal: Any, action: dict[str, Any]) -> None: + payload = action_payload(action) + goal.goal.accel_decel.start_velocity_ms = as_float(payload["start_velocity_ms"]) + goal.goal.accel_decel.target_velocity_ms = as_float(payload["target_velocity_ms"]) + goal.goal.accel_decel.target_accel_ms2 = as_float(payload["target_accel_ms2"]) + goal.goal.accel_decel.hold_time_sec = as_float(payload["hold_time_sec"]) + + +def fill_stop_accuracy_goal(goal: Any, action: dict[str, Any]) -> None: + payload = action_payload(action) + goal.goal.stop_accuracy.target_stop_x_m = as_float(payload["target_stop_x_m"]) + goal.goal.stop_accuracy.target_stop_y_m = as_float(payload["target_stop_y_m"]) + goal.goal.stop_accuracy.target_stop_yaw_rad = as_float(payload["target_stop_yaw_rad"]) + goal.goal.stop_accuracy.timeout_sec = as_float(payload["timeout_sec"]) + + +def build_goal( + execute_task_type: Any, + trajectory_point_type: Any, + config: dict[str, Any], + action: dict[str, Any], + args: argparse.Namespace, +) -> Any: + goal = execute_task_type.Goal() + fill_common_goal(goal, config, action, args) + selected_task = str(action["selected_task"]) + if selected_task == "trajectory_tracking": + fill_trajectory_goal(goal, action, trajectory_point_type) + elif selected_task == "velocity_step": + fill_velocity_step_goal(goal, action) + elif selected_task in {"acceleration_deceleration", "accel_decel"}: + fill_accel_decel_goal(goal, action) + elif selected_task == "stop_accuracy": + fill_stop_accuracy_goal(goal, action) + else: + raise ValueError(f"不支持的运控评估任务: {selected_task}") + return goal + + +def update_capture_for_action(capture: ControlSessionCapture, action: dict[str, Any], chassis_type: str) -> None: + capture.task_type = normalized_task_type(action) + capture.chassis_type = chassis_type + capture.control_axis = str(action["control_axis"]) + capture.controller_algorithm = str(action["controller_algorithm"]) + capture.control_role = str(action["control_role"]) + + +def execute_action( + rclpy_module: Any, + node: Any, + action_client: Any, + execute_task_type: Any, + trajectory_point_type: Any, + config: dict[str, Any], + action: dict[str, Any], + args: argparse.Namespace, +) -> Any: + if not action_client.wait_for_server(timeout_sec=args.service_timeout_sec): + raise TimeoutError(f"运控评估 action server 不可用: {args.action_server}") + + goal = build_goal(execute_task_type, trajectory_point_type, config, action, args) + send_future = action_client.send_goal_async(goal) + goal_handle = spin_until_future( + rclpy_module, + node, + send_future, + args.service_timeout_sec, + "发送运控评估 goal", + ) + if not goal_handle.accepted: + raise RuntimeError(f"运控评估任务被拒绝: {action['task_code']}") + + result_future = goal_handle.get_result_async() + return spin_until_future( + rclpy_module, + node, + result_future, + task_timeout_sec(action, args), + f"运控评估结果 {action['task_code']}", + ) + + +def import_ros_dependencies() -> dict[str, Any]: + try: + import rclpy + from rclpy.action import ActionClient + from calibration_chassis_interfaces.msg import ChassisTelemetry + from calibration_control_interfaces.action import ExecuteControllerEvaluation + from calibration_control_interfaces.msg import ControlTelemetry + from calibration_control_interfaces.msg import TrajectoryPoint + from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry + except ImportError as exc: + raise RuntimeError(f"缺少 ROS 2 Python 依赖或标定消息包: {exc}") from exc + + return { + "rclpy": rclpy, + "ActionClient": ActionClient, + "ExecuteControllerEvaluation": ExecuteControllerEvaluation, + "ControlTelemetry": ControlTelemetry, + "TrajectoryPoint": TrajectoryPoint, + "ChassisTelemetry": ChassisTelemetry, + "ExternalLocalizationTelemetry": ExternalLocalizationTelemetry, + } + + +def main() -> int: + args = parse_args() + config_path = Path(args.config).expanduser().resolve(strict=False) + config = load_config(config_path) + apply_cli_overrides(config, args) + if args.chassis_type: + config["chassis_type"] = args.chassis_type + + validation = validate_config(config, config_path) + if not validation.ok: + for error in validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + profile_path = Path(args.evaluation_profile).expanduser().resolve(strict=False) + evaluation_profile = validate_evaluation_profile(load_evaluation_profile_yaml(profile_path)) + chassis_type = str(config["chassis_type"]) + actions = select_actions(evaluation_profile, chassis_type, args.task_code) + data_capture = evaluation_profile.get("data_capture", {}) or {} + start_before_sec = ( + as_float(data_capture.get("start_before_task_sec"), 1.0) + if args.start_before_task_sec < 0.0 else args.start_before_task_sec + ) + stop_after_sec = ( + as_float(data_capture.get("stop_after_task_sec"), 1.0) + if args.stop_after_task_sec < 0.0 else args.stop_after_task_sec + ) + + try: + deps = import_ros_dependencies() + except RuntimeError as exc: + print(f"[错误] {exc}", file=sys.stderr) + return 1 + + rclpy_module = deps["rclpy"] + target_paths = make_target_paths(config) + control_topic = args.control_telemetry_topic or config_topic(config, "control_telemetry_topic", "/control/telemetry") + chassis_topic = args.chassis_telemetry_topic or config_topic(config, "chassis_telemetry_topic", "/chassis/telemetry") + external_topic = args.external_pose_topic or config_topic( + config, + "external_pose_topic", + "/workshop/external_localization/vehicle/pose", + ) + + rclpy_module.init() + node = rclpy_module.create_node("workshop_control_profile_capture") + capture = ControlSessionCapture(config, target_paths, args) + action_failed = False + finalize_code = 1 + + try: + node.create_subscription(deps["ControlTelemetry"], control_topic, capture.on_control_telemetry, 50) + node.create_subscription(deps["ChassisTelemetry"], chassis_topic, capture.on_chassis_telemetry, 50) + node.create_subscription(deps["ExternalLocalizationTelemetry"], external_topic, capture.on_external_pose, 50) + + action_client = deps["ActionClient"](node, deps["ExecuteControllerEvaluation"], args.action_server) + print(f"[*] 运控评估 profile: {profile_path}", file=sys.stderr) + print(f"[*] 底盘类型: {chassis_type}", file=sys.stderr) + print(f"[*] 任务数量: {len(actions)}", file=sys.stderr) + print(f"[*] 运控遥测采集: {control_topic}", file=sys.stderr) + print(f"[*] 底盘响应采集: {chassis_topic}", file=sys.stderr) + print(f"[*] 外部真值采集: {external_topic}", file=sys.stderr) + + for index, action in enumerate(actions, start=1): + update_capture_for_action(capture, action, chassis_type) + print(f"[*] 执行运控任务 {index}/{len(actions)}: {action['task_code']}", file=sys.stderr) + spin_for(rclpy_module, node, start_before_sec) + wrapped_result = execute_action( + rclpy_module, + node, + action_client, + deps["ExecuteControllerEvaluation"], + deps["TrajectoryPoint"], + config, + action, + args, + ) + spin_for(rclpy_module, node, stop_after_sec) + + job_result = wrapped_result.result.result + if not job_result.success: + action_failed = True + print(f"[错误] 运控任务失败 {action['task_code']}: {job_result.message}", file=sys.stderr) + if args.stop_on_action_failure: + break + else: + message = job_result.message or "任务完成。" + print(f"[*] 运控任务完成 {action['task_code']}: {message}", file=sys.stderr) + except Exception as exc: + action_failed = True + print(f"[错误] {exc}", file=sys.stderr) + finally: + capture.close() + node.destroy_node() + rclpy_module.shutdown() + finalize_code = finalize_capture(config, target_paths, config_path, args) + + if action_failed: + return 2 if finalize_code == 0 else finalize_code + return finalize_code + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/smoke_test_control_profile_capture.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/smoke_test_control_profile_capture.py new file mode 100644 index 0000000..a66b91a --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/smoke_test_control_profile_capture.py @@ -0,0 +1,443 @@ +#!/usr/bin/env python3 +"""本地验证运控评估 profile 执行和采集落盘链路。""" + +from __future__ import annotations + +import argparse +import os +import subprocess +import sys +import tempfile +import threading +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + +SCRIPT_DIR = Path(__file__).resolve().parent +if str(SCRIPT_DIR) not in sys.path: + sys.path.insert(0, str(SCRIPT_DIR)) + +from control_data_common import read_csv_rows + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="启动假的运控 action server 和遥测话题,验证 run_control_profile_capture.py。" + ) + parser.add_argument( + "--evaluation-profile", + default="src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml", + help="运控评估 profile YAML 路径。", + ) + parser.add_argument( + "--chassis-type", + default="ackermann", + choices=["ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"], + help="本次验证使用的底盘类型。", + ) + parser.add_argument( + "--task-code", + default="", + help="只验证指定任务;为空时默认取该底盘类型的第一个任务。", + ) + parser.add_argument("--session-dir", default="", help="输出目录;为空时使用临时目录。") + parser.add_argument("--keep-output", action="store_true", help="保留临时输出目录。") + parser.add_argument("--verbose", action="store_true", help="打印 profile runner 的完整输出。") + parser.add_argument("--timeout-sec", type=float, default=35.0, help="等待 profile runner 结束的超时。") + parser.add_argument("--rmw-implementation", default="rmw_fastrtps_cpp", help="本地 smoke 使用的 RMW 实现。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def first_task_code(profile_path: Path, chassis_type: str) -> str: + profile = load_yaml(profile_path) + actions = profile["chassis_profiles"][chassis_type]["actions"] + if not actions: + raise ValueError(f"{chassis_type} 没有配置运控评估任务。") + return str(actions[0]["task_code"]) + + +def write_smoke_config(path: Path, session_dir: Path, chassis_type: str) -> None: + config = { + "csv_contract_version": 1, + "session_id": "control_profile_capture_smoke", + "site_id": "local_smoke_site", + "vehicle_id": "smoke_agv_001", + "session_dir": str(session_dir), + "dataset_index_path": str(session_dir / "dataset_index.yaml"), + "chassis_type": chassis_type, + "control_axis": "combined", + "controller_algorithm": "pure_pursuit", + "control_role": "path_tracking_outer_loop", + "files": { + "control_evaluation_data_file": "control/control_eval.csv", + "reference_signal_file": "control/reference_signal.csv", + "chassis_response_file": "control/chassis_response.csv", + "truth_trajectory_file": "external/truth_trajectory.csv", + "diagnostics_file": "control/control_diagnostics.json", + }, + "capture": { + "control_telemetry_topic": "/smoke/control/telemetry", + "chassis_telemetry_topic": "/smoke/chassis/telemetry", + "external_pose_topic": "/smoke/external_localization/vehicle/pose", + }, + "validation": { + "min_control_evaluation_samples": 2, + "min_reference_signal_samples": 2, + "min_chassis_response_samples": 2, + "min_truth_trajectory_samples": 2, + "max_time_gap_ms": 500.0, + }, + "synthetic": { + "enabled": False, + }, + } + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(config, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +class FakeControlFixture: + def __init__(self, node: Any, action_name: str, chassis_type_name: str) -> None: + from rclpy.action import ActionServer + from calibration_chassis_interfaces.msg import ChassisTelemetry + from calibration_common_interfaces.msg import ErrorCode + from calibration_control_interfaces.action import ExecuteControllerEvaluation + from calibration_control_interfaces.msg import ControlTelemetry + from calibration_control_interfaces.msg import ControllerEvaluationTaskType + from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry + from calibration_vehicle_profile_interfaces.msg import ChassisType + from calibration_vehicle_profile_interfaces.msg import ControllerAlgorithmType + + self.node = node + self.ExecuteControllerEvaluation = ExecuteControllerEvaluation + self.ControlTelemetry = ControlTelemetry + self.ControllerEvaluationTaskType = ControllerEvaluationTaskType + self.ChassisTelemetry = ChassisTelemetry + self.ExternalLocalizationTelemetry = ExternalLocalizationTelemetry + self.ErrorCode = ErrorCode + self.ChassisType = ChassisType + self.ControllerAlgorithmType = ControllerAlgorithmType + self.chassis_type_value = { + "ackermann": ChassisType.ACKERMANN, + "differential": ChassisType.DIFFERENTIAL, + "single_steer_wheel": ChassisType.SINGLE_STEER_WHEEL, + "multi_steer_wheel": ChassisType.MULTI_STEER_WHEEL, + }[chassis_type_name] + self.lock = threading.Lock() + self.x_m = 0.0 + self.y_m = 0.0 + self.yaw_rad = 0.0 + self.speed_ms = 0.0 + self.target_speed_ms = 0.1 + self.active_job_id = "" + self.control_pub = node.create_publisher(ControlTelemetry, "/smoke/control/telemetry", 20) + self.chassis_pub = node.create_publisher(ChassisTelemetry, "/smoke/chassis/telemetry", 20) + self.truth_pub = node.create_publisher( + ExternalLocalizationTelemetry, + "/smoke/external_localization/vehicle/pose", + 20, + ) + self.timer = node.create_timer(0.05, self.publish_telemetry) + self.action_server = ActionServer( + node, + ExecuteControllerEvaluation, + action_name, + execute_callback=self.execute_control, + ) + + def publish_telemetry(self) -> None: + stamp = now_us() + with self.lock: + x_m = self.x_m + y_m = self.y_m + yaw_rad = self.yaw_rad + speed_ms = self.speed_ms + target_speed_ms = self.target_speed_ms + active_job_id = self.active_job_id + + control_msg = self.ControlTelemetry() + control_msg.hardware_timestamp_us = stamp + control_msg.odom_x_m = x_m + control_msg.odom_y_m = y_m + control_msg.odom_yaw_rad = yaw_rad + control_msg.linear_velocity_ms = speed_ms + control_msg.angular_velocity_rads = 0.02 + control_msg.lateral_error_m = 0.005 + control_msg.heading_error_rad = 0.003 + control_msg.speed_error_ms = target_speed_ms - speed_ms + control_msg.steering_output = 0.02 + control_msg.throttle_output = 0.1 + control_msg.brake_output = 0.0 + control_msg.saturation_flag = False + control_msg.active_job_id = active_job_id + control_msg.parameter_version = "smoke_fake_control" + control_msg.active_lateral_algorithm.value = self.ControllerAlgorithmType.PURE_PURSUIT + control_msg.active_longitudinal_algorithm.value = self.ControllerAlgorithmType.PID + control_msg.pose_source_name = "smoke_fake_truth" + control_msg.pose_from_external_truth = True + self.control_pub.publish(control_msg) + + chassis_msg = self.ChassisTelemetry() + chassis_msg.hardware_timestamp_us = stamp + chassis_msg.chassis_type.value = self.chassis_type_value + chassis_msg.odom_x_m = x_m + chassis_msg.odom_y_m = y_m + chassis_msg.odom_yaw_rad = yaw_rad + chassis_msg.linear_velocity_ms = speed_ms + chassis_msg.angular_velocity_rads = 0.02 + chassis_msg.estop_engaged = False + chassis_msg.driver_error_code = 0 + chassis_msg.active_job_id = active_job_id + chassis_msg.lateral_slip_estimate = 0.0 + chassis_msg.curvature_estimate = 0.0 + self.chassis_pub.publish(chassis_msg) + + truth_msg = self.ExternalLocalizationTelemetry() + truth_msg.hardware_timestamp_us = stamp + truth_msg.pose_valid = True + truth_msg.workshop_pose.x_m = x_m + truth_msg.workshop_pose.y_m = y_m + truth_msg.workshop_pose.z_m = 0.0 + truth_msg.workshop_pose.roll_rad = 0.0 + truth_msg.workshop_pose.pitch_rad = 0.0 + truth_msg.workshop_pose.yaw_rad = yaw_rad + truth_msg.position_stddev_m = 0.001 + truth_msg.yaw_stddev_rad = 0.001 + truth_msg.tracking_loss_ratio = 0.0 + truth_msg.time_sync_offset_ms = 1.0 + truth_msg.quality_score = 1.0 + truth_msg.observed_target_count = 4 + truth_msg.reference_source_name = "smoke_fake_truth" + truth_msg.active_job_id = active_job_id + self.truth_pub.publish(truth_msg) + + def execute_control(self, goal_handle: Any) -> Any: + request = goal_handle.request.goal + with self.lock: + self.active_job_id = request.header.request_id + + steps = 10 + for step_index in range(steps): + self.advance_state(request, step_index, steps) + self.publish_telemetry() + time.sleep(0.05) + + result = self.ExecuteControllerEvaluation.Result() + result.result.success = True + result.result.error_code.code = self.ErrorCode.OK + result.result.message = "本地 smoke 假运控任务完成。" + result.result.job_id = request.header.request_id + result.result.data_quality_passed = True + result.result.suitable_for_commit = False + result.result.recommended_parameter_version = "smoke_fake_control" + goal_handle.succeed() + with self.lock: + self.active_job_id = "" + return result + + def advance_state(self, request: Any, step_index: int, steps: int) -> None: + selected_task = request.selected_task.value + with self.lock: + if selected_task == self.ControllerEvaluationTaskType.TRAJECTORY_TRACKING: + path = list(request.trajectory_tracking.path) + if path: + point = path[min(len(path) - 1, int(step_index * len(path) / steps))] + self.x_m += (point.x_m - self.x_m) * 0.5 + self.y_m += (point.y_m - self.y_m) * 0.5 + self.yaw_rad += (point.yaw_rad - self.yaw_rad) * 0.5 + self.target_speed_ms = point.target_speed_ms + self.speed_ms += (point.target_speed_ms - self.speed_ms) * 0.5 + elif selected_task == self.ControllerEvaluationTaskType.VELOCITY_STEP: + self.target_speed_ms = request.velocity_step.target_velocity_ms + self.speed_ms += (self.target_speed_ms - self.speed_ms) * 0.4 + self.x_m += self.speed_ms * 0.05 + elif selected_task == self.ControllerEvaluationTaskType.ACCELERATION_DECELERATION: + self.target_speed_ms = request.accel_decel.target_velocity_ms + self.speed_ms += request.accel_decel.target_accel_ms2 * 0.05 + if self.speed_ms > self.target_speed_ms: + self.speed_ms = self.target_speed_ms + self.x_m += self.speed_ms * 0.05 + elif selected_task == self.ControllerEvaluationTaskType.STOP_ACCURACY: + self.x_m += (request.stop_accuracy.target_stop_x_m - self.x_m) * 0.4 + self.y_m += (request.stop_accuracy.target_stop_y_m - self.y_m) * 0.4 + self.yaw_rad += (request.stop_accuracy.target_stop_yaw_rad - self.yaw_rad) * 0.4 + self.target_speed_ms = 0.0 + self.speed_ms *= 0.5 + + +def run_profile_capture( + repo_root: Path, + config_path: Path, + evaluation_profile_path: Path, + session_dir: Path, + args: argparse.Namespace, + action_name: str, + task_code: str, +) -> subprocess.CompletedProcess[str]: + command = [ + sys.executable, + str(SCRIPT_DIR / "run_control_profile_capture.py"), + "--config", + str(config_path), + "--evaluation-profile", + str(evaluation_profile_path), + "--chassis-type", + args.chassis_type, + "--task-code", + task_code, + "--session-id", + "control_profile_capture_smoke", + "--site-id", + "local_smoke_site", + "--vehicle-id", + "smoke_agv_001", + "--session-dir", + str(session_dir), + "--dataset-index-path", + str(session_dir / "dataset_index.yaml"), + "--control-telemetry-topic", + "/smoke/control/telemetry", + "--chassis-telemetry-topic", + "/smoke/chassis/telemetry", + "--external-pose-topic", + "/smoke/external_localization/vehicle/pose", + "--action-server", + action_name, + "--start-before-task-sec", + "0.2", + "--stop-after-task-sec", + "0.2", + "--service-timeout-sec", + "5.0", + "--action-result-grace-sec", + "5.0", + ] + return subprocess.run( + command, + cwd=repo_root, + env=os.environ.copy(), + text=True, + capture_output=True, + timeout=args.timeout_sec, + check=False, + ) + + +def validate_outputs(session_dir: Path) -> None: + dataset_index_path = session_dir / "dataset_index.yaml" + if not dataset_index_path.exists(): + raise RuntimeError(f"未生成 dataset_index.yaml: {dataset_index_path}") + index = load_yaml(dataset_index_path) + control_inputs = index.get("data_inputs", {}).get("control", {}) + for field_name in ( + "control_evaluation_data_files", + "reference_signal_files", + "chassis_response_files", + "truth_trajectory_files", + ): + values = control_inputs.get(field_name) + if not values: + raise RuntimeError(f"dataset_index 缺少 data_inputs.control.{field_name}") + + control_rows = read_csv_rows(session_dir / "control/control_eval.csv") + reference_rows = read_csv_rows(session_dir / "control/reference_signal.csv") + chassis_rows = read_csv_rows(session_dir / "control/chassis_response.csv") + truth_rows = read_csv_rows(session_dir / "external/truth_trajectory.csv") + if len(control_rows) < 2 or len(reference_rows) < 2 or len(chassis_rows) < 2 or len(truth_rows) < 2: + raise RuntimeError( + "采集样本数不足: " + f"control={len(control_rows)}, reference={len(reference_rows)}, " + f"chassis={len(chassis_rows)}, truth={len(truth_rows)}" + ) + + +def main() -> int: + args = parse_args() + repo_root = Path.cwd() + evaluation_profile_path = Path(args.evaluation_profile).expanduser().resolve(strict=False) + task_code = args.task_code or first_task_code(evaluation_profile_path, args.chassis_type) + action_name = f"/smoke/control/execute_controller_evaluation_{now_us()}" + + temp_dir: Any = None + if args.session_dir: + session_dir = Path(args.session_dir).expanduser().resolve(strict=False) + session_dir.mkdir(parents=True, exist_ok=True) + else: + temp_dir = tempfile.TemporaryDirectory(prefix="agv_control_profile_capture_smoke_") + session_dir = Path(temp_dir.name) + + if args.rmw_implementation: + os.environ["RMW_IMPLEMENTATION"] = args.rmw_implementation + + try: + import rclpy + from rclpy.executors import MultiThreadedExecutor + except ImportError as exc: + print(f"[FAIL] 缺少 ROS2 Python 环境,请先 source install/setup.bash: {exc}", file=sys.stderr) + if temp_dir is not None: + temp_dir.cleanup() + return 1 + + config_path = session_dir / "control_data.yaml" + write_smoke_config(config_path, session_dir, args.chassis_type) + ros_log_dir = session_dir / "ros_log" + ros_log_dir.mkdir(parents=True, exist_ok=True) + os.environ.setdefault("ROS_LOG_DIR", str(ros_log_dir)) + + rclpy.init(args=None) + node = rclpy.create_node("smoke_control_profile_capture_fixture") + fixture = FakeControlFixture(node, action_name, args.chassis_type) + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + thread = threading.Thread(target=executor.spin, daemon=True) + thread.start() + + try: + process = run_profile_capture( + repo_root, + config_path, + evaluation_profile_path, + session_dir, + args, + action_name, + task_code, + ) + if args.verbose or process.returncode != 0: + print(process.stdout, end="") + print(process.stderr, end="", file=sys.stderr) + if process.returncode != 0: + print(f"[FAIL] run_control_profile_capture.py 返回 {process.returncode}", file=sys.stderr) + return process.returncode + validate_outputs(session_dir) + print(f"[PASS] 运控 profile 采集 smoke 通过: {session_dir}") + print(f"[PASS] task_code={task_code}") + return 0 + finally: + executor.shutdown() + fixture.action_server.destroy() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + thread.join(timeout=2.0) + if temp_dir is not None and not args.keep_output: + temp_dir.cleanup() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/stage_control_parameter_commit.py b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/stage_control_parameter_commit.py new file mode 100644 index 0000000..3072a0a --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/stage_control_parameter_commit.py @@ -0,0 +1,547 @@ +#!/usr/bin/env python3 +"""生成运控标定待提交参数包。""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +CHASSIS_TYPES = {"ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"} +CONTROL_AXES = {"lateral_control", "longitudinal_control", "combined"} +CONTROLLER_ALGORITHMS = {"pid", "mpc", "lqr", "pure_pursuit"} +SINGLE_PID_CONTROL_AXES = {"lateral_control", "longitudinal_control"} +CONTROL_ROLES = { + "path_tracking_outer_loop", + "speed_loop", + "acceleration_loop", + "heading_loop", + "yaw_rate_loop", + "steering_angle_inner_loop", + "module_steering_inner_loop", + "wheel_speed_inner_loop", + "multi_loop_pid", +} +CONTROL_ROLE_AXES = { + "path_tracking_outer_loop": {"lateral_control", "combined"}, + "speed_loop": {"longitudinal_control"}, + "acceleration_loop": {"longitudinal_control"}, + "heading_loop": {"lateral_control"}, + "yaw_rate_loop": {"lateral_control"}, + "steering_angle_inner_loop": {"lateral_control"}, + "module_steering_inner_loop": {"lateral_control"}, + "wheel_speed_inner_loop": {"longitudinal_control"}, + "multi_loop_pid": {"combined"}, +} +CONTROL_ROLE_ALGORITHMS = { + "path_tracking_outer_loop": {"mpc", "lqr", "pure_pursuit", "pid"}, + "speed_loop": {"pid", "mpc"}, + "acceleration_loop": {"pid", "mpc", "lqr"}, + "heading_loop": {"pid", "lqr"}, + "yaw_rate_loop": {"pid"}, + "steering_angle_inner_loop": {"pid"}, + "module_steering_inner_loop": {"pid"}, + "wheel_speed_inner_loop": {"pid"}, + "multi_loop_pid": {"pid"}, +} +CHASSIS_ROLE_COMPATIBILITY = { + "ackermann": { + "path_tracking_outer_loop", + "speed_loop", + "acceleration_loop", + "heading_loop", + "yaw_rate_loop", + "steering_angle_inner_loop", + "wheel_speed_inner_loop", + "multi_loop_pid", + }, + "differential": { + "path_tracking_outer_loop", + "speed_loop", + "acceleration_loop", + "heading_loop", + "yaw_rate_loop", + "wheel_speed_inner_loop", + "multi_loop_pid", + }, + "single_steer_wheel": { + "path_tracking_outer_loop", + "speed_loop", + "acceleration_loop", + "heading_loop", + "yaw_rate_loop", + "steering_angle_inner_loop", + "wheel_speed_inner_loop", + "multi_loop_pid", + }, + "multi_steer_wheel": { + "path_tracking_outer_loop", + "speed_loop", + "acceleration_loop", + "heading_loop", + "yaw_rate_loop", + "module_steering_inner_loop", + "wheel_speed_inner_loop", + "multi_loop_pid", + }, +} + +PID_FIELDS = ["kp", "ki", "kd"] +LATERAL_MPC_FIELDS = [ + "lateral_mpc.prediction_horizon", + "lateral_mpc.control_horizon", + "lateral_mpc.model_dt_s", + "lateral_mpc.q_lateral", + "lateral_mpc.q_heading", + "lateral_mpc.r_steering", + "lateral_mpc.r_steering_rate", +] +LONGITUDINAL_MPC_FIELDS = [ + "longitudinal_mpc.prediction_horizon", + "longitudinal_mpc.control_horizon", + "longitudinal_mpc.model_dt_s", + "longitudinal_mpc.q_speed", + "longitudinal_mpc.q_accel", + "longitudinal_mpc.r_throttle", + "longitudinal_mpc.r_brake", + "longitudinal_mpc.r_jerk", +] +LQR_LIST_FIELDS = ["lqr.q_state_weights", "lqr.r_input_weights"] +PURE_PURSUIT_FIELDS = ["pure_pursuit.lookahead_m"] + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="把运控算法输出整理成现场待审批/待提交参数包。") + parser.add_argument("--dataset-index", required=True, help="本次会话 dataset_index.yaml。") + parser.add_argument("--estimated-params", required=True, help="算法输出的运控参数 YAML。") + parser.add_argument("--output", default="", help="输出待提交参数包;为空时写到会话目录 control/pending_control_commit.yaml。") + parser.add_argument("--commit-reason", default="运控标定算法输出待提交", help="写入参数包的提交原因。") + parser.add_argument("--operator-id", default="", help="生成待提交包的操作员 ID。") + parser.add_argument("--previous-parameter-version", default="", help="当前车端已生效运控参数版本,用于回滚引用。") + parser.add_argument("--persistent-write", action=argparse.BooleanOptionalAction, default=True, help="最终提交到车端时是否持久化。") + parser.add_argument("--require-manual-approval", action=argparse.BooleanOptionalAction, default=True, help="是否要求人工审批。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否把待提交包路径写回 dataset_index.yaml。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def dotted_get(data: dict[str, Any], dotted_key: str) -> Any: + current: Any = data + for part in dotted_key.split("."): + if not isinstance(current, dict) or part not in current: + return None + current = current[part] + return current + + +def has_value(value: Any) -> bool: + if value in (None, ""): + return False + if isinstance(value, list): + return bool(value) + return True + + +def require_number(value: Any, field_name: str) -> float: + if not has_value(value): + raise ValueError(f"{field_name} 不能为空。") + try: + return float(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是数字,当前值为 {value!r}。") from exc + + +def require_integer(value: Any, field_name: str) -> int: + if not has_value(value): + raise ValueError(f"{field_name} 不能为空。") + try: + integer = int(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是整数,当前值为 {value!r}。") from exc + if integer <= 0: + raise ValueError(f"{field_name} 必须大于 0。") + return integer + + +def require_number_list(value: Any, field_name: str) -> None: + if not isinstance(value, list) or not value: + raise ValueError(f"{field_name} 必须是非空数字数组。") + for index, item in enumerate(value): + require_number(item, f"{field_name}[{index}]") + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def validate_dataset_index(index: dict[str, Any]) -> dict[str, Any]: + if int(index.get("schema_version", 0) or 0) != 1: + raise ValueError("dataset_index.schema_version 必须为 1。") + session = require_map(index.get("session"), "dataset_index.session") + for field_name in ("session_id", "vehicle_id", "dataset_root"): + require_string(session.get(field_name), f"dataset_index.session.{field_name}") + control_inputs = require_map( + require_map(index.get("data_inputs"), "dataset_index.data_inputs").get("control"), + "dataset_index.data_inputs.control", + ) + for field_name in ( + "control_evaluation_data_files", + "reference_signal_files", + "chassis_response_files", + "truth_trajectory_files", + ): + values = control_inputs.get(field_name) + if not isinstance(values, list) or not values: + raise ValueError(f"dataset_index.data_inputs.control.{field_name} 不能为空。") + return session + + +def dataset_chassis_type(index: dict[str, Any]) -> str: + metadata = index.get("metadata", {}) + if isinstance(metadata, dict): + summary = metadata.get("control_replay_summary", {}) + if isinstance(summary, dict): + value = str(summary.get("chassis_type", "") or "").strip() + if value: + return value + return "" + + +def validate_pid_loop( + loop: dict[str, Any], + field_prefix: str, + expected_control_axis: str | None = None, + expected_control_role: str | None = None, + chassis_type: str | None = None, +) -> None: + loop_name = require_string(loop.get("loop_name"), f"{field_prefix}.loop_name") + control_role = require_string(loop.get("control_role"), f"{field_prefix}.control_role") + if control_role not in CONTROL_ROLES: + raise ValueError(f"{field_prefix}.control_role 必须是 {sorted(CONTROL_ROLES)} 之一。") + if control_role == "multi_loop_pid": + raise ValueError(f"{field_prefix}.control_role 不能写 multi_loop_pid,必须描述具体 PID 回路。") + if expected_control_role is not None and control_role != expected_control_role: + raise ValueError(f"{field_prefix}.control_role 必须与顶层 control_role 一致。") + if expected_control_axis is not None: + loop_axis = str(loop.get("control_axis", expected_control_axis) or "").strip() + if loop_axis != expected_control_axis: + raise ValueError(f"{field_prefix}.control_axis 必须与顶层 control_axis 一致。") + elif str(loop.get("control_axis", "") or "").strip() not in SINGLE_PID_CONTROL_AXES: + raise ValueError(f"{field_prefix}.control_axis 必须是 {sorted(SINGLE_PID_CONTROL_AXES)} 之一。") + loop_axis = str(loop.get("control_axis", expected_control_axis) or "").strip() + validate_control_role("pid", loop_axis, control_role, field_prefix, chassis_type) + if loop_name == "combined": + raise ValueError(f"{field_prefix}.loop_name 不能写 combined,必须明确具体 PID 回路。") + for field_name in PID_FIELDS: + require_number(loop.get(field_name), f"{field_prefix}.{field_name}") + + +def validate_pid_params( + estimated: dict[str, Any], + control_axis: str, + control_role: str, + chassis_type: str, +) -> None: + if control_axis in SINGLE_PID_CONTROL_AXES: + validate_pid_loop( + require_map(estimated.get("pid"), "estimated_params.estimated_params.pid"), + "estimated_params.estimated_params.pid", + control_axis, + control_role, + chassis_type, + ) + return + + pid_loops = estimated.get("pid_loops") + if not isinstance(pid_loops, list) or not pid_loops: + raise ValueError( + "estimated_params.control_axis=combined 且 controller_algorithm=pid 时," + "必须使用 estimated_params.pid_loops 列表分别描述每个 PID 回路。" + ) + for index, loop in enumerate(pid_loops): + validate_pid_loop( + require_map(loop, f"estimated_params.estimated_params.pid_loops[{index}]"), + f"estimated_params.estimated_params.pid_loops[{index}]", + None, + None, + chassis_type, + ) + + +def validate_control_role( + controller_algorithm: str, + control_axis: str, + control_role: str, + field_prefix: str, + chassis_type: str | None = None, +) -> None: + if control_role not in CONTROL_ROLES: + raise ValueError(f"{field_prefix}.control_role 必须是 {sorted(CONTROL_ROLES)} 之一。") + allowed_axes = CONTROL_ROLE_AXES[control_role] + if control_axis not in allowed_axes: + raise ValueError( + f"{field_prefix}.control_role={control_role} 不适用于 control_axis={control_axis}。" + ) + allowed_algorithms = CONTROL_ROLE_ALGORITHMS[control_role] + if controller_algorithm not in allowed_algorithms: + raise ValueError( + f"{field_prefix}.control_role={control_role} 不适用于 controller_algorithm={controller_algorithm}。" + ) + if chassis_type: + allowed_roles = CHASSIS_ROLE_COMPATIBILITY[chassis_type] + if control_role not in allowed_roles: + raise ValueError( + f"{field_prefix}.control_role={control_role} 不适用于 chassis_type={chassis_type}。" + ) + + +def validate_mpc_params(estimated: dict[str, Any], control_axis: str) -> None: + require_lateral = control_axis in {"lateral_control", "combined"} + require_longitudinal = control_axis in {"longitudinal_control", "combined"} + if require_lateral: + for field_name in LATERAL_MPC_FIELDS: + value = dotted_get(estimated, field_name) + if field_name.endswith("prediction_horizon") or field_name.endswith("control_horizon"): + require_integer(value, f"estimated_params.estimated_params.{field_name}") + else: + require_number(value, f"estimated_params.estimated_params.{field_name}") + if require_longitudinal: + for field_name in LONGITUDINAL_MPC_FIELDS: + value = dotted_get(estimated, field_name) + if field_name.endswith("prediction_horizon") or field_name.endswith("control_horizon"): + require_integer(value, f"estimated_params.estimated_params.{field_name}") + else: + require_number(value, f"estimated_params.estimated_params.{field_name}") + + +def validate_lqr_params(estimated: dict[str, Any]) -> None: + for field_name in LQR_LIST_FIELDS: + require_number_list( + dotted_get(estimated, field_name), + f"estimated_params.estimated_params.{field_name}", + ) + + +def validate_pure_pursuit_params(estimated: dict[str, Any]) -> None: + for field_name in PURE_PURSUIT_FIELDS: + require_number(dotted_get(estimated, field_name), f"estimated_params.estimated_params.{field_name}") + + +def validate_estimated_params( + params: dict[str, Any], + session: dict[str, Any], + index: dict[str, Any], +) -> tuple[str, str, str, str, str, dict[str, Any]]: + if int(params.get("schema_version", 0) or 0) != 1: + raise ValueError("estimated_params.schema_version 必须为 1。") + parameter_version = require_string(params.get("parameter_version"), "estimated_params.parameter_version") + chassis_type = require_string(params.get("chassis_type"), "estimated_params.chassis_type") + control_axis = require_string(params.get("control_axis"), "estimated_params.control_axis") + controller_algorithm = require_string(params.get("controller_algorithm"), "estimated_params.controller_algorithm") + control_role = require_string(params.get("control_role"), "estimated_params.control_role") + if chassis_type not in CHASSIS_TYPES: + raise ValueError(f"estimated_params.chassis_type 必须是 {sorted(CHASSIS_TYPES)} 之一。") + index_chassis_type = dataset_chassis_type(index) + if index_chassis_type and index_chassis_type != chassis_type: + raise ValueError("estimated_params.chassis_type 与 dataset_index 中的底盘类型不一致。") + if control_axis not in CONTROL_AXES: + raise ValueError(f"estimated_params.control_axis 必须是 {sorted(CONTROL_AXES)} 之一。") + if controller_algorithm not in CONTROLLER_ALGORITHMS: + raise ValueError(f"estimated_params.controller_algorithm 必须是 {sorted(CONTROLLER_ALGORITHMS)} 之一。") + validate_control_role(controller_algorithm, control_axis, control_role, "estimated_params", chassis_type) + if controller_algorithm == "pid" and control_axis == "combined" and control_role != "multi_loop_pid": + raise ValueError("combined + pid 必须使用 control_role=multi_loop_pid,并在 pid_loops 中列出各 PID 回路。") + vehicle_id = str(params.get("vehicle_id", "") or "").strip() + if vehicle_id and vehicle_id != session.get("vehicle_id"): + raise ValueError("estimated_params.vehicle_id 与 dataset_index.session.vehicle_id 不一致。") + estimated = require_map(params.get("estimated_params"), "estimated_params.estimated_params") + + if controller_algorithm == "pid": + validate_pid_params(estimated, control_axis, control_role, chassis_type) + elif controller_algorithm == "mpc": + validate_mpc_params(estimated, control_axis) + elif controller_algorithm == "lqr": + validate_lqr_params(estimated) + elif controller_algorithm == "pure_pursuit": + validate_pure_pursuit_params(estimated) + + quality = require_map(params.get("quality"), "estimated_params.quality") + if not bool(quality.get("data_quality_passed", False)): + raise ValueError("estimated_params.quality.data_quality_passed 必须为 true。") + if not bool(quality.get("suitable_for_commit", False)): + raise ValueError("estimated_params.quality.suitable_for_commit 必须为 true。") + return parameter_version, chassis_type, control_axis, controller_algorithm, control_role, estimated + + +def resolve_output_path(args: argparse.Namespace, session: dict[str, Any]) -> Path: + if args.output: + return Path(args.output).expanduser().resolve(strict=False) + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + return dataset_root / "control" / "pending_control_commit.yaml" + + +def build_commit_package( + dataset_index_path: Path, + estimated_params_path: Path, + index: dict[str, Any], + params: dict[str, Any], + args: argparse.Namespace, + output_path: Path, +) -> dict[str, Any]: + session = index["session"] + parameter_version, chassis_type, control_axis, controller_algorithm, control_role, estimated = validate_estimated_params( + params, + session, + index, + ) + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + created_us = now_us() + return { + "schema_version": 1, + "commit_state": "pending_manual_approval" if args.require_manual_approval else "pending_vehicle_commit", + "created_timestamp_us": created_us, + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "source": { + "dataset_index_file": relative_to_root(dataset_index_path, dataset_root), + "estimated_params_file": relative_to_root(estimated_params_path, dataset_root), + "estimated_params_digest": { + "checksum_type": "sha256", + "checksum_value": sha256_file(estimated_params_path), + }, + }, + "approval": { + "required": bool(args.require_manual_approval), + "approved": False, + "operator_id": args.operator_id, + "approved_timestamp_us": 0, + }, + "commit_request": { + "parameter_version": parameter_version, + "chassis_type": chassis_type, + "control_axis": control_axis, + "controller_algorithm": controller_algorithm, + "control_role": control_role, + "commit_reason": args.commit_reason, + "persistent_write": bool(args.persistent_write), + "estimated_params": estimated, + }, + "rollback": { + "previous_parameter_version": args.previous_parameter_version, + "rollback_required_on_vehicle_commit_failure": True, + "rollback_verified": False, + }, + "trace": { + "pending_commit_file": relative_to_root(output_path, dataset_root), + "generated_by": "stage_control_parameter_commit.py", + }, + } + + +def update_dataset_index(index_path: Path, index: dict[str, Any], output_path: Path) -> None: + session = index["session"] + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + metadata = index.setdefault("metadata", {}) + commit_package = load_yaml(output_path) + source = require_map(commit_package.get("source"), "pending_commit.source") + digest = require_map(source.get("estimated_params_digest"), "pending_commit.source.estimated_params_digest") + approval = require_map(commit_package.get("approval"), "pending_commit.approval") + commit_request = require_map(commit_package.get("commit_request"), "pending_commit.commit_request") + metadata["control_estimated_params_file"] = source.get("estimated_params_file", "") + metadata["control_estimated_params_checksum_type"] = digest.get("checksum_type", "") + metadata["control_estimated_params_checksum_value"] = digest.get("checksum_value", "") + metadata["control_pending_commit_file"] = relative_to_root(output_path, dataset_root) + metadata["control_pending_commit_state"] = commit_package.get("commit_state", "") + metadata["control_pending_parameter_version"] = commit_request.get("parameter_version", "") + metadata["control_pending_commit_chassis_type"] = commit_request.get("chassis_type", "") + metadata["control_pending_commit_control_axis"] = commit_request.get("control_axis", "") + metadata["control_pending_commit_controller_algorithm"] = commit_request.get("controller_algorithm", "") + metadata["control_pending_commit_control_role"] = commit_request.get("control_role", "") + metadata["control_pending_commit_approval_required"] = bool(approval.get("required", False)) + metadata["control_pending_commit_approved"] = bool(approval.get("approved", False)) + write_yaml(index_path, index) + + +def main() -> int: + args = parse_args() + dataset_index_path = Path(args.dataset_index).expanduser().resolve(strict=False) + estimated_params_path = Path(args.estimated_params).expanduser().resolve(strict=False) + index = load_yaml(dataset_index_path) + params = load_yaml(estimated_params_path) + session = validate_dataset_index(index) + output_path = resolve_output_path(args, session) + commit_package = build_commit_package( + dataset_index_path, + estimated_params_path, + index, + params, + args, + output_path, + ) + write_yaml(output_path, commit_package) + if args.update_dataset_index: + update_dataset_index(dataset_index_path, index, output_path) + print(f"[OK] 已生成运控待提交参数包: {output_path}") + print(f"[OK] commit_state={commit_package['commit_state']}") + print(f"[OK] parameter_version={commit_package['commit_request']['parameter_version']}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/README.md b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/README.md new file mode 100644 index 0000000..a04cdde --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/README.md @@ -0,0 +1,145 @@ +# 四角 LiDAR 外部真值定位模板 + +这个目录定义真实车间外部真值定位的代码模板。目标场景是: + +```text +车间四角固定 LiDAR + -> 观测车端标定球 + -> 算法求解车辆 base_link 在车间坐标系下的位姿 + -> 发布 ExternalLocalizationTelemetry + -> external_localization_service 做 readiness、质量校验和 report 回填 +``` + +## 边界 + +算法实现者主要修改: + +- `four_lidar_ball_algorithm.py` + +模板维护者主要维护: + +- `workshop_external_lidar_localization_node.py` +- `workshop_external_lidar_common.py` +- `replay_four_lidar_smoke.py` +- `config/four_lidar_ball_template.yaml` +- 与总控、external service、report 的接口约定 + +车端 agent 不实现这套外部真值定位算法。车端如果需要 external pose,只接收车间电脑侧已经求出的位姿。 + +## 算法输入 + +`FourLidarBallLocalizationInput` 已固定这些输入: + +- `frames`:四角 LiDAR 最近一帧数据,点坐标保持在各自 LiDAR frame 下。 +- `lidar_mounts`:每台 LiDAR 的 topic、frame_id 和车间安装位姿。 +- `ball_targets`:车端标定球在 `base_link` 下的安装位置和半径。 +- `max_frame_age_ms`:允许参与融合的最大数据年龄。 +- `reference_source_name`:外部真值源名称。 +- `workcell_zone_id`:工位区域 ID。 + +## 算法输出 + +算法必须填充 `FourLidarBallLocalizationOutput`: + +- `pose_valid`:本帧是否得到有效车辆位姿。 +- `workshop_pose`:车辆 `base_link` 在车间坐标系下的位姿。 +- `position_stddev_m`:位置不确定度。 +- `yaw_stddev_rad`:航向不确定度。 +- `tracking_loss_ratio`:跟踪丢失比例。 +- `time_sync_offset_ms`:时间同步偏差。 +- `quality_score`:质量分数,建议范围 `[0, 1]`。 +- `observed_target_count`:本帧成功观测到的标定球数量。 +- `diagnostic_messages`:调试和质量诊断信息。 + +## 运行方式 + +先复制模板配置并填写真实现场参数: + +```bash +cp src/site_deployment/workshop_external_lidar_localization_real/config/four_lidar_ball_template.yaml \ + /data/agv_calib/site_a/four_lidar_ball.yaml +``` + +启动节点: + +```bash +source install/setup.bash +python3 src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_localization_node.py \ + --config /data/agv_calib/site_a/four_lidar_ball.yaml \ + --session-dir /data/agv_calib/site_a/session_001 \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id AGV-001 +``` + +启动后节点会额外发布诊断 JSON 字符串: + +```text +/workshop/external_localization/diagnostics +``` + +诊断内容包括每台 LiDAR 是否有帧、帧年龄、点数、缺失/过期 LiDAR、当前位姿质量、算法诊断消息和落盘文件路径。 + +## 配置校验 + +节点启动时会强校验现场配置: + +- `reference_source_name`、`workcell_zone_id` 不能为空或保留模板占位符。 +- `output_topic` 建议使用 `/workshop/` 前缀。 +- 默认要求 4 台 LiDAR 启用,至少 2 个 required 标定球。 +- 每台 LiDAR 必须填写真实 `topic`、`frame_id`、`message_type` 和车间安装位姿。 +- 每个标定球必须填写真实半径和 `base_link` 下安装坐标。 +- 启用落盘时,`recording.session_dir` 和 `recording.dataset_index_path` 不能保留模板占位符。 + +## 数据落盘 + +配置中 `recording.enabled=true` 时,节点会自动写入: + +- `external/external_observations.csv`:外部定位位姿、质量、标准差、有效 LiDAR 数等。 +- `external/marker_observations.jsonl`:算法输出的标定球观测列表。 +- `external/time_sync_diagnostics.jsonl`:每帧时间同步和 LiDAR 帧年龄诊断。 +- `dataset_index.yaml`:把以上三类文件写入 `data_inputs.external.*`。 + +后续可继续执行: + +```bash +python3 src/deployment/tools/finalize_site_session.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --session-dir /data/agv_calib/site_a/session_001 +``` + +生成的 `site_data_input.yaml` 会把这些外部定位数据注入 `external_localization_service`。 + +## 离线回放自检 + +没有 ROS 或真实 LiDAR 时,可以先验证配置、算法入口、诊断输出和落盘: + +```bash +python3 src/site_deployment/workshop_external_lidar_localization_real/replay_four_lidar_smoke.py \ + --config /data/agv_calib/site_a/four_lidar_ball.yaml \ + --session-dir /tmp/agv_calib_external_replay/session_001 \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id AGV-001 +``` + +如果有回放 JSONL,可通过 `--frames-jsonl` 输入。每行支持: + +```json +{"timestamp_us": 1711234567000000, "frames": [{"lidar_id": "corner_front_left", "frame_id": "lidar_corner_front_left", "points_xyz": [[1.0, 2.0, 0.5]]}]} +``` + +然后现场 launch 需要让 external service 订阅该节点输出: + +```text +external_telemetry_topic:=/workshop/external_localization/vehicle/pose +expected_reference_source_name:=workshop_four_lidar_ball_truth +``` + +## 关键约束 + +- 只有一个标定球时,通常只能确定车辆位置,不能唯一确定车辆 yaw。 +- 如果需要完整 `base_link` 位姿,建议至少两个球,三球及以上更稳健。 +- 四台 LiDAR 的外参必须先统一到同一个车间坐标系。 +- 算法输出的 `workshop_pose` 必须是车辆 `base_link` 位姿,不是某个标定球球心位姿。 +- 质量字段必须保守填写;质量不足时应发布 `pose_valid=false` 或降低 `quality_score`。 diff --git a/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/config/four_lidar_ball_template.yaml b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/config/four_lidar_ball_template.yaml new file mode 100644 index 0000000..b0ec1b3 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/config/four_lidar_ball_template.yaml @@ -0,0 +1,120 @@ +# 四角 LiDAR + 车端标定球外部真值定位配置模板。 +# 该文件只定义现场配置边界,真实数值必须由现场测量和安装标定得到。 + +reference_source_name: workshop_four_lidar_ball_truth +workcell_zone_id: workshop_external_lidar_zone_a +output_topic: /workshop/external_localization/vehicle/pose +diagnostics_topic: /workshop/external_localization/diagnostics +publish_hz: 20.0 +max_frame_age_ms: 100.0 +max_points_per_frame: 20000 + +# 现场配置强校验。 +# 四角 LiDAR 正式落地时建议四台全部在线;如果现场允许降级,可在真实配置中调低。 +config_validation: + min_enabled_lidar_count: 4 + min_required_ball_count: 2 + +# 外部定位观测落盘配置。 +# session_dir 表示本次标定会话根目录;以下三个文件会被写入 dataset_index.yaml 的 data_inputs.external。 +recording: + enabled: true + session_id: replace_with_session_id + site_id: replace_with_site_or_line_id + vehicle_id: replace_with_real_vehicle_id + session_dir: /data/agv_calib/replace_with_site_or_line_id/session_xxx + dataset_index_path: /data/agv_calib/replace_with_site_or_line_id/session_xxx/dataset_index.yaml + external_observation_file: external/external_observations.csv + marker_observation_file: external/marker_observations.jsonl + time_sync_diagnostic_file: external/time_sync_diagnostics.jsonl + dataset_index_update_every_n_samples: 20 + +# 四台固定 LiDAR 的现场安装位姿。 +# workshop_pose 表示 LiDAR 坐标系在车间坐标系下的位姿。 +# message_type 支持 pointcloud2 或 laser_scan。 +lidars: + - lidar_id: corner_front_left + enabled: true + topic: /site/lidar/corner_front_left/points + message_type: pointcloud2 + frame_id: lidar_corner_front_left + workshop_pose: + x_m: 0.0 + y_m: 0.0 + z_m: 1.2 + roll_rad: 0.0 + pitch_rad: 0.0 + yaw_rad: 0.0 + + - lidar_id: corner_front_right + enabled: true + topic: /site/lidar/corner_front_right/points + message_type: pointcloud2 + frame_id: lidar_corner_front_right + workshop_pose: + x_m: 8.0 + y_m: 0.0 + z_m: 1.2 + roll_rad: 0.0 + pitch_rad: 0.0 + yaw_rad: 3.1415926536 + + - lidar_id: corner_rear_left + enabled: true + topic: /site/lidar/corner_rear_left/points + message_type: pointcloud2 + frame_id: lidar_corner_rear_left + workshop_pose: + x_m: 0.0 + y_m: 6.0 + z_m: 1.2 + roll_rad: 0.0 + pitch_rad: 0.0 + yaw_rad: 0.0 + + - lidar_id: corner_rear_right + enabled: true + topic: /site/lidar/corner_rear_right/points + message_type: pointcloud2 + frame_id: lidar_corner_rear_right + workshop_pose: + x_m: 8.0 + y_m: 6.0 + z_m: 1.2 + roll_rad: 0.0 + pitch_rad: 0.0 + yaw_rad: 3.1415926536 + +# 车端标定球在 base_link 下的安装坐标。 +# 只有一个球通常只能确定位置,无法唯一确定车辆 yaw。 +# 需要输出完整车辆位姿时,建议至少配置两个球,三球及以上更稳健。 +ball_targets: + - ball_id: front_ball + radius_m: 0.075 + required: true + center_in_base_link: + x_m: 0.6 + y_m: 0.0 + z_m: 0.8 + roll_rad: 0.0 + pitch_rad: 0.0 + yaw_rad: 0.0 + + - ball_id: rear_ball + radius_m: 0.075 + required: true + center_in_base_link: + x_m: -0.6 + y_m: 0.0 + z_m: 0.8 + roll_rad: 0.0 + pitch_rad: 0.0 + yaw_rad: 0.0 + +# 算法实现者可使用这些阈值生成 quality_score、tracking_loss_ratio 和诊断信息。 +quality_thresholds: + min_visible_ball_count: 2 + max_position_stddev_m: 0.02 + max_yaw_stddev_rad: 0.01 + max_time_sync_offset_ms: 20.0 + min_quality_score: 0.8 diff --git a/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/four_lidar_ball_algorithm.py b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/four_lidar_ball_algorithm.py new file mode 100644 index 0000000..4f3663e --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/four_lidar_ball_algorithm.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +"""四角 LiDAR + 车端标定球外部真值定位算法模板。""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Sequence + + +@dataclass(frozen=True) +class Pose3DValue: + """车间坐标系下的三维位姿。""" + + x_m: float = 0.0 + y_m: float = 0.0 + z_m: float = 0.0 + roll_rad: float = 0.0 + pitch_rad: float = 0.0 + yaw_rad: float = 0.0 + + +@dataclass(frozen=True) +class LidarMountConfig: + """单台固定 LiDAR 在车间坐标系下的静态配置。""" + + lidar_id: str + topic: str + frame_id: str + workshop_pose: Pose3DValue + enabled: bool = True + + +@dataclass(frozen=True) +class BallTargetConfig: + """车端标定球在 base_link 坐标系下的安装配置。""" + + ball_id: str + radius_m: float + center_in_base_link: Pose3DValue + required: bool = True + + +@dataclass(frozen=True) +class LidarFrame: + """单帧 LiDAR 数据,点坐标保持在原始 LiDAR frame 下。""" + + lidar_id: str + frame_id: str + stamp_us: int + points_xyz: Sequence[tuple[float, float, float]] + point_count: int + + +@dataclass(frozen=True) +class BallObservation: + """算法识别出的单个标定球观测。""" + + ball_id: str + center_in_workshop: Pose3DValue + residual_m: float = 0.0 + quality_score: float = 0.0 + source_lidar_ids: Sequence[str] = field(default_factory=tuple) + + +@dataclass(frozen=True) +class FourLidarBallLocalizationInput: + """算法输入快照。""" + + frames: Sequence[LidarFrame] + lidar_mounts: Sequence[LidarMountConfig] + ball_targets: Sequence[BallTargetConfig] + max_frame_age_ms: float + publish_timestamp_us: int + reference_source_name: str + workcell_zone_id: str + + +@dataclass +class FourLidarBallLocalizationOutput: + """算法输出结果,最终会被节点转换成 ExternalLocalizationTelemetry。""" + + pose_valid: bool = False + workshop_pose: Pose3DValue = field(default_factory=Pose3DValue) + position_stddev_m: float = 0.0 + yaw_stddev_rad: float = 0.0 + tracking_loss_ratio: float = 1.0 + time_sync_offset_ms: float = 0.0 + quality_score: float = 0.0 + observed_target_count: int = 0 + hardware_timestamp_us: int = 0 + observed_ball_targets: list[BallObservation] = field(default_factory=list) + diagnostic_messages: list[str] = field(default_factory=list) + + +class FourLidarBallLocalizationAlgorithm: + """算法实现者只需要替换这个类里的 solve 方法。""" + + def solve(self, data: FourLidarBallLocalizationInput) -> FourLidarBallLocalizationOutput: + """根据四角 LiDAR 数据和标定球配置求解车辆 base_link 位姿。""" + output = FourLidarBallLocalizationOutput() + output.hardware_timestamp_us = data.publish_timestamp_us + output.diagnostic_messages.append( + "四角 LiDAR + 标定球定位算法尚未实现,请在 solve() 中填充真实求解逻辑。" + ) + return output diff --git a/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/replay_four_lidar_smoke.py b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/replay_four_lidar_smoke.py new file mode 100644 index 0000000..78f6e36 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/replay_four_lidar_smoke.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python3 +"""四角 LiDAR 外部真值定位离线回放和落盘自检入口。""" + +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path +from typing import Any + +from four_lidar_ball_algorithm import ( + FourLidarBallLocalizationAlgorithm, + FourLidarBallLocalizationInput, + FourLidarBallLocalizationOutput, + LidarFrame, +) +from workshop_external_lidar_common import ( + ExternalLocalizationRecorder, + build_diagnostics_payload, + build_frame_status, + load_config, + now_us, + parse_ball_targets, + parse_lidar_mounts, + validate_config, +) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="离线验证四角 LiDAR 外部真值定位配置、算法入口、诊断输出和数据落盘。" + ) + parser.add_argument("--config", required=True, help="四角 LiDAR + 标定球配置文件。") + parser.add_argument( + "--frames-jsonl", + default="", + help="可选回放数据,每行一个 JSON;不提供时使用空点云合成帧做链路自检。", + ) + parser.add_argument( + "--session-dir", + default="", + help="覆盖 recording.session_dir,并启用落盘。", + ) + parser.add_argument( + "--dataset-index", + default="", + help="覆盖 recording.dataset_index_path,并启用 dataset_index 写入。", + ) + parser.add_argument("--session-id", default="", help="覆盖 recording.session_id。") + parser.add_argument("--site-id", default="", help="覆盖 recording.site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 recording.vehicle_id。") + parser.add_argument( + "--disable-recording", + action="store_true", + help="关闭落盘,只检查配置和算法调用链路。", + ) + parser.add_argument( + "--require-valid-pose", + action="store_true", + help="要求至少输出一帧 pose_valid=true;当前算法模板未实现时不要开启。", + ) + return parser.parse_args() + + +def apply_overrides(config: dict[str, Any], args: argparse.Namespace) -> None: + recording = config.setdefault("recording", {}) + if args.session_dir: + recording["session_dir"] = args.session_dir + recording["enabled"] = True + if not args.dataset_index: + recording["dataset_index_path"] = "dataset_index.yaml" + if args.dataset_index: + recording["dataset_index_path"] = args.dataset_index + recording["enabled"] = True + if args.session_id: + recording["session_id"] = args.session_id + if args.site_id: + recording["site_id"] = args.site_id + if args.vehicle_id: + recording["vehicle_id"] = args.vehicle_id + if args.disable_recording: + recording["enabled"] = False + + +def points_from_value(value: Any) -> list[tuple[float, float, float]]: + result: list[tuple[float, float, float]] = [] + if not isinstance(value, list): + return result + for item in value: + if isinstance(item, dict): + result.append(( + float(item.get("x", item.get("x_m", 0.0)) or 0.0), + float(item.get("y", item.get("y_m", 0.0)) or 0.0), + float(item.get("z", item.get("z_m", 0.0)) or 0.0), + )) + elif isinstance(item, (list, tuple)) and len(item) >= 2: + z = item[2] if len(item) >= 3 else 0.0 + result.append((float(item[0]), float(item[1]), float(z))) + return result + + +def frame_from_dict(raw: dict[str, Any], fallback_timestamp_us: int) -> LidarFrame: + lidar_id = str(raw.get("lidar_id", "")).strip() + if not lidar_id: + raise ValueError(f"回放帧缺少 lidar_id: {raw!r}") + points = points_from_value(raw.get("points_xyz", raw.get("points", []))) + return LidarFrame( + lidar_id=lidar_id, + frame_id=str(raw.get("frame_id", lidar_id)), + stamp_us=int(raw.get("stamp_us", raw.get("timestamp_us", fallback_timestamp_us)) or fallback_timestamp_us), + points_xyz=points, + point_count=len(points), + ) + + +def load_replay_sets(path: Path) -> list[dict[str, LidarFrame]]: + frame_sets: list[dict[str, LidarFrame]] = [] + with path.open("r", encoding="utf-8") as stream: + for line_no, line in enumerate(stream, start=1): + line = line.strip() + if not line: + continue + raw = json.loads(line) + if not isinstance(raw, dict): + raise ValueError(f"{path}:{line_no} 每行必须是 JSON 对象。") + timestamp_us = int(raw.get("timestamp_us", now_us()) or now_us()) + frames: dict[str, LidarFrame] = {} + if isinstance(raw.get("frames"), list): + for item in raw["frames"]: + frame = frame_from_dict(item, timestamp_us) + frames[frame.lidar_id] = frame + elif isinstance(raw.get("lidars"), dict): + for lidar_id, item in raw["lidars"].items(): + item = item if isinstance(item, dict) else {} + item.setdefault("lidar_id", lidar_id) + frame = frame_from_dict(item, timestamp_us) + frames[frame.lidar_id] = frame + else: + frame = frame_from_dict(raw, timestamp_us) + frames[frame.lidar_id] = frame + frame_sets.append(frames) + return frame_sets + + +def synthetic_frame_set(config: dict[str, Any]) -> dict[str, LidarFrame]: + timestamp_us = now_us() + result: dict[str, LidarFrame] = {} + for item in config.get("lidars", []): + if not bool(item.get("enabled", True)): + continue + lidar_id = str(item["lidar_id"]) + result[lidar_id] = LidarFrame( + lidar_id=lidar_id, + frame_id=str(item.get("frame_id", lidar_id)), + stamp_us=timestamp_us, + points_xyz=[], + point_count=0, + ) + return result + + +def run_once( + config: dict[str, Any], + recorder: ExternalLocalizationRecorder, + latest_frames: dict[str, LidarFrame], +) -> FourLidarBallLocalizationOutput: + timestamp_us = now_us() + lidar_mounts = parse_lidar_mounts(config) + ball_targets = parse_ball_targets(config) + max_frame_age_ms = float(config.get("max_frame_age_ms", 100.0)) + frame_status = build_frame_status(timestamp_us, latest_frames, lidar_mounts, max_frame_age_ms) + max_age_us = int(max_frame_age_ms * 1000.0) + frames = [ + frame + for frame in latest_frames.values() + if timestamp_us - int(frame.stamp_us) <= max_age_us + ] + algorithm_input = FourLidarBallLocalizationInput( + frames=frames, + lidar_mounts=lidar_mounts, + ball_targets=ball_targets, + max_frame_age_ms=max_frame_age_ms, + publish_timestamp_us=timestamp_us, + reference_source_name=str(config.get("reference_source_name", "")), + workcell_zone_id=str(config.get("workcell_zone_id", "")), + ) + output = FourLidarBallLocalizationAlgorithm().solve(algorithm_input) + if not output.hardware_timestamp_us: + output.hardware_timestamp_us = timestamp_us + diagnostics = build_diagnostics_payload( + timestamp_us, + output, + frame_status, + str(config.get("reference_source_name", "")), + str(config.get("workcell_zone_id", "")), + recorder.recording_files(), + ) + recorder.record(output, frame_status, diagnostics) + print(json.dumps(diagnostics, ensure_ascii=False, separators=(",", ":"))) + return output + + +def main() -> int: + args = parse_args() + config_path = Path(args.config).expanduser().resolve(strict=False) + config = load_config(config_path) + apply_overrides(config, args) + validation = validate_config(config, config_path) + for warning in validation.warnings: + print(f"[WARN] {warning}", file=sys.stderr) + if not validation.ok: + for error in validation.errors: + print(f"[错误] {error}", file=sys.stderr) + return 1 + + recorder = ExternalLocalizationRecorder(config, config_path) + if args.frames_jsonl: + frame_sets = load_replay_sets(Path(args.frames_jsonl).expanduser().resolve(strict=False)) + else: + frame_sets = [synthetic_frame_set(config)] + + latest_frames: dict[str, LidarFrame] = {} + valid_count = 0 + for frame_set in frame_sets: + latest_frames.update(frame_set) + output = run_once(config, recorder, latest_frames) + if output.pose_valid: + valid_count += 1 + recorder.update_dataset_index() + + print(f"[OK] 回放完成: frames={len(frame_sets)}, valid_pose={valid_count}", file=sys.stderr) + if args.require_valid_pose and valid_count <= 0: + print("[错误] 没有输出有效外部定位位姿。", file=sys.stderr) + return 1 + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_common.py b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_common.py new file mode 100644 index 0000000..068b5ae --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_common.py @@ -0,0 +1,568 @@ +#!/usr/bin/env python3 +"""四角 LiDAR 外部真值定位的配置、诊断和落盘公共工具。""" + +from __future__ import annotations + +import csv +import json +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import yaml + +from four_lidar_ball_algorithm import ( + BallTargetConfig, + FourLidarBallLocalizationOutput, + LidarFrame, + LidarMountConfig, + Pose3DValue, +) + + +POINTCLOUD_TYPES = {"pointcloud2", "point_cloud2", "pointcloud"} +LASER_SCAN_TYPES = {"laserscan", "laser_scan", "scan"} +SUPPORTED_MESSAGE_TYPES = POINTCLOUD_TYPES | LASER_SCAN_TYPES + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def load_config(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + config = yaml.safe_load(stream) or {} + if not isinstance(config, dict): + raise ValueError(f"配置文件必须是 YAML 字典: {path}") + return config + + +def pose_from_dict(data: dict[str, Any] | None) -> Pose3DValue: + data = data or {} + return Pose3DValue( + x_m=float(data.get("x_m", 0.0) or 0.0), + y_m=float(data.get("y_m", 0.0) or 0.0), + z_m=float(data.get("z_m", 0.0) or 0.0), + roll_rad=float(data.get("roll_rad", 0.0) or 0.0), + pitch_rad=float(data.get("pitch_rad", 0.0) or 0.0), + yaw_rad=float(data.get("yaw_rad", 0.0) or 0.0), + ) + + +def parse_lidar_mounts(config: dict[str, Any]) -> list[LidarMountConfig]: + mounts: list[LidarMountConfig] = [] + for item in config.get("lidars", []): + mounts.append( + LidarMountConfig( + lidar_id=str(item["lidar_id"]), + topic=str(item["topic"]), + frame_id=str(item.get("frame_id", item["lidar_id"])), + workshop_pose=pose_from_dict(item.get("workshop_pose")), + enabled=bool(item.get("enabled", True)), + ) + ) + return mounts + + +def parse_ball_targets(config: dict[str, Any]) -> list[BallTargetConfig]: + targets: list[BallTargetConfig] = [] + for item in config.get("ball_targets", []): + targets.append( + BallTargetConfig( + ball_id=str(item["ball_id"]), + radius_m=float(item["radius_m"]), + center_in_base_link=pose_from_dict(item.get("center_in_base_link")), + required=bool(item.get("required", True)), + ) + ) + return targets + + +@dataclass +class ConfigValidationResult: + errors: list[str] = field(default_factory=list) + warnings: list[str] = field(default_factory=list) + + @property + def ok(self) -> bool: + return not self.errors + + +def _is_nonempty_string(value: Any) -> bool: + return isinstance(value, str) and bool(value.strip()) + + +def _has_placeholder(value: Any) -> bool: + if not isinstance(value, str): + return False + markers = ("replace_with", "measured_on_site", "session_xxx") + return any(marker in value for marker in markers) + + +def _require_pose_map(value: Any, field_name: str, result: ConfigValidationResult) -> None: + if not isinstance(value, dict): + result.errors.append(f"{field_name} 必须是 YAML 字典。") + return + for key in ("x_m", "y_m", "z_m", "roll_rad", "pitch_rad", "yaw_rad"): + try: + float(value.get(key, 0.0) or 0.0) + except (TypeError, ValueError): + result.errors.append(f"{field_name}.{key} 必须是数字。") + + +def validate_config(config: dict[str, Any], config_path: Path | None = None) -> ConfigValidationResult: + """校验现场配置是否满足四角 LiDAR 外部定位的最小落地要求。""" + + result = ConfigValidationResult() + validation = config.get("config_validation", {}) or {} + if not isinstance(validation, dict): + result.errors.append("config_validation 必须是 YAML 字典。") + validation = {} + + min_enabled_lidar_count = int(validation.get("min_enabled_lidar_count", 4) or 4) + min_required_ball_count = int(validation.get("min_required_ball_count", 2) or 2) + + output_topic = config.get("output_topic", "") + if not _is_nonempty_string(output_topic): + result.errors.append("output_topic 不能为空。") + elif not str(output_topic).startswith("/workshop/"): + result.warnings.append("output_topic 建议使用 /workshop/ 前缀,避免和仿真 topic 混淆。") + + reference_source_name = config.get("reference_source_name", "") + if not _is_nonempty_string(reference_source_name) or _has_placeholder(reference_source_name): + result.errors.append("reference_source_name 必须填写真实外部定位源名称。") + + workcell_zone_id = config.get("workcell_zone_id", "") + if not _is_nonempty_string(workcell_zone_id) or _has_placeholder(workcell_zone_id): + result.errors.append("workcell_zone_id 必须填写真实工位区域 ID。") + + publish_hz = float(config.get("publish_hz", 20.0) or 0.0) + if publish_hz <= 0.0: + result.errors.append("publish_hz 必须大于 0。") + elif publish_hz < 5.0: + result.warnings.append("publish_hz 低于 5Hz,运控使用外部位姿时可能不够平滑。") + + max_frame_age_ms = float(config.get("max_frame_age_ms", 100.0) or 0.0) + if max_frame_age_ms <= 0.0: + result.errors.append("max_frame_age_ms 必须大于 0。") + + lidars = config.get("lidars", []) + if not isinstance(lidars, list): + result.errors.append("lidars 必须是列表。") + lidars = [] + lidar_ids: set[str] = set() + enabled_lidar_count = 0 + for index, item in enumerate(lidars): + field = f"lidars[{index}]" + if not isinstance(item, dict): + result.errors.append(f"{field} 必须是 YAML 字典。") + continue + lidar_id = str(item.get("lidar_id", "")).strip() + if not lidar_id: + result.errors.append(f"{field}.lidar_id 不能为空。") + elif lidar_id in lidar_ids: + result.errors.append(f"{field}.lidar_id 重复: {lidar_id}") + lidar_ids.add(lidar_id) + if bool(item.get("enabled", True)): + enabled_lidar_count += 1 + topic = str(item.get("topic", "")).strip() + if not topic or _has_placeholder(topic): + result.errors.append(f"{field}.topic 必须填写真实 LiDAR topic。") + message_type = str(item.get("message_type", "pointcloud2")).strip().lower() + if message_type not in SUPPORTED_MESSAGE_TYPES: + result.errors.append(f"{field}.message_type 不支持: {message_type}") + frame_id = str(item.get("frame_id", "")).strip() + if not frame_id or _has_placeholder(frame_id): + result.errors.append(f"{field}.frame_id 必须填写真实 frame_id。") + _require_pose_map(item.get("workshop_pose"), f"{field}.workshop_pose", result) + + if enabled_lidar_count < min_enabled_lidar_count: + result.errors.append( + f"启用 LiDAR 数量不足,当前 {enabled_lidar_count},要求至少 {min_enabled_lidar_count}。" + ) + + ball_targets = config.get("ball_targets", []) + if not isinstance(ball_targets, list): + result.errors.append("ball_targets 必须是列表。") + ball_targets = [] + ball_ids: set[str] = set() + required_ball_count = 0 + for index, item in enumerate(ball_targets): + field = f"ball_targets[{index}]" + if not isinstance(item, dict): + result.errors.append(f"{field} 必须是 YAML 字典。") + continue + ball_id = str(item.get("ball_id", "")).strip() + if not ball_id: + result.errors.append(f"{field}.ball_id 不能为空。") + elif ball_id in ball_ids: + result.errors.append(f"{field}.ball_id 重复: {ball_id}") + ball_ids.add(ball_id) + if bool(item.get("required", True)): + required_ball_count += 1 + try: + radius_m = float(item.get("radius_m", 0.0) or 0.0) + except (TypeError, ValueError): + radius_m = 0.0 + if radius_m <= 0.0: + result.errors.append(f"{field}.radius_m 必须大于 0。") + _require_pose_map(item.get("center_in_base_link"), f"{field}.center_in_base_link", result) + + if required_ball_count < min_required_ball_count: + result.errors.append( + f"required 标定球数量不足,当前 {required_ball_count},要求至少 {min_required_ball_count}。" + ) + + recording = config.get("recording", {}) or {} + if not isinstance(recording, dict): + result.errors.append("recording 必须是 YAML 字典。") + recording = {} + if bool(recording.get("enabled", False)): + for field_name in ("session_id", "site_id", "vehicle_id"): + value = str(recording.get(field_name, "")).strip() + if not value or _has_placeholder(value): + result.errors.append(f"recording.{field_name} 必须填写真实值。") + session_dir = str(recording.get("session_dir", "")).strip() + if not session_dir or _has_placeholder(session_dir): + result.errors.append("recording.session_dir 必须填写真实会话目录。") + dataset_index_path = str(recording.get("dataset_index_path", "")).strip() + if dataset_index_path and _has_placeholder(dataset_index_path): + result.errors.append("recording.dataset_index_path 不能保留模板占位符。") + + if config_path is not None and not config_path.exists(): + result.errors.append(f"配置文件不存在: {config_path}") + return result + + +def message_type_for_lidar(config: dict[str, Any], lidar_id: str) -> str: + for item in config.get("lidars", []): + if str(item.get("lidar_id", "")) == lidar_id: + return str(item.get("message_type", "pointcloud2")).lower() + return "pointcloud2" + + +def build_frame_status( + timestamp_us: int, + latest_frames: dict[str, LidarFrame], + lidar_mounts: list[LidarMountConfig], + max_frame_age_ms: float, +) -> dict[str, Any]: + max_age_us = int(max_frame_age_ms * 1000.0) + lidar_status: list[dict[str, Any]] = [] + missing_lidar_ids: list[str] = [] + stale_lidar_ids: list[str] = [] + recent_lidar_ids: list[str] = [] + for mount in lidar_mounts: + if not mount.enabled: + continue + frame = latest_frames.get(mount.lidar_id) + if frame is None: + missing_lidar_ids.append(mount.lidar_id) + lidar_status.append({ + "lidar_id": mount.lidar_id, + "frame_id": mount.frame_id, + "present": False, + "fresh": False, + "age_ms": None, + "point_count": 0, + }) + continue + age_us = timestamp_us - int(frame.stamp_us) + fresh = age_us <= max_age_us + if fresh: + recent_lidar_ids.append(mount.lidar_id) + else: + stale_lidar_ids.append(mount.lidar_id) + lidar_status.append({ + "lidar_id": mount.lidar_id, + "frame_id": frame.frame_id, + "present": True, + "fresh": fresh, + "age_ms": max(0.0, float(age_us) / 1000.0), + "point_count": int(frame.point_count), + }) + return { + "lidars": lidar_status, + "enabled_lidar_count": sum(1 for mount in lidar_mounts if mount.enabled), + "recent_lidar_count": len(recent_lidar_ids), + "recent_lidar_ids": recent_lidar_ids, + "missing_lidar_ids": missing_lidar_ids, + "stale_lidar_ids": stale_lidar_ids, + } + + +def pose_to_dict(pose: Pose3DValue) -> dict[str, float]: + return { + "x_m": float(pose.x_m), + "y_m": float(pose.y_m), + "z_m": float(pose.z_m), + "roll_rad": float(pose.roll_rad), + "pitch_rad": float(pose.pitch_rad), + "yaw_rad": float(pose.yaw_rad), + } + + +def output_to_dict(output: FourLidarBallLocalizationOutput) -> dict[str, Any]: + return { + "pose_valid": bool(output.pose_valid), + "workshop_pose": pose_to_dict(output.workshop_pose), + "position_stddev_m": float(output.position_stddev_m), + "yaw_stddev_rad": float(output.yaw_stddev_rad), + "tracking_loss_ratio": float(output.tracking_loss_ratio), + "time_sync_offset_ms": float(output.time_sync_offset_ms), + "quality_score": float(output.quality_score), + "observed_target_count": int(output.observed_target_count), + "hardware_timestamp_us": int(output.hardware_timestamp_us), + "diagnostic_messages": list(output.diagnostic_messages), + } + + +def build_diagnostics_payload( + timestamp_us: int, + output: FourLidarBallLocalizationOutput, + frame_status: dict[str, Any], + reference_source_name: str, + workcell_zone_id: str, + recording_files: dict[str, str] | None = None, +) -> dict[str, Any]: + payload = { + "timestamp_us": int(timestamp_us), + "reference_source_name": reference_source_name, + "workcell_zone_id": workcell_zone_id, + "pose": output_to_dict(output), + "frame_status": frame_status, + "recording_files": recording_files or {}, + } + payload["healthy"] = ( + bool(output.pose_valid) + and frame_status.get("recent_lidar_count", 0) > 0 + and float(output.quality_score) > 0.0 + ) + return payload + + +def _resolve_recording_path(session_dir: Path, value: str, default_value: str) -> Path: + raw = value or default_value + path = Path(raw).expanduser() + if path.is_absolute(): + return path + return session_dir / path + + +def _relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def _load_yaml_if_exists(path: Path) -> dict[str, Any]: + if not path.exists(): + return {} + 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 字典。") + return data + + +class ExternalLocalizationRecorder: + """把外部定位实时输出写入本次会话数据集索引。""" + + def __init__(self, config: dict[str, Any], config_path: Path) -> None: + recording = config.get("recording", {}) or {} + self.enabled = bool(recording.get("enabled", False)) + self.sample_count = 0 + self.update_every_n_samples = int(recording.get("dataset_index_update_every_n_samples", 20) or 20) + self.start_us = now_us() + self.end_us = self.start_us + self.site_id = str(recording.get("site_id", "")) + self.vehicle_id = str(recording.get("vehicle_id", "")) + self.session_id = str(recording.get("session_id", "")) + self.config_path = config_path + self._paths: dict[str, Path] = {} + if not self.enabled: + self.session_dir = Path("") + self.dataset_index_path = Path("") + return + + session_dir_raw = str(recording.get("session_dir", "")).strip() + self.session_dir = Path(session_dir_raw).expanduser().resolve(strict=False) + self.dataset_index_path = _resolve_recording_path( + self.session_dir, + str(recording.get("dataset_index_path", "")), + "dataset_index.yaml", + ) + self._paths = { + "external_observation_files": _resolve_recording_path( + self.session_dir, + str(recording.get("external_observation_file", "")), + "external/external_observations.csv", + ), + "marker_observation_files": _resolve_recording_path( + self.session_dir, + str(recording.get("marker_observation_file", "")), + "external/marker_observations.jsonl", + ), + "time_sync_diagnostic_files": _resolve_recording_path( + self.session_dir, + str(recording.get("time_sync_diagnostic_file", "")), + "external/time_sync_diagnostics.jsonl", + ), + } + for path in self._paths.values(): + path.parent.mkdir(parents=True, exist_ok=True) + self.dataset_index_path.parent.mkdir(parents=True, exist_ok=True) + self._ensure_observation_header() + self.update_dataset_index() + + def recording_files(self) -> dict[str, str]: + if not self.enabled: + return {} + return {key: str(path) for key, path in self._paths.items()} + + def _ensure_observation_header(self) -> None: + path = self._paths["external_observation_files"] + if path.exists() and path.stat().st_size > 0: + return + with path.open("w", newline="", encoding="utf-8") as stream: + writer = csv.writer(stream) + writer.writerow([ + "hardware_timestamp_us", + "pose_valid", + "x_m", + "y_m", + "z_m", + "roll_rad", + "pitch_rad", + "yaw_rad", + "position_stddev_m", + "yaw_stddev_rad", + "tracking_loss_ratio", + "time_sync_offset_ms", + "quality_score", + "observed_target_count", + "recent_lidar_count", + "enabled_lidar_count", + "diagnostic_messages", + ]) + + def record( + self, + output: FourLidarBallLocalizationOutput, + frame_status: dict[str, Any], + diagnostics_payload: dict[str, Any], + ) -> None: + if not self.enabled: + return + self.sample_count += 1 + self.end_us = int(diagnostics_payload["timestamp_us"]) + self._append_external_observation(output, frame_status) + self._append_marker_observations(output, diagnostics_payload) + self._append_time_sync_diagnostic(diagnostics_payload) + if self.sample_count % max(1, self.update_every_n_samples) == 0: + self.update_dataset_index() + + def _append_external_observation( + self, + output: FourLidarBallLocalizationOutput, + frame_status: dict[str, Any], + ) -> None: + path = self._paths["external_observation_files"] + pose = output.workshop_pose + with path.open("a", newline="", encoding="utf-8") as stream: + writer = csv.writer(stream) + writer.writerow([ + int(output.hardware_timestamp_us), + int(bool(output.pose_valid)), + float(pose.x_m), + float(pose.y_m), + float(pose.z_m), + float(pose.roll_rad), + float(pose.pitch_rad), + float(pose.yaw_rad), + float(output.position_stddev_m), + float(output.yaw_stddev_rad), + float(output.tracking_loss_ratio), + float(output.time_sync_offset_ms), + float(output.quality_score), + int(output.observed_target_count), + int(frame_status.get("recent_lidar_count", 0)), + int(frame_status.get("enabled_lidar_count", 0)), + "|".join(output.diagnostic_messages), + ]) + + def _append_marker_observations( + self, + output: FourLidarBallLocalizationOutput, + diagnostics_payload: dict[str, Any], + ) -> None: + path = self._paths["marker_observation_files"] + entry = { + "timestamp_us": int(diagnostics_payload["timestamp_us"]), + "observed_target_count": int(output.observed_target_count), + "observed_ball_targets": [ + { + "ball_id": item.ball_id, + "center_in_workshop": pose_to_dict(item.center_in_workshop), + "residual_m": float(item.residual_m), + "quality_score": float(item.quality_score), + "source_lidar_ids": list(item.source_lidar_ids), + } + for item in output.observed_ball_targets + ], + } + with path.open("a", encoding="utf-8") as stream: + stream.write(json.dumps(entry, ensure_ascii=False, separators=(",", ":")) + "\n") + + def _append_time_sync_diagnostic(self, diagnostics_payload: dict[str, Any]) -> None: + path = self._paths["time_sync_diagnostic_files"] + entry = { + "timestamp_us": int(diagnostics_payload["timestamp_us"]), + "time_sync_offset_ms": diagnostics_payload["pose"]["time_sync_offset_ms"], + "frame_status": diagnostics_payload["frame_status"], + "healthy": diagnostics_payload["healthy"], + } + with path.open("a", encoding="utf-8") as stream: + stream.write(json.dumps(entry, ensure_ascii=False, separators=(",", ":")) + "\n") + + def update_dataset_index(self) -> None: + if not self.enabled: + return + index = _load_yaml_if_exists(self.dataset_index_path) + index.setdefault("schema_version", 1) + session = index.setdefault("session", {}) + if not isinstance(session, dict): + raise ValueError("dataset_index.yaml 中的 session 必须是 YAML 字典。") + session.setdefault("session_id", self.session_id) + session.setdefault("site_id", self.site_id) + session.setdefault("vehicle_id", self.vehicle_id) + session["dataset_root"] = str(self.session_dir) + session["data_window_start_timestamp_us"] = int(session.get("data_window_start_timestamp_us") or self.start_us) + session["data_window_end_timestamp_us"] = int(self.end_us) + + data_inputs = index.setdefault("data_inputs", {}) + external = data_inputs.setdefault("external", {}) + for field_name, path in self._paths.items(): + value = _relative_to_root(path, self.session_dir) + files = external.setdefault(field_name, []) + if isinstance(files, str): + files = [files] + external[field_name] = files + if value not in files: + files.append(value) + + metadata = index.setdefault("metadata", {}) + metadata["external_lidar_config_file"] = str(self.config_path) + metadata["external_lidar_sample_count"] = int(self.sample_count) + if self.session_id: + session["session_id"] = self.session_id + if self.site_id: + session["site_id"] = self.site_id + if self.vehicle_id: + session["vehicle_id"] = self.vehicle_id + self.dataset_index_path.write_text( + yaml.safe_dump(index, sort_keys=False, allow_unicode=True), + encoding="utf-8", + ) diff --git a/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_localization_node.py b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_localization_node.py new file mode 100644 index 0000000..4bee112 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_localization_node.py @@ -0,0 +1,290 @@ +#!/usr/bin/env python3 +"""车间电脑侧四角 LiDAR 外部真值定位节点模板。""" + +from __future__ import annotations + +import argparse +import json +import math +from pathlib import Path +from typing import Any + +import rclpy +from rclpy.node import Node + +from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry +from sensor_msgs.msg import LaserScan, PointCloud2 +from sensor_msgs_py import point_cloud2 +from std_msgs.msg import String + +from four_lidar_ball_algorithm import ( + FourLidarBallLocalizationAlgorithm, + FourLidarBallLocalizationInput, + FourLidarBallLocalizationOutput, + LidarFrame, +) +from workshop_external_lidar_common import ( + ExternalLocalizationRecorder, + build_diagnostics_payload, + build_frame_status, + load_config, + message_type_for_lidar, + now_us, + parse_ball_targets, + parse_lidar_mounts, + validate_config, +) + + +def msg_stamp_us(msg: Any) -> int: + stamp = getattr(getattr(msg, "header", None), "stamp", None) + if stamp is None: + return now_us() + return int(stamp.sec) * 1_000_000 + int(stamp.nanosec) // 1_000 + + +class WorkshopExternalLidarLocalizationNode(Node): + """固定 ROS 通信和配置加载,真实定位算法只在算法类中实现。""" + + def __init__(self, args: argparse.Namespace) -> None: + super().__init__("workshop_external_lidar_localization_real") + self.config_path = Path(args.config).expanduser().resolve() + self.config = load_config(self.config_path) + self.apply_cli_overrides(args) + validation = validate_config(self.config, self.config_path) + for warning in validation.warnings: + self.get_logger().warning(f"外部真值定位配置警告: {warning}") + if not validation.ok: + raise ValueError("外部真值定位配置无效:\n - " + "\n - ".join(validation.errors)) + + self.reference_source_name = str( + self.config.get("reference_source_name", "workshop_four_lidar_ball_truth") + ) + self.workcell_zone_id = str(self.config.get("workcell_zone_id", "workshop_external_lidar_zone")) + self.max_frame_age_ms = float(self.config.get("max_frame_age_ms", 100.0)) + self.max_points_per_frame = int(self.config.get("max_points_per_frame", 20000)) + self.lidar_mounts = parse_lidar_mounts(self.config) + self.ball_targets = parse_ball_targets(self.config) + self.latest_frames: dict[str, LidarFrame] = {} + self.algorithm = FourLidarBallLocalizationAlgorithm() + self.recorder = ExternalLocalizationRecorder(self.config, self.config_path) + self.publisher = self.create_publisher( + ExternalLocalizationTelemetry, + str(self.config.get("output_topic", "/workshop/external_localization/vehicle/pose")), + 10, + ) + self.diagnostics_publisher = self.create_publisher( + String, + str(self.config.get("diagnostics_topic", "/workshop/external_localization/diagnostics")), + 10, + ) + + for mount in self.lidar_mounts: + if not mount.enabled: + continue + message_type = message_type_for_lidar(self.config, mount.lidar_id) + if message_type in ("pointcloud2", "point_cloud2", "pointcloud"): + self.create_subscription( + PointCloud2, + mount.topic, + lambda msg, lidar_id=mount.lidar_id: self.on_point_cloud(lidar_id, msg), + 10, + ) + elif message_type in ("laserscan", "laser_scan", "scan"): + self.create_subscription( + LaserScan, + mount.topic, + lambda msg, lidar_id=mount.lidar_id: self.on_laser_scan(lidar_id, msg), + 10, + ) + else: + raise ValueError(f"不支持的 LiDAR 消息类型: {mount.lidar_id} {message_type}") + + publish_hz = float(self.config.get("publish_hz", 20.0)) + period_sec = 1.0 / publish_hz if publish_hz > 0.0 else 0.05 + self.create_timer(period_sec, self.on_timer) + self.get_logger().info( + f"四角 LiDAR 外部真值定位模板已启动: config={self.config_path}, source={self.reference_source_name}" + ) + if self.recorder.enabled: + self.get_logger().info( + f"外部真值定位数据落盘已启用: dataset_index={self.recorder.dataset_index_path}" + ) + + def apply_cli_overrides(self, args: argparse.Namespace) -> None: + recording = self.config.setdefault("recording", {}) + if args.session_dir: + recording["session_dir"] = args.session_dir + recording["enabled"] = True + if not args.dataset_index: + recording["dataset_index_path"] = "dataset_index.yaml" + if args.dataset_index: + recording["dataset_index_path"] = args.dataset_index + recording["enabled"] = True + if args.session_id: + recording["session_id"] = args.session_id + if args.site_id: + recording["site_id"] = args.site_id + if args.vehicle_id: + recording["vehicle_id"] = args.vehicle_id + if args.disable_recording: + recording["enabled"] = False + + def on_point_cloud(self, lidar_id: str, msg: PointCloud2) -> None: + points: list[tuple[float, float, float]] = [] + for point in point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): + if isinstance(point, dict): + x, y, z = point["x"], point["y"], point["z"] + else: + x, y, z = point[0], point[1], point[2] + points.append((float(x), float(y), float(z))) + if len(points) >= self.max_points_per_frame: + break + + frame_id = str(getattr(msg.header, "frame_id", "") or lidar_id) + self.latest_frames[lidar_id] = LidarFrame( + lidar_id=lidar_id, + frame_id=frame_id, + stamp_us=msg_stamp_us(msg), + points_xyz=points, + point_count=len(points), + ) + + def on_laser_scan(self, lidar_id: str, msg: LaserScan) -> None: + points: list[tuple[float, float, float]] = [] + angle = float(msg.angle_min) + for raw_range in msg.ranges: + distance = float(raw_range) + if math.isfinite(distance) and msg.range_min <= distance <= msg.range_max: + points.append((distance * math.cos(angle), distance * math.sin(angle), 0.0)) + if len(points) >= self.max_points_per_frame: + break + angle += float(msg.angle_increment) + + frame_id = str(getattr(msg.header, "frame_id", "") or lidar_id) + self.latest_frames[lidar_id] = LidarFrame( + lidar_id=lidar_id, + frame_id=frame_id, + stamp_us=msg_stamp_us(msg), + points_xyz=points, + point_count=len(points), + ) + + def recent_frames(self, timestamp_us: int) -> list[LidarFrame]: + max_age_us = int(self.max_frame_age_ms * 1000.0) + frames = [] + for frame in self.latest_frames.values(): + if timestamp_us - frame.stamp_us <= max_age_us: + frames.append(frame) + return frames + + def on_timer(self) -> None: + publish_timestamp_us = now_us() + frames = self.recent_frames(publish_timestamp_us) + frame_status = build_frame_status( + publish_timestamp_us, + self.latest_frames, + self.lidar_mounts, + self.max_frame_age_ms, + ) + algorithm_input = FourLidarBallLocalizationInput( + frames=frames, + lidar_mounts=self.lidar_mounts, + ball_targets=self.ball_targets, + max_frame_age_ms=self.max_frame_age_ms, + publish_timestamp_us=publish_timestamp_us, + reference_source_name=self.reference_source_name, + workcell_zone_id=self.workcell_zone_id, + ) + try: + output = self.algorithm.solve(algorithm_input) + except Exception as exc: + output = FourLidarBallLocalizationOutput() + output.hardware_timestamp_us = publish_timestamp_us + output.diagnostic_messages.append(f"外部真值定位算法异常: {exc}") + self.get_logger().warning(output.diagnostic_messages[-1]) + if not output.hardware_timestamp_us: + output.hardware_timestamp_us = publish_timestamp_us + diagnostics_payload = build_diagnostics_payload( + publish_timestamp_us, + output, + frame_status, + self.reference_source_name, + self.workcell_zone_id, + self.recorder.recording_files(), + ) + self.publish_telemetry(output, publish_timestamp_us) + self.publish_diagnostics(diagnostics_payload) + self.recorder.record(output, frame_status, diagnostics_payload) + + def publish_telemetry(self, output: FourLidarBallLocalizationOutput, publish_timestamp_us: int) -> None: + msg = ExternalLocalizationTelemetry() + msg.hardware_timestamp_us = int(output.hardware_timestamp_us or publish_timestamp_us) + msg.pose_valid = bool(output.pose_valid) + msg.workshop_pose.x_m = float(output.workshop_pose.x_m) + msg.workshop_pose.y_m = float(output.workshop_pose.y_m) + msg.workshop_pose.z_m = float(output.workshop_pose.z_m) + msg.workshop_pose.roll_rad = float(output.workshop_pose.roll_rad) + msg.workshop_pose.pitch_rad = float(output.workshop_pose.pitch_rad) + msg.workshop_pose.yaw_rad = float(output.workshop_pose.yaw_rad) + msg.position_stddev_m = float(output.position_stddev_m) + msg.yaw_stddev_rad = float(output.yaw_stddev_rad) + msg.tracking_loss_ratio = float(output.tracking_loss_ratio) + msg.time_sync_offset_ms = float(output.time_sync_offset_ms) + msg.quality_score = float(output.quality_score) + msg.observed_target_count = int(output.observed_target_count) + msg.reference_source_name = self.reference_source_name + msg.active_job_id = f"{self.workcell_zone_id}:four_lidar_ball_localization" + self.publisher.publish(msg) + + def publish_diagnostics(self, diagnostics_payload: dict[str, Any]) -> None: + msg = String() + msg.data = json.dumps(diagnostics_payload, ensure_ascii=False, separators=(",", ":")) + self.diagnostics_publisher.publish(msg) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="四角 LiDAR + 标定球外部真值定位节点模板") + parser.add_argument( + "--config", + required=True, + help="四角 LiDAR + 标定球定位配置文件路径", + ) + parser.add_argument( + "--session-dir", + default="", + help="覆盖 recording.session_dir,并启用本次会话数据落盘。", + ) + parser.add_argument( + "--dataset-index", + default="", + help="覆盖 recording.dataset_index_path,并启用数据集索引写入。", + ) + parser.add_argument("--session-id", default="", help="覆盖 recording.session_id。") + parser.add_argument("--site-id", default="", help="覆盖 recording.site_id。") + parser.add_argument("--vehicle-id", default="", help="覆盖 recording.vehicle_id。") + parser.add_argument( + "--disable-recording", + action="store_true", + help="关闭外部定位观测和诊断落盘。", + ) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + rclpy.init(args=None) + node = None + try: + node = WorkshopExternalLidarLocalizationNode(args) + rclpy.spin(node) + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/agv_calib_brain/src/site_deployment/workshop_external_pose_bridge_real/README.md b/agv_calib_brain/src/site_deployment/workshop_external_pose_bridge_real/README.md new file mode 100644 index 0000000..45d3e3c --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_pose_bridge_real/README.md @@ -0,0 +1,41 @@ +# 外部定位位姿车端桥 + +这个目录是现场正式桥接入口。它订阅车间电脑侧已经求出的车辆外部定位位姿,并通过 WiFi6/TCP 推送给车端 agent。 + +```text +/workshop/external_localization/vehicle/pose + -> workshop_external_pose_bridge.py + -> WiFi6/TCP frame type=31 + -> vehicle_agent_real +``` + +## 边界 + +- 这个 bridge 不做四角 LiDAR + 标定球定位算法。 +- 这个 bridge 只转发 `ExternalLocalizationTelemetry`。 +- 车端 agent 只消费已经求出的车辆位姿,不反算外部真值。 +- 仿真端也复用同一个 bridge,只是把 source topic 和车端地址改成仿真配置。 + +## 现场运行示例 + +```bash +source install/setup.bash +python3 src/site_deployment/workshop_external_pose_bridge_real/workshop_external_pose_bridge.py \ + --source-topic /workshop/external_localization/vehicle/pose \ + --vehicle-host 192.168.10.42 \ + --vehicle-port 9000 \ + --reference-source-name workshop_four_lidar_ball_truth +``` + +## 仿真复用方式 + +仿真启动栈会调用同一个脚本,并通过 `sim_workshop.yaml` 注入: + +```text +source_topic: /isaac/external_localization/vehicle/pose +vehicle_host: 127.0.0.1 +vehicle_port: 9000 +``` + +旧的 `src/simulation/tools/external_pose_wifi6_bridge.py` 只保留为兼容入口,内部转到本目录脚本。 + diff --git a/agv_calib_brain/src/site_deployment/workshop_external_pose_bridge_real/workshop_external_pose_bridge.py b/agv_calib_brain/src/site_deployment/workshop_external_pose_bridge_real/workshop_external_pose_bridge.py new file mode 100644 index 0000000..88a9115 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_external_pose_bridge_real/workshop_external_pose_bridge.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 +"""车间外部定位车辆位姿到车端 WiFi6/TCP 的桥接节点。""" + +from __future__ import annotations + +import argparse +import json +import socket +import struct +import time +from typing import Any + +import rclpy +from rclpy.executors import ExternalShutdownException + +try: + from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry +except ImportError: + ExternalLocalizationTelemetry = None + + +EXTERNAL_POSE_PUSH_REQ = 31 +EXTERNAL_POSE_PUSH_RSP = 32 + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def read_exactly(conn: socket.socket, size: int) -> bytes: + chunks: list[bytes] = [] + remaining = size + while remaining > 0: + chunk = conn.recv(remaining) + if not chunk: + raise ConnectionError("连接已关闭") + chunks.append(chunk) + remaining -= len(chunk) + return b"".join(chunks) + + +def send_request( + host: str, + port: int, + msg_type: int, + payload: dict[str, Any], + timeout_sec: float, +) -> tuple[int, dict[str, Any]]: + encoded = json.dumps(payload, ensure_ascii=False, separators=(",", ":")).encode("utf-8") + with socket.create_connection((host, port), timeout=timeout_sec) as conn: + conn.settimeout(timeout_sec) + conn.sendall(struct.pack(" {args.vehicle_host}:{args.vehicle_port}" + ) + + def rate_limited(self) -> bool: + if self.args.max_hz <= 0.0: + return False + now = time.monotonic() + period = 1.0 / self.args.max_hz + if now - self.last_send_monotonic < period: + return True + self.last_send_monotonic = now + return False + + def maybe_log_stats(self) -> None: + now = time.monotonic() + if now - self.last_stats_log_monotonic < self.args.stats_log_interval_sec: + return + self.last_stats_log_monotonic = now + self.node.get_logger().info( + "外部定位位姿桥统计: " + f"received={self.received_count}, sent={self.sent_count}, failed={self.fail_count}" + ) + + def build_payload(self, msg: Any) -> dict[str, Any]: + pose = msg.workshop_pose + source_name = self.args.reference_source_name or str( + msg.reference_source_name or "workshop_external_localization" + ) + return { + "hardware_timestamp_us": int(msg.hardware_timestamp_us or now_us()), + "pose_valid": bool(msg.pose_valid), + "workshop_pose": { + "x_m": float(pose.x_m), + "y_m": float(pose.y_m), + "z_m": float(pose.z_m), + "roll_rad": float(pose.roll_rad), + "pitch_rad": float(pose.pitch_rad), + "yaw_rad": float(pose.yaw_rad), + }, + "position_stddev_m": float(msg.position_stddev_m), + "yaw_stddev_rad": float(msg.yaw_stddev_rad), + "tracking_loss_ratio": float(msg.tracking_loss_ratio), + "time_sync_offset_ms": float(msg.time_sync_offset_ms), + "quality_score": float(msg.quality_score), + "observed_target_count": int(msg.observed_target_count), + "reference_source_name": source_name, + "active_job_id": str(msg.active_job_id or ""), + } + + def on_pose(self, msg: Any) -> None: + self.received_count += 1 + if self.rate_limited(): + return + + payload = self.build_payload(msg) + try: + rsp_type, rsp = send_request( + self.args.vehicle_host, + self.args.vehicle_port, + EXTERNAL_POSE_PUSH_REQ, + payload, + self.args.timeout_sec, + ) + if rsp_type != EXTERNAL_POSE_PUSH_RSP or not rsp.get("success", False): + raise RuntimeError(f"车端响应异常: type={rsp_type}, payload={rsp}") + self.sent_count += 1 + self.maybe_log_stats() + except Exception as exc: + self.fail_count += 1 + now = time.monotonic() + if rclpy.ok() and now - self.last_error_log_monotonic >= self.args.error_log_interval_sec: + self.last_error_log_monotonic = now + self.node.get_logger().warning( + f"外部定位位姿推送失败: {exc}; fail_count={self.fail_count}" + ) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="车间外部定位车辆位姿 ROS -> WiFi6/TCP 车端桥") + parser.add_argument("--source-topic", default="/workshop/external_localization/vehicle/pose") + parser.add_argument("--vehicle-host", default="192.168.10.42") + parser.add_argument("--vehicle-port", type=int, default=9000) + parser.add_argument("--reference-source-name", default="") + parser.add_argument("--max-hz", type=float, default=30.0) + parser.add_argument("--timeout-sec", type=float, default=2.0) + parser.add_argument("--error-log-interval-sec", type=float, default=2.0) + parser.add_argument("--stats-log-interval-sec", type=float, default=10.0) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + rclpy.init(args=None) + bridge = None + try: + bridge = WorkshopExternalPoseBridge(args) + rclpy.spin(bridge.node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + if bridge is not None: + bridge.node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) + diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/README.md b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/README.md new file mode 100644 index 0000000..2be5436 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/README.md @@ -0,0 +1,171 @@ +# 真实车间传感器标定 + +这个目录只处理传感器标定的车间电脑侧边界。真实采集程序负责把相机、LiDAR、IMU、机械臂或平台位姿数据落盘;算法服务或离线算法只读取 `data_input.*` 和 `dataset_index.yaml`。 + +## 数据流 + +```text +车间电脑总控 + -> 车端传感器 agent 或车间采集程序采集图像/点云/IMU/位姿 + -> 采集程序写入 sensor/* 数据文件 + -> dataset_index_to_site_data_input.py 生成 site_data_input.yaml + -> sensor_calibration_service 或离线算法读取 data_input.* + -> 算法输出 sensor/estimated_params.yaml + -> stage_sensor_parameter_commit.py 生成 pending_sensor_commit.yaml + -> approve_sensor_pending_parameters.py 生成 approved_sensor_parameter_handoff.yaml +``` + +## 任务 profile + +现场推荐任务固化在: + +```bash +src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml +``` + +校验并查看摘要: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py \ + --profile src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml +``` + +导出总控 `requested_tasks`: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py \ + --profile src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml \ + --tasks sensor_intrinsic,sensor_extrinsic,hand_eye \ + --format requested_tasks +``` + +## 数据输入 + +数据字段协议见: + +```bash +src/site_deployment/workshop_sensor_calibration_real/sensor_data_contract.md +``` + +字段样例见: + +```bash +src/site_deployment/workshop_sensor_calibration_real/config/data_examples/ +``` + +车间电脑侧如果已经拿到了相机、LiDAR、IMU、机械臂或平台位姿文件,可以先用本目录工具校验这些文件,并写入 `dataset_index.yaml`: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/sensor_dataset_manifest.py \ + --dataset-root /data/agv_calib/site_a/session_001 \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --session-id session_001 \ + --site-id site_a \ + --vehicle-id agv_001 \ + --image-index sensor/front_camera/images.yaml \ + --target-detections sensor/front_camera/charuco_detections.json \ + --imu-samples sensor/imu/imu_samples.csv \ + --imu-segments sensor/imu/imu_segments.yaml \ + --pointcloud-index sensor/lidar_3d/pointclouds.yaml \ + --laser-scan-index sensor/lidar_2d/laser_scans.yaml \ + --robot-poses sensor/hand_eye/robot_poses.csv +``` + +这个工具只做车间电脑侧的数据整理和格式校验,不负责连接车端 Windows 采集驱动。 + +车间电脑侧预处理和检测质量报告: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/sensor_preprocess_pipeline.py \ + --dataset-root /data/agv_calib/site_a/session_001 \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --image-index sensor/front_camera/images.yaml \ + --target-detections sensor/front_camera/charuco_detections.json \ + --imu-samples sensor/imu/imu_samples.csv \ + --imu-segments sensor/imu/imu_segments.yaml \ + --pointcloud-index sensor/lidar_3d/pointclouds.yaml \ + --laser-scan-index sensor/lidar_2d/laser_scans.yaml \ + --robot-poses sensor/hand_eye/robot_poses.csv +``` + +如果现场车间电脑安装了 OpenCV,也可以用同一个工具从图像生成棋盘格或 ChArUco 检测结果。没有 OpenCV 时,车端或其他视觉程序只要按 `sensor_data_contract.md` 输出 `target_detections.json` 即可。 + +真实采集或 ingest 写入索引: + +```bash +python3 src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --session-dir /data/agv_calib/site_a/session_001 \ + --site-id site_a \ + --vehicle-id agv_001 \ + --file sensor.image_sample_files=sensor/front_camera/images.yaml \ + --file sensor.target_detection_files=sensor/front_camera/charuco_detections.json \ + --finalize +``` + +校验索引: + +```bash +python3 src/deployment/tools/validate_dataset_index.py \ + /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --tasks sensor_intrinsic,sensor_extrinsic,hand_eye +``` + +## 参数结果交接 + +算法工程师应输出: + +```bash +/data/agv_calib/site_a/session_001/sensor/estimated_params.yaml +``` + +格式模板: + +```bash +src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml +``` + +生成待审批包: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/stage_sensor_parameter_commit.py \ + --dataset-index /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --estimated-params /data/agv_calib/site_a/session_001/sensor/estimated_params.yaml +``` + +审批并生成车间电脑侧交接包: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/approve_sensor_pending_parameters.py \ + /data/agv_calib/site_a/session_001/sensor/pending_sensor_commit.yaml \ + --operator-id operator_001 +``` + +当前交接包只表示“车间电脑侧已经审批通过,等待车端写参适配器处理”。车端写参实现暂不在本目录内处理。 + +车间电脑侧可以先把已审批交接包转换成车辆侧导入配置: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/apply_sensor_parameter_handoff.py \ + /data/agv_calib/site_a/session_001/sensor/approved_sensor_parameter_handoff.yaml \ + --operator-id operator_001 +``` + +该步骤会生成 `sensor/applied_parameters/*` 下的导入配置和回执,并把结果写回 `dataset_index.yaml` metadata。真正把配置导入 Windows 车端或厂商配置文件的步骤仍由车端适配器处理。 + +## 本地 smoke + +不依赖 ROS 和 Isaac 的传感器本地 smoke: + +```bash +python3 src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py \ + --keep-session-dir +``` + +这个 smoke 会验证: + +- 示例数据格式校验。 +- `dataset_index.yaml` 写入。 +- 预处理质量报告生成。 +- `site_data_input.yaml` 生成。 +- `sensor_intrinsic`、`sensor_extrinsic`、`hand_eye` 三类参数输出的待提交、审批、应用落盘链路。 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/apply_sensor_parameter_handoff.py b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/apply_sensor_parameter_handoff.py new file mode 100644 index 0000000..a756e42 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/apply_sensor_parameter_handoff.py @@ -0,0 +1,302 @@ +#!/usr/bin/env python3 +"""把已审批传感器参数交接包落成车间电脑侧车辆参数配置。""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="消费 approved_sensor_parameter_handoff.yaml,生成车间电脑侧车辆传感器参数配置。" + ) + parser.add_argument("handoff", help="已审批传感器参数交接包。") + parser.add_argument("--output-dir", default="", help="配置输出目录;为空时写到 dataset_root/sensor/applied_parameters。") + parser.add_argument("--dataset-index", default="", help="可选 dataset_index.yaml;为空时从 handoff.source.dataset_index_file 推断。") + parser.add_argument("--operator-id", default="", help="执行本次落盘的操作员 ID。") + parser.add_argument("--apply-note", default="", help="落盘备注。") + parser.add_argument("--dry-run", action="store_true", help="只校验并打印将要写入的文件,不实际写入。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否把落盘结果写回 dataset_index metadata。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def bool_value(value: Any) -> bool: + if isinstance(value, bool): + return value + if isinstance(value, str): + return value.strip().lower() in ("1", "true", "yes", "y") + return bool(value) + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def resolve_dataset_file(path_value: str, dataset_root: Path) -> Path: + path = Path(path_value).expanduser() + if path.is_absolute(): + return path + return (dataset_root / path).resolve(strict=False) + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def validate_handoff(handoff: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]: + if int(handoff.get("schema_version", 0) or 0) != 1: + raise ValueError("approved_sensor_parameter_handoff.schema_version 必须为 1。") + if handoff.get("handoff_state") != "ready_for_vehicle_adapter": + raise ValueError("handoff_state 必须为 ready_for_vehicle_adapter。") + session = require_map(handoff.get("session"), "handoff.session") + source = require_map(handoff.get("source"), "handoff.source") + approval = require_map(handoff.get("approval"), "handoff.approval") + vehicle_request = require_map(handoff.get("vehicle_commit_request"), "handoff.vehicle_commit_request") + require_string(session.get("session_id"), "session.session_id") + require_string(session.get("vehicle_id"), "session.vehicle_id") + require_string(session.get("dataset_root"), "session.dataset_root") + if not bool_value(approval.get("approved", False)): + raise ValueError("handoff.approval.approved 必须为 true。") + require_string(vehicle_request.get("parameter_version"), "vehicle_commit_request.parameter_version") + require_string(vehicle_request.get("selected_task"), "vehicle_commit_request.selected_task") + require_string(vehicle_request.get("task_subtype"), "vehicle_commit_request.task_subtype") + require_string(vehicle_request.get("sensor_id"), "vehicle_commit_request.sensor_id") + require_map(vehicle_request.get("estimated_params"), "vehicle_commit_request.estimated_params") + return session, source, approval, vehicle_request + + +def validate_source_digest(session: dict[str, Any], source: dict[str, Any]) -> None: + digest = require_map(source.get("estimated_params_digest"), "source.estimated_params_digest") + checksum_type = require_string(digest.get("checksum_type"), "source.estimated_params_digest.checksum_type") + checksum_value = require_string(digest.get("checksum_value"), "source.estimated_params_digest.checksum_value") + if checksum_type.lower() != "sha256": + raise ValueError("当前只支持 sha256 摘要校验。") + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + estimated_params_file = require_string(source.get("estimated_params_file"), "source.estimated_params_file") + estimated_params_path = resolve_dataset_file(estimated_params_file, dataset_root) + if not estimated_params_path.exists(): + raise FileNotFoundError(f"算法输出参数文件不存在:{estimated_params_path}") + if sha256_file(estimated_params_path) != checksum_value: + raise ValueError("算法输出参数摘要不一致,禁止生成车辆参数配置。") + + +def resolve_output_dir(args: argparse.Namespace, dataset_root: Path) -> Path: + if args.output_dir: + return Path(args.output_dir).expanduser().resolve(strict=False) + return (dataset_root / "sensor" / "applied_parameters").resolve(strict=False) + + +def build_vehicle_parameter_config( + handoff_path: Path, + output_file: Path, + session: dict[str, Any], + source: dict[str, Any], + approval: dict[str, Any], + vehicle_request: dict[str, Any], + args: argparse.Namespace, +) -> dict[str, Any]: + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + created_us = now_us() + return { + "schema_version": 1, + "config_state": "ready_for_vehicle_side_import", + "created_timestamp_us": created_us, + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "source": { + "approved_handoff_file": relative_to_root(handoff_path, dataset_root), + "pending_commit_file": source.get("pending_commit_file", ""), + "dataset_index_file": source.get("dataset_index_file", ""), + "estimated_params_file": source.get("estimated_params_file", ""), + "estimated_params_digest": source.get("estimated_params_digest", {}), + }, + "approval": { + "approved": True, + "operator_id": approval.get("operator_id", ""), + "approved_timestamp_us": approval.get("approved_timestamp_us", 0), + "approval_note": approval.get("approval_note", ""), + }, + "apply": { + "operator_id": args.operator_id, + "apply_note": args.apply_note, + "persistent_write": bool_value(vehicle_request.get("persistent_write", True)), + "config_file": relative_to_root(output_file, dataset_root), + }, + "sensor_parameter": { + "parameter_version": vehicle_request["parameter_version"], + "selected_task": vehicle_request["selected_task"], + "task_subtype": vehicle_request["task_subtype"], + "sensor_id": vehicle_request["sensor_id"], + "estimated_params": vehicle_request["estimated_params"], + }, + "vehicle_import_contract": { + "status": "waiting_vehicle_side_import", + "required_request": "vehicle_side_sensor_parameter_write_adapter", + "required_digest_check": "sha256", + }, + } + + +def build_receipt( + output_file: Path, + session: dict[str, Any], + vehicle_request: dict[str, Any], + args: argparse.Namespace, +) -> dict[str, Any]: + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + return { + "schema_version": 1, + "apply_state": "workshop_config_exported", + "created_timestamp_us": now_us(), + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "sensor_id": vehicle_request["sensor_id"], + "parameter_version": vehicle_request["parameter_version"], + "selected_task": vehicle_request["selected_task"], + "task_subtype": vehicle_request["task_subtype"], + "operator_id": args.operator_id, + "apply_note": args.apply_note, + "vehicle_parameter_config_file": relative_to_root(output_file, dataset_root), + "vehicle_parameter_config_digest": { + "checksum_type": "sha256", + "checksum_value": sha256_file(output_file), + }, + } + + +def resolve_dataset_index_path(args: argparse.Namespace, dataset_root: Path, source: dict[str, Any]) -> Path | None: + if args.dataset_index: + return Path(args.dataset_index).expanduser().resolve(strict=False) + dataset_index_file = source.get("dataset_index_file", "") + if not isinstance(dataset_index_file, str) or not dataset_index_file: + return None + return resolve_dataset_file(dataset_index_file, dataset_root) + + +def update_dataset_index( + dataset_index_path: Path | None, + dataset_root: Path, + output_file: Path, + receipt_file: Path, + receipt: dict[str, Any], +) -> None: + if dataset_index_path is None or not dataset_index_path.exists(): + return + index = load_yaml(dataset_index_path) + metadata = index.setdefault("metadata", {}) + metadata["sensor_parameter_vehicle_config_file"] = relative_to_root(output_file, dataset_root) + metadata["sensor_parameter_apply_receipt_file"] = relative_to_root(receipt_file, dataset_root) + metadata["sensor_parameter_apply_state"] = receipt["apply_state"] + metadata["sensor_parameter_apply_version"] = receipt["parameter_version"] + metadata["sensor_parameter_apply_sensor_id"] = receipt["sensor_id"] + metadata["sensor_parameter_apply_checksum_type"] = receipt["vehicle_parameter_config_digest"]["checksum_type"] + metadata["sensor_parameter_apply_checksum_value"] = receipt["vehicle_parameter_config_digest"]["checksum_value"] + write_yaml(dataset_index_path, index) + + +def main() -> int: + args = parse_args() + handoff_path = Path(args.handoff).expanduser().resolve(strict=False) + handoff = load_yaml(handoff_path) + session, source, approval, vehicle_request = validate_handoff(handoff) + validate_source_digest(session, source) + + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + output_dir = resolve_output_dir(args, dataset_root) + sensor_id = require_string(vehicle_request.get("sensor_id"), "vehicle_commit_request.sensor_id") + parameter_version = require_string( + vehicle_request.get("parameter_version"), + "vehicle_commit_request.parameter_version", + ) + output_file = output_dir / f"{sensor_id}_{parameter_version}.yaml" + receipt_file = output_dir / "sensor_parameter_apply_receipt.yaml" + config = build_vehicle_parameter_config( + handoff_path, + output_file, + session, + source, + approval, + vehicle_request, + args, + ) + + print(f"[OK] 已审批传感器参数交接包校验通过:{handoff_path}") + print(f"[OK] 目标传感器:{sensor_id}") + print(f"[OK] 参数版本:{parameter_version}") + print(f"[OK] 将生成车辆侧导入配置:{output_file}") + + if args.dry_run: + print("[OK] dry-run 模式,不写入文件。") + return 0 + + write_yaml(output_file, config) + receipt = build_receipt(output_file, session, vehicle_request, args) + write_yaml(receipt_file, receipt) + if args.update_dataset_index: + dataset_index_path = resolve_dataset_index_path(args, dataset_root, source) + update_dataset_index(dataset_index_path, dataset_root, output_file, receipt_file, receipt) + print(f"[OK] 已生成车辆侧导入配置:{output_file}") + print(f"[OK] 已生成车间电脑侧落盘回执:{receipt_file}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/approve_sensor_pending_parameters.py b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/approve_sensor_pending_parameters.py new file mode 100644 index 0000000..acc501a --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/approve_sensor_pending_parameters.py @@ -0,0 +1,261 @@ +#!/usr/bin/env python3 +"""审批传感器待提交参数包,并生成车间电脑侧交接包。""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="审批传感器待提交参数包,并生成车间电脑侧参数交接包。") + parser.add_argument("pending_commit", help="传感器待提交参数包 pending_sensor_commit.yaml。") + parser.add_argument("--operator-id", required=True, help="审批操作员 ID。") + parser.add_argument("--approval-note", default="", help="审批备注。") + parser.add_argument("--output", default="", help="交接包输出路径;为空时写到会话目录 sensor/approved_sensor_parameter_handoff.yaml。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否把交接包路径写回 dataset_index.yaml。") + parser.add_argument("--dry-run", action="store_true", help="只校验并打印结果,不修改文件。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def bool_value(value: Any) -> bool: + if isinstance(value, bool): + return value + if isinstance(value, str): + return value.strip().lower() in ("1", "true", "yes", "y") + return bool(value) + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def resolve_dataset_file(path_value: str, dataset_root: Path) -> Path: + path = Path(path_value).expanduser() + if path.is_absolute(): + return path + return (dataset_root / path).resolve(strict=False) + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def validate_pending_commit(package: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]: + if int(package.get("schema_version", 0) or 0) != 1: + raise ValueError("pending_sensor_commit.schema_version 必须为 1。") + session = require_map(package.get("session"), "pending_sensor_commit.session") + source = require_map(package.get("source"), "pending_sensor_commit.source") + approval = require_map(package.get("approval"), "pending_sensor_commit.approval") + commit_request = require_map(package.get("commit_request"), "pending_sensor_commit.commit_request") + require_string(session.get("session_id"), "session.session_id") + require_string(session.get("vehicle_id"), "session.vehicle_id") + require_string(session.get("dataset_root"), "session.dataset_root") + require_string(commit_request.get("parameter_version"), "commit_request.parameter_version") + require_string(commit_request.get("selected_task"), "commit_request.selected_task") + require_string(commit_request.get("task_subtype"), "commit_request.task_subtype") + require_string(commit_request.get("sensor_id"), "commit_request.sensor_id") + require_map(commit_request.get("estimated_params"), "commit_request.estimated_params") + return session, source, approval, commit_request + + +def validate_estimated_params_digest(session: dict[str, Any], source: dict[str, Any]) -> dict[str, str]: + digest = require_map(source.get("estimated_params_digest"), "source.estimated_params_digest") + checksum_type = require_string(digest.get("checksum_type"), "estimated_params_digest.checksum_type") + checksum_value = require_string(digest.get("checksum_value"), "estimated_params_digest.checksum_value") + if checksum_type.lower() != "sha256": + raise ValueError("当前只支持 sha256 参数摘要。") + + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + estimated_params_file = require_string(source.get("estimated_params_file"), "source.estimated_params_file") + estimated_params_path = resolve_dataset_file(estimated_params_file, dataset_root) + if not estimated_params_path.exists(): + raise FileNotFoundError(f"传感器算法输出参数文件不存在,无法校验摘要:{estimated_params_path}") + actual_checksum = sha256_file(estimated_params_path) + if actual_checksum != checksum_value: + raise ValueError("传感器算法输出参数摘要不一致,禁止审批交接。") + return { + "checksum_type": checksum_type, + "checksum_value": checksum_value, + "estimated_params_file": relative_to_root(estimated_params_path, dataset_root), + } + + +def resolve_output_path(args: argparse.Namespace, session: dict[str, Any]) -> Path: + if args.output: + return Path(args.output).expanduser().resolve(strict=False) + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + return (dataset_root / "sensor" / "approved_sensor_parameter_handoff.yaml").resolve(strict=False) + + +def build_handoff_package( + pending_path: Path, + output_path: Path, + package: dict[str, Any], + args: argparse.Namespace, +) -> dict[str, Any]: + session, source, approval, commit_request = validate_pending_commit(package) + digest = validate_estimated_params_digest(session, source) + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + approved_us = now_us() + + approval["required"] = bool_value(approval.get("required", True)) + approval["approved"] = True + approval["operator_id"] = args.operator_id + approval["approved_timestamp_us"] = approved_us + approval["approval_note"] = args.approval_note + package["commit_state"] = "approved_waiting_vehicle_handoff" + package["vehicle_handoff"] = { + "handoff_file": relative_to_root(output_path, dataset_root), + "handoff_state": "ready_for_vehicle_adapter", + "generated_timestamp_us": approved_us, + "generated_by": "approve_sensor_pending_parameters.py", + } + + return { + "schema_version": 1, + "handoff_state": "ready_for_vehicle_adapter", + "created_timestamp_us": approved_us, + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "source": { + "pending_commit_file": relative_to_root(pending_path, dataset_root), + "dataset_index_file": source.get("dataset_index_file", ""), + "estimated_params_file": digest["estimated_params_file"], + "estimated_params_digest": { + "checksum_type": digest["checksum_type"], + "checksum_value": digest["checksum_value"], + }, + }, + "approval": { + "approved": True, + "operator_id": args.operator_id, + "approved_timestamp_us": approved_us, + "approval_note": args.approval_note, + }, + "vehicle_commit_request": { + "parameter_version": commit_request["parameter_version"], + "selected_task": commit_request["selected_task"], + "task_subtype": commit_request["task_subtype"], + "sensor_id": commit_request["sensor_id"], + "commit_reason": commit_request.get("commit_reason", ""), + "persistent_write": bool_value(commit_request.get("persistent_write", True)), + "estimated_params": commit_request["estimated_params"], + }, + "vehicle_adapter_contract": { + "status": "waiting_vehicle_side_adapter", + "required_request": "vehicle_side_sensor_parameter_write_adapter", + "required_digest_check": "sha256", + "rollback_required_on_vehicle_commit_failure": bool_value( + package.get("rollback", {}).get("rollback_required_on_vehicle_commit_failure", True) + ), + }, + } + + +def update_dataset_index( + pending_package: dict[str, Any], + handoff_path: Path, + handoff_package: dict[str, Any], +) -> None: + session = require_map(pending_package.get("session"), "pending_sensor_commit.session") + source = require_map(pending_package.get("source"), "pending_sensor_commit.source") + dataset_root = Path(require_string(session.get("dataset_root"), "session.dataset_root")).expanduser() + dataset_index_file = source.get("dataset_index_file", "") + if not isinstance(dataset_index_file, str) or not dataset_index_file: + return + dataset_index_path = resolve_dataset_file(dataset_index_file, dataset_root) + if not dataset_index_path.exists(): + return + + index = load_yaml(dataset_index_path) + metadata = index.setdefault("metadata", {}) + metadata["sensor_parameter_handoff_file"] = relative_to_root(handoff_path, dataset_root) + metadata["sensor_parameter_handoff_state"] = handoff_package["handoff_state"] + metadata["sensor_pending_commit_state"] = pending_package.get("commit_state", "") + metadata["sensor_pending_commit_approved"] = True + metadata["sensor_pending_commit_approved_operator_id"] = handoff_package["approval"]["operator_id"] + metadata["sensor_pending_commit_approved_timestamp_us"] = handoff_package["approval"]["approved_timestamp_us"] + write_yaml(dataset_index_path, index) + + +def main() -> int: + args = parse_args() + pending_path = Path(args.pending_commit).expanduser().resolve(strict=False) + package = load_yaml(pending_path) + session, _, _, commit_request = validate_pending_commit(package) + output_path = resolve_output_path(args, session) + handoff_package = build_handoff_package(pending_path, output_path, package, args) + + print(f"[OK] 传感器待提交参数包校验通过:{pending_path}") + print(f"[OK] 参数版本:{commit_request['parameter_version']}") + print(f"[OK] 目标传感器:{commit_request['sensor_id']}") + print(f"[OK] 交接包状态:{handoff_package['handoff_state']}") + + if args.dry_run: + print("[OK] dry-run 模式,不写入文件。") + return 0 + + write_yaml(output_path, handoff_package) + write_yaml(pending_path, package) + if args.update_dataset_index: + update_dataset_index(package, output_path, handoff_package) + print(f"[OK] 已生成传感器参数交接包:{output_path}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/charuco_detections.json b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/charuco_detections.json new file mode 100644 index 0000000..0a416ad --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/charuco_detections.json @@ -0,0 +1,24 @@ +{ + "schema_version": 1, + "sensor_id": "demo_front_camera", + "sensor_type": "front_camera", + "frame_id": "front_camera_link", + "target_board_id": "front_camera_checkerboard", + "target_type": "charuco", + "detections": [ + { + "stamp_us": 1770000000000000, + "source_file": "sensor/front_camera/images/front_camera_000000.png", + "target_detected": true, + "quality_score": 0.97, + "corners": [ + {"id": 0, "u_px": 350.2, "v_px": 210.4}, + {"id": 1, "u_px": 410.1, "v_px": 211.0} + ], + "target_pose_in_sensor": { + "translation_xyz_m": [0.8, 0.0, 0.2], + "rotation_xyzw": [0.0, 0.0, 0.0, 1.0] + } + } + ] +} diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/images.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/images.yaml new file mode 100644 index 0000000..0f1e9d3 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/images.yaml @@ -0,0 +1,22 @@ +schema_version: 1 +sensor_id: demo_front_camera +sensor_type: front_camera +frame_id: front_camera_link +topic: /sensor/front_camera/image_raw +encoding: bgr8 +image_width: 1920 +image_height: 1080 +target_board_id: front_camera_checkerboard +frames: + - sequence: 0 + stamp_us: 1770000000000000 + file: sensor/front_camera/images/front_camera_000000.png + exposure_us: 8000 + gain: 1.0 + quality_score: 0.95 + - sequence: 1 + stamp_us: 1770000001000000 + file: sensor/front_camera/images/front_camera_000001.png + exposure_us: 8000 + gain: 1.0 + quality_score: 0.96 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/imu_samples.csv b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/imu_samples.csv new file mode 100644 index 0000000..1fb18d1 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/imu_samples.csv @@ -0,0 +1,3 @@ +stamp_us,sensor_id,frame_id,ax_mps2,ay_mps2,az_mps2,gx_rps,gy_rps,gz_rps,temperature_c +1770000000000000,demo_imu,imu_link,0.01,-0.02,9.81,0.001,0.000,0.002,32.5 +1770000001000000,demo_imu,imu_link,0.02,-0.01,9.80,0.001,0.001,0.002,32.5 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/imu_segments.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/imu_segments.yaml new file mode 100644 index 0000000..d48dc7e --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/imu_segments.yaml @@ -0,0 +1,13 @@ +schema_version: 1 +sensor_id: demo_imu +frame_id: imu_link +source_file: sensor/imu/imu_samples.csv +segments: + - segment_id: static_001 + segment_type: static + start_us: 1770000000000000 + end_us: 1770000005000000 + - segment_id: motion_001 + segment_type: motion + start_us: 1770000006000000 + end_us: 1770000012000000 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/laser_scans.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/laser_scans.yaml new file mode 100644 index 0000000..cc23c56 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/laser_scans.yaml @@ -0,0 +1,21 @@ +schema_version: 1 +sensor_id: demo_lidar_2d +sensor_type: lidar_2d +frame_id: lidar_2d_link +topic: /sensor/lidar_2d/scan +scan_format: csv +scans: + - sequence: 0 + stamp_us: 1770000000000000 + file: sensor/lidar_2d/scan_000000.csv + angle_min_rad: -3.14159 + angle_increment_rad: 0.00436 + range_min_m: 0.05 + range_max_m: 30.0 + - sequence: 1 + stamp_us: 1770000001000000 + file: sensor/lidar_2d/scan_000001.csv + angle_min_rad: -3.14159 + angle_increment_rad: 0.00436 + range_min_m: 0.05 + range_max_m: 30.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/pointclouds.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/pointclouds.yaml new file mode 100644 index 0000000..7301ab6 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/pointclouds.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +sensor_id: demo_lidar_3d +sensor_type: lidar_3d +frame_id: lidar_3d_link +topic: /sensor/lidar_3d/pointcloud +pointcloud_format: pcd +frames: + - sequence: 0 + stamp_us: 1770000000000000 + file: sensor/lidar_3d/cloud_000000.pcd + point_count: 120000 + quality_score: 0.92 + - sequence: 1 + stamp_us: 1770000001000000 + file: sensor/lidar_3d/cloud_000001.pcd + point_count: 121000 + quality_score: 0.93 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/robot_poses.csv b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/robot_poses.csv new file mode 100644 index 0000000..0cc74d3 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/robot_poses.csv @@ -0,0 +1,3 @@ +stamp_us,pose_id,parent_frame_id,child_frame_id,x_m,y_m,z_m,qx,qy,qz,qw,source,quality_score +1770000000000000,pose_000,base_link,tool0,0.3,0.0,0.5,0.0,0.0,0.0,1.0,robot_controller,1.0 +1770000001000000,pose_001,base_link,tool0,0.3,0.1,0.5,0.0,0.0,0.1,0.995,robot_controller,1.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/synchronized_sensor_dataset.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/synchronized_sensor_dataset.yaml new file mode 100644 index 0000000..755dc5a --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/data_examples/synchronized_sensor_dataset.yaml @@ -0,0 +1,24 @@ +schema_version: 1 +session_id: session_001 +vehicle_id: agv_001 +dataset_root: /data/agv_calib/site_a/session_001 +time_base: unix_us +data_window_start_timestamp_us: 1770000000000000 +data_window_end_timestamp_us: 1770000120000000 +time_sync: + max_sensor_time_offset_ms: 5.0 + source: ptp +files: + image_sample_files: + - sensor/front_camera/images.yaml + imu_sample_files: + - sensor/imu/imu_samples.csv + - sensor/imu/imu_segments.yaml + pointcloud_sample_files: + - sensor/lidar_3d/pointclouds.yaml + laser_scan_sample_files: + - sensor/lidar_2d/laser_scans.yaml + target_detection_files: + - sensor/front_camera/charuco_detections.json + robot_pose_sample_files: + - sensor/hand_eye/robot_poses.csv 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 new file mode 100644 index 0000000..029147a --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml @@ -0,0 +1,121 @@ +# 传感器标定现场任务配置模板。 +# 本文件固定真实标定车间中默认要跑的传感器内参、外参和手眼任务。 +# 现场落地时应把 sensor_id、标定板 ID、base frame 和采样数量替换为真实配置。 + +schema_version: 1 +profile_name: workshop_sensor_calibration_profile + +data_capture: + required_data_inputs: + - image_sample_files + - imu_sample_files + - pointcloud_sample_files + - laser_scan_sample_files + - target_detection_files + - robot_pose_sample_files + - synchronized_dataset_files + min_camera_frame_count: 20 + min_imu_static_segment_count: 3 + min_imu_motion_segment_count: 3 + min_lidar_sample_count: 10 + min_hand_eye_pose_count: 10 + +tasks: + - task_code: sensor.front_camera.intrinsic + display_name: 前视相机内参 + enabled: true + selected_task: camera_intrinsic + sensor_id: demo_front_camera + task_subtype: front_camera_intrinsic + camera_intrinsic: + required_image_count: 20 + target_board_id: front_camera_checkerboard + timeout_sec: 90.0 + + - task_code: sensor.downward_camera.intrinsic + display_name: 下视相机内参 + enabled: true + selected_task: camera_intrinsic + sensor_id: demo_down_camera + task_subtype: downward_camera_intrinsic + camera_intrinsic: + required_image_count: 20 + target_board_id: down_camera_charuco + timeout_sec: 90.0 + + - task_code: sensor.imu.intrinsic + display_name: IMU 内参 + enabled: true + selected_task: imu_intrinsic + sensor_id: demo_imu + task_subtype: imu_intrinsic + imu_intrinsic: + required_static_segment_count: 3 + required_motion_segment_count: 3 + timeout_sec: 120.0 + + - task_code: sensor.front_camera.extrinsic + display_name: 前视相机到 base_link 外参 + enabled: true + selected_task: sensor_extrinsic + sensor_id: demo_front_camera + task_subtype: front_camera_extrinsic + sensor_extrinsic: + base_frame_id: base_link + required_sample_count: 10 + timeout_sec: 90.0 + + - task_code: sensor.downward_camera.extrinsic + display_name: 下视相机到 base_link 外参 + enabled: true + selected_task: sensor_extrinsic + sensor_id: demo_down_camera + task_subtype: downward_camera_extrinsic + sensor_extrinsic: + base_frame_id: base_link + required_sample_count: 10 + timeout_sec: 90.0 + + - task_code: sensor.lidar_2d.extrinsic + display_name: 2D LiDAR 到 base_link 外参 + enabled: true + selected_task: sensor_extrinsic + sensor_id: demo_lidar_2d + task_subtype: lidar_2d_extrinsic + sensor_extrinsic: + base_frame_id: base_link + required_sample_count: 10 + timeout_sec: 90.0 + + - task_code: sensor.lidar_3d.extrinsic + display_name: 3D LiDAR 到 base_link 外参 + enabled: true + selected_task: sensor_extrinsic + sensor_id: demo_lidar_3d + task_subtype: lidar_3d_extrinsic + sensor_extrinsic: + base_frame_id: base_link + required_sample_count: 10 + timeout_sec: 90.0 + + - task_code: sensor.imu.extrinsic + display_name: IMU 到 base_link 外参 + enabled: true + selected_task: sensor_extrinsic + sensor_id: demo_imu + task_subtype: imu_extrinsic + sensor_extrinsic: + base_frame_id: base_link + required_sample_count: 10 + timeout_sec: 120.0 + + - task_code: sensor.front_camera.hand_eye + display_name: 前视相机眼在手内 + enabled: true + selected_task: hand_eye + sensor_id: demo_front_camera + task_subtype: eye_in_hand + hand_eye: + arm_id: demo_arm + required_pose_count: 10 + timeout_sec: 120.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml new file mode 100644 index 0000000..b8c0e6b --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml @@ -0,0 +1,98 @@ +# 传感器标定算法输出参数模板。 +# 算法同事应把本文件复制到本次会话目录的 sensor/estimated_params.yaml, +# 并按 selected_task / task_subtype 填写对应的 estimated_params 分支。 + +schema_version: 1 +session_id: session_xxx +vehicle_id: replace_with_real_vehicle_id +sensor_id: replace_with_sensor_id +selected_task: camera_intrinsic +task_subtype: front_camera_intrinsic +parameter_version: sensor_replace_with_sensor_id_session_xxx_v1 +algorithm: + name: replace_with_algorithm_name + version: replace_with_algorithm_version + +frames: + base_frame_id: base_link + sensor_frame_id: replace_with_sensor_frame + target_frame_id: replace_with_target_frame + +estimated_params: + camera_intrinsics: + image_width: 1920 + image_height: 1080 + camera_model: pinhole + distortion_model: plumb_bob + fx: 0.0 + fy: 0.0 + cx: 0.0 + cy: 0.0 + distortion_coefficients: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + + imu_intrinsics: + gyro_bias_xyz_rads: + - 0.0 + - 0.0 + - 0.0 + accel_bias_xyz_ms2: + - 0.0 + - 0.0 + - 0.0 + gyro_noise_density_rads_sqrt_hz: 0.0 + accel_noise_density_ms2_sqrt_hz: 0.0 + gyro_random_walk_rads_s_sqrt_hz: 0.0 + accel_random_walk_ms3_sqrt_hz: 0.0 + scale_matrix: + - [1.0, 0.0, 0.0] + - [0.0, 1.0, 0.0] + - [0.0, 0.0, 1.0] + + sensor_extrinsics: + parent_frame_id: base_link + child_frame_id: replace_with_sensor_frame + translation_xyz_m: + - 0.0 + - 0.0 + - 0.0 + rotation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + + hand_eye: + base_frame_id: base_link + tool_frame_id: replace_with_tool_frame + camera_frame_id: replace_with_camera_frame + translation_xyz_m: + - 0.0 + - 0.0 + - 0.0 + rotation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + +quality: + data_quality_passed: false + suitable_for_commit: false + validation_summary: + reprojection_error_px: 0.0 + translation_residual_m: 0.0 + rotation_residual_rad: 0.0 + plane_residual_m: 0.0 + repeatability_error_m: 0.0 + sample_count: 0 + auto_acceptance_passed: false + warnings: [] + +artifacts: + - file: sensor/diagnostics.json + description: 传感器标定诊断信息 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py new file mode 100644 index 0000000..4b61736 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py @@ -0,0 +1,405 @@ +#!/usr/bin/env python3 +"""校验传感器标定现场任务配置,并导出总控任务片段。""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +TASK_GROUPS = { + "camera_intrinsic", + "imu_intrinsic", + "sensor_extrinsic", + "hand_eye", +} + +TASK_GROUP_TO_STAGE_TYPE = { + "camera_intrinsic": "SENSOR_INTRINSIC_CALIBRATION_STAGE", + "imu_intrinsic": "SENSOR_INTRINSIC_CALIBRATION_STAGE", + "sensor_extrinsic": "SENSOR_EXTRINSIC_CALIBRATION_STAGE", + "hand_eye": "HAND_EYE_CALIBRATION_STAGE", +} + +TASK_GROUP_TO_REQUESTED_TASK = { + "camera_intrinsic": "sensor_intrinsic", + "imu_intrinsic": "sensor_intrinsic", + "sensor_extrinsic": "sensor_extrinsic", + "hand_eye": "hand_eye", +} + +SUPPORTED_SUBTYPES = { + "camera_intrinsic": { + "front_camera_intrinsic", + "downward_camera_intrinsic", + }, + "imu_intrinsic": { + "imu_intrinsic", + }, + "sensor_extrinsic": { + "front_camera_extrinsic", + "downward_camera_extrinsic", + "imu_extrinsic", + "lidar_2d_extrinsic", + "lidar_3d_extrinsic", + }, + "hand_eye": { + "eye_in_hand", + "eye_to_hand", + }, +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="校验并导出传感器标定现场 profile。") + parser.add_argument( + "--profile", + default="src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml", + help="传感器标定 profile YAML 路径。", + ) + parser.add_argument( + "--tasks", + default="sensor_intrinsic,sensor_extrinsic,hand_eye", + help="只导出指定总控任务,逗号分隔:sensor_intrinsic,sensor_extrinsic,hand_eye。", + ) + parser.add_argument( + "--format", + choices=["summary", "requested_tasks"], + default="summary", + help="输出格式:summary 为摘要,requested_tasks 为总控任务片段。", + ) + parser.add_argument("-o", "--output", help="输出文件路径;不填写时输出到标准输出。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + if data is None: + return {} + if not isinstance(data, dict): + raise ValueError(f"{path} 的顶层结构必须是 YAML map。") + return data + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_list(value: Any, field_name: str) -> list[Any]: + if not isinstance(value, list): + raise ValueError(f"{field_name} 必须是 YAML list。") + return value + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def as_int(value: Any, field_name: str) -> int: + try: + return int(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是整数,当前值为 {value!r}。") from exc + + +def as_float(value: Any, field_name: str) -> float: + try: + return float(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是数字,当前值为 {value!r}。") from exc + + +def validate_positive_int(value: Any, field_name: str) -> int: + number = as_int(value, field_name) + if number <= 0: + raise ValueError(f"{field_name} 必须大于 0。") + return number + + +def validate_positive_float(value: Any, field_name: str) -> float: + number = as_float(value, field_name) + if number <= 0.0: + raise ValueError(f"{field_name} 必须大于 0。") + return number + + +def stringify_value(value: Any) -> str: + if isinstance(value, bool): + return "true" if value else "false" + if isinstance(value, list): + return ",".join(stringify_value(item) for item in value) + return str(value) + + +def parse_requested_tasks(raw_tasks: str | list[str] | None) -> set[str]: + if raw_tasks is None: + return set() + if isinstance(raw_tasks, list): + items = raw_tasks + else: + items = raw_tasks.split(",") + result: set[str] = set() + for item in items: + task = str(item).strip() + if not task: + continue + if task == "all": + return {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"} + if task not in {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"}: + raise ValueError(f"不支持的传感器总控任务:{task}") + result.add(task) + return result + + +def validate_camera_intrinsic(task: dict[str, Any], task_name: str) -> None: + payload = require_map(task.get("camera_intrinsic"), f"{task_name}.camera_intrinsic") + validate_positive_int( + payload.get("required_image_count"), + f"{task_name}.camera_intrinsic.required_image_count", + ) + if "target_board_id" in payload and payload["target_board_id"] not in (None, ""): + require_string(payload["target_board_id"], f"{task_name}.camera_intrinsic.target_board_id") + if "timeout_sec" in payload: + validate_positive_float(payload["timeout_sec"], f"{task_name}.camera_intrinsic.timeout_sec") + + +def validate_imu_intrinsic(task: dict[str, Any], task_name: str) -> None: + payload = require_map(task.get("imu_intrinsic"), f"{task_name}.imu_intrinsic") + validate_positive_int( + payload.get("required_static_segment_count"), + f"{task_name}.imu_intrinsic.required_static_segment_count", + ) + validate_positive_int( + payload.get("required_motion_segment_count"), + f"{task_name}.imu_intrinsic.required_motion_segment_count", + ) + if "timeout_sec" in payload: + validate_positive_float(payload["timeout_sec"], f"{task_name}.imu_intrinsic.timeout_sec") + + +def validate_sensor_extrinsic(task: dict[str, Any], task_name: str) -> None: + payload = require_map(task.get("sensor_extrinsic"), f"{task_name}.sensor_extrinsic") + require_string(payload.get("base_frame_id"), f"{task_name}.sensor_extrinsic.base_frame_id") + validate_positive_int( + payload.get("required_sample_count"), + f"{task_name}.sensor_extrinsic.required_sample_count", + ) + if "timeout_sec" in payload: + validate_positive_float(payload["timeout_sec"], f"{task_name}.sensor_extrinsic.timeout_sec") + + +def validate_hand_eye(task: dict[str, Any], task_name: str) -> None: + payload = require_map(task.get("hand_eye"), f"{task_name}.hand_eye") + require_string(payload.get("arm_id"), f"{task_name}.hand_eye.arm_id") + validate_positive_int( + payload.get("required_pose_count"), + f"{task_name}.hand_eye.required_pose_count", + ) + if "timeout_sec" in payload: + validate_positive_float(payload["timeout_sec"], f"{task_name}.hand_eye.timeout_sec") + + +def validate_task(task: dict[str, Any], index: int) -> None: + task_name = f"tasks[{index}]" + require_string(task.get("task_code"), f"{task_name}.task_code") + selected_task = require_string(task.get("selected_task"), f"{task_name}.selected_task") + if selected_task not in TASK_GROUPS: + raise ValueError(f"{task_name}.selected_task 必须是 {sorted(TASK_GROUPS)} 之一。") + + require_string(task.get("sensor_id"), f"{task_name}.sensor_id") + task_subtype = require_string(task.get("task_subtype"), f"{task_name}.task_subtype") + if task_subtype not in SUPPORTED_SUBTYPES[selected_task]: + raise ValueError( + f"{task_name}.task_subtype={task_subtype} 不适用于 selected_task={selected_task}。" + ) + + if selected_task == "camera_intrinsic": + validate_camera_intrinsic(task, task_name) + elif selected_task == "imu_intrinsic": + validate_imu_intrinsic(task, task_name) + elif selected_task == "sensor_extrinsic": + validate_sensor_extrinsic(task, task_name) + elif selected_task == "hand_eye": + validate_hand_eye(task, task_name) + + +def validate_profile(profile: dict[str, Any]) -> dict[str, Any]: + schema_version = int(profile.get("schema_version", 0) or 0) + if schema_version != 1: + raise ValueError("schema_version 必须为 1。") + + data_capture = require_map(profile.get("data_capture", {}), "data_capture") + required_inputs = require_list( + data_capture.get("required_data_inputs", []), + "data_capture.required_data_inputs", + ) + for item in required_inputs: + require_string(item, "data_capture.required_data_inputs[]") + + tasks = require_list(profile.get("tasks"), "tasks") + if not tasks: + raise ValueError("tasks 不能为空。") + seen_task_codes: set[str] = set() + for index, task_value in enumerate(tasks): + task = require_map(task_value, f"tasks[{index}]") + task_code = require_string(task.get("task_code"), f"tasks[{index}].task_code") + if task_code in seen_task_codes: + raise ValueError(f"重复的 task_code:{task_code}") + seen_task_codes.add(task_code) + validate_task(task, index) + return profile + + +def make_task_param(key: str, value: Any) -> dict[str, str]: + return { + "key": key, + "value": stringify_value(value), + } + + +def task_matches_requested(task: dict[str, Any], requested_tasks: set[str]) -> bool: + if not requested_tasks: + return True + selected_task = str(task["selected_task"]) + return TASK_GROUP_TO_REQUESTED_TASK[selected_task] in requested_tasks + + +def flatten_task_metadata(task: dict[str, Any]) -> list[dict[str, str]]: + selected_task = str(task["selected_task"]) + params = [ + make_task_param("sensor.sensor_id", task["sensor_id"]), + make_task_param("sensor.task_subtype", task["task_subtype"]), + ] + + if selected_task == "camera_intrinsic": + payload = task["camera_intrinsic"] + params.append(make_task_param("camera_intrinsic.required_image_count", payload["required_image_count"])) + if payload.get("target_board_id") not in (None, ""): + params.append(make_task_param("camera_intrinsic.target_board_id", payload["target_board_id"])) + if payload.get("timeout_sec") not in (None, ""): + params.append(make_task_param("camera_intrinsic.timeout_sec", payload["timeout_sec"])) + elif selected_task == "imu_intrinsic": + payload = task["imu_intrinsic"] + params.append( + make_task_param( + "imu_intrinsic.required_static_segment_count", + payload["required_static_segment_count"], + ) + ) + params.append( + make_task_param( + "imu_intrinsic.required_motion_segment_count", + payload["required_motion_segment_count"], + ) + ) + if payload.get("timeout_sec") not in (None, ""): + params.append(make_task_param("imu_intrinsic.timeout_sec", payload["timeout_sec"])) + elif selected_task == "sensor_extrinsic": + payload = task["sensor_extrinsic"] + params.append(make_task_param("sensor_extrinsic.base_frame_id", payload["base_frame_id"])) + params.append(make_task_param("sensor_extrinsic.required_sample_count", payload["required_sample_count"])) + if payload.get("timeout_sec") not in (None, ""): + params.append(make_task_param("sensor_extrinsic.timeout_sec", payload["timeout_sec"])) + elif selected_task == "hand_eye": + payload = task["hand_eye"] + params.append(make_task_param("hand_eye.arm_id", payload["arm_id"])) + params.append(make_task_param("hand_eye.required_pose_count", payload["required_pose_count"])) + if payload.get("timeout_sec") not in (None, ""): + params.append(make_task_param("hand_eye.timeout_sec", payload["timeout_sec"])) + return params + + +def export_requested_tasks( + profile: dict[str, Any], + requested_tasks: set[str] | list[str] | str | None = None, +) -> dict[str, Any]: + selected_requested_tasks = ( + requested_tasks + if isinstance(requested_tasks, set) + else parse_requested_tasks(requested_tasks) + ) + tasks: list[dict[str, Any]] = [] + for task in profile["tasks"]: + if not bool(task.get("enabled", True)): + continue + if not task_matches_requested(task, selected_requested_tasks): + continue + selected_task = str(task["selected_task"]) + tasks.append({ + "stage_type": TASK_GROUP_TO_STAGE_TYPE[selected_task], + "enabled": True, + "require_manual_approval": bool(task.get("require_manual_approval", False)), + "execution_policy": str(task.get("execution_policy", "REQUIRED")), + "reason": str(task.get("reason", "现场传感器标定 profile")), + "task_code": str(task["task_code"]), + "target_id": str(task["sensor_id"]), + "task_params": flatten_task_metadata(task), + }) + return { + "schema_version": 1, + "requested_tasks": tasks, + } + + +def build_summary(profile: dict[str, Any]) -> dict[str, Any]: + return { + "schema_version": profile["schema_version"], + "profile_name": profile.get("profile_name", ""), + "task_count": len(profile["tasks"]), + "tasks": [ + { + "task_code": task["task_code"], + "enabled": bool(task.get("enabled", True)), + "selected_task": task["selected_task"], + "sensor_id": task["sensor_id"], + "task_subtype": task["task_subtype"], + "display_name": task.get("display_name", ""), + } + for task in profile["tasks"] + ], + } + + +def render_yaml(data: dict[str, Any]) -> str: + return yaml.safe_dump(data, sort_keys=False, allow_unicode=True) + + +def main() -> int: + args = parse_args() + profile_path = Path(args.profile).expanduser().resolve(strict=False) + profile = validate_profile(load_yaml(profile_path)) + + if args.format == "requested_tasks": + output = export_requested_tasks(profile, parse_requested_tasks(args.tasks)) + else: + output = build_summary(profile) + + rendered = render_yaml(output) + if args.output: + output_path = Path(args.output).expanduser().resolve(strict=False) + output_path.parent.mkdir(parents=True, exist_ok=True) + output_path.write_text(rendered, encoding="utf-8") + print(f"[OK] 已写入传感器标定 profile 输出:{output_path}", file=sys.stderr) + else: + print(rendered, end="") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_data_contract.md b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_data_contract.md new file mode 100644 index 0000000..b915125 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_data_contract.md @@ -0,0 +1,327 @@ +# 传感器标定数据合同 + +本文档固定真实车间传感器标定算法读取的数据入口。算法实现者不直接连相机、LiDAR、IMU 或机械臂驱动,只读取 `dataset_index.yaml` 或它转换出的 `data_input.*` ROS 参数。 + +## 数据索引字段 + +传感器数据统一写入: + +```yaml +data_inputs: + sensor: + image_sample_files: + - sensor/front_camera/images.yaml + imu_sample_files: + - sensor/imu/imu_samples.mcap + pointcloud_sample_files: + - sensor/lidar_3d/pointclouds.mcap + laser_scan_sample_files: + - sensor/lidar_2d/scans.mcap + target_detection_files: + - sensor/front_camera/charuco_detections.json + robot_pose_sample_files: + - sensor/hand_eye/robot_poses.csv + synchronized_dataset_files: + - dataset_index.yaml +``` + +字段含义: + +- `image_sample_files`:相机图像列表、图像目录索引、rosbag 或 mcap。 +- `imu_sample_files`:IMU 原始样本,必须能区分静止段和运动段。 +- `pointcloud_sample_files`:3D LiDAR 点云样本或点云帧索引。 +- `laser_scan_sample_files`:2D LiDAR LaserScan 样本或扫描帧索引。 +- `target_detection_files`:棋盘格、ChArUco、AprilTag、LiDAR 平面、角点、球心等检测结果。 +- `robot_pose_sample_files`:机械臂、平台或车辆位姿序列,主要用于手眼和联合外参。 +- `synchronized_dataset_files`:已经做过时间同步的数据集索引。真实算法推荐优先读取这个入口。 + +## 文件内部格式 + +以下格式是现场落地的稳定输入合同。真实采集程序可以直接生成这些文件;如果现场已经有 +rosbag、mcap 或厂商二进制文件,应额外生成这里定义的索引文件,把原始数据文件、时间戳、 +传感器 ID 和 frame 信息关联起来。 + +### 图像索引 `images.yaml` + +用于 `image_sample_files`。一个文件只描述一个相机的一组图像。 + +```yaml +schema_version: 1 +sensor_id: demo_front_camera +sensor_type: front_camera +frame_id: front_camera_link +topic: /sensor/front_camera/image_raw +encoding: bgr8 +image_width: 1920 +image_height: 1080 +target_board_id: front_camera_checkerboard +frames: + - sequence: 0 + stamp_us: 1770000000000000 + file: images/front_camera_000000.png + exposure_us: 8000 + gain: 1.0 + quality_score: 0.95 +``` + +必需字段: + +- 顶层:`schema_version`、`sensor_id`、`sensor_type`、`frame_id`、`frames`。 +- 每帧:`stamp_us`、`file`。 + +约定: + +- `file` 可以是相对 `dataset_root` 的路径,也可以是绝对路径。 +- `sensor_type` 使用 `front_camera` 或 `downward_camera`。 +- `quality_score` 范围为 0 到 1;采集端不知道时可以不填。 + +### 标定目标检测 `target_detections.json` + +用于 `target_detection_files`。相机、2D LiDAR、3D LiDAR 的检测结果都放在同一类文件里,通过 +`target_type` 和每帧的 `features` 区分。 + +```json +{ + "schema_version": 1, + "sensor_id": "demo_front_camera", + "sensor_type": "front_camera", + "frame_id": "front_camera_link", + "target_board_id": "front_camera_checkerboard", + "target_type": "charuco", + "detections": [ + { + "stamp_us": 1770000000000000, + "source_file": "images/front_camera_000000.png", + "target_detected": true, + "quality_score": 0.97, + "corners": [ + {"id": 0, "u_px": 350.2, "v_px": 210.4}, + {"id": 1, "u_px": 410.1, "v_px": 211.0} + ], + "target_pose_in_sensor": { + "translation_xyz_m": [0.8, 0.0, 0.2], + "rotation_xyzw": [0.0, 0.0, 0.0, 1.0] + } + } + ] +} +``` + +必需字段: + +- 顶层:`schema_version`、`sensor_id`、`target_type`、`detections`。 +- 每帧:`stamp_us`、`target_detected`。 + +常用 `target_type`: + +- `checkerboard` +- `charuco` +- `apriltag` +- `lidar_corner` +- `lidar_plane` +- `sphere` + +LiDAR 检测可使用 `features` 字段: + +```json +{ + "features": { + "corners_xy_m": [[1.0, 0.5], [1.0, -0.5]], + "planes": [ + {"normal_xyz": [1.0, 0.0, 0.0], "offset_m": -1.2, "residual_m": 0.01} + ], + "sphere_centers_xyz_m": [[1.0, 0.0, 0.5]] + } +} +``` + +### IMU 原始样本 `imu_samples.csv` + +用于 `imu_sample_files`。CSV 表头固定: + +```csv +stamp_us,sensor_id,frame_id,ax_mps2,ay_mps2,az_mps2,gx_rps,gy_rps,gz_rps,temperature_c +1770000000000000,demo_imu,imu_link,0.01,-0.02,9.81,0.001,0.000,0.002,32.5 +``` + +必需列: + +- `stamp_us` +- `sensor_id` +- `frame_id` +- `ax_mps2` +- `ay_mps2` +- `az_mps2` +- `gx_rps` +- `gy_rps` +- `gz_rps` + +### IMU 分段 `imu_segments.yaml` + +用于 `imu_sample_files`。同一字段可以同时登记 `imu_samples.csv` 和 `imu_segments.yaml`。 + +```yaml +schema_version: 1 +sensor_id: demo_imu +frame_id: imu_link +source_file: imu/imu_samples.csv +segments: + - segment_id: static_001 + segment_type: static + start_us: 1770000000000000 + end_us: 1770000005000000 + - segment_id: motion_001 + segment_type: motion + start_us: 1770000006000000 + end_us: 1770000012000000 +``` + +`segment_type` 只允许 `static` 或 `motion`。 + +### 3D 点云索引 `pointclouds.yaml` + +用于 `pointcloud_sample_files`。 + +```yaml +schema_version: 1 +sensor_id: demo_lidar_3d +sensor_type: lidar_3d +frame_id: lidar_3d_link +topic: /sensor/lidar_3d/pointcloud +pointcloud_format: pcd +frames: + - sequence: 0 + stamp_us: 1770000000000000 + file: lidar_3d/cloud_000000.pcd + point_count: 120000 + quality_score: 0.92 +``` + +必需字段: + +- 顶层:`schema_version`、`sensor_id`、`frame_id`、`frames`。 +- 每帧:`stamp_us`、`file`。 + +### 2D 激光扫描索引 `laser_scans.yaml` + +用于 `laser_scan_sample_files`。 + +```yaml +schema_version: 1 +sensor_id: demo_lidar_2d +sensor_type: lidar_2d +frame_id: lidar_2d_link +topic: /sensor/lidar_2d/scan +scan_format: csv +scans: + - sequence: 0 + stamp_us: 1770000000000000 + file: lidar_2d/scan_000000.csv + angle_min_rad: -3.14159 + angle_increment_rad: 0.00436 + range_min_m: 0.05 + range_max_m: 30.0 +``` + +单帧扫描 CSV 推荐表头: + +```csv +angle_rad,range_m,intensity +-1.57,2.31,120.0 +``` + +### 机械臂或平台位姿 `robot_poses.csv` + +用于 `robot_pose_sample_files`。CSV 表头固定: + +```csv +stamp_us,pose_id,parent_frame_id,child_frame_id,x_m,y_m,z_m,qx,qy,qz,qw,source,quality_score +1770000000000000,pose_000,base_link,tool0,0.3,0.0,0.5,0.0,0.0,0.0,1.0,robot_controller,1.0 +``` + +必需列: + +- `stamp_us` +- `parent_frame_id` +- `child_frame_id` +- `x_m` +- `y_m` +- `z_m` +- `qx` +- `qy` +- `qz` +- `qw` + +### 同步数据集 `synchronized_sensor_dataset.yaml` + +用于 `synchronized_dataset_files`。它把同一时间窗口内的相机、IMU、LiDAR、位姿和检测结果关联起来。 + +```yaml +schema_version: 1 +session_id: session_001 +vehicle_id: agv_001 +dataset_root: /data/agv_calib/site_a/session_001 +time_base: unix_us +data_window_start_timestamp_us: 1770000000000000 +data_window_end_timestamp_us: 1770000120000000 +time_sync: + max_sensor_time_offset_ms: 5.0 + source: ptp +files: + image_sample_files: + - sensor/front_camera/images.yaml + imu_sample_files: + - sensor/imu/imu_samples.csv + - sensor/imu/imu_segments.yaml + pointcloud_sample_files: + - sensor/lidar_3d/pointclouds.yaml + laser_scan_sample_files: + - sensor/lidar_2d/laser_scans.yaml + target_detection_files: + - sensor/front_camera/charuco_detections.json + robot_pose_sample_files: + - sensor/hand_eye/robot_poses.csv +``` + +## 任务到数据要求 + +相机内参: + +- 必需:`image_sample_files` 或 `synchronized_dataset_files`。 +- 推荐:`target_detection_files`。 +- 输出:`camera_intrinsics`。 + +IMU 内参: + +- 必需:`imu_sample_files` 或 `synchronized_dataset_files`。 +- 必须能区分静止段、运动段、采样频率和时间戳。 +- 输出:`imu_intrinsics`。 + +传感器到 `base_link` 外参: + +- 相机外参需要图像样本和标定目标检测结果。 +- 2D LiDAR 外参需要 `laser_scan_sample_files` 和角点/平面/球心检测结果。 +- 3D LiDAR 外参需要 `pointcloud_sample_files` 和平面/点云配准输入。 +- IMU 外参通常需要 `imu_sample_files`,并需要底盘运动或外部真值轨迹辅助。 +- 输出:`sensor_extrinsics`。 + +手眼标定: + +- 必需:图像或检测结果,以及 `robot_pose_sample_files`。 +- 输出:`hand_eye`。 + +## 输出参数文件 + +算法输出固定为: + +```bash +/sensor/estimated_params.yaml +``` + +格式模板: + +```bash +src/site_deployment/workshop_sensor_calibration_real/config/sensor_estimated_params_template.yaml +``` + +只有 `quality.data_quality_passed=true`、`quality.suitable_for_commit=true`,并且 `quality.validation_summary.auto_acceptance_passed=true` 的结果,才允许进入待提交包。 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_dataset_manifest.py b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_dataset_manifest.py new file mode 100644 index 0000000..e5edcc2 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_dataset_manifest.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +"""校验传感器标定数据文件,并可选写入 dataset_index.yaml。""" + +from __future__ import annotations + +import argparse +import csv +import json +import sys +from datetime import datetime, timezone +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +SENSOR_FIELDS = { + "image_sample_files", + "imu_sample_files", + "pointcloud_sample_files", + "laser_scan_sample_files", + "target_detection_files", + "robot_pose_sample_files", + "synchronized_dataset_files", +} + +IMU_SAMPLE_COLUMNS = { + "stamp_us", + "sensor_id", + "frame_id", + "ax_mps2", + "ay_mps2", + "az_mps2", + "gx_rps", + "gy_rps", + "gz_rps", +} + +ROBOT_POSE_COLUMNS = { + "stamp_us", + "parent_frame_id", + "child_frame_id", + "x_m", + "y_m", + "z_m", + "qx", + "qy", + "qz", + "qw", +} + + +def now_us() -> int: + return int(datetime.now(timezone.utc).timestamp() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="校验车间电脑侧已落盘的传感器标定数据,并可选写入 dataset_index.yaml。" + ) + parser.add_argument("--dataset-root", required=True, help="本次会话数据集根目录。") + parser.add_argument("--dataset-index", default="", help="可选 dataset_index.yaml;填写后写入 data_inputs.sensor。") + parser.add_argument("--session-id", default="", help="本次会话 ID;写 dataset_index 时使用。") + parser.add_argument("--site-id", default="", help="现场或产线 ID;写 dataset_index 时使用。") + parser.add_argument("--vehicle-id", default="", help="车辆 ID;写 dataset_index 时使用。") + parser.add_argument("--start-us", type=int, default=0, help="数据窗口开始时间;不填写时从文件推断。") + parser.add_argument("--end-us", type=int, default=0, help="数据窗口结束时间;不填写时从文件推断。") + parser.add_argument("--image-index", action="append", default=[], help="登记并校验 images.yaml,可重复。") + parser.add_argument("--imu-samples", action="append", default=[], help="登记并校验 imu_samples.csv,可重复。") + parser.add_argument("--imu-segments", action="append", default=[], help="登记并校验 imu_segments.yaml,可重复。") + parser.add_argument("--pointcloud-index", action="append", default=[], help="登记并校验 pointclouds.yaml,可重复。") + parser.add_argument("--laser-scan-index", action="append", default=[], help="登记并校验 laser_scans.yaml,可重复。") + parser.add_argument("--target-detections", action="append", default=[], help="登记并校验 target_detections.json,可重复。") + parser.add_argument("--robot-poses", action="append", default=[], help="登记并校验 robot_poses.csv,可重复。") + parser.add_argument("--synchronized-dataset", action="append", default=[], help="登记并校验 synchronized_sensor_dataset.yaml,可重复。") + parser.add_argument("--validate-only", action="store_true", help="只校验文件,不写 dataset_index.yaml。") + parser.add_argument("--overwrite", action="store_true", help="写 dataset_index 时覆盖已有 data_inputs.sensor。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def resolve_path(raw_path: str, dataset_root: Path) -> Path: + path = Path(raw_path).expanduser() + if path.is_absolute(): + return path + return (dataset_root / path).resolve(strict=False) + + +def relative_to_root(path: Path, dataset_root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(dataset_root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML/JSON map。") + return value + + +def require_list(value: Any, field_name: str) -> list[Any]: + if not isinstance(value, list): + raise ValueError(f"{field_name} 必须是列表。") + return value + + +def read_int(value: Any, field_name: str) -> int: + try: + return int(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是整数,当前值为 {value!r}。") from exc + + +def validate_schema_version(data: dict[str, Any], path: Path) -> None: + if read_int(data.get("schema_version"), f"{path}.schema_version") != 1: + raise ValueError(f"{path} 的 schema_version 必须为 1。") + + +def collect_stamp(stamps: list[int], value: Any, field_name: str) -> None: + stamp_us = read_int(value, field_name) + if stamp_us <= 0: + raise ValueError(f"{field_name} 必须大于 0。") + stamps.append(stamp_us) + + +def validate_index_frames( + path: Path, + data: dict[str, Any], + list_key: str, + stamp_field_name: str, +) -> list[int]: + validate_schema_version(data, path) + require_string(data.get("sensor_id"), f"{path}.sensor_id") + require_string(data.get("frame_id"), f"{path}.frame_id") + frames = require_list(data.get(list_key), f"{path}.{list_key}") + if not frames: + raise ValueError(f"{path}.{list_key} 不能为空。") + stamps: list[int] = [] + for index, frame_value in enumerate(frames): + frame = require_map(frame_value, f"{path}.{list_key}[{index}]") + collect_stamp(stamps, frame.get("stamp_us"), f"{path}.{list_key}[{index}].stamp_us") + require_string(frame.get("file"), f"{path}.{list_key}[{index}].file") + if stamps != sorted(stamps): + raise ValueError(f"{path}.{list_key} 必须按 {stamp_field_name} 递增排序。") + return stamps + + +def validate_image_index(path: Path) -> list[int]: + data = load_yaml(path) + require_string(data.get("sensor_type"), f"{path}.sensor_type") + return validate_index_frames(path, data, "frames", "stamp_us") + + +def validate_pointcloud_index(path: Path) -> list[int]: + data = load_yaml(path) + return validate_index_frames(path, data, "frames", "stamp_us") + + +def validate_laser_scan_index(path: Path) -> list[int]: + data = load_yaml(path) + return validate_index_frames(path, data, "scans", "stamp_us") + + +def validate_imu_segments(path: Path) -> list[int]: + data = load_yaml(path) + validate_schema_version(data, path) + require_string(data.get("sensor_id"), f"{path}.sensor_id") + require_string(data.get("frame_id"), f"{path}.frame_id") + segments = require_list(data.get("segments"), f"{path}.segments") + if not segments: + raise ValueError(f"{path}.segments 不能为空。") + stamps: list[int] = [] + for index, segment_value in enumerate(segments): + segment = require_map(segment_value, f"{path}.segments[{index}]") + segment_type = require_string(segment.get("segment_type"), f"{path}.segments[{index}].segment_type") + if segment_type not in {"static", "motion"}: + raise ValueError(f"{path}.segments[{index}].segment_type 必须是 static 或 motion。") + start_us = read_int(segment.get("start_us"), f"{path}.segments[{index}].start_us") + end_us = read_int(segment.get("end_us"), f"{path}.segments[{index}].end_us") + if start_us <= 0 or end_us <= start_us: + raise ValueError(f"{path}.segments[{index}] 的时间窗口无效。") + stamps.extend([start_us, end_us]) + return stamps + + +def validate_json_detections(path: Path) -> list[int]: + with path.open("r", encoding="utf-8") as stream: + data = json.load(stream) + if not isinstance(data, dict): + raise ValueError(f"{path} 顶层必须是 JSON object。") + validate_schema_version(data, path) + require_string(data.get("sensor_id"), f"{path}.sensor_id") + require_string(data.get("target_type"), f"{path}.target_type") + detections = require_list(data.get("detections"), f"{path}.detections") + if not detections: + raise ValueError(f"{path}.detections 不能为空。") + stamps: list[int] = [] + for index, detection_value in enumerate(detections): + detection = require_map(detection_value, f"{path}.detections[{index}]") + collect_stamp(stamps, detection.get("stamp_us"), f"{path}.detections[{index}].stamp_us") + if "target_detected" not in detection: + raise ValueError(f"{path}.detections[{index}].target_detected 不能为空。") + return stamps + + +def csv_header(path: Path) -> list[str]: + with path.open("r", encoding="utf-8", newline="") as stream: + reader = csv.reader(stream) + try: + header = next(reader) + except StopIteration as exc: + raise ValueError(f"{path} 不能为空 CSV。") from exc + return [item.strip() for item in header] + + +def validate_csv_columns(path: Path, required_columns: set[str]) -> list[int]: + header = csv_header(path) + missing = sorted(required_columns - set(header)) + if missing: + raise ValueError(f"{path} 缺少 CSV 列:{', '.join(missing)}。") + stamps: list[int] = [] + with path.open("r", encoding="utf-8", newline="") as stream: + reader = csv.DictReader(stream) + for row_index, row in enumerate(reader): + collect_stamp(stamps, row.get("stamp_us"), f"{path}.row[{row_index}].stamp_us") + if not stamps: + raise ValueError(f"{path} 至少需要一行数据。") + if stamps != sorted(stamps): + raise ValueError(f"{path} 必须按 stamp_us 递增排序。") + return stamps + + +def validate_synchronized_dataset(path: Path) -> list[int]: + data = load_yaml(path) + validate_schema_version(data, path) + require_string(data.get("session_id"), f"{path}.session_id") + require_string(data.get("vehicle_id"), f"{path}.vehicle_id") + files = require_map(data.get("files"), f"{path}.files") + if not any(files.get(field_name) for field_name in SENSOR_FIELDS): + raise ValueError(f"{path}.files 至少需要包含一个传感器数据字段。") + start_us = read_int(data.get("data_window_start_timestamp_us"), f"{path}.data_window_start_timestamp_us") + end_us = read_int(data.get("data_window_end_timestamp_us"), f"{path}.data_window_end_timestamp_us") + if start_us <= 0 or end_us <= start_us: + raise ValueError(f"{path} 的数据窗口无效。") + return [start_us, end_us] + + +def append_file( + sensor_inputs: dict[str, list[str]], + field_name: str, + raw_files: list[str], + dataset_root: Path, + validator, + stamps: list[int], +) -> None: + for raw_file in raw_files: + path = resolve_path(raw_file, dataset_root) + if not path.exists(): + raise FileNotFoundError(f"文件不存在:{path}") + stamps.extend(validator(path)) + value = relative_to_root(path, dataset_root) + sensor_inputs.setdefault(field_name, []) + if value not in sensor_inputs[field_name]: + sensor_inputs[field_name].append(value) + + +def load_or_create_index( + dataset_index_path: Path, + dataset_root: Path, + args: argparse.Namespace, + start_us: int, + end_us: int, +) -> dict[str, Any]: + if dataset_index_path.exists(): + index = load_yaml(dataset_index_path) + else: + index = {"schema_version": 1, "data_inputs": {}} + session = index.setdefault("session", {}) + if not isinstance(session, dict): + raise ValueError("dataset_index.session 必须是 YAML map。") + session["session_id"] = args.session_id or session.get("session_id") or dataset_root.name + session["site_id"] = args.site_id or session.get("site_id", "") + session["vehicle_id"] = args.vehicle_id or session.get("vehicle_id", "") + session["dataset_root"] = str(dataset_root) + session["data_window_start_timestamp_us"] = start_us + session["data_window_end_timestamp_us"] = end_us + index.setdefault("schema_version", 1) + index.setdefault("data_inputs", {}) + return index + + +def merge_sensor_inputs(index: dict[str, Any], sensor_inputs: dict[str, list[str]], overwrite: bool) -> None: + data_inputs = index.setdefault("data_inputs", {}) + if not isinstance(data_inputs, dict): + raise ValueError("dataset_index.data_inputs 必须是 YAML map。") + current = {} if overwrite else data_inputs.setdefault("sensor", {}) + if not isinstance(current, dict): + raise ValueError("dataset_index.data_inputs.sensor 必须是 YAML map。") + for field_name, values in sensor_inputs.items(): + existing_value = [] if overwrite else current.get(field_name, []) or [] + if isinstance(existing_value, str): + merged = [existing_value] + else: + merged = list(existing_value) + for value in values: + if value not in merged: + merged.append(value) + current[field_name] = merged + data_inputs["sensor"] = current + + +def main() -> int: + args = parse_args() + dataset_root = Path(args.dataset_root).expanduser().resolve(strict=False) + sensor_inputs: dict[str, list[str]] = {} + stamps: list[int] = [] + + append_file(sensor_inputs, "image_sample_files", args.image_index, dataset_root, validate_image_index, stamps) + append_file(sensor_inputs, "imu_sample_files", args.imu_samples, dataset_root, lambda path: validate_csv_columns(path, IMU_SAMPLE_COLUMNS), stamps) + append_file(sensor_inputs, "imu_sample_files", args.imu_segments, dataset_root, validate_imu_segments, stamps) + append_file(sensor_inputs, "pointcloud_sample_files", args.pointcloud_index, dataset_root, validate_pointcloud_index, stamps) + append_file(sensor_inputs, "laser_scan_sample_files", args.laser_scan_index, dataset_root, validate_laser_scan_index, stamps) + append_file(sensor_inputs, "target_detection_files", args.target_detections, dataset_root, validate_json_detections, stamps) + append_file(sensor_inputs, "robot_pose_sample_files", args.robot_poses, dataset_root, lambda path: validate_csv_columns(path, ROBOT_POSE_COLUMNS), stamps) + append_file(sensor_inputs, "synchronized_dataset_files", args.synchronized_dataset, dataset_root, validate_synchronized_dataset, stamps) + + if not sensor_inputs: + raise ValueError("至少需要提供一个传感器数据文件。") + + start_us = args.start_us or min(stamps) + end_us = args.end_us or max(stamps) + if end_us <= start_us: + end_us = start_us + 1 + + print(f"[OK] 传感器数据文件校验通过,文件组数:{sum(len(v) for v in sensor_inputs.values())}") + print(f"[OK] 数据窗口:{start_us} -> {end_us}") + + if args.validate_only: + return 0 + + if not args.dataset_index: + raise ValueError("未设置 --validate-only 时必须提供 --dataset-index。") + dataset_index_path = Path(args.dataset_index).expanduser().resolve(strict=False) + index = load_or_create_index(dataset_index_path, dataset_root, args, start_us, end_us) + merge_sensor_inputs(index, sensor_inputs, overwrite=args.overwrite) + write_yaml(dataset_index_path, index) + print(f"[OK] 已写入 dataset_index.yaml:{dataset_index_path}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_preprocess_pipeline.py b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_preprocess_pipeline.py new file mode 100644 index 0000000..4448189 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/sensor_preprocess_pipeline.py @@ -0,0 +1,528 @@ +#!/usr/bin/env python3 +"""传感器标定数据预处理和检测入口。""" + +from __future__ import annotations + +import argparse +import csv +import json +import math +import sys +import time +from pathlib import Path +from statistics import mean +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="汇总传感器标定数据质量,并可选生成相机标定目标检测结果。") + parser.add_argument("--dataset-root", required=True, help="本次会话数据集根目录。") + parser.add_argument("--dataset-index", default="", help="可选 dataset_index.yaml;填写后把预处理产物写回 metadata。") + parser.add_argument("--image-index", action="append", default=[], help="images.yaml,可重复。") + parser.add_argument("--target-detections", action="append", default=[], help="已有 target_detections.json,可重复。") + parser.add_argument("--imu-samples", action="append", default=[], help="imu_samples.csv,可重复。") + parser.add_argument("--imu-segments", action="append", default=[], help="imu_segments.yaml,可重复。") + parser.add_argument("--pointcloud-index", action="append", default=[], help="pointclouds.yaml,可重复。") + parser.add_argument("--laser-scan-index", action="append", default=[], help="laser_scans.yaml,可重复。") + parser.add_argument("--robot-poses", action="append", default=[], help="robot_poses.csv,可重复。") + parser.add_argument("--output-report", default="", help="预处理质量报告;默认写到 dataset_root/sensor/preprocess_quality_report.json。") + parser.add_argument("--generate-image-detections", action="store_true", help="使用 OpenCV 从 image-index 中生成相机检测结果。") + parser.add_argument("--output-target-detections", default="", help="生成的相机检测结果 JSON;默认写到 dataset_root/sensor/preprocessed_target_detections.json。") + parser.add_argument("--target-type", choices=["checkerboard", "charuco"], default="checkerboard", help="相机检测目标类型。") + parser.add_argument("--checkerboard-cols", type=int, default=0, help="棋盘格内角点列数。") + parser.add_argument("--checkerboard-rows", type=int, default=0, help="棋盘格内角点行数。") + parser.add_argument("--charuco-squares-x", type=int, default=0, help="ChArUco 棋盘格列数。") + parser.add_argument("--charuco-squares-y", type=int, default=0, help="ChArUco 棋盘格行数。") + parser.add_argument("--charuco-square-size-m", type=float, default=0.0, help="ChArUco 方格边长。") + parser.add_argument("--charuco-marker-size-m", type=float, default=0.0, help="ChArUco marker 边长。") + parser.add_argument("--aruco-dictionary", default="DICT_4X4_50", help="OpenCV ArUco 字典名。") + parser.add_argument("--max-image-missing-ratio", type=float, default=0.0, help="允许图像文件缺失比例。") + parser.add_argument("--max-imu-gyro-rps", type=float, default=20.0, help="IMU 角速度饱和检查阈值。") + parser.add_argument("--max-imu-accel-mps2", type=float, default=80.0, help="IMU 加速度饱和检查阈值。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否写回 dataset_index metadata。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_json(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(json.dumps(data, ensure_ascii=False, indent=2), encoding="utf-8") + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def resolve_path(raw_path: str, dataset_root: Path) -> Path: + path = Path(raw_path).expanduser() + if path.is_absolute(): + return path + return (dataset_root / path).resolve(strict=False) + + +def relative_to_root(path: Path, dataset_root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(dataset_root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def as_float(value: Any, default: float = 0.0) -> float: + try: + return float(value) + except (TypeError, ValueError): + return default + + +def as_int(value: Any, default: int = 0) -> int: + try: + return int(value) + except (TypeError, ValueError): + return default + + +def quality_summary(values: list[float]) -> dict[str, Any]: + if not values: + return {"count": 0, "average": 0.0, "minimum": 0.0, "maximum": 0.0} + return { + "count": len(values), + "average": mean(values), + "minimum": min(values), + "maximum": max(values), + } + + +def read_image_index(path: Path, dataset_root: Path) -> dict[str, Any]: + data = load_yaml(path) + frames = data.get("frames", []) + if not isinstance(frames, list): + raise ValueError(f"{path}.frames 必须是列表。") + missing_count = 0 + stamps: list[int] = [] + quality_values: list[float] = [] + for frame in frames: + if not isinstance(frame, dict): + continue + stamp_us = as_int(frame.get("stamp_us")) + if stamp_us > 0: + stamps.append(stamp_us) + if "quality_score" in frame: + quality_values.append(as_float(frame.get("quality_score"))) + image_file = str(frame.get("file", "") or "") + if image_file and not resolve_path(image_file, dataset_root).exists(): + missing_count += 1 + return { + "file": str(path), + "sensor_id": data.get("sensor_id", ""), + "sensor_type": data.get("sensor_type", ""), + "frame_id": data.get("frame_id", ""), + "target_board_id": data.get("target_board_id", ""), + "frame_count": len(frames), + "missing_file_count": missing_count, + "missing_file_ratio": missing_count / max(len(frames), 1), + "start_us": min(stamps) if stamps else 0, + "end_us": max(stamps) if stamps else 0, + "quality": quality_summary(quality_values), + } + + +def read_detection_file(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = json.load(stream) + if not isinstance(data, dict): + raise ValueError(f"{path} 顶层必须是 JSON object。") + detections = data.get("detections", []) + if not isinstance(detections, list): + raise ValueError(f"{path}.detections 必须是列表。") + detected_count = 0 + quality_values: list[float] = [] + stamps: list[int] = [] + for detection in detections: + if not isinstance(detection, dict): + continue + if bool(detection.get("target_detected", False)): + detected_count += 1 + if "quality_score" in detection: + quality_values.append(as_float(detection.get("quality_score"))) + stamp_us = as_int(detection.get("stamp_us")) + if stamp_us > 0: + stamps.append(stamp_us) + return { + "file": str(path), + "sensor_id": data.get("sensor_id", ""), + "target_type": data.get("target_type", ""), + "detection_count": len(detections), + "target_detected_count": detected_count, + "target_detected_ratio": detected_count / max(len(detections), 1), + "start_us": min(stamps) if stamps else 0, + "end_us": max(stamps) if stamps else 0, + "quality": quality_summary(quality_values), + } + + +def read_csv_rows(path: Path) -> list[dict[str, str]]: + with path.open("r", encoding="utf-8", newline="") as stream: + reader = csv.DictReader(stream) + return list(reader) + + +def read_imu_samples(path: Path, args: argparse.Namespace) -> dict[str, Any]: + rows = read_csv_rows(path) + stamps: list[int] = [] + accel_norms: list[float] = [] + gyro_norms: list[float] = [] + saturation_count = 0 + for row in rows: + stamp_us = as_int(row.get("stamp_us")) + if stamp_us > 0: + stamps.append(stamp_us) + ax = as_float(row.get("ax_mps2")) + ay = as_float(row.get("ay_mps2")) + az = as_float(row.get("az_mps2")) + gx = as_float(row.get("gx_rps")) + gy = as_float(row.get("gy_rps")) + gz = as_float(row.get("gz_rps")) + accel_norm = math.sqrt(ax * ax + ay * ay + az * az) + gyro_norm = math.sqrt(gx * gx + gy * gy + gz * gz) + accel_norms.append(accel_norm) + gyro_norms.append(gyro_norm) + if accel_norm > args.max_imu_accel_mps2 or gyro_norm > args.max_imu_gyro_rps: + saturation_count += 1 + return { + "file": str(path), + "sample_count": len(rows), + "start_us": min(stamps) if stamps else 0, + "end_us": max(stamps) if stamps else 0, + "accel_norm_mps2": quality_summary(accel_norms), + "gyro_norm_rps": quality_summary(gyro_norms), + "saturation_count": saturation_count, + "saturation_ratio": saturation_count / max(len(rows), 1), + } + + +def read_imu_segments(path: Path) -> dict[str, Any]: + data = load_yaml(path) + segments = data.get("segments", []) + if not isinstance(segments, list): + raise ValueError(f"{path}.segments 必须是列表。") + static_count = 0 + motion_count = 0 + durations: list[float] = [] + for segment in segments: + if not isinstance(segment, dict): + continue + segment_type = str(segment.get("segment_type", "") or "") + if segment_type == "static": + static_count += 1 + elif segment_type == "motion": + motion_count += 1 + start_us = as_int(segment.get("start_us")) + end_us = as_int(segment.get("end_us")) + if end_us > start_us > 0: + durations.append((end_us - start_us) / 1_000_000.0) + return { + "file": str(path), + "sensor_id": data.get("sensor_id", ""), + "segment_count": len(segments), + "static_segment_count": static_count, + "motion_segment_count": motion_count, + "duration_sec": quality_summary(durations), + } + + +def read_frame_index(path: Path, list_key: str) -> dict[str, Any]: + data = load_yaml(path) + frames = data.get(list_key, []) + if not isinstance(frames, list): + raise ValueError(f"{path}.{list_key} 必须是列表。") + stamps: list[int] = [] + quality_values: list[float] = [] + for frame in frames: + if not isinstance(frame, dict): + continue + stamp_us = as_int(frame.get("stamp_us")) + if stamp_us > 0: + stamps.append(stamp_us) + if "quality_score" in frame: + quality_values.append(as_float(frame.get("quality_score"))) + return { + "file": str(path), + "sensor_id": data.get("sensor_id", ""), + "sensor_type": data.get("sensor_type", ""), + "frame_id": data.get("frame_id", ""), + "frame_count": len(frames), + "start_us": min(stamps) if stamps else 0, + "end_us": max(stamps) if stamps else 0, + "quality": quality_summary(quality_values), + } + + +def read_robot_poses(path: Path) -> dict[str, Any]: + rows = read_csv_rows(path) + stamps: list[int] = [] + quality_values: list[float] = [] + for row in rows: + stamp_us = as_int(row.get("stamp_us")) + if stamp_us > 0: + stamps.append(stamp_us) + if row.get("quality_score") not in (None, ""): + quality_values.append(as_float(row.get("quality_score"))) + return { + "file": str(path), + "pose_count": len(rows), + "start_us": min(stamps) if stamps else 0, + "end_us": max(stamps) if stamps else 0, + "quality": quality_summary(quality_values), + } + + +def import_cv2(): + try: + import cv2 + except ImportError as exc: + raise RuntimeError("生成图像检测结果需要安装 OpenCV Python 模块 cv2。") from exc + return cv2 + + +def detect_checkerboard(cv2, image_path: Path, pattern_size: tuple[int, int]) -> tuple[bool, list[dict[str, Any]]]: + image = cv2.imread(str(image_path)) + if image is None: + return False, [] + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + found, corners = cv2.findChessboardCorners(gray, pattern_size) + if not found: + return False, [] + criteria = ( + cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, + 30, + 0.001, + ) + corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + result: list[dict[str, Any]] = [] + for index, corner in enumerate(corners.reshape(-1, 2)): + result.append({"id": index, "u_px": float(corner[0]), "v_px": float(corner[1])}) + return True, result + + +def detect_charuco(cv2, image_path: Path, args: argparse.Namespace) -> tuple[bool, list[dict[str, Any]]]: + if not hasattr(cv2, "aruco"): + raise RuntimeError("当前 OpenCV 未启用 aruco 模块,无法生成 ChArUco 检测结果。") + if args.charuco_squares_x <= 0 or args.charuco_squares_y <= 0: + raise ValueError("ChArUco 检测必须设置 --charuco-squares-x 和 --charuco-squares-y。") + if args.charuco_square_size_m <= 0.0 or args.charuco_marker_size_m <= 0.0: + raise ValueError("ChArUco 检测必须设置 --charuco-square-size-m 和 --charuco-marker-size-m。") + dictionary_id = getattr(cv2.aruco, args.aruco_dictionary, None) + if dictionary_id is None: + raise ValueError(f"未知 ArUco 字典:{args.aruco_dictionary}") + dictionary = cv2.aruco.getPredefinedDictionary(dictionary_id) + board = cv2.aruco.CharucoBoard( + (args.charuco_squares_x, args.charuco_squares_y), + args.charuco_square_size_m, + args.charuco_marker_size_m, + dictionary, + ) + image = cv2.imread(str(image_path)) + if image is None: + return False, [] + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + marker_corners, marker_ids, _ = cv2.aruco.detectMarkers(gray, dictionary) + if marker_ids is None or len(marker_ids) == 0: + return False, [] + _, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco( + marker_corners, + marker_ids, + gray, + board, + ) + if charuco_ids is None or charuco_corners is None: + return False, [] + result: list[dict[str, Any]] = [] + for index, corner in zip(charuco_ids.flatten(), charuco_corners.reshape(-1, 2)): + result.append({"id": int(index), "u_px": float(corner[0]), "v_px": float(corner[1])}) + return bool(result), result + + +def generate_image_detections( + image_index_paths: list[Path], + dataset_root: Path, + args: argparse.Namespace, +) -> dict[str, Any]: + if not image_index_paths: + raise ValueError("生成图像检测结果需要至少一个 --image-index。") + cv2 = import_cv2() + if args.target_type == "checkerboard": + if args.checkerboard_cols <= 0 or args.checkerboard_rows <= 0: + raise ValueError("棋盘格检测必须设置 --checkerboard-cols 和 --checkerboard-rows。") + pattern_size = (args.checkerboard_cols, args.checkerboard_rows) + else: + pattern_size = (0, 0) + + first_index = load_yaml(image_index_paths[0]) + detections: list[dict[str, Any]] = [] + for index_path in image_index_paths: + image_index = load_yaml(index_path) + frames = image_index.get("frames", []) + if not isinstance(frames, list): + continue + for frame in frames: + if not isinstance(frame, dict): + continue + source_file = str(frame.get("file", "") or "") + image_path = resolve_path(source_file, dataset_root) + if args.target_type == "checkerboard": + found, corners = detect_checkerboard(cv2, image_path, pattern_size) + else: + found, corners = detect_charuco(cv2, image_path, args) + detections.append({ + "stamp_us": as_int(frame.get("stamp_us")), + "source_file": source_file, + "target_detected": found, + "quality_score": 1.0 if found else 0.0, + "corners": corners, + }) + + return { + "schema_version": 1, + "sensor_id": first_index.get("sensor_id", ""), + "sensor_type": first_index.get("sensor_type", ""), + "frame_id": first_index.get("frame_id", ""), + "target_board_id": first_index.get("target_board_id", ""), + "target_type": args.target_type, + "generated_timestamp_us": now_us(), + "generated_by": "sensor_preprocess_pipeline.py", + "detections": detections, + } + + +def update_dataset_index( + dataset_index_path: Path, + dataset_root: Path, + report_path: Path, + detection_path: Path | None, +) -> None: + if not dataset_index_path.exists(): + return + index = load_yaml(dataset_index_path) + metadata = index.setdefault("metadata", {}) + metadata["sensor_preprocess_report_file"] = relative_to_root(report_path, dataset_root) + metadata["sensor_preprocess_state"] = "completed" + metadata["sensor_preprocess_generated_timestamp_us"] = now_us() + if detection_path is not None: + data_inputs = index.setdefault("data_inputs", {}) + sensor_inputs = data_inputs.setdefault("sensor", {}) + detections = sensor_inputs.setdefault("target_detection_files", []) + if isinstance(detections, str): + detections = [detections] + detection_file = relative_to_root(detection_path, dataset_root) + if detection_file not in detections: + detections.append(detection_file) + sensor_inputs["target_detection_files"] = detections + metadata["sensor_preprocess_target_detection_file"] = detection_file + write_yaml(dataset_index_path, index) + + +def resolved_files(raw_files: list[str], dataset_root: Path) -> list[Path]: + return [resolve_path(raw_file, dataset_root) for raw_file in raw_files] + + +def main() -> int: + args = parse_args() + dataset_root = Path(args.dataset_root).expanduser().resolve(strict=False) + output_report = ( + Path(args.output_report).expanduser().resolve(strict=False) + if args.output_report + else (dataset_root / "sensor" / "preprocess_quality_report.json").resolve(strict=False) + ) + + image_index_paths = resolved_files(args.image_index, dataset_root) + target_detection_paths = resolved_files(args.target_detections, dataset_root) + generated_detection_path: Path | None = None + generated_detection: dict[str, Any] | None = None + + if args.generate_image_detections: + generated_detection = generate_image_detections(image_index_paths, dataset_root, args) + generated_detection_path = ( + Path(args.output_target_detections).expanduser().resolve(strict=False) + if args.output_target_detections + else (dataset_root / "sensor" / "preprocessed_target_detections.json").resolve(strict=False) + ) + write_json(generated_detection_path, generated_detection) + target_detection_paths.append(generated_detection_path) + + image_summaries = [read_image_index(path, dataset_root) for path in image_index_paths] + detection_summaries = [read_detection_file(path) for path in target_detection_paths] + imu_sample_summaries = [read_imu_samples(path, args) for path in resolved_files(args.imu_samples, dataset_root)] + imu_segment_summaries = [read_imu_segments(path) for path in resolved_files(args.imu_segments, dataset_root)] + pointcloud_summaries = [read_frame_index(path, "frames") for path in resolved_files(args.pointcloud_index, dataset_root)] + laser_scan_summaries = [read_frame_index(path, "scans") for path in resolved_files(args.laser_scan_index, dataset_root)] + robot_pose_summaries = [read_robot_poses(path) for path in resolved_files(args.robot_poses, dataset_root)] + + warnings: list[str] = [] + for summary in image_summaries: + if summary["missing_file_ratio"] > args.max_image_missing_ratio: + warnings.append( + f"{summary['file']} 图像缺失比例 {summary['missing_file_ratio']:.3f} 超过阈值。" + ) + for summary in imu_sample_summaries: + if summary["saturation_count"] > 0: + warnings.append(f"{summary['file']} 存在 IMU 饱和样本:{summary['saturation_count']}。") + + report = { + "schema_version": 1, + "generated_timestamp_us": now_us(), + "generated_by": "sensor_preprocess_pipeline.py", + "dataset_root": str(dataset_root), + "image_samples": image_summaries, + "target_detections": detection_summaries, + "imu_samples": imu_sample_summaries, + "imu_segments": imu_segment_summaries, + "pointcloud_samples": pointcloud_summaries, + "laser_scan_samples": laser_scan_summaries, + "robot_poses": robot_pose_summaries, + "warnings": warnings, + "passed": not warnings, + } + if generated_detection_path is not None: + report["generated_target_detection_file"] = str(generated_detection_path) + report["generated_target_detection_count"] = len(generated_detection["detections"] if generated_detection else []) + + write_json(output_report, report) + if args.dataset_index and args.update_dataset_index: + update_dataset_index( + Path(args.dataset_index).expanduser().resolve(strict=False), + dataset_root, + output_report, + generated_detection_path, + ) + print(f"[OK] 已生成传感器预处理报告:{output_report}") + if generated_detection_path is not None: + print(f"[OK] 已生成相机检测结果:{generated_detection_path}") + if warnings: + for warning in warnings: + print(f"[WARN] {warning}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) 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 new file mode 100644 index 0000000..cfadcc5 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/smoke_test_sensor_calibration_real.py @@ -0,0 +1,361 @@ +#!/usr/bin/env python3 +"""验证传感器标定车间电脑侧数据、交接和应用链路。""" + +from __future__ import annotations + +import argparse +import shutil +import subprocess +import sys +import tempfile +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +SCRIPT_DIR = Path(__file__).resolve().parent +REPO_ROOT = Path(__file__).resolve().parents[3] +DEPLOYMENT_TOOLS = REPO_ROOT / "src" / "deployment" / "tools" + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="本地验证 sensor 数据回放、待提交包、审批交接、参数配置落盘链路。") + parser.add_argument("--session-dir", default="", help="smoke 会话目录;为空时使用临时目录。") + parser.add_argument("--keep-session-dir", action="store_true", help="使用临时目录时保留输出。") + parser.add_argument("--vehicle-id", default="agv_sensor_smoke_001") + parser.add_argument("--site-id", default="site_a") + parser.add_argument("--operator-id", default="sensor_smoke_operator") + return parser.parse_args() + + +def run_command(args: list[str], cwd: Path) -> None: + result = subprocess.run(args, cwd=cwd, text=True, capture_output=True, check=False) + if result.returncode != 0: + if result.stdout: + print(result.stdout, file=sys.stderr) + if result.stderr: + print(result.stderr, file=sys.stderr) + raise RuntimeError(f"命令执行失败:{' '.join(args)}") + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def copy_examples(session_dir: Path) -> None: + source_dir = SCRIPT_DIR / "config" / "data_examples" + shutil.copytree(source_dir, session_dir, dirs_exist_ok=True) + + +def make_estimated_params( + session_dir: Path, + vehicle_id: str, + selected_task: str, + task_subtype: str, + sensor_id: str, +) -> Path: + parameter_version = f"{sensor_id}_{task_subtype}_smoke_v1" + if selected_task == "camera_intrinsic": + estimated_params = { + "camera_intrinsics": { + "image_width": 1920, + "image_height": 1080, + "camera_model": "pinhole", + "distortion_model": "plumb_bob", + "fx": 1000.0, + "fy": 1001.0, + "cx": 960.0, + "cy": 540.0, + "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0], + } + } + validation_summary = { + "reprojection_error_px": 0.25, + "sample_count": 20, + "auto_acceptance_passed": True, + } + elif selected_task == "sensor_extrinsic": + estimated_params = { + "sensor_extrinsics": { + "parent_frame_id": "base_link", + "child_frame_id": "lidar_3d_link", + "translation_xyz_m": [0.5, 0.0, 1.2], + "rotation_xyzw": [0.0, 0.0, 0.0, 1.0], + } + } + validation_summary = { + "translation_residual_m": 0.01, + "rotation_residual_rad": 0.002, + "plane_residual_m": 0.01, + "sample_count": 10, + "auto_acceptance_passed": True, + } + elif selected_task == "hand_eye": + estimated_params = { + "hand_eye": { + "base_frame_id": "base_link", + "tool_frame_id": "tool0", + "camera_frame_id": "front_camera_link", + "translation_xyz_m": [0.1, 0.0, 0.2], + "rotation_xyzw": [0.0, 0.0, 0.0, 1.0], + } + } + validation_summary = { + "reprojection_error_px": 0.3, + "translation_residual_m": 0.012, + "rotation_residual_rad": 0.003, + "repeatability_error_m": 0.004, + "sample_count": 10, + "auto_acceptance_passed": True, + } + else: + raise ValueError(f"不支持的 selected_task:{selected_task}") + + output_path = session_dir / "sensor" / f"estimated_params_{task_subtype}.yaml" + output = { + "schema_version": 1, + "session_id": session_dir.name, + "vehicle_id": vehicle_id, + "sensor_id": sensor_id, + "selected_task": selected_task, + "task_subtype": task_subtype, + "parameter_version": parameter_version, + "estimated_params": estimated_params, + "quality": { + "data_quality_passed": True, + "suitable_for_commit": True, + "validation_summary": validation_summary, + }, + } + write_yaml(output_path, output) + return output_path + + +def stage_approve_apply( + session_dir: Path, + dataset_index: Path, + estimated_params: Path, + task_subtype: str, + operator_id: str, +) -> dict[str, str]: + pending = session_dir / "sensor" / f"pending_sensor_commit_{task_subtype}.yaml" + approved = session_dir / "sensor" / f"approved_sensor_parameter_handoff_{task_subtype}.yaml" + applied_dir = session_dir / "sensor" / "applied_parameters" / task_subtype + + run_command( + [ + sys.executable, + str(SCRIPT_DIR / "stage_sensor_parameter_commit.py"), + "--dataset-index", + str(dataset_index), + "--estimated-params", + str(estimated_params), + "--output", + str(pending), + "--operator-id", + operator_id, + ], + REPO_ROOT, + ) + run_command( + [ + sys.executable, + str(SCRIPT_DIR / "approve_sensor_pending_parameters.py"), + str(pending), + "--operator-id", + operator_id, + "--output", + str(approved), + ], + REPO_ROOT, + ) + run_command( + [ + sys.executable, + str(SCRIPT_DIR / "apply_sensor_parameter_handoff.py"), + str(approved), + "--output-dir", + str(applied_dir), + "--operator-id", + operator_id, + ], + REPO_ROOT, + ) + return { + "estimated_params": str(estimated_params), + "pending_commit": str(pending), + "approved_handoff": str(approved), + "applied_dir": str(applied_dir), + } + + +def run_smoke(session_dir: Path, args: argparse.Namespace) -> dict[str, Any]: + copy_examples(session_dir) + dataset_index = session_dir / "dataset_index.yaml" + site_data_input = session_dir / "site_data_input.yaml" + preprocess_report = session_dir / "sensor" / "preprocess_quality_report.json" + + run_command( + [ + sys.executable, + str(SCRIPT_DIR / "sensor_dataset_manifest.py"), + "--dataset-root", + str(session_dir), + "--dataset-index", + str(dataset_index), + "--session-id", + session_dir.name, + "--site-id", + args.site_id, + "--vehicle-id", + args.vehicle_id, + "--image-index", + "images.yaml", + "--target-detections", + "charuco_detections.json", + "--imu-samples", + "imu_samples.csv", + "--imu-segments", + "imu_segments.yaml", + "--pointcloud-index", + "pointclouds.yaml", + "--laser-scan-index", + "laser_scans.yaml", + "--robot-poses", + "robot_poses.csv", + "--synchronized-dataset", + "synchronized_sensor_dataset.yaml", + "--overwrite", + ], + REPO_ROOT, + ) + run_command( + [ + sys.executable, + str(SCRIPT_DIR / "sensor_preprocess_pipeline.py"), + "--dataset-root", + str(session_dir), + "--dataset-index", + str(dataset_index), + "--image-index", + "images.yaml", + "--target-detections", + "charuco_detections.json", + "--imu-samples", + "imu_samples.csv", + "--imu-segments", + "imu_segments.yaml", + "--pointcloud-index", + "pointclouds.yaml", + "--laser-scan-index", + "laser_scans.yaml", + "--robot-poses", + "robot_poses.csv", + "--output-report", + str(preprocess_report), + "--max-image-missing-ratio", + "1.0", + ], + REPO_ROOT, + ) + run_command( + [ + sys.executable, + str(DEPLOYMENT_TOOLS / "dataset_index_to_site_data_input.py"), + str(dataset_index), + "-o", + str(site_data_input), + "--strict", + ], + REPO_ROOT, + ) + run_command( + [ + sys.executable, + str(DEPLOYMENT_TOOLS / "validate_dataset_index.py"), + str(dataset_index), + "--tasks", + "sensor_intrinsic,sensor_extrinsic,hand_eye", + ], + REPO_ROOT, + ) + + 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"), + ] + generated: list[dict[str, str]] = [] + for selected_task, task_subtype, sensor_id in tasks: + estimated_params = make_estimated_params( + session_dir, + args.vehicle_id, + selected_task, + task_subtype, + sensor_id, + ) + generated.append( + stage_approve_apply( + session_dir, + dataset_index, + estimated_params, + task_subtype, + args.operator_id, + ) + ) + + report = { + "schema_version": 1, + "success": True, + "session_dir": str(session_dir), + "dataset_index": str(dataset_index), + "site_data_input": str(site_data_input), + "preprocess_report": str(preprocess_report), + "generated": generated, + } + report_path = session_dir / "sensor" / "sensor_smoke_report.yaml" + write_yaml(report_path, report) + return report + + +def main() -> int: + args = parse_args() + if args.session_dir: + session_dir = Path(args.session_dir).expanduser().resolve(strict=False) + session_dir.mkdir(parents=True, exist_ok=True) + report = run_smoke(session_dir, args) + print(f"[OK] sensor 标定本地 smoke 通过:{report['session_dir']}") + return 0 + + with tempfile.TemporaryDirectory(prefix="agv_sensor_smoke_") as temp_dir: + session_dir = Path(temp_dir).resolve(strict=False) + report = run_smoke(session_dir, args) + print(f"[OK] sensor 标定本地 smoke 通过:{report['session_dir']}") + if args.keep_session_dir: + keep_dir = Path("/tmp") / f"{session_dir.name}_kept" + if keep_dir.exists(): + shutil.rmtree(keep_dir) + shutil.copytree(session_dir, keep_dir) + print(f"[OK] 已保留 smoke 输出:{keep_dir}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/stage_sensor_parameter_commit.py b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/stage_sensor_parameter_commit.py new file mode 100644 index 0000000..44b7594 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/stage_sensor_parameter_commit.py @@ -0,0 +1,372 @@ +#!/usr/bin/env python3 +"""生成传感器标定待提交参数包。""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +import time +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +TASK_PARAM_BRANCH = { + "camera_intrinsic": "camera_intrinsics", + "imu_intrinsic": "imu_intrinsics", + "sensor_extrinsic": "sensor_extrinsics", + "hand_eye": "hand_eye", +} + +SUPPORTED_SUBTYPES = { + "camera_intrinsic": {"front_camera_intrinsic", "downward_camera_intrinsic"}, + "imu_intrinsic": {"imu_intrinsic"}, + "sensor_extrinsic": { + "front_camera_extrinsic", + "downward_camera_extrinsic", + "imu_extrinsic", + "lidar_2d_extrinsic", + "lidar_3d_extrinsic", + }, + "hand_eye": {"eye_in_hand", "eye_to_hand"}, +} + + +def now_us() -> int: + return int(time.time() * 1_000_000) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="把传感器算法输出整理成现场待审批/待提交参数包。") + parser.add_argument("--dataset-index", required=True, help="本次会话 dataset_index.yaml。") + parser.add_argument("--estimated-params", required=True, help="算法输出的传感器参数 YAML。") + parser.add_argument("--output", default="", help="输出待提交参数包;为空时写到会话目录 sensor/pending_sensor_commit.yaml。") + parser.add_argument("--commit-reason", default="传感器标定算法输出待提交", help="写入参数包的提交原因。") + parser.add_argument("--operator-id", default="", help="生成待提交包的操作员 ID。") + parser.add_argument("--previous-parameter-version", default="", help="当前车端已生效传感器参数版本,用于回滚引用。") + parser.add_argument("--persistent-write", action=argparse.BooleanOptionalAction, default=True, help="最终提交到车端时是否持久化。") + parser.add_argument("--require-manual-approval", action=argparse.BooleanOptionalAction, default=True, help="是否要求人工审批。") + parser.add_argument("--update-dataset-index", action=argparse.BooleanOptionalAction, default=True, help="是否把待提交包路径写回 dataset_index.yaml。") + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + 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 + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(yaml.safe_dump(data, sort_keys=False, allow_unicode=True), encoding="utf-8") + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def require_string(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} 必须是非空字符串。") + return value.strip() + + +def require_map(value: Any, field_name: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{field_name} 必须是 YAML map。") + return value + + +def require_list(value: Any, field_name: str) -> list[Any]: + if not isinstance(value, list): + raise ValueError(f"{field_name} 必须是 YAML list。") + return value + + +def has_file_values(value: Any) -> bool: + if value is None: + return False + if isinstance(value, list): + return bool(value) + return bool(str(value).strip()) + + +def require_number(value: Any, field_name: str) -> float: + if value in (None, ""): + raise ValueError(f"{field_name} 不能为空。") + try: + return float(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是数字,当前值为 {value!r}。") from exc + + +def require_integer(value: Any, field_name: str) -> int: + if value in (None, ""): + raise ValueError(f"{field_name} 不能为空。") + try: + return int(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是整数,当前值为 {value!r}。") from exc + + +def require_number_list(value: Any, field_name: str, length: int | None = None) -> None: + values = require_list(value, field_name) + if not values: + raise ValueError(f"{field_name} 不能为空。") + if length is not None and len(values) != length: + raise ValueError(f"{field_name} 必须包含 {length} 个数字。") + for index, item in enumerate(values): + require_number(item, f"{field_name}[{index}]") + + +def require_number_matrix(value: Any, field_name: str, row_count: int, column_count: int) -> None: + rows = require_list(value, field_name) + if len(rows) != row_count: + raise ValueError(f"{field_name} 必须包含 {row_count} 行。") + for row_index, row in enumerate(rows): + require_number_list(row, f"{field_name}[{row_index}]", column_count) + + +def relative_to_root(path: Path, root: Path) -> str: + try: + return str(path.resolve(strict=False).relative_to(root.resolve(strict=False))) + except ValueError: + return str(path.resolve(strict=False)) + + +def validate_dataset_index(index: dict[str, Any]) -> dict[str, Any]: + if int(index.get("schema_version", 0) or 0) != 1: + raise ValueError("dataset_index.schema_version 必须为 1。") + session = require_map(index.get("session"), "dataset_index.session") + for field_name in ("session_id", "vehicle_id", "dataset_root"): + require_string(session.get(field_name), f"dataset_index.session.{field_name}") + sensor_inputs = require_map( + require_map(index.get("data_inputs"), "dataset_index.data_inputs").get("sensor"), + "dataset_index.data_inputs.sensor", + ) + if not any(has_file_values(value) for value in sensor_inputs.values()): + raise ValueError("dataset_index.data_inputs.sensor 至少需要一个传感器数据文件引用。") + return session + + +def validate_camera_intrinsics(params: dict[str, Any]) -> None: + require_integer(params.get("image_width"), "estimated_params.camera_intrinsics.image_width") + require_integer(params.get("image_height"), "estimated_params.camera_intrinsics.image_height") + require_string(params.get("camera_model"), "estimated_params.camera_intrinsics.camera_model") + require_string(params.get("distortion_model"), "estimated_params.camera_intrinsics.distortion_model") + for field_name in ("fx", "fy", "cx", "cy"): + require_number(params.get(field_name), f"estimated_params.camera_intrinsics.{field_name}") + require_number_list( + params.get("distortion_coefficients"), + "estimated_params.camera_intrinsics.distortion_coefficients", + ) + + +def validate_imu_intrinsics(params: dict[str, Any]) -> None: + require_number_list(params.get("gyro_bias_xyz_rads"), "estimated_params.imu_intrinsics.gyro_bias_xyz_rads", 3) + require_number_list(params.get("accel_bias_xyz_ms2"), "estimated_params.imu_intrinsics.accel_bias_xyz_ms2", 3) + for field_name in ( + "gyro_noise_density_rads_sqrt_hz", + "accel_noise_density_ms2_sqrt_hz", + "gyro_random_walk_rads_s_sqrt_hz", + "accel_random_walk_ms3_sqrt_hz", + ): + require_number(params.get(field_name), f"estimated_params.imu_intrinsics.{field_name}") + if "scale_matrix" in params: + require_number_matrix(params["scale_matrix"], "estimated_params.imu_intrinsics.scale_matrix", 3, 3) + + +def validate_transform_params(params: dict[str, Any], field_prefix: str) -> None: + require_number_list(params.get("translation_xyz_m"), f"{field_prefix}.translation_xyz_m", 3) + require_number_list(params.get("rotation_xyzw"), f"{field_prefix}.rotation_xyzw", 4) + + +def validate_sensor_extrinsics(params: dict[str, Any]) -> None: + require_string(params.get("parent_frame_id"), "estimated_params.sensor_extrinsics.parent_frame_id") + require_string(params.get("child_frame_id"), "estimated_params.sensor_extrinsics.child_frame_id") + validate_transform_params(params, "estimated_params.sensor_extrinsics") + + +def validate_hand_eye(params: dict[str, Any]) -> None: + require_string(params.get("base_frame_id"), "estimated_params.hand_eye.base_frame_id") + require_string(params.get("tool_frame_id"), "estimated_params.hand_eye.tool_frame_id") + require_string(params.get("camera_frame_id"), "estimated_params.hand_eye.camera_frame_id") + validate_transform_params(params, "estimated_params.hand_eye") + + +def validate_quality(params: dict[str, Any]) -> None: + quality = require_map(params.get("quality"), "estimated_params.quality") + if not bool(quality.get("data_quality_passed", False)): + raise ValueError("estimated_params.quality.data_quality_passed 必须为 true。") + if not bool(quality.get("suitable_for_commit", False)): + raise ValueError("estimated_params.quality.suitable_for_commit 必须为 true。") + validation_summary = require_map( + quality.get("validation_summary"), + "estimated_params.quality.validation_summary", + ) + if "auto_acceptance_passed" in validation_summary and not bool(validation_summary["auto_acceptance_passed"]): + raise ValueError("estimated_params.quality.validation_summary.auto_acceptance_passed 必须为 true。") + + +def validate_estimated_params( + params: dict[str, Any], + session: dict[str, Any], +) -> tuple[str, str, str, str, dict[str, Any]]: + if int(params.get("schema_version", 0) or 0) != 1: + raise ValueError("estimated_params.schema_version 必须为 1。") + parameter_version = require_string(params.get("parameter_version"), "estimated_params.parameter_version") + selected_task = require_string(params.get("selected_task"), "estimated_params.selected_task") + if selected_task not in TASK_PARAM_BRANCH: + raise ValueError(f"estimated_params.selected_task 必须是 {sorted(TASK_PARAM_BRANCH)} 之一。") + task_subtype = require_string(params.get("task_subtype"), "estimated_params.task_subtype") + if task_subtype not in SUPPORTED_SUBTYPES[selected_task]: + raise ValueError(f"estimated_params.task_subtype={task_subtype} 不适用于 selected_task={selected_task}。") + sensor_id = require_string(params.get("sensor_id"), "estimated_params.sensor_id") + vehicle_id = str(params.get("vehicle_id", "") or "").strip() + if vehicle_id and vehicle_id != session.get("vehicle_id"): + raise ValueError("estimated_params.vehicle_id 与 dataset_index.session.vehicle_id 不一致。") + + estimated = require_map(params.get("estimated_params"), "estimated_params.estimated_params") + branch_name = TASK_PARAM_BRANCH[selected_task] + selected_params = require_map(estimated.get(branch_name), f"estimated_params.estimated_params.{branch_name}") + if selected_task == "camera_intrinsic": + validate_camera_intrinsics(selected_params) + elif selected_task == "imu_intrinsic": + validate_imu_intrinsics(selected_params) + elif selected_task == "sensor_extrinsic": + validate_sensor_extrinsics(selected_params) + elif selected_task == "hand_eye": + validate_hand_eye(selected_params) + validate_quality(params) + return parameter_version, selected_task, task_subtype, sensor_id, {branch_name: selected_params} + + +def resolve_output_path(args: argparse.Namespace, session: dict[str, Any]) -> Path: + if args.output: + return Path(args.output).expanduser().resolve(strict=False) + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + return dataset_root / "sensor" / "pending_sensor_commit.yaml" + + +def build_commit_package( + dataset_index_path: Path, + estimated_params_path: Path, + index: dict[str, Any], + params: dict[str, Any], + args: argparse.Namespace, + output_path: Path, +) -> dict[str, Any]: + session = index["session"] + parameter_version, selected_task, task_subtype, sensor_id, estimated = validate_estimated_params(params, session) + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + created_us = now_us() + return { + "schema_version": 1, + "commit_state": "pending_manual_approval" if args.require_manual_approval else "pending_vehicle_commit", + "created_timestamp_us": created_us, + "session": { + "session_id": session["session_id"], + "site_id": session.get("site_id", ""), + "vehicle_id": session["vehicle_id"], + "dataset_root": session["dataset_root"], + }, + "source": { + "dataset_index_file": relative_to_root(dataset_index_path, dataset_root), + "estimated_params_file": relative_to_root(estimated_params_path, dataset_root), + "estimated_params_digest": { + "checksum_type": "sha256", + "checksum_value": sha256_file(estimated_params_path), + }, + }, + "approval": { + "required": bool(args.require_manual_approval), + "approved": False, + "operator_id": args.operator_id, + "approved_timestamp_us": 0, + }, + "commit_request": { + "parameter_version": parameter_version, + "selected_task": selected_task, + "task_subtype": task_subtype, + "sensor_id": sensor_id, + "commit_reason": args.commit_reason, + "persistent_write": bool(args.persistent_write), + "estimated_params": estimated, + }, + "rollback": { + "previous_parameter_version": args.previous_parameter_version, + "rollback_required_on_vehicle_commit_failure": True, + "rollback_verified": False, + }, + "trace": { + "pending_commit_file": relative_to_root(output_path, dataset_root), + "generated_by": "stage_sensor_parameter_commit.py", + }, + } + + +def update_dataset_index(index_path: Path, index: dict[str, Any], output_path: Path) -> None: + session = index["session"] + dataset_root = Path(str(session["dataset_root"])).expanduser().resolve(strict=False) + metadata = index.setdefault("metadata", {}) + commit_package = load_yaml(output_path) + source = require_map(commit_package.get("source"), "pending_commit.source") + digest = require_map(source.get("estimated_params_digest"), "pending_commit.source.estimated_params_digest") + approval = require_map(commit_package.get("approval"), "pending_commit.approval") + commit_request = require_map(commit_package.get("commit_request"), "pending_commit.commit_request") + metadata["sensor_estimated_params_file"] = source.get("estimated_params_file", "") + metadata["sensor_estimated_params_checksum_type"] = digest.get("checksum_type", "") + metadata["sensor_estimated_params_checksum_value"] = digest.get("checksum_value", "") + metadata["sensor_pending_commit_file"] = relative_to_root(output_path, dataset_root) + metadata["sensor_pending_commit_state"] = commit_package.get("commit_state", "") + metadata["sensor_pending_parameter_version"] = commit_request.get("parameter_version", "") + metadata["sensor_pending_commit_selected_task"] = commit_request.get("selected_task", "") + metadata["sensor_pending_commit_task_subtype"] = commit_request.get("task_subtype", "") + metadata["sensor_pending_commit_sensor_id"] = commit_request.get("sensor_id", "") + metadata["sensor_pending_commit_approval_required"] = bool(approval.get("required", False)) + metadata["sensor_pending_commit_approved"] = bool(approval.get("approved", False)) + write_yaml(index_path, index) + + +def main() -> int: + args = parse_args() + dataset_index_path = Path(args.dataset_index).expanduser().resolve(strict=False) + estimated_params_path = Path(args.estimated_params).expanduser().resolve(strict=False) + index = load_yaml(dataset_index_path) + params = load_yaml(estimated_params_path) + session = validate_dataset_index(index) + output_path = resolve_output_path(args, session) + commit_package = build_commit_package( + dataset_index_path, + estimated_params_path, + index, + params, + args, + output_path, + ) + write_yaml(output_path, commit_package) + if args.update_dataset_index: + update_dataset_index(dataset_index_path, index, output_path) + print(f"[OK] 已生成传感器待提交参数包: {output_path}") + print(f"[OK] commit_state={commit_package['commit_state']}") + print(f"[OK] parameter_version={commit_package['commit_request']['parameter_version']}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1)