diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json index dc46835..c95b9cc 100644 --- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json @@ -652,28 +652,11 @@ }, "chassis_calibration": { "chassis_type": "ackermann", - "straight_track_length_m": 5.0, - "straight_track_width_m": 0.7, - "arc_track_radius_m": 1.6, "wheel_radius_m": 0.1, "wheel_track_m": 0.52, "wheel_base_m": 0.8 }, "control_calibration": { - "reference_path": [ - { - "x_m": -2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - }, - { - "x_m": 2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - } - ], "parameter_version": "isaac_control_baseline_v1" }, "external_truth": { @@ -682,7 +665,36 @@ "workcell_zone_id": "isaac_workcell_zone_a", "expected_position_stddev_m": 0.01, "expected_yaw_stddev_rad": 0.01, - "expected_time_sync_offset_ms": 2.0 + "expected_time_sync_offset_ms": 2.0, + "visualization_enabled": false, + "visualization_root_prim": "/World/ExternalTruthVisualization", + "target_ball_radius_m": 0.075, + "target_balls": [ + { + "ball_id": "front_left", + "center_in_base_link": { + "x_m": 0.58, + "y_m": 0.32, + "z_m": 1.28 + } + }, + { + "ball_id": "front_right", + "center_in_base_link": { + "x_m": 0.58, + "y_m": -0.32, + "z_m": 1.28 + } + }, + { + "ball_id": "rear_center", + "center_in_base_link": { + "x_m": -0.5, + "y_m": 0.0, + "z_m": 1.22 + } + } + ] }, "sensors": [ { diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json index 4e1c895..413732e 100644 --- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis.json @@ -61,28 +61,11 @@ }, "chassis_calibration": { "chassis_type": "ackermann", - "straight_track_length_m": 5.0, - "straight_track_width_m": 0.7, - "arc_track_radius_m": 1.6, "wheel_radius_m": 0.1, "wheel_track_m": 0.52, "wheel_base_m": 0.8 }, "control_calibration": { - "reference_path": [ - { - "x_m": -2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - }, - { - "x_m": 2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - } - ], "parameter_version": "isaac_control_baseline_v1" }, "external_truth": { @@ -91,7 +74,8 @@ "workcell_zone_id": "isaac_workcell_zone_a", "expected_position_stddev_m": 0.01, "expected_yaw_stddev_rad": 0.01, - "expected_time_sync_offset_ms": 2.0 + "expected_time_sync_offset_ms": 2.0, + "visualization_enabled": false }, "sensors": [ { diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json index 4e1c895..413732e 100644 --- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_gateway.json @@ -61,28 +61,11 @@ }, "chassis_calibration": { "chassis_type": "ackermann", - "straight_track_length_m": 5.0, - "straight_track_width_m": 0.7, - "arc_track_radius_m": 1.6, "wheel_radius_m": 0.1, "wheel_track_m": 0.52, "wheel_base_m": 0.8 }, "control_calibration": { - "reference_path": [ - { - "x_m": -2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - }, - { - "x_m": 2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - } - ], "parameter_version": "isaac_control_baseline_v1" }, "external_truth": { @@ -91,7 +74,8 @@ "workcell_zone_id": "isaac_workcell_zone_a", "expected_position_stddev_m": 0.01, "expected_yaw_stddev_rad": 0.01, - "expected_time_sync_offset_ms": 2.0 + "expected_time_sync_offset_ms": 2.0, + "visualization_enabled": false }, "sensors": [ { diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json index 4e1c895..413732e 100644 --- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_control_sensor_intrinsic.json @@ -61,28 +61,11 @@ }, "chassis_calibration": { "chassis_type": "ackermann", - "straight_track_length_m": 5.0, - "straight_track_width_m": 0.7, - "arc_track_radius_m": 1.6, "wheel_radius_m": 0.1, "wheel_track_m": 0.52, "wheel_base_m": 0.8 }, "control_calibration": { - "reference_path": [ - { - "x_m": -2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - }, - { - "x_m": 2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - } - ], "parameter_version": "isaac_control_baseline_v1" }, "external_truth": { @@ -91,7 +74,8 @@ "workcell_zone_id": "isaac_workcell_zone_a", "expected_position_stddev_m": 0.01, "expected_yaw_stddev_rad": 0.01, - "expected_time_sync_offset_ms": 2.0 + "expected_time_sync_offset_ms": 2.0, + "visualization_enabled": false }, "sensors": [ { diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json index 4e1c895..413732e 100644 --- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_chassis_gateway.json @@ -61,28 +61,11 @@ }, "chassis_calibration": { "chassis_type": "ackermann", - "straight_track_length_m": 5.0, - "straight_track_width_m": 0.7, - "arc_track_radius_m": 1.6, "wheel_radius_m": 0.1, "wheel_track_m": 0.52, "wheel_base_m": 0.8 }, "control_calibration": { - "reference_path": [ - { - "x_m": -2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - }, - { - "x_m": 2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - } - ], "parameter_version": "isaac_control_baseline_v1" }, "external_truth": { @@ -91,7 +74,8 @@ "workcell_zone_id": "isaac_workcell_zone_a", "expected_position_stddev_m": 0.01, "expected_yaw_stddev_rad": 0.01, - "expected_time_sync_offset_ms": 2.0 + "expected_time_sync_offset_ms": 2.0, + "visualization_enabled": false }, "sensors": [ { diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json index 4e1c895..413732e 100644 --- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json +++ b/agv_calib_brain/.isaac_cache/workshop_scene_manifest_external_only.json @@ -61,28 +61,11 @@ }, "chassis_calibration": { "chassis_type": "ackermann", - "straight_track_length_m": 5.0, - "straight_track_width_m": 0.7, - "arc_track_radius_m": 1.6, "wheel_radius_m": 0.1, "wheel_track_m": 0.52, "wheel_base_m": 0.8 }, "control_calibration": { - "reference_path": [ - { - "x_m": -2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - }, - { - "x_m": 2.5, - "y_m": 0.0, - "yaw_rad": 0.0, - "target_speed_ms": 0.3 - } - ], "parameter_version": "isaac_control_baseline_v1" }, "external_truth": { @@ -91,7 +74,8 @@ "workcell_zone_id": "isaac_workcell_zone_a", "expected_position_stddev_m": 0.01, "expected_yaw_stddev_rad": 0.01, - "expected_time_sync_offset_ms": 2.0 + "expected_time_sync_offset_ms": 2.0, + "visualization_enabled": false }, "sensors": [ { diff --git a/agv_calib_brain/run_isaac_real_sim_test.sh b/agv_calib_brain/run_isaac_real_sim_test.sh index 5113f3b..c2b96b6 100755 --- a/agv_calib_brain/run_isaac_real_sim_test.sh +++ b/agv_calib_brain/run_isaac_real_sim_test.sh @@ -15,7 +15,7 @@ CONDA_ENV_NAME="${CONDA_ENV_NAME:-AutoCalib_Workshop}" ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}" RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/isaac_real_sim_$(date +%Y%m%d_%H%M%S)}" -HEADLESS=1 +HEADLESS=0 DO_BUILD=0 RUN_SMOKE=1 KEEP_RUNNING=0 @@ -499,6 +499,7 @@ log "工作空间: ${WORKSPACE_DIR}" log "ROS_LOG_DIR: ${ROS_LOG_DIR}" log "进程日志目录: ${RUN_LOG_ROOT}" log "Isaac conda 环境: ${CONDA_ENV_NAME}" +log "Isaac headless: ${HEADLESS}" log "总控底盘/运控模式 use_gateway=${WORKSHOP_USE_GATEWAY_VALUE}" if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then log "数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}" @@ -589,13 +590,19 @@ start_bg "workshop-demo" bash -lc " source '${WORKSPACE_DIR}/install/setup.bash' set -u export ROS_LOG_DIR='${ROS_LOG_DIR}' - exec ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \ - use_gateway:=${WORKSHOP_USE_GATEWAY_VALUE} \ - chassis_host:=127.0.0.1 \ - control_host:=127.0.0.1 \ - sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu \ - data_input_params_file:='${DATA_INPUT_PARAMS_FILE}' \ - dataset_index_file:='${DATASET_INDEX_FILE}' + launch_args=( + 'use_gateway:=${WORKSHOP_USE_GATEWAY_VALUE}' + 'chassis_host:=127.0.0.1' + 'control_host:=127.0.0.1' + 'sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu' + ) + if [[ -n '${DATA_INPUT_PARAMS_FILE}' ]]; then + launch_args+=('data_input_params_file:=${DATA_INPUT_PARAMS_FILE}') + fi + if [[ -n '${DATASET_INDEX_FILE}' ]]; then + launch_args+=('dataset_index_file:=${DATASET_INDEX_FILE}') + fi + exec ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \"\${launch_args[@]}\" " wait_for_ros_service "/workshop_v2/create_session" 60 || exit 1 diff --git a/agv_calib_brain/src/apps/operator_ui/README.md b/agv_calib_brain/src/apps/operator_ui/README.md index 35bbf5e..b31b843 100644 --- a/agv_calib_brain/src/apps/operator_ui/README.md +++ b/agv_calib_brain/src/apps/operator_ui/README.md @@ -19,6 +19,12 @@ source install/setup.bash python3 src/apps/operator_ui/main.py ``` +仿真联调时可以让界面直接加载仿真 profile： + +```bash +AGV_OPERATOR_SITE_PROFILE=src/deployment/profiles/sim_workshop.yaml python3 src/apps/operator_ui/main.py +``` + 如果环境里还没有界面依赖： ```bash @@ -50,6 +56,29 @@ pip install -r src/apps/operator_ui/requirements.txt 8. 点击“开始执行本轮标定”，在“报告”页查看最终结果。 9. 验证结束后点击“停止现场服务”。 +## Isaac 中测试底盘选路采集 + +如果只是验证“底盘标定流程里能不能按选定路径让车动起来、采集底盘遥测和外部真值”，先启动 Isaac 仿真链路： + +```bash +./run_isaac_real_sim_test.sh --no-smoke --keep-running +``` + +再启动 UI： + +```bash +AGV_OPERATOR_SITE_PROFILE=src/deployment/profiles/sim_workshop.yaml python3 src/apps/operator_ui/main.py +``` + +在“底盘标定”页选择底盘类型和“参考路径”，然后点击“开始底盘路径采集测试”。该按钮会调用 +`run_chassis_profile_capture.py`，自动传入当前选中的 `reference_path_id`，并把采集结果写到 `/tmp/agv_calib_chassis_sim/ui_*`。 + +## 在 UI 中录制参考路径 + +底盘标定、运控参数和传感器标定页的“参考路径”下拉框旁都有“录制”和“停止录制”按钮。点击“录制”后填写中文名称、路径 ID、录制来源、话题、路径类型和目标速度；录制来源可选外部真值定位或底盘遥测。录制时长填 `0` 表示一直录，点击“停止录制”后会收尾并把单条路径 YAML 写入对应模块的 `reference_paths/` 目录。 + +底盘路径会按当前底盘类型写入 `allowed_chassis_types`，因此录制完成后只会出现在对应底盘类型下。需要注意的是，“开始底盘路径采集测试”仍然走 `chassis_action_profile.yaml` 的动作原语执行链路；如果一条新录制底盘路径还没有绑定到动作 profile，UI 会阻止直接执行，只把它作为参考路径文件用于路径管理和任务预览。 + ## 注意事项 - “跳过网络质量检查（仅调试）”只适合本地联调，真实部署默认不要勾选。 @@ -65,6 +94,7 @@ pip install -r src/apps/operator_ui/requirements.txt - “车间定位”窗口只显示 3D 位置、本轮计划轨迹和定位数据，不允许修改固定 topic 或定位系统名称。 - 3D 车间图支持鼠标左键拖动旋转视角，左键双击恢复默认视角。 - 本轮计划轨迹用绿色点显示，来自当前勾选的底盘动作和运控轨迹参数，不要求操作员额外输入。 +- 底盘标定、运控参数标定和传感器标定页都有“参考路径”下拉框，选项来自对应模块的 `reference_paths/*.yaml`；运控轨迹跟踪会把选中的路径展开为实际下发的轨迹点。 - 标定流程运行时，界面订阅 `/workshop_v2/events`，上一项任务完成后会自动切到下一项需要动车采集的轨迹。 - 界面会把明细选择写入本轮任务文件，并让标定流程读取这些任务。 - 界面通过现有 CLI 和 ROS launch 运行流程，没有绕过总控。 diff --git a/agv_calib_brain/src/apps/operator_ui/constants.py b/agv_calib_brain/src/apps/operator_ui/constants.py index dadf308..2946919 100644 --- a/agv_calib_brain/src/apps/operator_ui/constants.py +++ b/agv_calib_brain/src/apps/operator_ui/constants.py @@ -2,13 +2,26 @@ from __future__ import annotations +import os from pathlib import Path REPO_ROOT = Path(__file__).resolve().parents[3] -DEFAULT_SITE_PROFILE = REPO_ROOT / "src/deployment/profiles/site_template.yaml" +_DEFAULT_SITE_PROFILE_RAW = Path( + os.environ.get("AGV_OPERATOR_SITE_PROFILE", "src/deployment/profiles/site_template.yaml") +).expanduser() +DEFAULT_SITE_PROFILE = ( + _DEFAULT_SITE_PROFILE_RAW.resolve(strict=False) + if _DEFAULT_SITE_PROFILE_RAW.is_absolute() + else (REPO_ROOT / _DEFAULT_SITE_PROFILE_RAW).resolve(strict=False) +) DEFAULT_VEHICLE_PROFILE_DIR = REPO_ROOT / "src/deployment/profiles/vehicle_profiles" DEFAULT_VEHICLE_PROFILE = DEFAULT_VEHICLE_PROFILE_DIR / "ackermann_default.yaml" +DEFAULT_REFERENCE_PATH_DIRS = { + "chassis": REPO_ROOT / "src/site_deployment/workshop_chassis_calibration_real/reference_paths", + "control": REPO_ROOT / "src/site_deployment/workshop_control_calibration_real/reference_paths", + "sensor": REPO_ROOT / "src/site_deployment/workshop_sensor_calibration_real/reference_paths", +} VEHICLE_PROFILE_SAVE_EXTENSION = ".ymal" DEFAULT_SESSION_CONFIG = Path("/tmp/agv_calib_operator_ui/workshop_session_config.yaml") PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx") @@ -373,5 +386,3 @@ for sensor_key, sensor_label, subtype, _label, stage_type in SENSOR_TASK_OPTIONS SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}外参标定" elif stage_type == STAGE_HAND_EYE: SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}手眼标定" - - diff --git a/agv_calib_brain/src/apps/operator_ui/main.py b/agv_calib_brain/src/apps/operator_ui/main.py index 2bb619b..53d176a 100644 --- a/agv_calib_brain/src/apps/operator_ui/main.py +++ b/agv_calib_brain/src/apps/operator_ui/main.py @@ -80,6 +80,9 @@ class OperatorMainWindow( self.generated_session_config: dict[str, Any] = {} self.launch_process = QProcess(self) self.smoke_process = QProcess(self) + self.chassis_capture_process = QProcess(self) + self.reference_path_record_process = QProcess(self) + self.reference_path_record_module = "" self.report_metadata: dict[str, str] = {} self.report_stages: list[dict[str, str]] = [] self.last_unresolved_signature = "" @@ -97,6 +100,8 @@ class OperatorMainWindow( self._setup_process(self.launch_process, "现场服务") self._setup_process(self.smoke_process, "标定流程") + self._setup_process(self.chassis_capture_process, "底盘路径采集") + self._setup_process(self.reference_path_record_process, "参考路径录制") self._build_ui() self._connect_live_refresh() self.load_profile() @@ -302,16 +307,24 @@ class OperatorMainWindow( self.stop_launch_btn = QPushButton("停止现场服务") self.smoke_btn = QPushButton("开始执行本轮标定") self.stop_smoke_btn = QPushButton("停止本轮标定") + self.chassis_capture_btn = QPushButton("开始底盘路径采集测试") + self.stop_chassis_capture_btn = QPushButton("停止底盘路径采集") self.build_config_btn.clicked.connect(self.build_session_config) self.launch_btn.clicked.connect(self.start_launch_stack) self.stop_launch_btn.clicked.connect(lambda: self.stop_process(self.launch_process, "现场服务")) self.smoke_btn.clicked.connect(self.start_smoke_test) self.stop_smoke_btn.clicked.connect(lambda: self.stop_process(self.smoke_process, "标定流程")) + self.chassis_capture_btn.clicked.connect(self.start_chassis_path_capture) + self.stop_chassis_capture_btn.clicked.connect( + lambda: self.stop_process(self.chassis_capture_process, "底盘路径采集") + ) action_layout.addWidget(self.build_config_btn, 0, 0) action_layout.addWidget(self.launch_btn, 0, 1) action_layout.addWidget(self.stop_launch_btn, 1, 1) action_layout.addWidget(self.smoke_btn, 1, 0) action_layout.addWidget(self.stop_smoke_btn, 2, 0, 1, 2) + action_layout.addWidget(self.chassis_capture_btn, 3, 0) + action_layout.addWidget(self.stop_chassis_capture_btn, 3, 1) layout.addWidget(action_box) layout.addStretch(1) return panel @@ -325,10 +338,26 @@ class OperatorMainWindow( for value, label in CHASSIS_TYPES: self.chassis_type_combo.addItem(label, value) self.chassis_type_combo.currentIndexChanged.connect(self.rebuild_chassis_parameter_checks) + self.chassis_type_combo.currentIndexChanged.connect(self.refresh_reference_path_combos) type_row.addWidget(self.chassis_type_combo) type_row.addStretch(1) layout.addLayout(type_row) + path_row = QHBoxLayout() + path_row.addWidget(QLabel("参考路径")) + self.chassis_reference_path_combo = QComboBox() + self.chassis_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview) + path_row.addWidget(self.chassis_reference_path_combo, 1) + self.record_chassis_path_btn = QPushButton("录制") + self.stop_chassis_path_record_btn = QPushButton("停止录制") + self.record_chassis_path_btn.clicked.connect(lambda: self.start_reference_path_recording("chassis")) + self.stop_chassis_path_record_btn.clicked.connect( + lambda: self.stop_process(self.reference_path_record_process, "参考路径录制") + ) + path_row.addWidget(self.record_chassis_path_btn) + path_row.addWidget(self.stop_chassis_path_record_btn) + layout.addLayout(path_row) + self.chassis_param_area = QWidget() self.chassis_param_layout = QVBoxLayout(self.chassis_param_area) self.chassis_param_layout.setContentsMargins(0, 0, 0, 0) @@ -341,6 +370,21 @@ class OperatorMainWindow( def _build_control_task_tab(self) -> QWidget: tab = QWidget() layout = QVBoxLayout(tab) + path_row = QHBoxLayout() + path_row.addWidget(QLabel("参考路径")) + self.control_reference_path_combo = QComboBox() + self.control_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview) + path_row.addWidget(self.control_reference_path_combo, 1) + self.record_control_path_btn = QPushButton("录制") + self.stop_control_path_record_btn = QPushButton("停止录制") + self.record_control_path_btn.clicked.connect(lambda: self.start_reference_path_recording("control")) + self.stop_control_path_record_btn.clicked.connect( + lambda: self.stop_process(self.reference_path_record_process, "参考路径录制") + ) + path_row.addWidget(self.record_control_path_btn) + path_row.addWidget(self.stop_control_path_record_btn) + layout.addLayout(path_row) + self.control_param_checks: dict[str, QCheckBox] = {} for option in CONTROL_PARAMETER_OPTIONS: check = QCheckBox(f"{option['label']} · {option['axis']} · {option['algorithm']}") @@ -354,6 +398,21 @@ class OperatorMainWindow( def _build_sensor_task_tab(self) -> QWidget: tab = QWidget() layout = QVBoxLayout(tab) + path_row = QHBoxLayout() + path_row.addWidget(QLabel("参考路径")) + self.sensor_reference_path_combo = QComboBox() + self.sensor_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview) + path_row.addWidget(self.sensor_reference_path_combo, 1) + self.record_sensor_path_btn = QPushButton("录制") + self.stop_sensor_path_record_btn = QPushButton("停止录制") + self.record_sensor_path_btn.clicked.connect(lambda: self.start_reference_path_recording("sensor")) + self.stop_sensor_path_record_btn.clicked.connect( + lambda: self.stop_process(self.reference_path_record_process, "参考路径录制") + ) + path_row.addWidget(self.record_sensor_path_btn) + path_row.addWidget(self.stop_sensor_path_record_btn) + layout.addLayout(path_row) + self.sensor_id_edits: dict[str, QLineEdit] = { "front_camera": QLineEdit("demo_front_camera"), "down_camera": QLineEdit("demo_down_camera"), @@ -391,7 +450,7 @@ class OperatorMainWindow( chassis_type = self.chassis_type_combo.currentData() or "ackermann" for option in CHASSIS_PARAMETER_OPTIONS[str(chassis_type)]: check = QCheckBox(f"{option['label']} · {option['target']}") - check.setChecked(True) + check.setChecked(False) check.stateChanged.connect(self.refresh_command_preview) self.chassis_param_checks[str(option["code"])] = check self.chassis_param_layout.addWidget(check) @@ -476,6 +535,8 @@ class OperatorMainWindow( return panel def closeEvent(self, event) -> None: + self.stop_process(self.reference_path_record_process, "参考路径录制") + self.stop_process(self.chassis_capture_process, "底盘路径采集") self.stop_process(self.smoke_process, "标定流程") self.stop_process(self.launch_process, "现场服务") if self.localization_spin_timer.isActive(): diff --git a/agv_calib_brain/src/apps/operator_ui/process_report.py b/agv_calib_brain/src/apps/operator_ui/process_report.py index 3d4723d..fb0cb5e 100644 --- a/agv_calib_brain/src/apps/operator_ui/process_report.py +++ b/agv_calib_brain/src/apps/operator_ui/process_report.py @@ -4,19 +4,328 @@ from __future__ import annotations import re import sys +from datetime import datetime +from pathlib import Path from PySide6.QtCore import QProcess -from PySide6.QtWidgets import QMessageBox +from PySide6.QtWidgets import ( + QCheckBox, + QComboBox, + QDialog, + QDialogButtonBox, + QFormLayout, + QLineEdit, + QMessageBox, + QVBoxLayout, +) try: - from .profile_io import launch_arg, shell_join + from .constants import REPO_ROOT + from .profile_io import launch_arg, read_nested, shell_join from .ui_helpers import table_item except ImportError: - from profile_io import launch_arg, shell_join + from constants import REPO_ROOT + from profile_io import launch_arg, read_nested, shell_join from ui_helpers import table_item +REFERENCE_PATH_RECORDER = "src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py" +RECORD_MODULE_LABELS = { + "chassis": "底盘标定", + "control": "运控参数", + "sensor": "传感器标定", +} +RECORD_DEFAULT_PATH_TYPES = { + "chassis": "recorded", + "control": "recorded", + "sensor": "recorded", +} +RECORD_DEFAULT_RECOMMENDED_TASKS = { + "chassis": "recorded", + "control": "trajectory_tracking", + "sensor": "sensor_extrinsic", +} + + +class ReferencePathRecordDialog(QDialog): + def __init__(self, module: str, defaults: dict[str, str], parent=None) -> None: + super().__init__(parent) + self.setWindowTitle(f"录制{RECORD_MODULE_LABELS.get(module, module)}参考路径") + self.module = module + self.default_topics = { + "external_pose": defaults.get("external_pose_topic", ""), + "chassis_telemetry": defaults.get("chassis_telemetry_topic", ""), + } + + layout = QVBoxLayout(self) + form = QFormLayout() + layout.addLayout(form) + + self.display_name_edit = QLineEdit(defaults.get("display_name", "")) + self.path_id_edit = QLineEdit(defaults.get("path_id", "")) + self.source_combo = QComboBox() + self.source_combo.addItem("外部真值定位", "external_pose") + self.source_combo.addItem("底盘遥测里程计", "chassis_telemetry") + self.topic_edit = QLineEdit(defaults.get("topic", "")) + self.path_type_combo = QComboBox() + for item in defaults.get("path_type_options", "").split(","): + item = item.strip() + if item: + self.path_type_combo.addItem(item) + self.path_type_combo.setEditable(True) + current_path_type = defaults.get("path_type", "") + if current_path_type: + index = self.path_type_combo.findText(current_path_type) + if index >= 0: + self.path_type_combo.setCurrentIndex(index) + else: + self.path_type_combo.setEditText(current_path_type) + self.recommended_task_types_edit = QLineEdit(defaults.get("recommended_task_types", "")) + self.target_speed_edit = QLineEdit(defaults.get("target_speed_ms", "0.05")) + self.duration_edit = QLineEdit(defaults.get("duration_sec", "0")) + self.min_distance_step_edit = QLineEdit(defaults.get("min_distance_step_m", "0.03")) + self.min_yaw_step_edit = QLineEdit(defaults.get("min_yaw_step_rad", "0.03")) + self.description_edit = QLineEdit(defaults.get("description", "")) + self.overwrite_check = QCheckBox("允许覆盖同名路径文件") + + source = defaults.get("source", "external_pose") + source_index = self.source_combo.findData(source) + if source_index >= 0: + self.source_combo.setCurrentIndex(source_index) + self.source_combo.currentIndexChanged.connect(self._apply_default_topic_for_source) + + form.addRow("中文名称", self.display_name_edit) + form.addRow("路径 ID", self.path_id_edit) + form.addRow("录制来源", self.source_combo) + form.addRow("话题", self.topic_edit) + form.addRow("路径类型", self.path_type_combo) + form.addRow("推荐任务类型", self.recommended_task_types_edit) + form.addRow("目标速度 m/s", self.target_speed_edit) + form.addRow("录制时长 s", self.duration_edit) + form.addRow("最小点间距 m", self.min_distance_step_edit) + form.addRow("最小 yaw 间隔 rad", self.min_yaw_step_edit) + form.addRow("说明", self.description_edit) + form.addRow("", self.overwrite_check) + + buttons = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel) + buttons.accepted.connect(self.accept) + buttons.rejected.connect(self.reject) + layout.addWidget(buttons) + + def _apply_default_topic_for_source(self, *_args) -> None: + source = str(self.source_combo.currentData() or "external_pose") + self.topic_edit.setText(self.default_topics.get(source, "")) + + def values(self) -> dict[str, object]: + return { + "module": self.module, + "display_name": self.display_name_edit.text().strip(), + "path_id": self.path_id_edit.text().strip(), + "source": str(self.source_combo.currentData() or "external_pose"), + "topic": self.topic_edit.text().strip(), + "path_type": self.path_type_combo.currentText().strip() or "recorded", + "recommended_task_types": [ + item.strip() + for item in re.split(r"[,;，；\s]+", self.recommended_task_types_edit.text().strip()) + if item.strip() + ], + "target_speed_ms": self.target_speed_edit.text().strip() or "0.0", + "duration_sec": self.duration_edit.text().strip() or "0", + "min_distance_step_m": self.min_distance_step_edit.text().strip() or "0.03", + "min_yaw_step_rad": self.min_yaw_step_edit.text().strip() or "0.03", + "description": self.description_edit.text().strip(), + "overwrite": self.overwrite_check.isChecked(), + } + + class ProcessReportMixin: + def _resolved_repo_path(self, raw_path: str) -> str: + path = Path(raw_path).expanduser() + if path.is_absolute(): + return str(path.resolve(strict=False)) + return str((REPO_ROOT / path).resolve(strict=False)) + + def _resolved_bridge_script(self, raw_path: str) -> str: + return self._resolved_repo_path(raw_path) + + def _configured_path_or_default(self, keys: tuple[str, ...], default_path: str) -> str: + raw_path = read_nested(getattr(self, "site_profile", {}), keys, "") + return self._resolved_repo_path(raw_path or default_path) + + def default_record_topic(self, source: str) -> str: + if source == "chassis_telemetry": + return ( + read_nested(self.site_profile, ("vehicle_agent", "chassis_telemetry_topic")) + or read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic")) + or "/chassis/telemetry" + ) + return self.external_topic_edit.text().strip() or "/workshop/external_localization/vehicle/pose" + + def reference_path_record_defaults(self, module: str) -> dict[str, str]: + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + selected = self.selected_reference_path(module) if hasattr(self, "selected_reference_path") else None + recommended = selected.get("recommended_task_types", []) if isinstance(selected, dict) else [] + recommended_text = ",".join(str(item) for item in recommended) if recommended else RECORD_DEFAULT_RECOMMENDED_TASKS[module] + path_type = str(selected.get("path_type", "")) if isinstance(selected, dict) else "" + path_type_options = { + "chassis": "recorded,straight_line,arc,s_curve,in_place_rotation,lateral_translation,diagonal_motion", + "control": "recorded,straight_line,arc,s_curve,lateral_offset,stop_accuracy", + "sensor": "recorded,static_station,sampling_line,imu_motion,pose_sweep", + }[module] + return { + "display_name": f"{RECORD_MODULE_LABELS[module]}录制路径 {timestamp}", + "path_id": f"{module}_recorded_{timestamp}", + "source": "external_pose", + "topic": self.default_record_topic("external_pose"), + "external_pose_topic": self.default_record_topic("external_pose"), + "chassis_telemetry_topic": self.default_record_topic("chassis_telemetry"), + "path_type": path_type or RECORD_DEFAULT_PATH_TYPES[module], + "path_type_options": path_type_options, + "recommended_task_types": recommended_text, + "target_speed_ms": "0.05" if module != "chassis" else "0.08", + "duration_sec": "0", + "min_distance_step_m": "0.03", + "min_yaw_step_rad": "0.03", + "description": "操作台录制参考路径", + } + + def validate_reference_path_record_values(self, values: dict[str, object]) -> bool: + path_id = str(values.get("path_id", "")).strip() + display_name = str(values.get("display_name", "")).strip() + if not display_name: + QMessageBox.warning(self, "路径名称缺失", "请填写中文名称。") + return False + if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_-]*", path_id): + QMessageBox.warning(self, "路径 ID 不合法", "路径 ID 只能使用英文、数字、下划线和短横线，且不能以符号开头。") + return False + for key, label in [ + ("target_speed_ms", "目标速度"), + ("duration_sec", "录制时长"), + ("min_distance_step_m", "最小点间距"), + ("min_yaw_step_rad", "最小 yaw 间隔"), + ]: + try: + value = float(str(values.get(key, "0"))) + except ValueError: + QMessageBox.warning(self, "数值格式错误", f"{label} 必须是数字。") + return False + if key != "target_speed_ms" and value < 0.0: + QMessageBox.warning(self, "数值格式错误", f"{label} 不能小于 0。") + return False + if key == "target_speed_ms" and value < 0.0: + QMessageBox.warning(self, "数值格式错误", f"{label} 不能小于 0。") + return False + return True + + def build_reference_path_record_command(self, values: dict[str, object]) -> str: + module = str(values["module"]) + recorder = self._configured_path_or_default( + ("reference_paths", "recorder"), + REFERENCE_PATH_RECORDER, + ) + args = [ + sys.executable, + recorder, + "--output-dir", + str(self.reference_path_dir(module)), + "--path-id", + str(values["path_id"]), + "--display-name", + str(values["display_name"]), + "--module-type", + module, + "--path-type", + str(values["path_type"]), + "--frame-id", + "workshop", + "--target-speed-ms", + str(values["target_speed_ms"]), + "--duration-sec", + str(values["duration_sec"]), + "--min-distance-step-m", + str(values["min_distance_step_m"]), + "--min-yaw-step-rad", + str(values["min_yaw_step_rad"]), + "--source", + str(values["source"]), + ] + topic = str(values.get("topic", "")).strip() + if topic: + args.extend(["--topic", topic]) + description = str(values.get("description", "")).strip() + if description: + args.extend(["--description", description]) + for task_type in values.get("recommended_task_types", []) or []: + args.extend(["--recommended-task-type", str(task_type)]) + if module == "chassis": + args.extend(["--allowed-chassis-type", str(self.chassis_type_combo.currentData() or "ackermann")]) + if bool(values.get("overwrite", False)): + args.append("--overwrite") + return "source install/setup.bash && exec " + shell_join(args) + + def build_chassis_path_capture_command(self) -> str: + reference_path = self.selected_reference_path("chassis") + reference_path_id = str(reference_path.get("path_id", "")).strip() if reference_path else "" + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + session_dir = Path("/tmp/agv_calib_chassis_sim") / f"ui_{timestamp}" + vehicle_id = self.vehicle_id_edit.text().strip() or "demo_agv_001" + chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") + chassis_topic = ( + read_nested(self.site_profile, ("vehicle_agent", "chassis_telemetry_topic")) + or read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic")) + or "/chassis/telemetry" + ) + ackermann_topic = read_nested( + self.site_profile, + ("vehicle_agent", "internal_ackermann_command_topic"), + f"/vehicle/{vehicle_id}/internal/ackermann_cmd", + ) + runner = self._configured_path_or_default( + ("chassis_calibration", "action_profile_capture_runner"), + "src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py", + ) + config_file = self._configured_path_or_default( + ("chassis_calibration", "data_config_file"), + "src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_sim.yaml", + ) + action_profile = self._configured_path_or_default( + ("chassis_calibration", "action_profile_file"), + "src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml", + ) + args = [ + sys.executable, + runner, + "--config", + config_file, + "--action-profile", + action_profile, + "--chassis-type", + chassis_type, + "--session-id", + f"ui_chassis_path_{timestamp}", + "--site-id", + self.workcell_zone_edit.text().strip() or "isaac_workcell_zone_a", + "--vehicle-id", + vehicle_id, + "--session-dir", + str(session_dir), + "--dataset-index-path", + str(session_dir / "dataset_index.yaml"), + "--chassis-telemetry-topic", + chassis_topic, + "--external-pose-topic", + self.external_topic_edit.text().strip() or "/isaac/external_localization/vehicle/pose", + "--ackermann-command-topic", + ackermann_topic, + "--command-source", + "auto", + "--action-server", + "/chassis/execute_motion_primitive", + ] + if reference_path_id: + args.extend(["--reference-path-id", reference_path_id]) + return "source install/setup.bash && " + shell_join(args) + def build_launch_command(self) -> str: args = [ "ros2", @@ -36,6 +345,45 @@ class ProcessReportMixin: args.append(launch_arg("data_input_params_file", self.data_input_edit.text().strip())) if self.dataset_index_edit.text().strip(): args.append(launch_arg("dataset_index_file", self.dataset_index_edit.text().strip())) + bridge_config = getattr(self, "site_profile", {}).get("chassis_telemetry_bridge", {}) + if isinstance(bridge_config, dict) and bridge_config: + bridge_script = read_nested( + self.site_profile, + ("chassis_telemetry_bridge", "bridge_tool"), + "src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py", + ) + args.extend([ + launch_arg("enable_chassis_telemetry_bridge", "true"), + launch_arg("chassis_telemetry_bridge_script", self._resolved_bridge_script(bridge_script)), + launch_arg( + "chassis_telemetry_bind_host", + read_nested(self.site_profile, ("chassis_telemetry_bridge", "bind_host"), "0.0.0.0"), + ), + launch_arg( + "chassis_telemetry_bind_port", + read_nested(self.site_profile, ("chassis_telemetry_bridge", "bind_port"), "9010"), + ), + launch_arg( + "chassis_telemetry_protocol", + read_nested(self.site_profile, ("chassis_telemetry_bridge", "protocol"), "frame"), + ), + launch_arg( + "chassis_telemetry_expected_msg_type", + read_nested(self.site_profile, ("chassis_telemetry_bridge", "expected_msg_type"), "0"), + ), + launch_arg( + "chassis_telemetry_topic", + read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic"), "/chassis/telemetry"), + ), + launch_arg( + "chassis_telemetry_default_chassis_type", + read_nested(self.site_profile, ("chassis_telemetry_bridge", "default_chassis_type"), "ackermann"), + ), + launch_arg( + "chassis_telemetry_max_payload_bytes", + read_nested(self.site_profile, ("chassis_telemetry_bridge", "max_payload_bytes"), "262144"), + ), + ]) return "source install/setup.bash && " + shell_join(args) def build_smoke_command(self) -> str: @@ -61,6 +409,7 @@ class ProcessReportMixin: "240", "--service-timeout-sec", "10", + "--verbose-feedback", ] if self.vehicle_profile_path_edit.text().strip(): args.extend(["--vehicle-profile-file", self.vehicle_profile_path_edit.text().strip()]) @@ -115,6 +464,61 @@ class ProcessReportMixin: self.smoke_state.setText("标定流程：运行中") self.smoke_process.start("bash", ["-lc", command]) + def start_chassis_path_capture(self) -> None: + if self.chassis_capture_process.state() != QProcess.NotRunning: + QMessageBox.warning(self, "底盘路径采集已运行", "底盘路径采集进程已经在运行。") + return + if not self.selected_reference_path("chassis"): + QMessageBox.warning(self, "未选择路径", "请先在底盘标定页选择参考路径。") + return + reference_path = self.selected_reference_path("chassis") + reference_path_id = str(reference_path.get("path_id", "")).strip() if reference_path else "" + chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") + if reference_path_id not in self.allowed_chassis_reference_path_ids(chassis_type): + QMessageBox.warning( + self, + "路径未绑定动作", + "这条路径是录制参考路径，但还没有绑定到底盘动作 profile，不能直接用于“底盘路径采集测试”。" + "请先在 chassis_action_profile.yaml 中为当前底盘类型添加对应动作，或仅用于任务预览/路径管理。", + ) + return + if self.launch_process.state() == QProcess.NotRunning: + reply = QMessageBox.question( + self, + "现场服务未运行", + "当前界面没有检测到由本界面启动的现场服务。若你已经用脚本启动 Isaac 仿真链路，可以继续执行底盘路径采集测试。", + ) + if reply != QMessageBox.Yes: + return + command = self.build_chassis_path_capture_command() + self.append_log("[底盘路径采集] 启动：" + command) + self.smoke_state.setText("底盘路径采集：运行中") + self.chassis_capture_process.start("bash", ["-lc", command]) + + def start_reference_path_recording(self, module: str) -> None: + if self.reference_path_record_process.state() != QProcess.NotRunning: + QMessageBox.warning(self, "参考路径录制已运行", "参考路径录制进程已经在运行。") + return + if self.launch_process.state() == QProcess.NotRunning: + reply = QMessageBox.question( + self, + "现场服务未运行", + "当前界面没有检测到由本界面启动的现场服务。若你已经用脚本启动 Isaac 或 ROS 现场链路，可以继续录制参考路径。", + ) + if reply != QMessageBox.Yes: + return + dialog = ReferencePathRecordDialog(module, self.reference_path_record_defaults(module), self) + if dialog.exec() != QDialog.Accepted: + return + values = dialog.values() + if not self.validate_reference_path_record_values(values): + return + command = self.build_reference_path_record_command(values) + self.reference_path_record_module = module + self.append_log("[参考路径录制] 启动：" + command) + self.smoke_state.setText("参考路径录制：运行中") + self.reference_path_record_process.start("bash", ["-lc", command]) + def stop_process(self, process: QProcess, name: str) -> None: if process.state() == QProcess.NotRunning: self.append_log(f"[{name}] 当前没有运行中的进程。") @@ -142,11 +546,16 @@ class ProcessReportMixin: self.append_log(f"[{name}] {status_text}") if name == "现场服务": self.stack_state.setText("现场服务：未启动") - else: + elif name == "标定流程": self.trajectory_runtime_active = False self.trajectory_last_completed_stage_id = "" self.refresh_planned_trajectory() self.smoke_state.setText("标定流程：空闲") + elif name == "底盘路径采集": + self.smoke_state.setText("底盘路径采集：空闲") + elif name == "参考路径录制": + self.smoke_state.setText("参考路径录制：空闲") + self.refresh_reference_path_combos() def append_log(self, text: str) -> None: self.log_view.appendPlainText(text) diff --git a/agv_calib_brain/src/apps/operator_ui/profile_handlers.py b/agv_calib_brain/src/apps/operator_ui/profile_handlers.py index c6a6fac..212baa4 100644 --- a/agv_calib_brain/src/apps/operator_ui/profile_handlers.py +++ b/agv_calib_brain/src/apps/operator_ui/profile_handlers.py @@ -380,3 +380,5 @@ class ProfileHandlersMixin: self.vehicle_profile_path_edit.setText(str(resolve_vehicle_profile_path(vehicle_profile_file))) if Path(self.vehicle_profile_path_edit.text()).exists(): self.load_vehicle_profile_from_current_path() + if hasattr(self, "refresh_reference_path_combos"): + self.refresh_reference_path_combos() diff --git a/agv_calib_brain/src/apps/operator_ui/session_builder.py b/agv_calib_brain/src/apps/operator_ui/session_builder.py index 4955f1f..3a83b51 100644 --- a/agv_calib_brain/src/apps/operator_ui/session_builder.py +++ b/agv_calib_brain/src/apps/operator_ui/session_builder.py @@ -13,6 +13,7 @@ try: from .constants import ( CHASSIS_PARAMETER_OPTIONS, CONTROL_PARAMETER_OPTIONS, + DEFAULT_REFERENCE_PATH_DIRS, DEFAULT_VEHICLE_PROFILE, POLICY_DISPLAY_NAMES, REPO_ROOT, @@ -41,6 +42,7 @@ except ImportError: from constants import ( CHASSIS_PARAMETER_OPTIONS, CONTROL_PARAMETER_OPTIONS, + DEFAULT_REFERENCE_PATH_DIRS, DEFAULT_VEHICLE_PROFILE, POLICY_DISPLAY_NAMES, REPO_ROOT, @@ -89,6 +91,219 @@ class SessionBuilderMixin: tasks = self.selected_tasks() return ",".join(tasks) if tasks else "external" + def reference_path_dir(self, module: str) -> Path: + config_keys = { + "chassis": ("chassis_calibration", "reference_path_dir"), + "control": ("control_calibration", "reference_path_dir"), + "sensor": ("sensor_calibration", "reference_path_dir"), + } + raw_path = read_nested(self.site_profile, config_keys[module]) + if raw_path: + path = Path(raw_path).expanduser() + if path.is_absolute(): + return path.resolve(strict=False) + profile_path = resolve_profile_path(self.profile_path_edit.text().strip()) + return (profile_path.parent / path).resolve(strict=False) + return DEFAULT_REFERENCE_PATH_DIRS[module] + + def resolve_site_or_repo_path(self, raw_path: str) -> Path: + path = Path(raw_path).expanduser() + if path.is_absolute(): + return path.resolve(strict=False) + profile_path = resolve_profile_path(self.profile_path_edit.text().strip()) + profile_relative = (profile_path.parent / path).resolve(strict=False) + if profile_relative.exists(): + return profile_relative + return (REPO_ROOT / path).resolve(strict=False) + + def chassis_action_profile_path(self) -> Path: + raw_path = read_nested( + self.site_profile, + ("chassis_calibration", "action_profile_file"), + "src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml", + ) + return self.resolve_site_or_repo_path(raw_path) + + def allowed_chassis_reference_path_ids(self, chassis_type: str) -> set[str]: + profile_path = self.chassis_action_profile_path() + if not profile_path.exists(): + return set() + try: + profile = load_yaml(profile_path) + except Exception as exc: + self.append_log(f"[参考路径] 底盘动作 profile 加载失败 {profile_path}: {exc}") + return set() + section = profile.get("chassis_profiles", {}).get(chassis_type, {}) + actions = section.get("actions", []) if isinstance(section, dict) else [] + if not isinstance(actions, list): + return set() + return { + str(action.get("reference_path_id", "")).strip() + for action in actions + if isinstance(action, dict) and str(action.get("reference_path_id", "")).strip() + } + + def load_reference_path_options(self, module: str) -> tuple[dict[str, Any], list[dict[str, Any]]]: + path_dir = self.reference_path_dir(module) + index_path = path_dir / "index.yaml" + index = load_yaml(index_path) if index_path.exists() else {} + paths: list[dict[str, Any]] = [] + if not path_dir.exists(): + return index, paths + for path_file in sorted(path_dir.glob("*.yaml")): + if path_file.name in {"index.yaml", "_index.yaml"}: + continue + try: + payload = load_yaml(path_file) + except Exception as exc: + self.append_log(f"[参考路径] 跳过 {path_file}: {exc}") + continue + path_id = str(payload.get("path_id", "")).strip() + display_name = str(payload.get("display_name", "")).strip() + points = payload.get("points", []) + if not path_id or not display_name or not isinstance(points, list) or not points: + self.append_log(f"[参考路径] 跳过无效路径文件: {path_file}") + continue + payload["_source_file"] = str(path_file.resolve(strict=False)) + payload["_path_dir"] = str(path_dir.resolve(strict=False)) + paths.append(payload) + if module == "chassis": + chassis_type = ( + str(self.chassis_type_combo.currentData() or "ackermann") + if hasattr(self, "chassis_type_combo") + else "ackermann" + ) + allowed_ids = self.allowed_chassis_reference_path_ids(chassis_type) + if allowed_ids: + def recorded_path_chassis_types(path: dict[str, Any]) -> set[str]: + raw_types = path.get("allowed_chassis_types", []) or [] + if isinstance(raw_types, str): + return {raw_types} + if isinstance(raw_types, list): + return {str(item) for item in raw_types} + return set() + + paths = [ + path + for path in paths + if str(path.get("path_id", "")).strip() in allowed_ids + or chassis_type in recorded_path_chassis_types(path) + ] + return index, paths + + def default_reference_path_id(self, module: str, index: dict[str, Any], paths: list[dict[str, Any]]) -> str: + chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") if hasattr(self, "chassis_type_combo") else "ackermann" + by_chassis = index.get("default_path_id_by_chassis", {}) if isinstance(index, dict) else {} + by_task = index.get("default_path_id_by_task_type", {}) if isinstance(index, dict) else {} + if module in {"chassis", "control"} and isinstance(by_chassis, dict): + default_id = str(by_chassis.get(chassis_type, "")).strip() + if default_id: + return default_id + if isinstance(by_task, dict): + task_key = { + "chassis": "straight_line", + "control": "trajectory_tracking", + "sensor": "camera_intrinsic", + }[module] + default_id = str(by_task.get(task_key, "")).strip() + if default_id: + return default_id + return str(paths[0].get("path_id", "")) if paths else "" + + def refresh_reference_path_combos(self) -> None: + combo_specs = { + "chassis": "chassis_reference_path_combo", + "control": "control_reference_path_combo", + "sensor": "sensor_reference_path_combo", + } + for module, attr_name in combo_specs.items(): + if not hasattr(self, attr_name): + continue + combo = getattr(self, attr_name) + previous = combo.currentData() + previous_id = str(previous.get("path_id", "")) if isinstance(previous, dict) else "" + index, paths = self.load_reference_path_options(module) + ids = {str(path.get("path_id", "")) for path in paths} + selected_id = previous_id if previous_id in ids else self.default_reference_path_id(module, index, paths) + if selected_id not in ids and paths: + selected_id = str(paths[0].get("path_id", "")) + combo.blockSignals(True) + combo.clear() + if not paths: + combo.addItem("未找到参考路径", None) + combo.setEnabled(False) + else: + combo.setEnabled(True) + for path in paths: + label = f"{path.get('display_name', path.get('path_id'))} ({path.get('path_id')})" + combo.addItem(label, path) + if str(path.get("path_id", "")) == selected_id: + combo.setCurrentIndex(combo.count() - 1) + combo.blockSignals(False) + if hasattr(self, "launch_command_preview"): + self.refresh_command_preview() + + def selected_reference_path(self, module: str) -> dict[str, Any] | None: + combo = getattr(self, f"{module}_reference_path_combo", None) + if combo is None: + return None + path = combo.currentData() + return path if isinstance(path, dict) else None + + @staticmethod + def stringify_reference_value(value: Any) -> str: + if isinstance(value, bool): + return "true" if value else "false" + if isinstance(value, float): + return f"{value:.9g}" + return str(value) + + def add_reference_path_metadata(self, params: dict[str, Any], path: dict[str, Any] | None) -> None: + if not path: + return + points = path.get("points", []) + if not isinstance(points, list): + return + params["reference_path.id"] = path.get("path_id", "") + params["reference_path.display_name"] = path.get("display_name", "") + params["reference_path.frame_id"] = path.get("frame_id", "workshop") + params["reference_path.path_type"] = path.get("path_type", "polyline") + params["reference_path.point_count"] = len(points) + if path.get("_path_dir"): + params["reference_path.path_dir"] = path["_path_dir"] + if path.get("_source_file"): + params["reference_path.file"] = path["_source_file"] + if path.get("module_type"): + params["reference_path.module_type"] = path["module_type"] + if path.get("description"): + params["reference_path.description"] = path["description"] + for index, point in enumerate(points): + if not isinstance(point, dict): + continue + prefix = f"reference_path.pt_{index}" + params[f"{prefix}_x_m"] = self.stringify_reference_value(point.get("x_m", 0.0)) + params[f"{prefix}_y_m"] = self.stringify_reference_value(point.get("y_m", 0.0)) + params[f"{prefix}_z_m"] = self.stringify_reference_value(point.get("z_m", 0.0)) + params[f"{prefix}_yaw_rad"] = self.stringify_reference_value(point.get("yaw_rad", 0.0)) + params[f"{prefix}_speed_ms"] = self.stringify_reference_value(point.get("target_speed_ms", 0.0)) + + def apply_reference_path_as_trajectory(self, params: dict[str, Any], path: dict[str, Any] | None) -> None: + if not path: + return + points = path.get("points", []) + if not isinstance(points, list) or len(points) < 2: + return + for key in list(params): + if key.startswith("traj_pt_"): + del params[key] + for index, point in enumerate(points): + if not isinstance(point, dict): + continue + params[f"traj_pt_{index}_x_m"] = self.stringify_reference_value(point.get("x_m", 0.0)) + params[f"traj_pt_{index}_y_m"] = self.stringify_reference_value(point.get("y_m", 0.0)) + params[f"traj_pt_{index}_yaw_rad"] = self.stringify_reference_value(point.get("yaw_rad", 0.0)) + params[f"traj_pt_{index}_speed_ms"] = self.stringify_reference_value(point.get("target_speed_ms", 0.0)) + def refresh_all(self) -> None: self.refresh_command_preview() self.refresh_task_preview_from_config() @@ -158,6 +373,10 @@ class SessionBuilderMixin: def trajectory_segments_from_task_params(self, params: dict[str, str]) -> list[list[tuple[float, float, float]]]: segments: list[list[tuple[float, float, float]]] = [] + segment = self.trajectory_points_from_reference_path(params) + if segment: + segments.append(segment) + return segments segment = self.trajectory_points_from_explicit_params(params) if segment: segments.append(segment) @@ -212,6 +431,21 @@ class SessionBuilderMixin: indexed_points[index] = (x, y) return [(x, y, 0.04) for _, (x, y) in sorted(indexed_points.items())] + def trajectory_points_from_reference_path(self, params: dict[str, str]) -> list[tuple[float, float, float]]: + indexes = sorted({ + int(match.group(1)) + for key in params + if (match := re.fullmatch(r"reference_path\.pt_(\d+)_x_m", key)) + }) + points: list[tuple[float, float, float]] = [] + for index in indexes: + x = float_or_none(params.get(f"reference_path.pt_{index}_x_m")) + y = float_or_none(params.get(f"reference_path.pt_{index}_y_m")) + z = float_or_none(params.get(f"reference_path.pt_{index}_z_m")) + if x is not None and y is not None: + points.append((x, y, z if z is not None else 0.04)) + return points + def trajectory_points_from_motion_primitive(self, params: dict[str, str]) -> list[tuple[float, float, float]]: primitive_type = params.get("primitive_type", "") if primitive_type == "straight_line": @@ -268,6 +502,7 @@ class SessionBuilderMixin: if not hasattr(self, "chassis_type_combo"): return [] chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") + reference_path = self.selected_reference_path("chassis") tasks: list[dict[str, Any]] = [] for option in CHASSIS_PARAMETER_OPTIONS[chassis_type]: check = self.chassis_param_checks.get(str(option["code"])) @@ -280,6 +515,7 @@ class SessionBuilderMixin: } params.setdefault("brake_when_finished", "true") params.setdefault("timeout_sec", "20.0") + self.add_reference_path_metadata(params, reference_path) tasks.append(self.make_task( STAGE_CHASSIS, str(option["code"]), @@ -292,6 +528,7 @@ class SessionBuilderMixin: def selected_control_tasks(self) -> list[dict[str, Any]]: if not hasattr(self, "control_param_checks"): return [] + reference_path = self.selected_reference_path("control") tasks: list[dict[str, Any]] = [] for option in CONTROL_PARAMETER_OPTIONS: check = self.control_param_checks.get(str(option["code"])) @@ -308,6 +545,8 @@ class SessionBuilderMixin: "trajectory_tracking.required_external_pose_source_id", self.reference_source_edit.text().strip(), ) + self.apply_reference_path_as_trajectory(params, reference_path) + self.add_reference_path_metadata(params, reference_path) tasks.append(self.make_task( STAGE_CONTROL, str(option["code"]), @@ -320,6 +559,7 @@ class SessionBuilderMixin: def selected_sensor_tasks(self) -> list[dict[str, Any]]: if not hasattr(self, "sensor_task_checks"): return [] + reference_path = self.selected_reference_path("sensor") tasks: list[dict[str, Any]] = [] for sensor_key, sensor_label, subtype, label, stage_type in SENSOR_TASK_OPTIONS: check = self.sensor_task_checks.get((sensor_key, subtype)) @@ -358,6 +598,7 @@ class SessionBuilderMixin: "hand_eye.required_pose_count": "1", "hand_eye.timeout_sec": "5.0", }) + self.add_reference_path_metadata(params, reference_path) tasks.append(self.make_task( stage_type, f"sensor.{sensor_key}.{subtype}", 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 e2a8fb0..93ed7da 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 @@ -1,5 +1,7 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +import sys + +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -13,6 +15,8 @@ from launch_ros.actions import Node # 传感器 readiness 的最小真实配置入口。 # external_telemetry_topic / expected_reference_source_name / expected_workcell_zone_id： # 外部真值定位源的现场配置入口。 +# enable_chassis_telemetry_bridge / chassis_telemetry_*： +# 车间电脑侧 WiFi/TCP 底盘遥测接收桥，发布 /chassis/telemetry。 # data_input_params_file：可选 ROS 参数文件，用于把真实采集/ingest 产物路径注入算法输入。 # dataset_index_file：可选数据集索引文件；为空时主控会按 data_input_params_file 同目录自动查找。 def generate_launch_description(): @@ -29,6 +33,16 @@ def generate_launch_description(): "control_host", default_value="192.168.1.100", description="Windows 车端 IP（use_gateway=true 时生效）", ) + chassis_timeout_ms_arg = DeclareLaunchArgument( + "chassis_timeout_ms", + default_value="120000", + description="底盘网关 TCP 请求超时，动作原语需要覆盖完整运动时长", + ) + control_timeout_ms_arg = DeclareLaunchArgument( + "control_timeout_ms", + default_value="120000", + description="运控网关 TCP 请求超时，轨迹评估需要覆盖完整运动时长", + ) profile_storage_path_arg = DeclareLaunchArgument( "profile_storage_path", default_value="/tmp/agv_calib_vehicle_profiles.db", @@ -89,11 +103,63 @@ def generate_launch_description(): default_value="", description="external_localization_service 期望的工位区域 ID，留空表示不校验", ) + enable_chassis_telemetry_bridge_arg = DeclareLaunchArgument( + "enable_chassis_telemetry_bridge", + default_value="false", + description="true: 启动车间电脑侧 WiFi/TCP 底盘遥测 bridge，发布 chassis_telemetry_topic", + ) + chassis_telemetry_bridge_script_arg = DeclareLaunchArgument( + "chassis_telemetry_bridge_script", + default_value="src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py", + description="车间电脑侧底盘遥测 bridge Python 脚本路径", + ) + chassis_telemetry_bind_host_arg = DeclareLaunchArgument( + "chassis_telemetry_bind_host", + default_value="0.0.0.0", + description="底盘遥测 TCP 监听地址", + ) + chassis_telemetry_bind_port_arg = DeclareLaunchArgument( + "chassis_telemetry_bind_port", + default_value="9010", + description="底盘遥测 TCP 监听端口", + ) + chassis_telemetry_protocol_arg = DeclareLaunchArgument( + "chassis_telemetry_protocol", + default_value="frame", + description="底盘遥测包协议：frame/json_lines/raw_json/auto", + ) + chassis_telemetry_expected_msg_type_arg = DeclareLaunchArgument( + "chassis_telemetry_expected_msg_type", + default_value="0", + description="frame 协议期望 msg_type；0 表示接受任意类型", + ) + chassis_telemetry_topic_arg = DeclareLaunchArgument( + "chassis_telemetry_topic", + default_value="/chassis/telemetry", + description="底盘遥测 bridge 输出 topic", + ) + chassis_telemetry_default_chassis_type_arg = DeclareLaunchArgument( + "chassis_telemetry_default_chassis_type", + default_value="ackermann", + description="底盘遥测包未携带 chassis_type 时使用的默认车型", + ) + chassis_telemetry_max_payload_bytes_arg = DeclareLaunchArgument( + "chassis_telemetry_max_payload_bytes", + default_value="262144", + description="底盘遥测单包最大 payload 字节数", + ) + chassis_telemetry_send_ack_arg = DeclareLaunchArgument( + "chassis_telemetry_send_ack", + default_value="true", + description="true: bridge 收到遥测后向 TCP 连接回 ACK", + ) def make_nodes(context): use_gateway = LaunchConfiguration("use_gateway").perform(context).lower() == "true" chassis_host = LaunchConfiguration("chassis_host").perform(context) control_host = LaunchConfiguration("control_host").perform(context) + chassis_timeout_ms = int(LaunchConfiguration("chassis_timeout_ms").perform(context)) + control_timeout_ms = int(LaunchConfiguration("control_timeout_ms").perform(context)) profile_storage_path = LaunchConfiguration("profile_storage_path").perform(context) sensor_storage_root = LaunchConfiguration("sensor_storage_root").perform(context) sensor_registry = LaunchConfiguration("sensor_registry").perform(context) @@ -106,6 +172,26 @@ def generate_launch_description(): 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() + enable_chassis_telemetry_bridge = ( + LaunchConfiguration("enable_chassis_telemetry_bridge").perform(context).lower() == "true" + ) + chassis_telemetry_bridge_script = LaunchConfiguration("chassis_telemetry_bridge_script").perform(context).strip() + chassis_telemetry_bind_host = LaunchConfiguration("chassis_telemetry_bind_host").perform(context).strip() + chassis_telemetry_bind_port = LaunchConfiguration("chassis_telemetry_bind_port").perform(context).strip() + chassis_telemetry_protocol = LaunchConfiguration("chassis_telemetry_protocol").perform(context).strip() + chassis_telemetry_expected_msg_type = ( + LaunchConfiguration("chassis_telemetry_expected_msg_type").perform(context).strip() + ) + chassis_telemetry_topic = LaunchConfiguration("chassis_telemetry_topic").perform(context).strip() + chassis_telemetry_default_chassis_type = ( + LaunchConfiguration("chassis_telemetry_default_chassis_type").perform(context).strip() + ) + chassis_telemetry_max_payload_bytes = ( + LaunchConfiguration("chassis_telemetry_max_payload_bytes").perform(context).strip() + ) + chassis_telemetry_send_ack = ( + LaunchConfiguration("chassis_telemetry_send_ack").perform(context).lower() == "true" + ) def with_data_input_params(params=None): merged = [] @@ -172,10 +258,10 @@ def generate_launch_description(): parameters=[{ "chassis_host": chassis_host, "chassis_port": 9000, - "chassis_timeout_ms": 5000, + "chassis_timeout_ms": chassis_timeout_ms, "control_host": control_host, "control_port": 9000, - "control_timeout_ms": 5000, + "control_timeout_ms": control_timeout_ms, }], ), ] @@ -198,12 +284,44 @@ def generate_launch_description(): ), ] - return common_nodes + chassis_control_nodes + telemetry_bridge_nodes = [] + if enable_chassis_telemetry_bridge: + ack_arg = "--send-ack" if chassis_telemetry_send_ack else "--no-send-ack" + telemetry_bridge_nodes.append( + ExecuteProcess( + cmd=[ + sys.executable, + chassis_telemetry_bridge_script, + "--bind-host", + chassis_telemetry_bind_host, + "--bind-port", + chassis_telemetry_bind_port, + "--protocol", + chassis_telemetry_protocol, + "--expected-msg-type", + chassis_telemetry_expected_msg_type, + "--output-topic", + chassis_telemetry_topic, + "--default-chassis-type", + chassis_telemetry_default_chassis_type, + "--max-payload-bytes", + chassis_telemetry_max_payload_bytes, + ack_arg, + ], + name="workshop_chassis_telemetry_bridge", + output="screen", + emulate_tty=True, + ) + ) + + return common_nodes + chassis_control_nodes + telemetry_bridge_nodes return LaunchDescription([ use_gateway_arg, chassis_host_arg, control_host_arg, + chassis_timeout_ms_arg, + control_timeout_ms_arg, profile_storage_path_arg, sensor_storage_root_arg, sensor_registry_arg, @@ -216,5 +334,15 @@ def generate_launch_description(): external_telemetry_topic_arg, expected_reference_source_name_arg, expected_workcell_zone_id_arg, + enable_chassis_telemetry_bridge_arg, + chassis_telemetry_bridge_script_arg, + chassis_telemetry_bind_host_arg, + chassis_telemetry_bind_port_arg, + chassis_telemetry_protocol_arg, + chassis_telemetry_expected_msg_type_arg, + chassis_telemetry_topic_arg, + chassis_telemetry_default_chassis_type_arg, + chassis_telemetry_max_payload_bytes_arg, + chassis_telemetry_send_ack_arg, OpaqueFunction(function=make_nodes), ]) diff --git a/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml b/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml index 5307d09..d48a3a3 100644 --- a/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml +++ b/agv_calib_brain/src/deployment/profiles/sim_workshop.yaml @@ -50,9 +50,14 @@ vehicle_sensor_agent: external_pose_bridge: type: external_pose_wifi6_bridge source_topic: /isaac/external_localization/vehicle/pose - reference_source_name: isaac_external_truth + reference_source_name: isaac_sim_truth_source max_hz: 30.0 +external_localization: + output_topic: /isaac/external_localization/vehicle/pose + reference_source_name: isaac_sim_truth_source + workcell_zone_id: isaac_workcell_zone_a + workshop_sensor_ingest: type: workshop_sensor_ingest_sim publish_prefix: /workshop/vehicle_sensor diff --git a/agv_calib_brain/src/deployment/profiles/site_template.yaml b/agv_calib_brain/src/deployment/profiles/site_template.yaml index 11cb4b2..6478316 100644 --- a/agv_calib_brain/src/deployment/profiles/site_template.yaml +++ b/agv_calib_brain/src/deployment/profiles/site_template.yaml @@ -29,6 +29,17 @@ vehicle_agent: wheel_base_m: measured_on_site max_steering_angle_rad: measured_on_site +chassis_telemetry_bridge: + type: wifi6_tcp_chassis_telemetry_bridge + bridge_tool: src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py + bind_host: 0.0.0.0 + bind_port: 9010 + protocol: frame + expected_msg_type: 0 + output_topic: /chassis/telemetry + default_chassis_type: ackermann + max_payload_bytes: 262144 + vehicle_sensor_agent: type: real_vehicle_sensor_agent front_camera_sensor_id: replace_with_front_camera_id @@ -64,6 +75,7 @@ chassis_calibration: 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 + reference_path_dir: /data/agv_calib/replace_with_site_or_line_id/chassis_reference_paths 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 @@ -89,6 +101,7 @@ control_calibration: 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 + reference_path_dir: /data/agv_calib/replace_with_site_or_line_id/control_reference_paths 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 @@ -115,6 +128,7 @@ sensor_calibration: # 这里只定义传感器标定算法的数据输入、任务 profile 和参数交接产物。 type: workshop_sensor_data profile_file: src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml + reference_path_dir: /data/agv_calib/replace_with_site_or_line_id/sensor_reference_paths 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 diff --git a/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py b/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py index 3c8843c..aa58310 100644 --- a/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py +++ b/agv_calib_brain/src/deployment/tools/build_workshop_session_config.py @@ -194,27 +194,45 @@ def normalize_profile_task(raw_task: dict[str, Any]) -> dict[str, Any]: return task -def export_chassis_profile_tasks(profile_path: Path, chassis_type: str) -> list[dict[str, Any]]: +def export_chassis_profile_tasks( + profile_path: Path, + chassis_type: str, + reference_path_dir: Path | None = None, +) -> 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) + if reference_path_dir is not None: + profile["reference_path_dir"] = str(reference_path_dir) + exported = tool.export_requested_tasks(profile, chassis_type, profile_path) 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]]: +def export_control_profile_tasks( + profile_path: Path, + chassis_type: str, + reference_path_dir: Path | None = None, +) -> 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) + if reference_path_dir is not None: + profile["reference_path_dir"] = str(reference_path_dir) + exported = tool.export_requested_tasks(profile, chassis_type, profile_path) 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]]: +def export_sensor_profile_tasks( + profile_path: Path, + task_name: str, + reference_path_dir: Path | None = None, +) -> 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}) + if reference_path_dir is not None: + profile["reference_path_dir"] = str(reference_path_dir) + exported = tool.export_requested_tasks(profile, {task_name}, profile_path) return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]] @@ -327,32 +345,62 @@ def build_requested_tasks( site_profile_path, ("chassis_calibration", "action_profile_file"), ) + chassis_reference_path_dir = resolve_configured_path( + "", + site_profile, + site_profile_path, + ("chassis_calibration", "reference_path_dir"), + ) control_profile_path = resolve_configured_path( args.control_evaluation_profile, site_profile, site_profile_path, ("control_calibration", "evaluation_profile_file"), ) + control_reference_path_dir = resolve_configured_path( + "", + site_profile, + site_profile_path, + ("control_calibration", "reference_path_dir"), + ) sensor_profile_path = resolve_configured_path( args.sensor_calibration_profile, site_profile, site_profile_path, ("sensor_calibration", "profile_file"), ) + sensor_reference_path_dir = resolve_configured_path( + "", + site_profile, + site_profile_path, + ("sensor_calibration", "reference_path_dir"), + ) 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)) + requested_tasks.extend(export_chassis_profile_tasks( + chassis_profile_path, + chassis_type, + chassis_reference_path_dir, + )) 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)) + requested_tasks.extend(export_control_profile_tasks( + control_profile_path, + chassis_type, + control_reference_path_dir, + )) 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)) + requested_tasks.extend(export_sensor_profile_tasks( + sensor_profile_path, + task_name, + sensor_reference_path_dir, + )) continue requested_tasks.append(minimal_task(task_name, args, site_profile, chassis_type)) return requested_tasks diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/README.md b/agv_calib_brain/src/simulation/isaac_workshop_sim/README.md index 3ae9805..a970e2f 100644 --- a/agv_calib_brain/src/simulation/isaac_workshop_sim/README.md +++ b/agv_calib_brain/src/simulation/isaac_workshop_sim/README.md @@ -10,7 +10,8 @@ - 墙面棋盘格阵列。 - 2D LiDAR 外参标定角落。 - 下视相机 3D ChArUco 内参标定台。 -- 底盘标定路线和停车标记。 +- 标定路径不在 Isaac 场景中固化，由各标定模块的 `reference_paths/*.yaml` 选择和录制。 +- 外部真值遥测发布；Isaac 默认不显示黄球/观测连线这类定位调试可视化。 - 场景 manifest 生成。 默认启动： @@ -34,6 +35,13 @@ headless 模式： python3 src/simulation/tools/launch_sim_stack.py --component isaac --headless ``` +外部真值定位调试可视化默认关闭。需要临时查看四角 LiDAR、车端标定球和观测连线时： + +```bash +python3 src/simulation/tools/launch_sim_stack.py --component isaac \ + --isaac-arg=--enable-external-truth-visualization +``` + 边界规则： - Isaac API 只能留在这里。 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 3323f51..3989876 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 @@ -48,6 +48,11 @@ def parse_args(): parser.add_argument("--camera-focal-length", type=float, default=5.0, help="相机焦距") parser.add_argument("--ros-topic-prefix", type=str, default="/AutoCalib_Workshop", help="相机与激光雷达话题前缀") parser.add_argument("--cmd-vel-topic", type=str, default="/cmd_vel", help="车辆速度控制话题") + parser.add_argument( + "--disable-kinematic-command-follow", + action="store_true", + help="禁用仿真车辆按 cmd_vel 直接积分更新位姿；默认启用，便于在 Isaac 中可视化路径执行。", + ) parser.add_argument("--lidar-config", type=str, default="Example_Rotary", help="Isaac LiDAR 配置名") parser.add_argument("--lidar-3d-config", type=str, default="Hesai_XT32_SD10", help="车载3D LiDAR 配置名（默认32线）") parser.add_argument("--disable-camera", action="store_true", help="不创建顶置相机") @@ -105,6 +110,9 @@ def parse_args(): parser.add_argument("--external-quality-score", type=float, default=0.98, help="上报的 external 质量分数") parser.add_argument("--external-observed-target-count", type=int, default=4, help="上报的目标观测数量") parser.add_argument("--external-pose-drop-rate", type=float, default=0.0, help="位姿失效率，取值 [0,1]") + parser.add_argument("--enable-external-truth-visualization", action="store_true", help="在 Isaac 场景中显示外部真值定位调试可视化") + parser.add_argument("--disable-external-truth-visualization", action="store_true", help=argparse.SUPPRESS) + parser.add_argument("--external-target-ball-radius", type=float, default=0.075, help="车端外部定位标定球可视化半径（米）") parser.add_argument("--disable-chassis-telemetry", action="store_true", help="不发布底盘标定遥测") parser.add_argument("--chassis-telemetry-topic", type=str, default="/chassis/telemetry", help="底盘标定遥测话题") parser.add_argument("--chassis-telemetry-hz", type=float, default=20.0, help="底盘标定遥测发布频率（Hz）") @@ -122,8 +130,8 @@ def parse_args(): parser.add_argument("--disable-control-telemetry", action="store_true", help="不发布运控评估遥测") parser.add_argument("--control-telemetry-topic", type=str, default="/control/telemetry", help="运控评估遥测话题") parser.add_argument("--control-telemetry-hz", type=float, default=20.0, help="运控评估遥测发布频率（Hz）") - parser.add_argument("--control-reference-speed-ms", type=float, default=0.3, help="运控遥测参考速度（m/s）") - parser.add_argument("--control-reference-y-m", type=float, default=0.0, help="运控直线参考轨迹的 Y 坐标（米）") + parser.add_argument("--control-reference-speed-ms", type=float, default=0.3, help="兼容旧启动参数；Isaac 不再生成运控参考速度") + parser.add_argument("--control-reference-y-m", type=float, default=0.0, help="兼容旧启动参数；Isaac 不再生成运控参考轨迹") parser.add_argument("--control-parameter-version", type=str, default="isaac_control_baseline_v1", help="当前控制参数版本") parser.add_argument("--disable-vehicle-tf", action="store_true", help="不发布车辆 TF 变换") parser.add_argument("--vehicle-tf-publish-hz", type=float, default=50.0, help="车辆 TF 发布频率（Hz）") @@ -135,10 +143,10 @@ def parse_args(): parser.add_argument("--sensor-quality-score", type=float, default=0.95, help="传感器观测质量评分，取值 [0,1]") parser.add_argument("--sensor-target-missing", action="store_true", help="发布 target_detected=false 的传感器遥测") parser.add_argument("--disable-calibration-boards", action="store_true", help="不创建多姿态棋盘格标定目标阵列") - parser.add_argument("--disable-calibration-fixtures", action="store_true", help="不创建地面标定路线和停车目标标识") - parser.add_argument("--straight-track-length", type=float, default=5.0, help="底盘直线标定路线长度（米）") - parser.add_argument("--straight-track-width", type=float, default=0.7, help="底盘直线标定路线宽度（米）") - parser.add_argument("--arc-track-radius", type=float, default=1.6, help="圆弧/转向标定路线半径（米）") + parser.add_argument("--disable-calibration-fixtures", action="store_true", help="兼容旧启动参数；Isaac 不再创建地面标定路线") + parser.add_argument("--straight-track-length", type=float, default=5.0, help="兼容旧启动参数；标定路径由 reference_paths/*.yaml 提供") + parser.add_argument("--straight-track-width", type=float, default=0.7, help="兼容旧启动参数；标定路径由 reference_paths/*.yaml 提供") + parser.add_argument("--arc-track-radius", type=float, default=1.6, help="兼容旧启动参数；标定路径由 reference_paths/*.yaml 提供") parser.add_argument("--random-seed", type=int, default=42, help="随机种子，保证实验可重复") parser.add_argument("--vehicle-id", type=str, default="demo_agv_001", help="车辆 ID，用于 manifest 和后端会话配置对齐") parser.add_argument("--vehicle-initial-x", type=float, default=0.0, help="车辆初始 X 坐标") @@ -257,12 +265,12 @@ def validate_args(args): raise ValueError("sensor_quality_score 必须在 [0,1] 范围内。") if args.external_observed_target_count < 0 or args.sensor_target_sample_count < 0: raise ValueError("目标数量/样本数不能为负数。") + if args.external_target_ball_radius <= 0: + raise ValueError("external_target_ball_radius 必须大于 0。") if args.wheel_radius <= 0 or args.wheel_track <= 0 or args.wheel_base <= 0: raise ValueError("wheel_radius、wheel_track 和 wheel_base 必须大于 0。") if args.odom_noise_stddev_m < 0 or args.yaw_noise_stddev_rad < 0: raise ValueError("里程计噪声不能为负数。") - if args.straight_track_length <= 0 or args.straight_track_width <= 0 or args.arc_track_radius <= 0: - raise ValueError("标定路线尺寸必须大于 0。") if args.lidar_2d_target_width <= 0 or args.lidar_2d_target_height <= 0 or args.lidar_2d_target_thickness <= 0: raise ValueError("2D LiDAR 靶标尺寸必须大于 0。") if args.lidar_2d_target_bottom_z < 0: @@ -318,7 +326,7 @@ import omni.graph.core as og import omni.kit.commands import omni.usd import omni.replicator.core as rep -from pxr import Gf, PhysxSchema, Sdf, UsdGeom, UsdShade, Vt +from pxr import Gf, PhysxSchema, Sdf, UsdGeom, UsdShade from omni.isaac.core import World from omni.isaac.core.objects import FixedCuboid @@ -331,7 +339,6 @@ from omni.isaac.core.utils.viewports import set_camera_view from usd_utils import create_raw_usd_material, create_textured_board, create_textured_top_strip from calibration_targets import ( add_calibration_boards, - add_calibration_floor_fixtures, add_down_camera_intrinsic_target, add_2d_lidar_calibration_targets, lidar_2d_checkerboard_reserved_zones, @@ -381,6 +388,45 @@ except ImportError: Imu = None LaserScan = None +try: + from geometry_msgs.msg import Twist +except ImportError: + Twist = None + + +def external_lidar_mount_specs(room_length, room_width, height): + """Return the four fixed workshop LiDAR poses used by external truth visualization.""" + offset = 0.3 + x_pos = (room_length / 2.0) - offset + y_pos = (room_width / 2.0) - offset + return [ + {"name": "FL", "pos": [x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, -x_pos))}, + {"name": "FR", "pos": [x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, -x_pos))}, + {"name": "BL", "pos": [-x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, x_pos))}, + {"name": "BR", "pos": [-x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, x_pos))}, + ] + + +def create_preview_material(stage, mat_path, color, opacity=1.0, emissive_scale=0.0): + material = UsdShade.Material.Define(stage, mat_path) + shader = UsdShade.Shader.Define(stage, f"{mat_path}/PreviewSurface") + shader.CreateIdAttr("UsdPreviewSurface") + shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*color)) + shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.55) + shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0) + if emissive_scale > 0.0: + shader.CreateInput("emissiveColor", Sdf.ValueTypeNames.Color3f).Set( + Gf.Vec3f(*(min(1.0, channel * emissive_scale) for channel in color)) + ) + if opacity < 1.0: + shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(float(opacity)) + material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") + return material + + +def bind_material(prim, material): + UsdShade.MaterialBindingAPI.Apply(prim).Bind(material) + def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topic_prefix): """添加四角旋转 LiDAR""" @@ -390,16 +436,7 @@ def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topi import omni.replicator.core as rep from pxr import Gf - offset = 0.3 - x_pos = (room_length / 2.0) - offset - y_pos = (room_width / 2.0) - offset - - lidar_configs = [ - {"name": "FL", "pos": [x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, -x_pos))}, - {"name": "FR", "pos": [x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, -x_pos))}, - {"name": "BL", "pos": [-x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, x_pos))}, - {"name": "BR", "pos": [-x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, x_pos))}, - ] + lidar_configs = external_lidar_mount_specs(room_length, room_width, height) keys = og.Controller.Keys graph_path = "/World/ROS2_Lidar_Graph" @@ -447,6 +484,194 @@ def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topi ) +class ExternalTruthSceneVisualization: + """Isaac viewport visualization for the external truth localization flow.""" + + TARGET_BALLS = [ + {"name": "front_left", "pos": np.array([0.58, 0.32, 1.28], dtype=float)}, + {"name": "front_right", "pos": np.array([0.58, -0.32, 1.28], dtype=float)}, + {"name": "rear_center", "pos": np.array([-0.50, 0.0, 1.22], dtype=float)}, + ] + + def __init__(self, args, world, vehicle_mount_parent_path): + self.enabled = args.enable_external_truth_visualization and not args.disable_external_truth_visualization + self.args = args + self.stage = omni.usd.get_context().get_stage() + self.root_path = "/World/ExternalTruthVisualization" + self.vehicle_mount_parent_path = vehicle_mount_parent_path + self.lidar_specs = external_lidar_mount_specs( + args.room_length, + args.room_width, + args.room_height - 0.2, + ) + self.observation_segments = [] + self.heading_segments = [] + self.uncertainty_segments = [] + self.last_update_wall_time = 0.0 + + if not self.enabled: + print("[*] Isaac 外部真值定位调试可视化未启用。") + return + + UsdGeom.Xform.Define(self.stage, self.root_path) + UsdGeom.Xform.Define(self.stage, f"{self.root_path}/Materials") + UsdGeom.Xform.Define(self.stage, f"{self.root_path}/LidarStations") + UsdGeom.Xform.Define(self.stage, f"{self.root_path}/ObservationLines") + UsdGeom.Xform.Define(self.stage, f"{self.root_path}/PoseOverlay") + self.materials = { + "tower": create_preview_material(self.stage, f"{self.root_path}/Materials/TowerCyan", (0.10, 0.72, 0.84), emissive_scale=0.35), + "beam": create_preview_material(self.stage, f"{self.root_path}/Materials/ObservationBeam", (0.20, 0.85, 1.0), opacity=0.72, emissive_scale=0.25), + "ball": create_preview_material(self.stage, f"{self.root_path}/Materials/TargetBall", (1.0, 0.72, 0.12), emissive_scale=0.2), + "pose": create_preview_material(self.stage, f"{self.root_path}/Materials/PoseArrow", (0.24, 0.90, 0.45), emissive_scale=0.35), + "uncertainty": create_preview_material(self.stage, f"{self.root_path}/Materials/PoseUncertainty", (0.98, 0.84, 0.25), opacity=0.85, emissive_scale=0.15), + } + self._create_lidar_station_visuals(world) + self._create_vehicle_target_balls() + self._create_dynamic_segments() + print("[*] Isaac 外部真值定位可视化已启用：四角 LiDAR、车端标定球、观测连线、真值姿态箭头。") + + def _create_lidar_station_visuals(self, world): + for spec in self.lidar_specs: + name = spec["name"] + x, y, z = spec["pos"] + stand_height = max(z, 0.4) + world.scene.add(FixedCuboid( + prim_path=f"{self.root_path}/LidarStations/{name}_Stand", + name=f"external_truth_lidar_{name.lower()}_stand", + position=np.array([x, y, stand_height / 2.0]), + scale=np.array([0.08, 0.08, stand_height]), + color=np.array([0.08, 0.52, 0.62]), + )) + world.scene.add(FixedCuboid( + prim_path=f"{self.root_path}/LidarStations/{name}_Head", + name=f"external_truth_lidar_{name.lower()}_head", + position=np.array([x, y, z]), + scale=np.array([0.22, 0.22, 0.16]), + color=np.array([0.12, 0.78, 0.90]), + )) + + def _create_vehicle_target_balls(self): + target_root = f"{self.vehicle_mount_parent_path}/ExternalTruthTargets" + UsdGeom.Xform.Define(self.stage, target_root) + for target in self.TARGET_BALLS: + sphere_path = f"{target_root}/Ball_{target['name']}" + sphere = UsdGeom.Sphere.Define(self.stage, sphere_path) + sphere.GetRadiusAttr().Set(float(self.args.external_target_ball_radius)) + xform = UsdGeom.Xformable(sphere.GetPrim()) + xform.AddTranslateOp().Set(Gf.Vec3d(*target["pos"])) + bind_material(sphere.GetPrim(), self.materials["ball"]) + + def _make_segment(self, path, width, material): + cube = UsdGeom.Cube.Define(self.stage, path) + cube.CreateSizeAttr(1.0) + xform = UsdGeom.Xformable(cube.GetPrim()) + segment = { + "translate": xform.AddTranslateOp(), + "rotate": xform.AddRotateXYZOp(), + "scale": xform.AddScaleOp(), + "width": float(width), + } + segment["translate"].Set(Gf.Vec3d(0.0, 0.0, 0.0)) + segment["rotate"].Set(Gf.Vec3f(0.0, 0.0, 0.0)) + segment["scale"].Set(Gf.Vec3f(0.001, float(width), float(width))) + bind_material(cube.GetPrim(), material) + return segment + + def _create_dynamic_segments(self): + for lidar in self.lidar_specs: + for target in self.TARGET_BALLS: + segment = self._make_segment( + f"{self.root_path}/ObservationLines/{lidar['name']}_to_{target['name']}", + 0.014, + self.materials["beam"], + ) + self.observation_segments.append((segment, lidar, target)) + + for name in ("Shaft", "ArrowLeft", "ArrowRight"): + self.heading_segments.append(self._make_segment( + f"{self.root_path}/PoseOverlay/ExternalPoseHeading{name}", + 0.045, + self.materials["pose"], + )) + for index in range(32): + self.uncertainty_segments.append(self._make_segment( + f"{self.root_path}/PoseOverlay/ExternalPoseUncertainty_{index:02d}", + 0.025, + self.materials["uncertainty"], + )) + + @staticmethod + def _rotate_local_by_yaw(local_pos, yaw_rad): + cos_yaw = math.cos(yaw_rad) + sin_yaw = math.sin(yaw_rad) + return np.array([ + cos_yaw * local_pos[0] - sin_yaw * local_pos[1], + sin_yaw * local_pos[0] + cos_yaw * local_pos[1], + local_pos[2], + ], dtype=float) + + def _target_world_position(self, vehicle_position, yaw_rad, target): + return np.array(vehicle_position, dtype=float) + self._rotate_local_by_yaw(target["pos"], yaw_rad) + + @staticmethod + def _update_segment(segment, start, end): + start = np.array(start, dtype=float) + end = np.array(end, dtype=float) + delta = end - start + length = float(np.linalg.norm(delta)) + width = float(segment["width"]) + if length < 1e-4: + midpoint = start + yaw_deg = 0.0 + pitch_deg = 0.0 + length = 1e-4 + else: + midpoint = (start + end) * 0.5 + horizontal = math.hypot(float(delta[0]), float(delta[1])) + yaw_deg = math.degrees(math.atan2(float(delta[1]), float(delta[0]))) + pitch_deg = math.degrees(math.atan2(float(delta[2]), horizontal)) + + segment["translate"].Set(Gf.Vec3d(float(midpoint[0]), float(midpoint[1]), float(midpoint[2]))) + segment["rotate"].Set(Gf.Vec3f(0.0, float(-pitch_deg), float(yaw_deg))) + segment["scale"].Set(Gf.Vec3f(float(length), width, width)) + + def update(self, agv): + if not self.enabled or agv is None: + return + + now = time.time() + if now - self.last_update_wall_time < 0.02: + return + self.last_update_wall_time = now + + vehicle_position, quat = agv.get_world_pose() + yaw_rad = quat_wxyz_to_yaw(quat) + target_positions = { + target["name"]: self._target_world_position(vehicle_position, yaw_rad, target) + for target in self.TARGET_BALLS + } + + for segment, lidar, target in self.observation_segments: + self._update_segment(segment, np.array(lidar["pos"], dtype=float), target_positions[target["name"]]) + + pose_anchor = np.array(vehicle_position, dtype=float) + np.array([0.0, 0.0, 1.55]) + heading_length = 0.80 + heading = np.array([math.cos(yaw_rad), math.sin(yaw_rad), 0.0], dtype=float) + side = np.array([-math.sin(yaw_rad), math.cos(yaw_rad), 0.0], dtype=float) + tip = pose_anchor + heading * heading_length + self._update_segment(self.heading_segments[0], pose_anchor, tip) + self._update_segment(self.heading_segments[1], tip, tip - heading * 0.22 + side * 0.13) + self._update_segment(self.heading_segments[2], tip, tip - heading * 0.22 - side * 0.13) + + radius = max(0.18, min(0.65, self.args.external_position_stddev_m * 18.0)) + ring_center = np.array(vehicle_position, dtype=float) + np.array([0.0, 0.0, 0.045]) + ring_points = [] + for index in range(len(self.uncertainty_segments) + 1): + angle = 2.0 * math.pi * index / len(self.uncertainty_segments) + ring_points.append(ring_center + np.array([math.cos(angle) * radius, math.sin(angle) * radius, 0.0])) + for index, segment in enumerate(self.uncertainty_segments): + self._update_segment(segment, ring_points[index], ring_points[index + 1]) + class ExternalTruthTelemetryPublisher: def __init__(self, args): @@ -1068,8 +1293,6 @@ class ControlCalibrationTelemetryPublisher: self.enabled = not args.disable_control_telemetry self.topic = args.control_telemetry_topic self.publish_period_sec = 0.0 if args.control_telemetry_hz <= 0.0 else 1.0 / args.control_telemetry_hz - self.reference_speed_ms = args.control_reference_speed_ms - self.reference_y_m = args.control_reference_y_m self.parameter_version = args.control_parameter_version self.last_publish_wall_time = 0.0 self.node = None @@ -1102,12 +1325,13 @@ class ControlCalibrationTelemetryPublisher: position, yaw_rad, linear_velocity, angular_velocity = vehicle_state_from_isaac(agv) forward_axis = np.array([math.cos(yaw_rad), math.sin(yaw_rad)]) forward_velocity = float(np.dot(np.array([linear_velocity[0], linear_velocity[1]]), forward_axis)) - lateral_error = float(position[1] - self.reference_y_m) - heading_error = float(normalize_angle(yaw_rad)) - speed_error = float(forward_velocity - self.reference_speed_ms) - steering_output = float(clamp(-0.8 * lateral_error - 1.2 * heading_error, -1.0, 1.0)) - throttle_output = float(clamp(-2.0 * speed_error, 0.0, 1.0)) - brake_output = float(clamp(2.0 * speed_error, 0.0, 1.0)) + # Isaac 不再持有标定参考路径；路径误差由标定任务按所选 YAML 路径计算。 + lateral_error = 0.0 + heading_error = 0.0 + speed_error = 0.0 + steering_output = 0.0 + throttle_output = 0.0 + brake_output = 0.0 msg = ControlTelemetry() msg.hardware_timestamp_us = time.time_ns() // 1000 @@ -1274,6 +1498,87 @@ class AckermannUrdfJointDriver: self.warned = True +class VehicleCmdVelSubscriber: + """Direct ROS subscriber used by Isaac's Python loop to avoid OmniGraph command lag.""" + + def __init__(self, args): + self.topic = args.cmd_vel_topic + self.node = None + self.latest_linear_x = 0.0 + self.latest_angular_z = 0.0 + self.last_message_wall_time = 0.0 + + if rclpy is None or Twist is None: + print("[WARN] 未找到 rclpy 或 geometry_msgs/Twist，Isaac 直接 cmd_vel 订阅已禁用。") + return + + if not rclpy.ok(): + rclpy.init(args=None) + + self.node = rclpy.create_node("isaac_vehicle_cmd_vel_subscriber") + self.node.create_subscription(Twist, self.topic, self.on_cmd_vel, 10) + print(f"[*] Isaac 直接订阅车辆 cmd_vel: topic={self.topic}") + + def on_cmd_vel(self, msg): + self.latest_linear_x = float(msg.linear.x) + self.latest_angular_z = float(msg.angular.z) + self.last_message_wall_time = time.time() + + def spin_once(self): + if self.node is None or not rclpy.ok(): + return + rclpy.spin_once(self.node, timeout_sec=0.0) + + def command(self, stale_timeout_sec=0.5): + if self.node is None or self.last_message_wall_time <= 0.0: + return None + age_sec = time.time() - self.last_message_wall_time + if age_sec > stale_timeout_sec: + return 0.0, 0.0 + return self.latest_linear_x, self.latest_angular_z + + def shutdown(self): + if self.node is not None: + self.node.destroy_node() + self.node = None + + +class KinematicCommandFollower: + def __init__(self, enabled=True): + self.enabled = enabled + self.last_wall_time = time.time() + + def apply(self, agv, linear_x, angular_z, ackermann_joint_driver): + now = time.time() + dt = max(0.0, min(now - self.last_wall_time, 0.1)) + self.last_wall_time = now + + position, quat = agv.get_world_pose() + yaw = quat_wxyz_to_yaw(quat) + current_linear_velocity = agv.get_linear_velocity() + world_linear_velocity = np.array([ + linear_x * math.cos(yaw), + linear_x * math.sin(yaw), + current_linear_velocity[2], + ]) + agv.set_linear_velocity(world_linear_velocity) + agv.set_angular_velocity(np.array([0.0, 0.0, angular_z])) + ackermann_joint_driver.apply(agv, linear_x, angular_z) + + if not self.enabled or dt <= 0.0: + return + if abs(linear_x) <= 1e-4 and abs(angular_z) <= 1e-4: + return + + next_yaw = normalize_angle(yaw + angular_z * dt) + travel_yaw = yaw + 0.5 * angular_z * dt + next_position = np.array(position, dtype=float) + next_position[0] += linear_x * math.cos(travel_yaw) * dt + next_position[1] += linear_x * math.sin(travel_yaw) * dt + next_quat = euler_angles_to_quat(np.array([0.0, 0.0, next_yaw]), degrees=False) + agv.set_world_pose(position=next_position, orientation=np.array(next_quat)) + + class IsaacWorkshopRuntime: def __init__(self, args): self.args = args @@ -1304,6 +1609,7 @@ class IsaacWorkshopRuntime: self.calibration_board_specs = [] self.lidar_2d_target_specs = [] self.down_camera_target_specs = [] + self.external_truth_visualization = None self.scene_manifest_path = ( Path(args.scene_manifest_path).expanduser().resolve() if args.scene_manifest_path @@ -1390,18 +1696,11 @@ class IsaacWorkshopRuntime: }, "chassis_calibration": { "chassis_type": self.args.chassis_type, - "straight_track_length_m": self.args.straight_track_length, - "straight_track_width_m": self.args.straight_track_width, - "arc_track_radius_m": self.args.arc_track_radius, "wheel_radius_m": self.args.wheel_radius, "wheel_track_m": self.args.wheel_track, "wheel_base_m": self.args.wheel_base, }, "control_calibration": { - "reference_path": [ - {"x_m": -self.args.straight_track_length / 2.0, "y_m": self.args.control_reference_y_m, "yaw_rad": 0.0, "target_speed_ms": self.args.control_reference_speed_ms}, - {"x_m": self.args.straight_track_length / 2.0, "y_m": self.args.control_reference_y_m, "yaw_rad": 0.0, "target_speed_ms": self.args.control_reference_speed_ms}, - ], "parameter_version": self.args.control_parameter_version, }, "external_truth": { @@ -1411,6 +1710,23 @@ class IsaacWorkshopRuntime: "expected_position_stddev_m": self.args.external_position_stddev_m, "expected_yaw_stddev_rad": self.args.external_yaw_stddev_rad, "expected_time_sync_offset_ms": self.args.external_time_sync_offset_ms, + "visualization_enabled": ( + self.args.enable_external_truth_visualization + and not self.args.disable_external_truth_visualization + ), + "visualization_root_prim": "/World/ExternalTruthVisualization", + "target_ball_radius_m": self.args.external_target_ball_radius, + "target_balls": [ + { + "ball_id": target["name"], + "center_in_base_link": { + "x_m": float(target["pos"][0]), + "y_m": float(target["pos"][1]), + "z_m": float(target["pos"][2]), + }, + } + for target in ExternalTruthSceneVisualization.TARGET_BALLS + ], }, "sensors": [ { @@ -1756,6 +2072,14 @@ class IsaacWorkshopRuntime: {keys.CREATE_NODES: nodes, keys.CONNECT: connections, keys.SET_VALUES: set_values}, ) + def add_external_truth_visualization(self, world): + vehicle_mount_parent_path = self.get_vehicle_sensor_mount_parent_path() + self.external_truth_visualization = ExternalTruthSceneVisualization( + self.args, + world, + vehicle_mount_parent_path, + ) + def build_workshop(self): world = World(stage_units_in_meters=1.0) room_length = self.args.room_length @@ -1868,9 +2192,6 @@ class IsaacWorkshopRuntime: self.lidar_2d_target_specs = add_2d_lidar_calibration_targets(world, self.args) print(f"[*] 已创建 {len(self.lidar_2d_target_specs)} 个 2D LiDAR 角落外参标定靶标。") - if not self.args.disable_calibration_fixtures: - add_calibration_floor_fixtures(world, self.args) - if self.args.disable_down_camera_intrinsic_target: print("[*] 已禁用下视相机 3D ChArUco 内参标定台。") else: @@ -1918,6 +2239,7 @@ class IsaacWorkshopRuntime: ) self.import_vehicle_to_stage(world) + self.add_external_truth_visualization(world) self.hide_sensor_placeholder_visuals() self.add_vehicle_sensor_publishers() @@ -1968,7 +2290,12 @@ def main(): print(f" - 2D LiDAR topic: {runtime.vehicle_2d_lidar_topic}") print(f" - IMU topic: {runtime.vehicle_imu_topic}") print(f" - /cmd_vel topic: {runtime.cmd_vel_topic}") + print(f" - kinematic command follow: enabled={not ARGS.disable_kinematic_command_follow}") print(f" - external telemetry topic: {ARGS.external_telemetry_topic}") + print( + " - external truth visualization: " + f"enabled={ARGS.enable_external_truth_visualization and not ARGS.disable_external_truth_visualization}" + ) print(f" - chassis telemetry topic: {ARGS.chassis_telemetry_topic}") print(f" - vehicle TF: enabled={not ARGS.disable_vehicle_tf}, hz={ARGS.vehicle_tf_publish_hz}") print(f" - control telemetry topic: {ARGS.control_telemetry_topic}") @@ -1979,6 +2306,8 @@ def main(): agv = runtime.vehicle or world.scene.get_object("agv_vehicle") ackermann_joint_driver = AckermannUrdfJointDriver(ARGS) ackermann_joint_driver.configure(agv) + cmd_vel_subscriber = VehicleCmdVelSubscriber(ARGS) + command_follower = KinematicCommandFollower(enabled=not ARGS.disable_kinematic_command_follow) render_sensor_outputs = ( not ARGS.disable_camera or not ARGS.disable_lidars @@ -2003,7 +2332,9 @@ def main(): (control_telemetry_publisher, (agv,)), (sensor_telemetry_publisher, ()), ) - ros_context_required = any(getattr(publisher, "node", None) is not None for publisher, _ in ros_publishers) + ros_context_required = any(getattr(publisher, "node", None) is not None for publisher, _ in ros_publishers) or ( + cmd_vel_subscriber.node is not None + ) try: while True: if SHUTDOWN_REQUESTED: @@ -2017,29 +2348,29 @@ def main(): exit_reason = "simulation_app.is_running() returned False" break try: - lin_vel = og.Controller.get(og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:linearVelocity")) - ang_vel = og.Controller.get(og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:angularVelocity")) + cmd_vel_subscriber.spin_once() + command = cmd_vel_subscriber.command() + if command is None: + lin_vel = og.Controller.get( + og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:linearVelocity") + ) + ang_vel = og.Controller.get( + og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:angularVelocity") + ) + if lin_vel is not None and ang_vel is not None and len(lin_vel) == 3: + command = float(lin_vel[0]), float(ang_vel[2]) - if lin_vel is not None and ang_vel is not None and len(lin_vel) == 3: - _, quat = agv.get_world_pose() - current_linear_velocity = agv.get_linear_velocity() - - q = Gf.Quatd(float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3])) - rot_mat = Gf.Matrix3d(Gf.Rotation(q)) - world_linear_velocity = Gf.Vec3d(*lin_vel) * rot_mat - world_angular_velocity = Gf.Vec3d(*ang_vel) * rot_mat - - target_linear_velocity = np.array([world_linear_velocity[0], world_linear_velocity[1], current_linear_velocity[2]]) - target_angular_velocity = np.array([0.0, 0.0, world_angular_velocity[2]]) - agv.set_linear_velocity(target_linear_velocity) - agv.set_angular_velocity(target_angular_velocity) - ackermann_joint_driver.apply(agv, lin_vel[0], ang_vel[2]) - - if abs(lin_vel[0]) > 0.01 or abs(ang_vel[2]) > 0.01: - print(f"\r[ROS2 Debug] 车辆移动中: 线速度 {lin_vel[0]:.2f}, 角速度 {ang_vel[2]:.2f}", end="") + if command is not None: + linear_x, angular_z = command + command_follower.apply(agv, linear_x, angular_z, ackermann_joint_driver) + if abs(linear_x) > 0.01 or abs(angular_z) > 0.01: + print(f"\r[ROS2 Debug] 车辆移动中: 线速度 {linear_x:.2f}, 角速度 {angular_z:.2f}", end="") except Exception as exc: print(f"\n[ERROR] 运动学控制循环异常: {exc}") + if runtime.external_truth_visualization is not None: + runtime.external_truth_visualization.update(agv) + world.step(render=render_frame) try: for publisher, publish_args in ros_publishers: @@ -2079,6 +2410,7 @@ def main(): chassis_telemetry_publisher.shutdown() control_telemetry_publisher.shutdown() sensor_telemetry_publisher.shutdown() + cmd_vel_subscriber.shutdown() if rclpy is not None and rclpy.ok(): rclpy.shutdown() if SHUTDOWN_REQUESTED or ros_context_shutdown_seen: diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/calibration_targets.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/calibration_targets.py index 7dddf4e..fe2d67a 100644 --- a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/calibration_targets.py +++ b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/calibration_targets.py @@ -360,93 +360,3 @@ def add_2d_lidar_calibration_targets(world, args): ) return specs - - -def add_floor_marker(world, prim_path, name, position, scale, color): - """添加地面标记""" - world.scene.add(FixedCuboid( - prim_path=prim_path, - name=name, - position=np.array(position), - scale=np.array(scale), - color=np.array(color), - )) - - -def add_calibration_floor_fixtures(world, args): - """添加地面标定设施""" - z = 0.012 - thickness = 0.012 - length = args.straight_track_length - width = args.straight_track_width - - add_floor_marker( - world, - "/World/Workshop/CalibrationFixtures/StraightTrack", - "straight_track", - [0.0, 0.0, z], - [length, width, thickness], - [0.08, 0.12, 0.10], - ) - add_floor_marker( - world, - "/World/Workshop/CalibrationFixtures/StraightCenterLine", - "straight_center_line", - [0.0, 0.0, z + thickness], - [length, 0.035, thickness], - [0.95, 0.95, 0.15], - ) - for side, y in (("Left", width / 2.0), ("Right", -width / 2.0)): - add_floor_marker( - world, - f"/World/Workshop/CalibrationFixtures/StraightBoundary{side}", - f"straight_boundary_{side.lower()}", - [0.0, y, z + thickness], - [length, 0.025, thickness], - [0.95, 0.78, 0.08], - ) - - stop_x = min(length / 2.0 - 0.35, args.room_length / 2.0 - 1.0) - add_floor_marker( - world, - "/World/Workshop/CalibrationFixtures/StopAccuracyTarget", - "stop_accuracy_target", - [stop_x, 0.0, z + 2.0 * thickness], - [0.6, 0.9, thickness], - [0.88, 0.10, 0.10], - ) - - lateral_x = -min(length / 2.0 - 0.8, args.room_length / 2.0 - 1.2) - add_floor_marker( - world, - "/World/Workshop/CalibrationFixtures/LateralMotionPad", - "lateral_motion_pad", - [lateral_x, 0.0, z + thickness], - [0.75, 2.0, thickness], - [0.10, 0.34, 0.88], - ) - - arc_center = np.array([0.0, -args.room_width * 0.20]) - arc_radius = min(args.arc_track_radius, args.room_width * 0.32) - marker_count = 25 - for index in range(marker_count): - theta = math.radians(20.0 + 140.0 * index / (marker_count - 1)) - x = arc_center[0] + arc_radius * math.cos(theta) - y = arc_center[1] + arc_radius * math.sin(theta) - add_floor_marker( - world, - f"/World/Workshop/CalibrationFixtures/ArcMarker_{index:02d}", - f"arc_marker_{index:02d}", - [x, y, z + 3.0 * thickness], - [0.10, 0.10, thickness], - [0.12, 0.75, 0.35], - ) - - add_floor_marker( - world, - "/World/Workshop/CalibrationFixtures/SensorCapturePad", - "sensor_capture_pad", - [0.0, args.room_width * 0.28, z + thickness], - [1.2, 0.9, thickness], - [0.42, 0.18, 0.78], - ) diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/utils.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/utils.py index b8b1eb4..ae29b3e 100644 --- a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/utils.py +++ b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/utils.py @@ -42,7 +42,9 @@ def chassis_type_value(name, chassis_type_cls=None): "ackermann": chassis_type_cls.ACKERMANN, "differential": chassis_type_cls.DIFFERENTIAL, "single_steer": chassis_type_cls.SINGLE_STEER_WHEEL, + "single_steer_wheel": chassis_type_cls.SINGLE_STEER_WHEEL, "multi_steer": chassis_type_cls.MULTI_STEER_WHEEL, + "multi_steer_wheel": chassis_type_cls.MULTI_STEER_WHEEL, } return mapping.get(name, chassis_type_cls.CHASSIS_TYPE_UNSPECIFIED) 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 482c6bb..8ca965a 100755 --- a/agv_calib_brain/src/simulation/tools/launch_sim_stack.py +++ b/agv_calib_brain/src/simulation/tools/launch_sim_stack.py @@ -205,6 +205,8 @@ def build_isaac_command(profile: dict, python_executable: str, headless: bool, e isaac = profile["isaac"] targets = profile.get("targets", {}) down_camera = targets.get("down_camera_charuco", {}) + external_localization = profile.get("external_localization", {}) + external_pose_bridge = profile.get("external_pose_bridge", {}) command = [ python_executable, str(repo_path(isaac["scene_script"])), @@ -222,6 +224,20 @@ def build_isaac_command(profile: dict, python_executable: str, headless: bool, e str(get_nested(profile, "vehicle_sensor_agent.lidar_2d_topic", "/sensor/lidar_2d/scan")), "--vehicle-imu-topic", str(get_nested(profile, "vehicle_sensor_agent.imu_topic", "/sensor/imu/data")), + "--external-telemetry-topic", + str( + external_localization.get("output_topic") + or external_pose_bridge.get("source_topic") + or get_nested(profile, "vehicle_agent.external_pose_topic", "/isaac/external_localization/vehicle/pose") + ), + "--external-reference-source-name", + str( + external_localization.get("reference_source_name") + or external_pose_bridge.get("reference_source_name") + or "isaac_sim_truth_source" + ), + "--external-workcell-zone-id", + str(external_localization.get("workcell_zone_id", "isaac_workcell_zone_a")), "--scene-manifest-path", str(repo_path(isaac["scene_manifest_path"])), ] 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 e72b1c6..1bae5b0 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 @@ -625,7 +625,7 @@ def make_chassis_profile_tasks(args: argparse.Namespace) -> list[RequestedCalibr ) 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) + exported = tool.export_requested_tasks(profile, args.chassis_profile_type, profile_path) return [ make_requested_task_from_profile( raw_task, @@ -643,7 +643,7 @@ def make_control_profile_tasks(args: argparse.Namespace) -> list[RequestedCalibr ) 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) + exported = tool.export_requested_tasks(profile, args.chassis_profile_type, profile_path) return [ make_requested_task_from_profile( raw_task, @@ -661,7 +661,7 @@ def make_sensor_profile_tasks(task_name: str, args: argparse.Namespace) -> list[ ) 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}) + exported = tool.export_requested_tasks(profile, {task_name}, profile_path) return [ make_requested_task_from_profile( raw_task, 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 index 81eed79..08583f0 100644 --- 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 @@ -56,6 +56,14 @@ src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_prof 这个 profile 固定了四类底盘在真实标定车间中建议执行的动作序列、速度、距离、超时和采集要求。现场部署时需要先按工位尺寸、安全速度、真实模块 ID 修改它，再用工具校验： +底盘参考路径单独放在： + +```text +src/site_deployment/workshop_chassis_calibration_real/reference_paths/ +``` + +每条路径一个 YAML 文件，必须配置 `path_id` 和中文 `display_name`。`chassis_action_profile.yaml` 中每个动作通过 `reference_path_id` 选择要用的路径，导出任务时会自动展开为 `reference_path.*` metadata。 + ```bash python3 src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py \ --profile /data/agv_calib/site_a/chassis_action_profile.yaml @@ -125,6 +133,73 @@ python3 src/site_deployment/workshop_chassis_calibration_real/run_chassis_profil - 命令话题不可用时，`command_source=auto` 会按动作 profile 写计划命令行。 - 全部动作结束后统一写 `dataset_index.yaml`。 +## 车间电脑侧底盘遥测 WiFi/TCP bridge + +如果真实车端通过 WiFi/TCP 把底盘遥测发到车间电脑，而不是直接发布 ROS2 topic，先启动桥接节点： + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash + +python3 src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py \ + --bind-host 0.0.0.0 \ + --bind-port 9010 \ + --protocol frame \ + --output-topic /chassis/telemetry \ + --default-chassis-type ackermann +``` + +桥接节点接收 JSON payload，并发布 `calibration_chassis_interfaces/msg/ChassisTelemetry`。支持两种常用输入： + +- `frame`：项目统一 TCP 帧，``，建议车端使用 `msg_type=7`。 +- `json_lines`：每行一条 UTF-8 JSON。 + +payload 字段示例： + +```json +{ + "hardware_timestamp_us": 1711234567000000, + "chassis_type": "ackermann", + "odom_x_m": 0.12, + "odom_y_m": 0.0, + "odom_yaw_rad": 0.0, + "linear_velocity_ms": 0.1, + "angular_velocity_rads": 0.0, + "estop_engaged": false, + "driver_error_code": 0, + "active_job_id": "chassis_req_001", + "modules": [ + { + "module_id": "rear_left", + "encoder_ticks": 12345, + "wheel_speed_rpm": 18.0, + "steer_angle_deg": 0.0, + "motor_current_amp": 1.2 + } + ] +} +``` + +确认车间电脑侧已经收到并转成 ROS topic： + +```bash +ros2 topic hz /chassis/telemetry +ros2 topic echo /chassis/telemetry --once +``` + +通过操作台 UI 启动“现场服务”时，如果现场配置里存在 `chassis_telemetry_bridge`，UI 会把监听地址、端口、协议和输出 topic 传给 +`minimal_workshop_demo.launch.py`，并自动启动这个 bridge。也可以手动通过 launch 参数启用： + +```bash +ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \ + use_gateway:=true \ + enable_chassis_telemetry_bridge:=true \ + chassis_telemetry_bind_host:=0.0.0.0 \ + chassis_telemetry_bind_port:=9010 \ + chassis_telemetry_protocol:=frame \ + chassis_telemetry_topic:=/chassis/telemetry +``` + 本地最小 smoke： ```bash 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 index ad92e43..6f0ce76 100644 --- 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 @@ -176,6 +176,8 @@ class ChassisSessionCapture: def on_chassis_telemetry(self, msg: Any) -> None: chassis_type = normalize_chassis_type_name(getattr(getattr(msg, "chassis_type", None), "value", "")) + if not chassis_type: + chassis_type = normalize_chassis_type_name(self.config.get("chassis_type", "")) self.chassis_csv.write_row({ "hardware_timestamp_us": int_value(getattr(msg, "hardware_timestamp_us", 0), now_us()), "chassis_type": chassis_type, 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 index 3452a23..1782c82 100644 --- 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 @@ -13,6 +13,13 @@ try: except ImportError as exc: raise SystemExit("缺少 PyYAML，请先在当前 Python 环境安装 yaml 模块。") from exc +SCRIPT_DIR = Path(__file__).resolve().parent +REFERENCE_PATH_TOOL_DIR = SCRIPT_DIR.parents[0] / "workshop_reference_paths" +if str(REFERENCE_PATH_TOOL_DIR) not in sys.path: + sys.path.insert(0, str(REFERENCE_PATH_TOOL_DIR)) + +from calibration_reference_paths import selected_path_metadata # noqa: E402 + SUPPORTED_CHASSIS_TYPES = { "ackermann", @@ -230,6 +237,8 @@ def validate_action( for key, value in metadata.items(): validate_metadata_value(key, value, task_code, max_linear_speed_ms) + if action.get("reference_path_id") not in (None, ""): + require_string(action.get("reference_path_id"), f"{action_name}.reference_path_id") def validate_profile(profile: dict[str, Any]) -> dict[str, Any]: @@ -281,12 +290,25 @@ def make_task_param(key: str, value: Any) -> dict[str, str]: } -def export_requested_tasks(profile: dict[str, Any], chassis_type: str) -> dict[str, Any]: +def export_requested_tasks( + profile: dict[str, Any], + chassis_type: str, + profile_path: Path | None = None, +) -> dict[str, Any]: section = profile["chassis_profiles"][chassis_type] + reference_path_dir = profile.get("reference_path_dir", "reference_paths") tasks: list[dict[str, Any]] = [] for action in section["actions"]: metadata = dict(action["metadata"]) metadata["primitive_type"] = action["primitive_type"] + reference_metadata, _ = selected_path_metadata( + reference_path_dir, + profile_path, + str(action.get("reference_path_id", "")), + chassis_type, + str(action["primitive_type"]), + ) + metadata.update(reference_metadata) task_params = [ make_task_param(key, metadata[key]) for key in sorted(metadata) @@ -341,7 +363,7 @@ def main() -> int: 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) + output = export_requested_tasks(profile, args.chassis_type, profile_path) else: output = build_summary(profile) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_reference_paths.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_reference_paths.py new file mode 100644 index 0000000..11474e1 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/chassis_reference_paths.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +"""兼容入口：底盘参考路径已迁移到通用标定参考路径工具。""" + +from __future__ import annotations + +import sys +from pathlib import Path + +COMMON_TOOL_DIR = Path(__file__).resolve().parents[1] / "workshop_reference_paths" +if str(COMMON_TOOL_DIR) not in sys.path: + sys.path.insert(0, str(COMMON_TOOL_DIR)) + +from calibration_reference_paths import main # noqa: E402 + + +DEFAULT_PATH_DIR = Path(__file__).resolve().parent / "reference_paths" + + +if __name__ == "__main__": + if "--path-dir" not in sys.argv: + sys.argv.extend(["--path-dir", str(DEFAULT_PATH_DIR)]) + raise SystemExit(main()) 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 index 9631ccf..ebc0cc2 100644 --- 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 @@ -4,6 +4,7 @@ schema_version: 1 profile_name: workshop_chassis_calibration_action_profile +reference_path_dir: ../reference_paths safety: max_linear_speed_ms: 0.1 @@ -33,6 +34,7 @@ chassis_profiles: - task_code: chassis.ackermann.straight_forward display_name: 阿克曼直线前进 primitive_type: straight_line + reference_path_id: chassis_straight_forward_1m metadata: straight_line.target_distance_m: 1.0 straight_line.target_speed_ms: 0.1 @@ -42,6 +44,7 @@ chassis_profiles: - task_code: chassis.ackermann.straight_reverse display_name: 阿克曼直线倒车 primitive_type: straight_line + reference_path_id: chassis_straight_reverse_0_6m metadata: straight_line.target_distance_m: 0.6 straight_line.target_speed_ms: 0.08 @@ -51,6 +54,7 @@ chassis_profiles: - task_code: chassis.ackermann.arc_left display_name: 阿克曼左圆弧 primitive_type: arc + reference_path_id: chassis_arc_left_r1_45deg metadata: arc.target_speed_ms: 0.08 arc.radius_m: 1.0 @@ -61,6 +65,7 @@ chassis_profiles: - task_code: chassis.ackermann.arc_right display_name: 阿克曼右圆弧 primitive_type: arc + reference_path_id: chassis_arc_right_r1_45deg metadata: arc.target_speed_ms: 0.08 arc.radius_m: 1.0 @@ -68,9 +73,54 @@ chassis_profiles: arc.clockwise: true brake_when_finished: true timeout_sec: 45.0 + - task_code: chassis.ackermann.s_curve_left_entry + display_name: 阿克曼 S 形 1/4 左入弯 + primitive_type: arc + reference_path_id: chassis_ackermann_s_curve_r1_5 + metadata: + arc.target_speed_ms: 0.1 + arc.radius_m: 1.5 + arc.sweep_angle_deg: 25.0 + arc.clockwise: false + brake_when_finished: false + timeout_sec: 30.0 + - task_code: chassis.ackermann.s_curve_right_middle + display_name: 阿克曼 S 形 2/4 右转 + primitive_type: arc + reference_path_id: chassis_ackermann_s_curve_r1_5 + metadata: + arc.target_speed_ms: 0.1 + arc.radius_m: 1.5 + arc.sweep_angle_deg: 60.0 + arc.clockwise: true + brake_when_finished: false + timeout_sec: 40.0 + - task_code: chassis.ackermann.s_curve_left_middle + display_name: 阿克曼 S 形 3/4 左转 + primitive_type: arc + reference_path_id: chassis_ackermann_s_curve_r1_5 + metadata: + arc.target_speed_ms: 0.1 + arc.radius_m: 1.5 + arc.sweep_angle_deg: 60.0 + arc.clockwise: false + brake_when_finished: false + timeout_sec: 40.0 + - task_code: chassis.ackermann.s_curve_right_exit + display_name: 阿克曼 S 形 4/4 右出弯 + primitive_type: arc + reference_path_id: chassis_ackermann_s_curve_r1_5 + metadata: + arc.target_speed_ms: 0.1 + arc.radius_m: 1.5 + arc.sweep_angle_deg: 25.0 + arc.clockwise: true + brake_when_finished: true + timeout_sec: 30.0 - task_code: chassis.ackermann.steering_sweep display_name: 阿克曼舵角扫动 primitive_type: steering_sweep + reference_path_id: chassis_static_station metadata: steering_sweep.target_angle_deg: 0.0 steering_sweep.sweep_amplitude_deg: 8.0 @@ -89,6 +139,7 @@ chassis_profiles: - task_code: chassis.differential.straight_forward display_name: 差速直线前进 primitive_type: straight_line + reference_path_id: chassis_straight_forward_1m metadata: straight_line.target_distance_m: 1.0 straight_line.target_speed_ms: 0.1 @@ -98,6 +149,7 @@ chassis_profiles: - task_code: chassis.differential.straight_reverse display_name: 差速直线倒车 primitive_type: straight_line + reference_path_id: chassis_straight_reverse_0_6m metadata: straight_line.target_distance_m: 0.6 straight_line.target_speed_ms: 0.08 @@ -107,6 +159,7 @@ chassis_profiles: - task_code: chassis.differential.rotate_left display_name: 差速左原地旋转 primitive_type: in_place_rotation + reference_path_id: chassis_rotate_left_90deg metadata: in_place_rotation.target_yaw_deg: 90.0 in_place_rotation.target_angular_vel_deg_s: 10.0 @@ -115,6 +168,7 @@ chassis_profiles: - task_code: chassis.differential.rotate_right display_name: 差速右原地旋转 primitive_type: in_place_rotation + reference_path_id: chassis_rotate_right_90deg metadata: in_place_rotation.target_yaw_deg: -90.0 in_place_rotation.target_angular_vel_deg_s: 10.0 @@ -123,6 +177,7 @@ chassis_profiles: - task_code: chassis.differential.arc_left display_name: 差速左转圆弧 primitive_type: arc + reference_path_id: chassis_arc_left_r1_45deg metadata: arc.target_speed_ms: 0.08 arc.radius_m: 1.0 @@ -133,6 +188,7 @@ chassis_profiles: - task_code: chassis.differential.arc_right display_name: 差速右转圆弧 primitive_type: arc + reference_path_id: chassis_arc_right_r1_45deg metadata: arc.target_speed_ms: 0.08 arc.radius_m: 1.0 @@ -152,6 +208,7 @@ chassis_profiles: - task_code: chassis.single_steer.straight_forward display_name: 单舵轮直线前进 primitive_type: straight_line + reference_path_id: chassis_straight_forward_1m metadata: straight_line.target_distance_m: 1.0 straight_line.target_speed_ms: 0.1 @@ -161,6 +218,7 @@ chassis_profiles: - task_code: chassis.single_steer.steering_sweep display_name: 单舵轮舵角扫动 primitive_type: steering_sweep + reference_path_id: chassis_static_station metadata: steering_sweep.target_angle_deg: 0.0 steering_sweep.sweep_amplitude_deg: 10.0 @@ -171,6 +229,7 @@ chassis_profiles: - task_code: chassis.single_steer.arc_left display_name: 单舵轮左圆弧 primitive_type: arc + reference_path_id: chassis_arc_left_r1_45deg metadata: arc.target_speed_ms: 0.08 arc.radius_m: 1.0 @@ -181,6 +240,7 @@ chassis_profiles: - task_code: chassis.single_steer.arc_right display_name: 单舵轮右圆弧 primitive_type: arc + reference_path_id: chassis_arc_right_r1_45deg metadata: arc.target_speed_ms: 0.08 arc.radius_m: 1.0 @@ -200,6 +260,7 @@ chassis_profiles: - task_code: chassis.multi_steer.straight_forward display_name: 多舵轮直线前进 primitive_type: straight_line + reference_path_id: chassis_straight_forward_1m metadata: straight_line.target_distance_m: 1.0 straight_line.target_speed_ms: 0.1 @@ -209,6 +270,7 @@ chassis_profiles: - task_code: chassis.multi_steer.lateral_left display_name: 多舵轮左横移 primitive_type: lateral_translation + reference_path_id: chassis_lateral_left_0_5m metadata: lateral_translation.target_speed_ms: 0.06 lateral_translation.target_distance_m: 0.5 @@ -218,6 +280,7 @@ chassis_profiles: - task_code: chassis.multi_steer.lateral_right display_name: 多舵轮右横移 primitive_type: lateral_translation + reference_path_id: chassis_lateral_right_0_5m metadata: lateral_translation.target_speed_ms: 0.06 lateral_translation.target_distance_m: 0.5 @@ -227,6 +290,7 @@ chassis_profiles: - task_code: chassis.multi_steer.diagonal_forward_left display_name: 多舵轮左前斜移 primitive_type: diagonal_motion + reference_path_id: chassis_diagonal_forward_left_0_5m metadata: diagonal_motion.target_speed_ms: 0.06 diagonal_motion.target_distance_m: 0.5 @@ -236,6 +300,7 @@ chassis_profiles: - task_code: chassis.multi_steer.diagonal_forward_right display_name: 多舵轮右前斜移 primitive_type: diagonal_motion + reference_path_id: chassis_diagonal_forward_right_0_5m metadata: diagonal_motion.target_speed_ms: 0.06 diagonal_motion.target_distance_m: 0.5 @@ -245,6 +310,7 @@ chassis_profiles: - task_code: chassis.multi_steer.module_alignment display_name: 多舵轮模块零位检查 primitive_type: module_alignment + reference_path_id: chassis_static_station metadata: module_alignment.module_ids: - front_left @@ -258,6 +324,7 @@ chassis_profiles: - task_code: chassis.multi_steer.coordinated_steering display_name: 多舵轮协同转向 primitive_type: coordinated_steering + reference_path_id: chassis_static_station metadata: coordinated_steering.module_ids: - front_left diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_sim.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_sim.yaml new file mode 100644 index 0000000..e08141d --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_sim.yaml @@ -0,0 +1,36 @@ +# Isaac 仿真底盘路径执行/采集配置。 +# 用于验证：选择底盘 reference path -> 执行动作 -> 采集底盘遥测和 external truth。 + +csv_contract_version: 1 +chassis_type: ackermann +action_profile_file: src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml +session_id: sim_chassis_path_test +site_id: isaac_workcell_zone_a +vehicle_id: demo_agv_001 +session_dir: /tmp/agv_calib_chassis_sim/session_latest +dataset_index_path: /tmp/agv_calib_chassis_sim/session_latest/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: /chassis/telemetry + external_pose_topic: /isaac/external_localization/vehicle/pose + ackermann_command_topic: /vehicle/demo_agv_001/internal/ackermann_cmd + command_source: auto + +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: 300.0 + +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/reference_paths/chassis_ackermann_s_curve_r1_5.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_ackermann_s_curve_r1_5.yaml new file mode 100644 index 0000000..53b828c --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_ackermann_s_curve_r1_5.yaml @@ -0,0 +1,47 @@ +schema_version: 1 +path_id: chassis_ackermann_s_curve_r1_5 +display_name: 阿克曼 S 形路径 R1.5 +module_type: chassis +frame_id: workshop +path_type: s_curve +recommended_task_types: + - arc + - s_curve +description: 由左 25deg、右 60deg、左 60deg、右 25deg 四段 R1.5 圆弧组成的低速阿克曼 S 形路径。 +points: + - x_m: 0.000 + y_m: 0.000 + yaw_rad: 0.000 + target_speed_ms: 0.10 + - x_m: 0.325 + y_m: 0.036 + yaw_rad: 0.218 + target_speed_ms: 0.10 + - x_m: 0.634 + y_m: 0.141 + yaw_rad: 0.436 + target_speed_ms: 0.10 + - x_m: 1.399 + y_m: 0.275 + yaw_rad: -0.087 + target_speed_ms: 0.10 + - x_m: 2.128 + y_m: 0.010 + yaw_rad: -0.611 + target_speed_ms: 0.10 + - x_m: 2.858 + y_m: -0.256 + yaw_rad: -0.087 + target_speed_ms: 0.10 + - x_m: 3.623 + y_m: -0.121 + yaw_rad: 0.436 + target_speed_ms: 0.10 + - x_m: 3.932 + y_m: -0.016 + yaw_rad: 0.218 + target_speed_ms: 0.10 + - x_m: 4.256 + y_m: 0.020 + yaw_rad: 0.000 + target_speed_ms: 0.10 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_arc_left_r1_45deg.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_arc_left_r1_45deg.yaml new file mode 100644 index 0000000..5341c38 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_arc_left_r1_45deg.yaml @@ -0,0 +1,21 @@ +schema_version: 1 +path_id: chassis_arc_left_r1_45deg +display_name: 底盘左圆弧 R1 45deg +module_type: chassis +frame_id: workshop +path_type: arc +recommended_task_types: + - arc +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 0.383 + y_m: 0.076 + yaw_rad: 0.393 + target_speed_ms: 0.08 + - x_m: 0.707 + y_m: 0.293 + yaw_rad: 0.785 + target_speed_ms: 0.08 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_arc_right_r1_45deg.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_arc_right_r1_45deg.yaml new file mode 100644 index 0000000..c020162 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_arc_right_r1_45deg.yaml @@ -0,0 +1,21 @@ +schema_version: 1 +path_id: chassis_arc_right_r1_45deg +display_name: 底盘右圆弧 R1 45deg +module_type: chassis +frame_id: workshop +path_type: arc +recommended_task_types: + - arc +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: 0.383 + y_m: -0.076 + yaw_rad: -0.393 + target_speed_ms: 0.08 + - x_m: 0.707 + y_m: -0.293 + yaw_rad: -0.785 + target_speed_ms: 0.08 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_diagonal_forward_left_0_5m.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_diagonal_forward_left_0_5m.yaml new file mode 100644 index 0000000..3468fed --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_diagonal_forward_left_0_5m.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: chassis_diagonal_forward_left_0_5m +display_name: 多舵轮左前斜移 0.5m +module_type: chassis +frame_id: workshop +path_type: diagonal_motion +recommended_task_types: + - diagonal_motion +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - x_m: 0.354 + y_m: 0.354 + yaw_rad: 0.0 + target_speed_ms: 0.06 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_diagonal_forward_right_0_5m.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_diagonal_forward_right_0_5m.yaml new file mode 100644 index 0000000..a0a07eb --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_diagonal_forward_right_0_5m.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: chassis_diagonal_forward_right_0_5m +display_name: 多舵轮右前斜移 0.5m +module_type: chassis +frame_id: workshop +path_type: diagonal_motion +recommended_task_types: + - diagonal_motion +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - x_m: 0.354 + y_m: -0.354 + yaw_rad: 0.0 + target_speed_ms: 0.06 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_lateral_left_0_5m.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_lateral_left_0_5m.yaml new file mode 100644 index 0000000..32679e5 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_lateral_left_0_5m.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: chassis_lateral_left_0_5m +display_name: 多舵轮左横移 0.5m +module_type: chassis +frame_id: workshop +path_type: lateral_translation +recommended_task_types: + - lateral_translation +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - x_m: 0.0 + y_m: 0.5 + yaw_rad: 0.0 + target_speed_ms: 0.06 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_lateral_right_0_5m.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_lateral_right_0_5m.yaml new file mode 100644 index 0000000..ae159ce --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_lateral_right_0_5m.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: chassis_lateral_right_0_5m +display_name: 多舵轮右横移 0.5m +module_type: chassis +frame_id: workshop +path_type: lateral_translation +recommended_task_types: + - lateral_translation +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.06 + - x_m: 0.0 + y_m: -0.5 + yaw_rad: 0.0 + target_speed_ms: 0.06 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_rotate_left_90deg.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_rotate_left_90deg.yaml new file mode 100644 index 0000000..21aecaa --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_rotate_left_90deg.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: chassis_rotate_left_90deg +display_name: 底盘原地左转 90deg +module_type: chassis +frame_id: workshop +path_type: in_place_rotation +recommended_task_types: + - in_place_rotation +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.0 + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 1.570796327 + target_speed_ms: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_rotate_right_90deg.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_rotate_right_90deg.yaml new file mode 100644 index 0000000..04b7e46 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_rotate_right_90deg.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: chassis_rotate_right_90deg +display_name: 底盘原地右转 90deg +module_type: chassis +frame_id: workshop +path_type: in_place_rotation +recommended_task_types: + - in_place_rotation +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.0 + - x_m: 0.0 + y_m: 0.0 + yaw_rad: -1.570796327 + target_speed_ms: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_static_station.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_static_station.yaml new file mode 100644 index 0000000..0a52783 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_static_station.yaml @@ -0,0 +1,15 @@ +schema_version: 1 +path_id: chassis_static_station +display_name: 底盘静态检查点 +module_type: chassis +frame_id: workshop +path_type: static_station +recommended_task_types: + - steering_sweep + - module_alignment + - coordinated_steering +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_straight_forward_1m.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_straight_forward_1m.yaml new file mode 100644 index 0000000..18e4bfc --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_straight_forward_1m.yaml @@ -0,0 +1,18 @@ +schema_version: 1 +path_id: chassis_straight_forward_1m +display_name: 底盘直线前进 1m +description: 用于轮径、里程计比例和直线跑偏检查。 +module_type: chassis +frame_id: workshop +path_type: straight_line +recommended_task_types: + - straight_line +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.10 + - x_m: 1.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.10 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_straight_reverse_0_6m.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_straight_reverse_0_6m.yaml new file mode 100644 index 0000000..e00a2a7 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/chassis_straight_reverse_0_6m.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: chassis_straight_reverse_0_6m +display_name: 底盘直线倒车 0.6m +module_type: chassis +frame_id: workshop +path_type: straight_line +recommended_task_types: + - straight_line +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.08 + - x_m: -0.6 + y_m: 0.0 + yaw_rad: 3.141592654 + target_speed_ms: 0.08 diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/index.yaml b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/index.yaml new file mode 100644 index 0000000..5225416 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/reference_paths/index.yaml @@ -0,0 +1,19 @@ +profile_name: chassis_calibration_reference_paths +frame_id: workshop + +default_path_id_by_chassis: + ackermann: chassis_straight_forward_1m + differential: chassis_straight_forward_1m + single_steer_wheel: chassis_straight_forward_1m + multi_steer_wheel: chassis_lateral_left_0_5m + +default_path_id_by_task_type: + straight_line: chassis_straight_forward_1m + arc: chassis_arc_left_r1_45deg + in_place_rotation: chassis_rotate_left_90deg + s_curve: chassis_ackermann_s_curve_r1_5 + steering_sweep: chassis_static_station + lateral_translation: chassis_lateral_left_0_5m + diagonal_motion: chassis_diagonal_forward_left_0_5m + module_alignment: chassis_static_station + coordinated_steering: chassis_static_station 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 index 7cc6bce..9ef84e6 100644 --- 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 @@ -51,6 +51,7 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type，并选择对应动作序列。") parser.add_argument("--task-code", default="", help="只执行指定 task_code；为空时执行该底盘类型全部动作。") + parser.add_argument("--reference-path-id", default="", help="只执行绑定到指定 reference_path_id 的动作。") 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。") @@ -130,14 +131,28 @@ def spin_until_future( return future.result() -def select_actions(profile: dict[str, Any], chassis_type: str, task_code: str) -> list[dict[str, Any]]: +def select_actions( + profile: dict[str, Any], + chassis_type: str, + task_code: str, + reference_path_id: str, +) -> list[dict[str, Any]]: section = profile["chassis_profiles"][chassis_type] actions = list(section["actions"]) - if not task_code: + if task_code: + actions = [action for action in actions if action.get("task_code") == task_code] + if reference_path_id: + actions = [action for action in actions if action.get("reference_path_id") == reference_path_id] + if not task_code and not reference_path_id: return actions - selected = [action for action in actions if action.get("task_code") == task_code] + selected = actions if not selected: - raise ValueError(f"动作 profile 中没有 task_code={task_code!r}。") + detail = [] + if task_code: + detail.append(f"task_code={task_code!r}") + if reference_path_id: + detail.append(f"reference_path_id={reference_path_id!r}") + raise ValueError(f"动作 profile 中没有 {' 且 '.join(detail)} 的动作。") return selected @@ -349,7 +364,7 @@ def main() -> int: 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) + actions = select_actions(action_profile, chassis_type, args.task_code, args.reference_path_id) data_capture = action_profile.get("data_capture", {}) or {} start_before_sec = ( as_float(data_capture.get("start_before_motion_sec"), 1.0) diff --git a/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py new file mode 100644 index 0000000..dc64845 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py @@ -0,0 +1,322 @@ +#!/usr/bin/env python3 +"""车间电脑侧 WiFi/TCP 底盘遥测接收桥。 + +车端通过 TCP 推送 JSON 遥测，本节点转换成标准 ChassisTelemetry 并发布到 ROS2。 +""" + +from __future__ import annotations + +import argparse +import json +import socket +import socketserver +import struct +import threading +import time +from typing import Any + +try: + import rclpy + from calibration_chassis_interfaces.msg import ChassisTelemetry, WheelModuleState +except ImportError: + rclpy = None + ChassisTelemetry = None + WheelModuleState = None + + +CHASSIS_TELEMETRY_PUSH_REQ = 7 +CHASSIS_TELEMETRY_PUSH_RSP = 8 + +CHASSIS_TYPE_BY_NAME = { + "ackermann": 1, + "differential": 2, + "single_steer_wheel": 3, + "multi_steer_wheel": 4, +} + + +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_frame(conn: socket.socket, msg_type: int, payload: dict[str, Any]) -> None: + encoded = json.dumps(payload, ensure_ascii=False, separators=(",", ":")).encode("utf-8") + conn.sendall(struct.pack(" float: + if value in (None, ""): + return default + return float(value) + + +def as_int(value: Any, default: int = 0) -> int: + if value in (None, ""): + return default + return int(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 chassis_type_value(value: Any, default_name: str) -> int: + if isinstance(value, dict): + value = value.get("value", "") + if value in (None, ""): + return CHASSIS_TYPE_BY_NAME.get(default_name, 0) + if isinstance(value, (int, float)): + return int(value) + text = str(value).strip().lower() + if text.isdigit(): + return int(text) + return CHASSIS_TYPE_BY_NAME.get(text, CHASSIS_TYPE_BY_NAME.get(default_name, 0)) + + +def nested_payload(payload: dict[str, Any]) -> dict[str, Any]: + for key in ("telemetry", "chassis_telemetry", "chassis"): + value = payload.get(key) + if isinstance(value, dict): + return value + return payload + + +def module_payloads(payload: dict[str, Any]) -> list[dict[str, Any]]: + raw = payload.get("modules", payload.get("module_states", [])) + if isinstance(raw, dict): + modules = [] + for module_id, value in raw.items(): + if isinstance(value, dict): + modules.append({"module_id": module_id, **value}) + return modules + if isinstance(raw, list): + return [value for value in raw if isinstance(value, dict)] + return [] + + +def build_module_msg(payload: dict[str, Any]) -> Any: + module = WheelModuleState() + module.module_id = str(payload.get("module_id", payload.get("id", ""))) + module.encoder_ticks = as_int(payload.get("encoder_ticks", payload.get("ticks", 0))) + module.wheel_speed_rpm = as_float(payload.get("wheel_speed_rpm", payload.get("rpm", 0.0))) + module.steer_angle_deg = as_float(payload.get("steer_angle_deg", payload.get("steer_deg", 0.0))) + module.motor_current_amp = as_float(payload.get("motor_current_amp", payload.get("current_amp", 0.0))) + return module + + +def build_chassis_telemetry(payload: dict[str, Any], default_chassis_type: str) -> Any: + data = nested_payload(payload) + msg = ChassisTelemetry() + raw_timestamp = data.get("hardware_timestamp_us", data.get("timestamp_us", data.get("timestamp", ""))) + msg.hardware_timestamp_us = as_int(raw_timestamp, now_us()) + msg.chassis_type.value = chassis_type_value(data.get("chassis_type", ""), default_chassis_type) + msg.odom_x_m = as_float(data.get("odom_x_m", data.get("x_m", 0.0))) + msg.odom_y_m = as_float(data.get("odom_y_m", data.get("y_m", 0.0))) + msg.odom_yaw_rad = as_float(data.get("odom_yaw_rad", data.get("yaw_rad", 0.0))) + msg.linear_velocity_ms = as_float(data.get("linear_velocity_ms", data.get("linear_speed_ms", 0.0))) + msg.angular_velocity_rads = as_float(data.get("angular_velocity_rads", data.get("angular_speed_rads", 0.0))) + msg.estop_engaged = as_bool(data.get("estop_engaged", data.get("estop", False))) + msg.driver_error_code = as_int(data.get("driver_error_code", data.get("error_code", 0))) + msg.active_job_id = str(data.get("active_job_id", data.get("job_id", ""))) + msg.lateral_slip_estimate = as_float(data.get("lateral_slip_estimate", 0.0)) + msg.curvature_estimate = as_float(data.get("curvature_estimate", 0.0)) + msg.modules = [build_module_msg(module) for module in module_payloads(data)] + return msg + + +class TelemetryTCPServer(socketserver.ThreadingMixIn, socketserver.TCPServer): + allow_reuse_address = True + daemon_threads = True + + +class TelemetryHandler(socketserver.BaseRequestHandler): + def handle(self) -> None: + self.server.bridge.handle_connection(self.request, self.client_address) + + +class WorkshopChassisTelemetryBridge: + def __init__(self, args: argparse.Namespace) -> None: + if rclpy is None or ChassisTelemetry is None or WheelModuleState is None: + raise RuntimeError("缺少 ROS 2 Python 依赖或 calibration_chassis_interfaces。") + + self.args = args + self.node = rclpy.create_node("workshop_chassis_telemetry_bridge") + self.publisher = self.node.create_publisher(ChassisTelemetry, args.output_topic, 50) + self.received_count = 0 + self.published_count = 0 + self.error_count = 0 + self.lock = threading.Lock() + self.last_stats_log_monotonic = 0.0 + self.server = TelemetryTCPServer((args.bind_host, args.bind_port), TelemetryHandler) + self.server.bridge = self + self.thread = threading.Thread(target=self.server.serve_forever, name="chassis-telemetry-bridge", daemon=True) + + def start(self) -> None: + self.thread.start() + self.node.get_logger().info( + "底盘遥测 WiFi/TCP bridge 已启动: " + f"{self.args.bind_host}:{self.args.bind_port} -> {self.args.output_topic}, " + f"protocol={self.args.protocol}" + ) + + def shutdown(self) -> None: + self.server.shutdown() + self.server.server_close() + + def 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( + "底盘遥测 bridge 统计: " + f"received={self.received_count}, published={self.published_count}, errors={self.error_count}" + ) + + def detect_protocol(self, conn: socket.socket) -> str: + if self.args.protocol != "auto": + return self.args.protocol + peek = conn.recv(8, socket.MSG_PEEK) + first = peek.lstrip()[:1] + if first in (b"{", b"["): + return "json_lines" + return "frame" + + def handle_connection(self, conn: socket.socket, address: tuple[str, int]) -> None: + conn.settimeout(self.args.connection_timeout_sec) + try: + protocol = self.detect_protocol(conn) + if protocol == "frame": + self.handle_frame_connection(conn) + elif protocol == "json_lines": + self.handle_json_lines_connection(conn) + elif protocol == "raw_json": + self.handle_raw_json_connection(conn) + else: + raise ValueError(f"未知协议: {protocol}") + except ConnectionError: + return + except Exception as exc: + with self.lock: + self.error_count += 1 + self.node.get_logger().warning(f"底盘遥测连接处理失败 {address}: {exc}") + + def publish_payload(self, payload: dict[str, Any]) -> None: + msg = build_chassis_telemetry(payload, self.args.default_chassis_type) + self.publisher.publish(msg) + with self.lock: + self.received_count += 1 + self.published_count += 1 + self.log_stats() + + def handle_frame_connection(self, conn: socket.socket) -> None: + while rclpy.ok(): + header = read_exactly(conn, 8) + msg_type, payload_len = struct.unpack(" self.args.max_payload_bytes: + raise ValueError(f"payload 过大: {payload_len} > {self.args.max_payload_bytes}") + raw_payload = read_exactly(conn, payload_len) if payload_len else b"{}" + payload = json.loads(raw_payload.decode("utf-8")) + if self.args.expected_msg_type and msg_type != self.args.expected_msg_type: + raise ValueError(f"msg_type 不匹配: expected={self.args.expected_msg_type}, actual={msg_type}") + self.publish_payload(payload) + if self.args.send_ack: + send_frame( + conn, + self.args.ack_msg_type, + {"success": True, "message": "chassis telemetry accepted", "timestamp_us": now_us()}, + ) + + def handle_json_lines_connection(self, conn: socket.socket) -> None: + with conn.makefile("rb") as stream: + for raw_line in stream: + line = raw_line.strip() + if not line: + continue + if len(line) > self.args.max_payload_bytes: + raise ValueError(f"payload 过大: {len(line)} > {self.args.max_payload_bytes}") + payload = json.loads(line.decode("utf-8")) + self.publish_payload(payload) + if self.args.send_ack: + conn.sendall( + (json.dumps({"success": True, "timestamp_us": now_us()}, separators=(",", ":")) + "\n").encode( + "utf-8" + ) + ) + + def handle_raw_json_connection(self, conn: socket.socket) -> None: + chunks: list[bytes] = [] + total = 0 + while True: + chunk = conn.recv(4096) + if not chunk: + break + chunks.append(chunk) + total += len(chunk) + if total > self.args.max_payload_bytes: + raise ValueError(f"payload 过大: {total} > {self.args.max_payload_bytes}") + if not chunks: + return + payload = json.loads(b"".join(chunks).decode("utf-8")) + self.publish_payload(payload) + if self.args.send_ack: + conn.sendall(json.dumps({"success": True, "timestamp_us": now_us()}).encode("utf-8")) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="车间电脑侧 WiFi/TCP 底盘遥测 -> ROS2 /chassis/telemetry bridge") + parser.add_argument("--bind-host", default="0.0.0.0") + parser.add_argument("--bind-port", type=int, default=9010) + parser.add_argument("--output-topic", default="/chassis/telemetry") + parser.add_argument("--protocol", choices=["auto", "frame", "json_lines", "raw_json"], default="frame") + parser.add_argument("--expected-msg-type", type=int, default=0, help="0 表示接受任意帧类型；建议车端使用 7。") + parser.add_argument("--ack-msg-type", type=int, default=CHASSIS_TELEMETRY_PUSH_RSP) + parser.add_argument("--send-ack", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--default-chassis-type", choices=sorted(CHASSIS_TYPE_BY_NAME), default="ackermann") + parser.add_argument("--max-payload-bytes", type=int, default=262144) + parser.add_argument("--connection-timeout-sec", type=float, default=30.0) + parser.add_argument("--stats-log-interval-sec", type=float, default=10.0) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + if rclpy is None: + print("[错误] 缺少 ROS 2 Python 依赖，无法启动底盘遥测 bridge。") + return 1 + rclpy.init(args=None) + bridge = None + try: + bridge = WorkshopChassisTelemetryBridge(args) + bridge.start() + while rclpy.ok(): + rclpy.spin_once(bridge.node, timeout_sec=0.2) + except KeyboardInterrupt: + pass + finally: + if bridge is not None: + bridge.shutdown() + 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_control_calibration_real/README.md b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/README.md index 16a2d1f..a6ef3a4 100644 --- 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 @@ -95,7 +95,15 @@ python3 src/site_deployment/workshop_control_calibration_real/capture_control_se 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` 和必要的外部真值质量门限。 +该文件按 `ackermann`、`differential`、`single_steer_wheel`、`multi_steer_wheel` 拆分，固定每类底盘建议跑的轨迹跟踪、速度阶跃、加减速和停车精度任务。每个任务同时声明 `control_axis`、`controller_algorithm`、`control_role`、`reference_path_id` 和必要的外部真值质量门限。 + +运控参考路径单独放在： + +```bash +src/site_deployment/workshop_control_calibration_real/reference_paths/ +``` + +每条路径一个 YAML 文件，必须配置 `path_id` 和中文 `display_name`。导出 `requested_tasks` 时，`reference_path_id` 会被展开成 `reference_path.*` metadata；轨迹跟踪任务还会同步展开为现有 `traj_pt_*` 字段，供运控执行链路读取。 校验并查看摘要： 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 index c5c3dc4..5e8bf82 100644 --- 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 @@ -4,6 +4,7 @@ schema_version: 1 profile_name: workshop_control_evaluation_profile +reference_path_dir: ../reference_paths safety: max_linear_speed_ms: 0.2 @@ -34,6 +35,7 @@ chassis_profiles: - task_code: control.ackermann.path_tracking_s_curve display_name: 阿克曼低速 S 形轨迹跟踪 selected_task: trajectory_tracking + reference_path_id: control_s_curve_low_speed control_axis: combined controller_algorithm: pure_pursuit control_role: path_tracking_outer_loop @@ -47,26 +49,10 @@ chassis_profiles: 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 + reference_path_id: control_line_low_speed control_axis: longitudinal_control controller_algorithm: pid control_role: speed_loop @@ -78,6 +64,7 @@ chassis_profiles: - task_code: control.ackermann.steering_response_arc display_name: 阿克曼转角响应轨迹 selected_task: trajectory_tracking + reference_path_id: control_arc_low_speed control_axis: lateral_control controller_algorithm: pid control_role: steering_angle_inner_loop @@ -89,22 +76,10 @@ chassis_profiles: 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 + reference_path_id: control_stop_accuracy_1_2m control_axis: longitudinal_control controller_algorithm: pid control_role: speed_loop @@ -126,6 +101,7 @@ chassis_profiles: - task_code: control.differential.path_tracking_line display_name: 差速低速直线轨迹跟踪 selected_task: trajectory_tracking + reference_path_id: control_line_low_speed control_axis: combined controller_algorithm: pure_pursuit control_role: path_tracking_outer_loop @@ -136,22 +112,10 @@ chassis_profiles: 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 + reference_path_id: control_line_low_speed control_axis: longitudinal_control controller_algorithm: pid control_role: speed_loop @@ -163,6 +127,7 @@ chassis_profiles: - task_code: control.differential.yaw_rate_arc display_name: 差速角速度响应圆弧 selected_task: trajectory_tracking + reference_path_id: control_arc_low_speed control_axis: lateral_control controller_algorithm: pid control_role: yaw_rate_loop @@ -174,22 +139,10 @@ chassis_profiles: 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 + reference_path_id: control_line_low_speed control_axis: longitudinal_control controller_algorithm: pid control_role: acceleration_loop @@ -211,6 +164,7 @@ chassis_profiles: - task_code: control.single_steer.path_tracking_arc display_name: 单舵轮低速圆弧轨迹跟踪 selected_task: trajectory_tracking + reference_path_id: control_arc_low_speed control_axis: combined controller_algorithm: pure_pursuit control_role: path_tracking_outer_loop @@ -221,22 +175,10 @@ chassis_profiles: 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 + reference_path_id: control_line_low_speed control_axis: longitudinal_control controller_algorithm: pid control_role: speed_loop @@ -248,6 +190,7 @@ chassis_profiles: - task_code: control.single_steer.steering_response_s_curve display_name: 单舵轮舵角响应 S 形轨迹 selected_task: trajectory_tracking + reference_path_id: control_s_curve_low_speed control_axis: lateral_control controller_algorithm: pid control_role: steering_angle_inner_loop @@ -259,26 +202,10 @@ chassis_profiles: 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 + reference_path_id: control_stop_accuracy_1_2m control_axis: longitudinal_control controller_algorithm: pid control_role: speed_loop @@ -300,6 +227,7 @@ chassis_profiles: - task_code: control.multi_steer.path_tracking_lateral_offset display_name: 多舵轮横向偏移轨迹跟踪 selected_task: trajectory_tracking + reference_path_id: control_lateral_offset_low_speed control_axis: combined controller_algorithm: mpc control_role: path_tracking_outer_loop @@ -310,26 +238,10 @@ chassis_profiles: 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 + reference_path_id: control_line_low_speed control_axis: longitudinal_control controller_algorithm: pid control_role: wheel_speed_inner_loop @@ -341,6 +253,7 @@ chassis_profiles: - task_code: control.multi_steer.module_steering_response display_name: 多舵轮模块转角响应轨迹 selected_task: trajectory_tracking + reference_path_id: control_lateral_offset_low_speed control_axis: lateral_control controller_algorithm: pid control_role: module_steering_inner_loop @@ -352,26 +265,10 @@ chassis_profiles: 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 + reference_path_id: control_stop_accuracy_1_2m control_axis: longitudinal_control controller_algorithm: pid control_role: wheel_speed_inner_loop 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 index 36c9b2a..edf31a4 100644 --- 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 @@ -16,6 +16,12 @@ except ImportError as exc: SCRIPT_DIR = Path(__file__).resolve().parent if str(SCRIPT_DIR) not in sys.path: sys.path.insert(0, str(SCRIPT_DIR)) +REFERENCE_PATH_TOOL_DIR = SCRIPT_DIR.parents[0] / "workshop_reference_paths" +if str(REFERENCE_PATH_TOOL_DIR) not in sys.path: + sys.path.insert(0, str(REFERENCE_PATH_TOOL_DIR)) + +from calibration_reference_paths import path_points_as_trajectory # noqa: E402 +from calibration_reference_paths import selected_path_metadata # noqa: E402 from stage_control_parameter_commit import ( CHASSIS_TYPES, @@ -204,25 +210,31 @@ def validate_trajectory_payload( payload: dict[str, Any], action_name: str, safety: dict[str, float], + allow_reference_path: bool = False, ) -> 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 个轨迹点。") + raw_path = payload.get("path") + if raw_path in (None, ""): + if not allow_reference_path: + raise ValueError(f"{action_name}.trajectory_tracking.path 不能为空，或在动作上配置 reference_path_id。") + else: + path = require_list(raw_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, - ) + 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( @@ -289,8 +301,15 @@ def validate_action( payload_key = TASK_PAYLOAD_KEYS[selected_task] payload = require_map(action.get(payload_key), f"{action_name}.{payload_key}") + if action.get("reference_path_id") not in (None, ""): + require_string(action.get("reference_path_id"), f"{action_name}.reference_path_id") if selected_task == "trajectory_tracking": - validate_trajectory_payload(payload, action_name, safety) + validate_trajectory_payload( + payload, + action_name, + safety, + allow_reference_path=action.get("reference_path_id") not in (None, ""), + ) elif selected_task == "velocity_step": validate_velocity_step_payload(payload, action_name, safety) elif selected_task in {"acceleration_deceleration", "accel_decel"}: @@ -349,7 +368,11 @@ def add_if_present(metadata: dict[str, Any], key: str, payload: dict[str, Any], metadata[key] = payload[source_key] -def flatten_trajectory_payload(metadata: dict[str, Any], payload: dict[str, Any]) -> None: +def flatten_trajectory_payload( + metadata: dict[str, Any], + payload: dict[str, Any], + selected_path_points: list[dict[str, float]] | None = None, +) -> 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") @@ -360,17 +383,30 @@ def flatten_trajectory_payload(metadata: dict[str, Any], payload: dict[str, Any] 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"]): + path = selected_path_points if selected_path_points is not None else payload["path"] + for index, point in enumerate(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]: +def flatten_action_metadata( + action: dict[str, Any], + chassis_type: str, + profile: dict[str, Any], + profile_path: Path | None = None, +) -> dict[str, Any]: selected_task = action["selected_task"] payload_key = TASK_PAYLOAD_KEYS[selected_task] payload = action[payload_key] + reference_metadata, selected_path = selected_path_metadata( + profile.get("reference_path_dir", "reference_paths"), + profile_path, + str(action.get("reference_path_id", "")), + chassis_type, + TASK_TYPE_TO_METADATA[selected_task], + ) metadata: dict[str, Any] = { "control.task_type": TASK_TYPE_TO_METADATA[selected_task], "control.chassis_type": chassis_type, @@ -379,9 +415,13 @@ def flatten_action_metadata(action: dict[str, Any], chassis_type: str) -> dict[s "control.role": action["control_role"], } add_if_present(metadata, "control.loop_name", action, "loop_name") + metadata.update(reference_metadata) if selected_task == "trajectory_tracking": - flatten_trajectory_payload(metadata, payload) + selected_points = path_points_as_trajectory(selected_path) + if len(selected_points) < 2: + raise ValueError(f"{action['task_code']} 选择的 reference_path 至少需要 2 个点。") + flatten_trajectory_payload(metadata, payload, selected_points) 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"] @@ -399,11 +439,15 @@ def flatten_action_metadata(action: dict[str, Any], chassis_type: str) -> dict[s return metadata -def export_requested_tasks(profile: dict[str, Any], chassis_type: str) -> dict[str, Any]: +def export_requested_tasks( + profile: dict[str, Any], + chassis_type: str, + profile_path: Path | None = None, +) -> 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) + metadata = flatten_action_metadata(action, chassis_type, profile, profile_path) task_params = [make_task_param(key, metadata[key]) for key in sorted(metadata)] tasks.append({ "stage_type": "CONTROL_CALIBRATION_STAGE", @@ -458,7 +502,7 @@ def main() -> int: 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) + output = export_requested_tasks(profile, args.chassis_type, profile_path) else: output = build_summary(profile) diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_arc_low_speed.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_arc_low_speed.yaml new file mode 100644 index 0000000..47120d2 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_arc_low_speed.yaml @@ -0,0 +1,21 @@ +schema_version: 1 +path_id: control_arc_low_speed +display_name: 运控低速圆弧跟踪 +module_type: control +frame_id: workshop +path_type: arc +recommended_task_types: + - trajectory_tracking +points: + - 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 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_lateral_offset_low_speed.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_lateral_offset_low_speed.yaml new file mode 100644 index 0000000..c66470e --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_lateral_offset_low_speed.yaml @@ -0,0 +1,25 @@ +schema_version: 1 +path_id: control_lateral_offset_low_speed +display_name: 运控横向偏移低速跟踪 +module_type: control +frame_id: workshop +path_type: lateral_offset +recommended_task_types: + - trajectory_tracking +points: + - 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 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_line_low_speed.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_line_low_speed.yaml new file mode 100644 index 0000000..2dff8e4 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_line_low_speed.yaml @@ -0,0 +1,23 @@ +schema_version: 1 +path_id: control_line_low_speed +display_name: 运控低速直线跟踪 +module_type: control +frame_id: workshop +path_type: straight_line +recommended_task_types: + - trajectory_tracking + - velocity_step + - accel_decel +points: + - 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 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_s_curve_low_speed.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_s_curve_low_speed.yaml new file mode 100644 index 0000000..cf652fd --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_s_curve_low_speed.yaml @@ -0,0 +1,25 @@ +schema_version: 1 +path_id: control_s_curve_low_speed +display_name: 运控低速 S 形跟踪 +module_type: control +frame_id: workshop +path_type: s_curve +recommended_task_types: + - trajectory_tracking +points: + - 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 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_stop_accuracy_1_2m.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_stop_accuracy_1_2m.yaml new file mode 100644 index 0000000..0cb3325 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/control_stop_accuracy_1_2m.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: control_stop_accuracy_1_2m +display_name: 运控停车精度 1.2m +module_type: control +frame_id: workshop +path_type: stop_accuracy +recommended_task_types: + - stop_accuracy +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.10 + - x_m: 1.2 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/index.yaml b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/index.yaml new file mode 100644 index 0000000..de87bd3 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_control_calibration_real/reference_paths/index.yaml @@ -0,0 +1,15 @@ +profile_name: control_calibration_reference_paths +frame_id: workshop + +default_path_id_by_chassis: + ackermann: control_s_curve_low_speed + differential: control_line_low_speed + single_steer_wheel: control_arc_low_speed + multi_steer_wheel: control_lateral_offset_low_speed + +default_path_id_by_task_type: + trajectory_tracking: control_s_curve_low_speed + velocity_step: control_line_low_speed + accel_decel: control_line_low_speed + acceleration_deceleration: control_line_low_speed + stop_accuracy: control_stop_accuracy_1_2m diff --git a/agv_calib_brain/src/site_deployment/workshop_reference_paths/README.md b/agv_calib_brain/src/site_deployment/workshop_reference_paths/README.md new file mode 100644 index 0000000..98e1c48 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_reference_paths/README.md @@ -0,0 +1,51 @@ +# 标定参考路径工具 + +三类标定任务都使用同一套路径 YAML 协议，但路径文件分别放在各自模块目录： + +- `src/site_deployment/workshop_chassis_calibration_real/reference_paths/` +- `src/site_deployment/workshop_control_calibration_real/reference_paths/` +- `src/site_deployment/workshop_sensor_calibration_real/reference_paths/` + +每条路径一个 YAML 文件，必须包含 `path_id` 和中文 `display_name`。`index.yaml` 只负责给 `chassis_type` 或任务类型配置默认路径，不存放路径点。 + +查看某个目录中的可选路径： + +```bash +python3 src/site_deployment/workshop_reference_paths/calibration_reference_paths.py \ + --path-dir src/site_deployment/workshop_control_calibration_real/reference_paths +``` + +导出一条路径的总控 metadata： + +```bash +python3 src/site_deployment/workshop_reference_paths/calibration_reference_paths.py \ + --path-dir src/site_deployment/workshop_control_calibration_real/reference_paths \ + --path-id control_s_curve_low_speed \ + --format metadata +``` + +从外部真值话题录制新路径： + +```bash +python3 src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py \ + --output-dir src/site_deployment/workshop_control_calibration_real/reference_paths \ + --path-id site_a_control_s_curve \ + --display-name 现场A运控S形路径 \ + --module-type control \ + --source external_pose \ + --duration-sec 20.0 \ + --target-speed-ms 0.10 \ + --recommended-task-type trajectory_tracking +``` + +也可以从已有 CSV 生成路径，CSV 支持 `x_m/y_m/yaw_rad` 或 `odom_x_m/odom_y_m/odom_yaw_rad`： + +```bash +python3 src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py \ + --output-dir src/site_deployment/workshop_chassis_calibration_real/reference_paths \ + --path-id site_a_chassis_line \ + --display-name 现场A底盘直线路径 \ + --module-type chassis \ + --input-csv /data/agv_calib/site_a/session_001/external/truth_trajectory.csv \ + --recommended-task-type straight_line +``` diff --git a/agv_calib_brain/src/site_deployment/workshop_reference_paths/calibration_reference_paths.py b/agv_calib_brain/src/site_deployment/workshop_reference_paths/calibration_reference_paths.py new file mode 100644 index 0000000..fc690ea --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_reference_paths/calibration_reference_paths.py @@ -0,0 +1,377 @@ +#!/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 +SUPPORTED_CHASSIS_TYPES = { + "ackermann", + "differential", + "single_steer_wheel", + "multi_steer_wheel", +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="校验并导出标定参考路径目录。") + parser.add_argument( + "--path-dir", + required=True, + help="参考路径 YAML 文件目录。目录中每条路径一个 YAML 文件。", + ) + parser.add_argument("--path-id", default="", help="要导出的路径 ID。") + parser.add_argument( + "--chassis-type", + choices=sorted(SUPPORTED_CHASSIS_TYPES), + default="", + help="未指定 --path-id 时，从目录 index.yaml 的 default_path_id_by_chassis 中选择。", + ) + parser.add_argument( + "--task-type", + default="", + help="未指定 --path-id/--chassis-type 时，从目录 index.yaml 的 default_path_id_by_task_type 中选择。", + ) + parser.add_argument( + "--format", + choices=["summary", "metadata", "path"], + default="summary", + help="summary 输出摘要；metadata 输出总控 task_params；path 输出单条路径。", + ) + parser.add_argument("-o", "--output", default="", 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, float): + return f"{value:.9g}" + return str(value) + + +def resolve_path_dir(raw_path: str | Path, owner_profile_path: Path | None = None) -> Path: + path = Path(str(raw_path)).expanduser() + if path.is_absolute(): + return path.resolve(strict=False) + if owner_profile_path is not None: + owner_relative = (owner_profile_path.parent / path).resolve(strict=False) + if owner_relative.exists(): + return owner_relative + return path.resolve(strict=False) + + +def path_yaml_files(path_dir: Path) -> list[Path]: + if not path_dir.exists(): + raise FileNotFoundError(f"参考路径目录不存在: {path_dir}") + if not path_dir.is_dir(): + raise ValueError(f"参考路径路径必须是目录: {path_dir}") + return sorted( + path + for path in path_dir.glob("*.yaml") + if path.name not in {"index.yaml", "_index.yaml"} + ) + + +def load_path_index(path_dir: Path) -> dict[str, Any]: + for filename in ("index.yaml", "_index.yaml"): + index_path = path_dir / filename + if index_path.exists(): + return load_yaml(index_path) + return {} + + +def validate_point(point_value: Any, path_name: str, point_index: int) -> None: + point = require_map(point_value, f"{path_name}.points[{point_index}]") + as_float(point.get("x_m"), f"{path_name}.points[{point_index}].x_m") + as_float(point.get("y_m"), f"{path_name}.points[{point_index}].y_m") + if "z_m" in point: + as_float(point.get("z_m"), f"{path_name}.points[{point_index}].z_m") + if "yaw_rad" in point: + as_float(point.get("yaw_rad"), f"{path_name}.points[{point_index}].yaw_rad") + if "target_speed_ms" in point: + speed = as_float(point.get("target_speed_ms"), f"{path_name}.points[{point_index}].target_speed_ms") + if speed < 0.0: + raise ValueError(f"{path_name}.points[{point_index}].target_speed_ms 不能小于 0。") + + +def validate_reference_path(path: dict[str, Any], source_path: Path | None = None) -> dict[str, Any]: + schema_version = int(path.get("schema_version", 0)) + if schema_version != 1: + raise ValueError(f"{source_path or path.get('path_id', '')} schema_version 必须为 1。") + path_id = require_string(path.get("path_id"), "path_id") + require_string(path.get("display_name"), f"{path_id}.display_name") + if "frame_id" in path: + require_string(path.get("frame_id"), f"{path_id}.frame_id") + if "path_type" in path: + require_string(path.get("path_type"), f"{path_id}.path_type") + if "module_type" in path: + require_string(path.get("module_type"), f"{path_id}.module_type") + points = require_list(path.get("points"), f"{path_id}.points") + if not points: + raise ValueError(f"{path_id}.points 不能为空。") + for index, point in enumerate(points): + validate_point(point, path_id, index) + for task_type in path.get("recommended_task_types", []) or []: + require_string(task_type, f"{path_id}.recommended_task_types[]") + if source_path is not None: + path["_source_file"] = str(source_path.resolve(strict=False)) + return path + + +def load_reference_path_dir(raw_path_dir: str | Path, owner_profile_path: Path | None = None) -> tuple[dict[str, Any], Path]: + path_dir = resolve_path_dir(raw_path_dir, owner_profile_path) + index = load_path_index(path_dir) + paths: list[dict[str, Any]] = [] + seen: set[str] = set() + for path_file in path_yaml_files(path_dir): + path = validate_reference_path(load_yaml(path_file), path_file) + path_id = str(path["path_id"]) + if path_id in seen: + raise ValueError(f"重复的 path_id：{path_id}") + seen.add(path_id) + paths.append(path) + if not paths: + raise ValueError(f"参考路径目录没有路径 YAML: {path_dir}") + profile = { + "schema_version": 1, + "profile_name": index.get("profile_name", path_dir.name), + "frame_id": index.get("frame_id", "workshop"), + "default_path_id_by_chassis": index.get("default_path_id_by_chassis", {}) or {}, + "default_path_id_by_task_type": index.get("default_path_id_by_task_type", {}) or {}, + "paths": paths, + } + validate_path_dir_profile(profile) + return profile, path_dir + + +def validate_path_dir_profile(profile: dict[str, Any]) -> dict[str, Any]: + indexed = path_map(profile) + defaults_by_chassis = require_map( + profile.get("default_path_id_by_chassis", {}) or {}, + "default_path_id_by_chassis", + ) + for chassis_type, path_id in defaults_by_chassis.items(): + if chassis_type not in SUPPORTED_CHASSIS_TYPES: + raise ValueError(f"default_path_id_by_chassis 包含未知底盘类型：{chassis_type}") + if str(path_id) not in indexed: + raise ValueError(f"default_path_id_by_chassis.{chassis_type} 引用了不存在的路径：{path_id}") + defaults_by_task_type = require_map( + profile.get("default_path_id_by_task_type", {}) or {}, + "default_path_id_by_task_type", + ) + for task_type, path_id in defaults_by_task_type.items(): + require_string(str(task_type), "default_path_id_by_task_type key") + if str(path_id) not in indexed: + raise ValueError(f"default_path_id_by_task_type.{task_type} 引用了不存在的路径：{path_id}") + return profile + + +def path_map(profile: dict[str, Any]) -> dict[str, dict[str, Any]]: + indexed: dict[str, dict[str, Any]] = {} + for path in require_list(profile.get("paths"), "paths"): + path_id = require_string(path.get("path_id"), "path_id") + indexed[path_id] = path + return indexed + + +def select_path( + profile: dict[str, Any], + path_id: str = "", + chassis_type: str = "", + task_type: str = "", +) -> dict[str, Any]: + indexed = path_map(profile) + selected_id = path_id.strip() + if not selected_id and chassis_type: + selected_id = str((profile.get("default_path_id_by_chassis", {}) or {}).get(chassis_type, "")) + if not selected_id and task_type: + selected_id = str((profile.get("default_path_id_by_task_type", {}) or {}).get(task_type, "")) + if not selected_id: + raise ValueError("未指定 path_id，也没有可用的默认参考路径。") + if selected_id not in indexed: + raise ValueError(f"参考路径不存在：{selected_id}") + return indexed[selected_id] + + +def flatten_path_metadata(path: dict[str, Any], profile: dict[str, Any], path_dir: Path | None = None) -> dict[str, str]: + points = require_list(path.get("points"), f"{path.get('path_id', '')}.points") + metadata: dict[str, str] = { + "reference_path.id": require_string(path.get("path_id"), "path_id"), + "reference_path.display_name": str(path.get("display_name", path.get("path_id", ""))), + "reference_path.frame_id": str(path.get("frame_id", profile.get("frame_id", "workshop"))), + "reference_path.path_type": str(path.get("path_type", "polyline")), + "reference_path.point_count": str(len(points)), + } + if path_dir is not None: + metadata["reference_path.path_dir"] = str(path_dir) + if path.get("_source_file"): + metadata["reference_path.file"] = str(path["_source_file"]) + if path.get("module_type"): + metadata["reference_path.module_type"] = str(path["module_type"]) + if path.get("description"): + metadata["reference_path.description"] = str(path["description"]) + + for index, point_value in enumerate(points): + point = require_map(point_value, f"{path['path_id']}.points[{index}]") + prefix = f"reference_path.pt_{index}" + metadata[f"{prefix}_x_m"] = stringify_value(as_float(point.get("x_m"), f"{prefix}_x_m")) + metadata[f"{prefix}_y_m"] = stringify_value(as_float(point.get("y_m"), f"{prefix}_y_m")) + metadata[f"{prefix}_z_m"] = stringify_value(as_float(point.get("z_m", 0.0), f"{prefix}_z_m")) + metadata[f"{prefix}_yaw_rad"] = stringify_value(as_float(point.get("yaw_rad", 0.0), f"{prefix}_yaw_rad")) + metadata[f"{prefix}_speed_ms"] = stringify_value(as_float(point.get("target_speed_ms", 0.0), f"{prefix}_speed_ms")) + return metadata + + +def selected_path_metadata( + reference_path_dir: str | Path, + owner_profile_path: Path | None = None, + path_id: str = "", + chassis_type: str = "", + task_type: str = "", +) -> tuple[dict[str, str], dict[str, Any]]: + profile, path_dir = load_reference_path_dir(reference_path_dir, owner_profile_path) + selected = select_path(profile, path_id, chassis_type, task_type) + return flatten_path_metadata(selected, profile, path_dir), selected + + +def path_points_as_trajectory(path: dict[str, Any]) -> list[dict[str, float]]: + points = require_list(path.get("points"), f"{path.get('path_id', '')}.points") + trajectory: list[dict[str, float]] = [] + for index, point_value in enumerate(points): + point = require_map(point_value, f"{path.get('path_id', '')}.points[{index}]") + trajectory.append({ + "x_m": as_float(point.get("x_m"), f"points[{index}].x_m"), + "y_m": as_float(point.get("y_m"), f"points[{index}].y_m"), + "yaw_rad": as_float(point.get("yaw_rad", 0.0), f"points[{index}].yaw_rad"), + "target_speed_ms": as_float( + point.get("target_speed_ms", 0.0), + f"points[{index}].target_speed_ms", + ), + }) + return trajectory + + +def make_task_param(key: str, value: Any) -> dict[str, str]: + return {"key": key, "value": stringify_value(value)} + + +def build_summary(profile: dict[str, Any], path_dir: Path) -> dict[str, Any]: + return { + "schema_version": profile["schema_version"], + "profile_name": profile.get("profile_name", ""), + "path_dir": str(path_dir), + "frame_id": profile.get("frame_id", "workshop"), + "default_path_id_by_chassis": profile.get("default_path_id_by_chassis", {}) or {}, + "default_path_id_by_task_type": profile.get("default_path_id_by_task_type", {}) or {}, + "paths": [ + { + "path_id": path["path_id"], + "display_name": path.get("display_name", ""), + "module_type": path.get("module_type", ""), + "path_type": path.get("path_type", "polyline"), + "point_count": len(path.get("points", []) or []), + "recommended_task_types": path.get("recommended_task_types", []) or [], + "file": path.get("_source_file", ""), + } + for path in profile["paths"] + ], + } + + +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_dir = load_reference_path_dir(args.path_dir) + + if args.format == "summary": + output = build_summary(profile, path_dir) + else: + selected = select_path(profile, args.path_id, args.chassis_type, args.task_type) + if args.format == "path": + output = { + "schema_version": 1, + "path_dir": str(path_dir), + "reference_path": { + key: value + for key, value in selected.items() + if not key.startswith("_") + }, + } + else: + metadata = flatten_path_metadata(selected, profile, path_dir) + output = { + "schema_version": 1, + "reference_path_id": metadata["reference_path.id"], + "display_name": metadata["reference_path.display_name"], + "task_params": [ + make_task_param(key, metadata[key]) + for key in sorted(metadata) + ], + } + + 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] 已写入参考路径输出：{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_reference_paths/record_calibration_reference_path.py b/agv_calib_brain/src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py new file mode 100644 index 0000000..5ac1ad5 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python3 +"""从 ROS 话题或 CSV 录制一条标定参考路径 YAML。""" + +from __future__ import annotations + +import argparse +import csv +import math +import signal +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 + +SCRIPT_DIR = Path(__file__).resolve().parent +if str(SCRIPT_DIR) not in sys.path: + sys.path.insert(0, str(SCRIPT_DIR)) + +from calibration_reference_paths import validate_reference_path # noqa: E402 + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="录制一条标定参考路径，并写成单独 YAML 文件。") + parser.add_argument("--output-dir", required=True, help="路径 YAML 输出目录，例如某模块的 reference_paths。") + parser.add_argument("--path-id", required=True, help="路径 ID，同时作为默认文件名。") + parser.add_argument("--display-name", required=True, help="中文路径名称，例如：底盘直线前进 1m。") + parser.add_argument( + "--module-type", + choices=["chassis", "control", "sensor"], + required=True, + help="路径归属模块。", + ) + parser.add_argument( + "--allowed-chassis-type", + choices=["ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"], + action="append", + default=[], + help="底盘路径允许出现在哪些底盘类型下，可重复传入。", + ) + parser.add_argument("--path-type", default="recorded", help="路径类型，例如 straight_line、arc、recorded。") + parser.add_argument("--frame-id", default="workshop", help="路径坐标系。") + parser.add_argument("--description", default="", help="路径说明。") + parser.add_argument( + "--recommended-task-type", + action="append", + default=[], + help="推荐使用的任务类型，可重复传入。", + ) + parser.add_argument("--target-speed-ms", type=float, default=0.0, help="写入路径点的默认目标速度。") + parser.add_argument("--duration-sec", type=float, default=0.0, help="ROS 录制时长；0 表示 Ctrl+C 结束。") + parser.add_argument("--min-distance-step-m", type=float, default=0.03, help="相邻保留点的最小平移距离。") + parser.add_argument("--min-yaw-step-rad", type=float, default=0.03, help="相邻保留点的最小 yaw 变化。") + parser.add_argument("--overwrite", action="store_true", help="允许覆盖同名路径文件。") + + source = parser.add_mutually_exclusive_group(required=True) + source.add_argument("--input-csv", default="", help="从 CSV 生成路径，不启动 ROS。") + source.add_argument( + "--source", + choices=["external_pose", "chassis_telemetry"], + help="从 ROS 话题录制路径。", + ) + parser.add_argument("--topic", default="", help="ROS 话题；不填时按 source 使用默认话题。") + return parser.parse_args() + + +def yaw_delta(a: float, b: float) -> float: + delta = (a - b + math.pi) % (2.0 * math.pi) - math.pi + return abs(delta) + + +def should_keep_point( + points: list[dict[str, float]], + point: dict[str, float], + min_distance_step_m: float, + min_yaw_step_rad: float, +) -> bool: + if not points: + return True + previous = points[-1] + distance = math.hypot(point["x_m"] - previous["x_m"], point["y_m"] - previous["y_m"]) + return distance >= min_distance_step_m or yaw_delta(point["yaw_rad"], previous["yaw_rad"]) >= min_yaw_step_rad + + +def append_decimated(points: list[dict[str, float]], point: dict[str, float], args: argparse.Namespace) -> None: + if should_keep_point(points, point, args.min_distance_step_m, args.min_yaw_step_rad): + points.append(point) + + +def csv_float(row: dict[str, str], keys: tuple[str, ...], default: float = 0.0) -> float: + for key in keys: + value = row.get(key) + if value not in (None, ""): + return float(value) + return default + + +def read_points_from_csv(path: Path, args: argparse.Namespace) -> list[dict[str, float]]: + points: list[dict[str, float]] = [] + with path.open("r", newline="", encoding="utf-8") as stream: + reader = csv.DictReader(stream) + for row in reader: + if row.get("pose_valid") not in (None, "", "1", "true", "True", "TRUE"): + continue + point = { + "x_m": csv_float(row, ("x_m", "odom_x_m")), + "y_m": csv_float(row, ("y_m", "odom_y_m")), + "z_m": csv_float(row, ("z_m",), 0.0), + "yaw_rad": csv_float(row, ("yaw_rad", "odom_yaw_rad"), 0.0), + "target_speed_ms": args.target_speed_ms, + } + append_decimated(points, point, args) + return points + + +def default_topic(source: str) -> str: + if source == "external_pose": + return "/workshop/external_localization/vehicle/pose" + if source == "chassis_telemetry": + return "/chassis/telemetry" + raise ValueError(f"不支持的 source: {source}") + + +def read_points_from_ros(args: argparse.Namespace) -> list[dict[str, float]]: + try: + import rclpy + 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 + + points: list[dict[str, float]] = [] + stop_requested = False + topic = args.topic or default_topic(args.source) + + def on_signal(signum: int, frame: Any) -> None: + nonlocal stop_requested + (void_signum, void_frame) = (signum, frame) + _ = (void_signum, void_frame) + stop_requested = True + + previous_sigint_handler = signal.signal(signal.SIGINT, on_signal) + previous_sigterm_handler = signal.signal(signal.SIGTERM, on_signal) + rclpy.init(args=None) + node = rclpy.create_node("calibration_reference_path_recorder") + + def on_external_pose(msg: Any) -> None: + if hasattr(msg, "pose_valid") and not bool(msg.pose_valid): + return + pose = msg.workshop_pose + point = { + "x_m": float(pose.x_m), + "y_m": float(pose.y_m), + "z_m": float(getattr(pose, "z_m", 0.0)), + "yaw_rad": float(pose.yaw_rad), + "target_speed_ms": args.target_speed_ms, + } + append_decimated(points, point, args) + + def on_chassis_telemetry(msg: Any) -> None: + point = { + "x_m": float(msg.odom_x_m), + "y_m": float(msg.odom_y_m), + "z_m": 0.0, + "yaw_rad": float(msg.odom_yaw_rad), + "target_speed_ms": args.target_speed_ms, + } + append_decimated(points, point, args) + + try: + if args.source == "external_pose": + node.create_subscription(ExternalLocalizationTelemetry, topic, on_external_pose, 50) + elif args.source == "chassis_telemetry": + node.create_subscription(ChassisTelemetry, topic, on_chassis_telemetry, 50) + else: + raise ValueError(f"不支持的 source: {args.source}") + + print(f"[*] 正在录制参考路径: topic={topic}, display_name={args.display_name}", file=sys.stderr) + start = time.monotonic() + while rclpy.ok() and not stop_requested: + rclpy.spin_once(node, timeout_sec=0.1) + if args.duration_sec > 0.0 and time.monotonic() - start >= args.duration_sec: + break + finally: + node.destroy_node() + rclpy.shutdown() + signal.signal(signal.SIGINT, previous_sigint_handler) + signal.signal(signal.SIGTERM, previous_sigterm_handler) + return points + + +def build_path_document(points: list[dict[str, float]], args: argparse.Namespace) -> dict[str, Any]: + if not points: + raise ValueError("没有录到任何路径点。") + document: dict[str, Any] = { + "schema_version": 1, + "path_id": args.path_id, + "display_name": args.display_name, + "module_type": args.module_type, + "frame_id": args.frame_id, + "path_type": args.path_type, + "points": points, + } + if args.description: + document["description"] = args.description + if args.recommended_task_type: + document["recommended_task_types"] = args.recommended_task_type + if args.module_type == "chassis" and args.allowed_chassis_type: + document["allowed_chassis_types"] = sorted(set(args.allowed_chassis_type)) + validate_reference_path(dict(document)) + return document + + +def write_path_document(document: dict[str, Any], args: argparse.Namespace) -> Path: + output_dir = Path(args.output_dir).expanduser().resolve(strict=False) + output_dir.mkdir(parents=True, exist_ok=True) + output_path = output_dir / f"{args.path_id}.yaml" + if output_path.exists() and not args.overwrite: + raise FileExistsError(f"路径文件已存在，若要覆盖请加 --overwrite: {output_path}") + output_path.write_text( + yaml.safe_dump(document, sort_keys=False, allow_unicode=True), + encoding="utf-8", + ) + return output_path + + +def main() -> int: + args = parse_args() + if args.input_csv: + points = read_points_from_csv(Path(args.input_csv).expanduser().resolve(strict=False), args) + else: + points = read_points_from_ros(args) + document = build_path_document(points, args) + output_path = write_path_document(document, args) + print(f"[OK] 已写入参考路径: {output_path}", file=sys.stderr) + print(yaml.safe_dump({"path_file": str(output_path), "point_count": len(points)}, 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/site_deployment/workshop_sensor_calibration_real/README.md b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/README.md index 2be5436..5d6c327 100644 --- 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 @@ -23,6 +23,14 @@ src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml ``` +传感器参考路径单独放在： + +```bash +src/site_deployment/workshop_sensor_calibration_real/reference_paths/ +``` + +每条路径一个 YAML 文件，必须配置 `path_id` 和中文 `display_name`。`sensor_calibration_profile.yaml` 中每个任务通过 `reference_path_id` 选择路径，导出任务时会自动带上 `reference_path.*` metadata，便于 UI 或 Isaac 显示当前标定采集路径。 + 校验并查看摘要： ```bash diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml index b6a6133..06db4db 100644 --- a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml @@ -4,6 +4,7 @@ schema_version: 1 profile_name: workshop_sensor_calibration_profile +reference_path_dir: ../reference_paths data_capture: required_data_inputs: @@ -25,6 +26,7 @@ tasks: display_name: 前视相机内参 enabled: true selected_task: camera_intrinsic + reference_path_id: sensor_static_front_station sensor_id: demo_front_camera task_subtype: front_camera_intrinsic camera_intrinsic: @@ -36,6 +38,7 @@ tasks: display_name: 下视相机内参 enabled: true selected_task: camera_intrinsic + reference_path_id: sensor_static_front_station sensor_id: demo_down_camera task_subtype: downward_camera_intrinsic camera_intrinsic: @@ -47,6 +50,7 @@ tasks: display_name: IMU 内参 enabled: true selected_task: imu_intrinsic + reference_path_id: sensor_imu_motion_line sensor_id: demo_imu task_subtype: imu_intrinsic imu_intrinsic: @@ -58,6 +62,7 @@ tasks: display_name: 前视相机到 base_link 外参 enabled: true selected_task: sensor_extrinsic + reference_path_id: sensor_extrinsic_slow_line sensor_id: demo_front_camera task_subtype: front_camera_extrinsic sensor_extrinsic: @@ -69,6 +74,7 @@ tasks: display_name: 下视相机到 base_link 外参 enabled: true selected_task: sensor_extrinsic + reference_path_id: sensor_extrinsic_slow_line sensor_id: demo_down_camera task_subtype: downward_camera_extrinsic sensor_extrinsic: @@ -80,6 +86,7 @@ tasks: display_name: 2D LiDAR 到 base_link 外参 enabled: true selected_task: sensor_extrinsic + reference_path_id: sensor_extrinsic_slow_line sensor_id: demo_lidar_2d task_subtype: lidar_2d_extrinsic sensor_extrinsic: @@ -91,6 +98,7 @@ tasks: display_name: 3D LiDAR 到 base_link 外参 enabled: true selected_task: sensor_extrinsic + reference_path_id: sensor_extrinsic_slow_line sensor_id: demo_lidar_3d task_subtype: lidar_3d_extrinsic sensor_extrinsic: @@ -102,6 +110,7 @@ tasks: display_name: IMU 到 base_link 外参 enabled: true selected_task: sensor_extrinsic + reference_path_id: sensor_extrinsic_slow_line sensor_id: demo_imu task_subtype: imu_extrinsic sensor_extrinsic: @@ -113,6 +122,7 @@ tasks: display_name: 手眼相机眼在手上 enabled: true selected_task: hand_eye + reference_path_id: hand_eye_pose_sweep sensor_id: demo_arm_camera task_subtype: eye_in_hand hand_eye: diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/hand_eye_pose_sweep.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/hand_eye_pose_sweep.yaml new file mode 100644 index 0000000..48127d1 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/hand_eye_pose_sweep.yaml @@ -0,0 +1,21 @@ +schema_version: 1 +path_id: hand_eye_pose_sweep +display_name: 手眼标定位姿序列 +module_type: sensor +frame_id: workshop +path_type: pose_sweep +recommended_task_types: + - hand_eye +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.0 + - x_m: 0.15 + y_m: 0.0 + yaw_rad: 0.15 + target_speed_ms: 0.0 + - x_m: -0.15 + y_m: 0.0 + yaw_rad: -0.15 + target_speed_ms: 0.0 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/index.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/index.yaml new file mode 100644 index 0000000..ad95c51 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/index.yaml @@ -0,0 +1,8 @@ +profile_name: sensor_calibration_reference_paths +frame_id: workshop + +default_path_id_by_task_type: + camera_intrinsic: sensor_static_front_station + imu_intrinsic: sensor_imu_motion_line + sensor_extrinsic: sensor_extrinsic_slow_line + hand_eye: hand_eye_pose_sweep diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_extrinsic_slow_line.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_extrinsic_slow_line.yaml new file mode 100644 index 0000000..2a675af --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_extrinsic_slow_line.yaml @@ -0,0 +1,21 @@ +schema_version: 1 +path_id: sensor_extrinsic_slow_line +display_name: 传感器外参低速直线采样 +module_type: sensor +frame_id: workshop +path_type: sampling_line +recommended_task_types: + - sensor_extrinsic +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.05 + - x_m: 0.4 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.05 + - x_m: 0.8 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.05 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_imu_motion_line.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_imu_motion_line.yaml new file mode 100644 index 0000000..98a2b2b --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_imu_motion_line.yaml @@ -0,0 +1,17 @@ +schema_version: 1 +path_id: sensor_imu_motion_line +display_name: IMU 内参低速运动段 +module_type: sensor +frame_id: workshop +path_type: imu_motion +recommended_task_types: + - imu_intrinsic +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.05 + - x_m: 0.5 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.05 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_static_front_station.yaml b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_static_front_station.yaml new file mode 100644 index 0000000..8ff41a0 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_calibration_real/reference_paths/sensor_static_front_station.yaml @@ -0,0 +1,13 @@ +schema_version: 1 +path_id: sensor_static_front_station +display_name: 传感器静态采集点 +module_type: sensor +frame_id: workshop +path_type: static_station +recommended_task_types: + - camera_intrinsic +points: + - x_m: 0.0 + y_m: 0.0 + yaw_rad: 0.0 + target_speed_ms: 0.0 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 index 4b61736..23b1d18 100644 --- 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 @@ -13,6 +13,13 @@ try: except ImportError as exc: raise SystemExit("缺少 PyYAML，请先在当前 Python 环境安装 yaml 模块。") from exc +SCRIPT_DIR = Path(__file__).resolve().parent +REFERENCE_PATH_TOOL_DIR = SCRIPT_DIR.parents[0] / "workshop_reference_paths" +if str(REFERENCE_PATH_TOOL_DIR) not in sys.path: + sys.path.insert(0, str(REFERENCE_PATH_TOOL_DIR)) + +from calibration_reference_paths import selected_path_metadata # noqa: E402 + TASK_GROUPS = { "camera_intrinsic", @@ -224,6 +231,8 @@ def validate_task(task: dict[str, Any], index: int) -> None: raise ValueError( f"{task_name}.task_subtype={task_subtype} 不适用于 selected_task={selected_task}。" ) + if task.get("reference_path_id") not in (None, ""): + require_string(task.get("reference_path_id"), f"{task_name}.reference_path_id") if selected_task == "camera_intrinsic": validate_camera_intrinsic(task, task_name) @@ -276,12 +285,24 @@ def task_matches_requested(task: dict[str, Any], requested_tasks: set[str]) -> b return TASK_GROUP_TO_REQUESTED_TASK[selected_task] in requested_tasks -def flatten_task_metadata(task: dict[str, Any]) -> list[dict[str, str]]: +def flatten_task_metadata( + task: dict[str, Any], + profile: dict[str, Any], + profile_path: Path | None = None, +) -> list[dict[str, str]]: selected_task = str(task["selected_task"]) + reference_metadata, _ = selected_path_metadata( + profile.get("reference_path_dir", "reference_paths"), + profile_path, + str(task.get("reference_path_id", "")), + "", + selected_task, + ) params = [ make_task_param("sensor.sensor_id", task["sensor_id"]), make_task_param("sensor.task_subtype", task["task_subtype"]), ] + params.extend(make_task_param(key, reference_metadata[key]) for key in sorted(reference_metadata)) if selected_task == "camera_intrinsic": payload = task["camera_intrinsic"] @@ -324,6 +345,7 @@ def flatten_task_metadata(task: dict[str, Any]) -> list[dict[str, str]]: def export_requested_tasks( profile: dict[str, Any], requested_tasks: set[str] | list[str] | str | None = None, + profile_path: Path | None = None, ) -> dict[str, Any]: selected_requested_tasks = ( requested_tasks @@ -345,7 +367,7 @@ def export_requested_tasks( "reason": str(task.get("reason", "现场传感器标定 profile")), "task_code": str(task["task_code"]), "target_id": str(task["sensor_id"]), - "task_params": flatten_task_metadata(task), + "task_params": flatten_task_metadata(task, profile, profile_path), }) return { "schema_version": 1, @@ -382,7 +404,7 @@ def main() -> int: profile = validate_profile(load_yaml(profile_path)) if args.format == "requested_tasks": - output = export_requested_tasks(profile, parse_requested_tasks(args.tasks)) + output = export_requested_tasks(profile, parse_requested_tasks(args.tasks), profile_path) else: output = build_summary(profile)