diff --git a/agv_calib_brain/.codex b/agv_calib_brain/.codex deleted file mode 100644 index e69de29..0000000 diff --git a/agv_calib_brain/.gitignore b/agv_calib_brain/.gitignore new file mode 100644 index 0000000..43ae0e2 --- /dev/null +++ b/agv_calib_brain/.gitignore @@ -0,0 +1,2 @@ +__pycache__/ +*.py[cod] diff --git a/agv_calib_brain/run_isaac_real_sim_test.sh b/agv_calib_brain/run_isaac_real_sim_test.sh index ceae4a7..d99c304 100755 --- a/agv_calib_brain/run_isaac_real_sim_test.sh +++ b/agv_calib_brain/run_isaac_real_sim_test.sh @@ -15,16 +15,21 @@ CONDA_ENV_NAME="${CONDA_ENV_NAME:-AutoCalib_Workshop}" ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}" RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/isaac_real_sim_$(date +%Y%m%d_%H%M%S)}" -HEADLESS=0 +HEADLESS=1 DO_BUILD=0 RUN_SMOKE=1 KEEP_RUNNING=0 WITH_SENSOR_INGEST=1 WAIT_FOR_ISAAC_TOPICS=1 +WORKSHOP_USE_GATEWAY=1 ISAAC_WAIT_SEC="${ISAAC_WAIT_SEC:-180}" WORKSHOP_WARMUP_SEC="${WORKSHOP_WARMUP_SEC:-5}" START_GRACE_SEC="${START_GRACE_SEC:-2}" +STOP_GRACE_SEC="${STOP_GRACE_SEC:-8}" +TERM_GRACE_SEC="${TERM_GRACE_SEC:-3}" SMOKE_TASKS="" +DATA_INPUT_PARAMS_FILE="" +DATASET_INDEX_FILE="" PIDS=() NAMES=() @@ -54,6 +59,12 @@ usage() { --keep-running 验收结束后保持仿真链路运行,按 Ctrl+C 停止。 --no-sensor-ingest 不启动车间侧传感器 ingest 轮询进程。 --skip-topic-check 不等待 Isaac 真值/传感器 topic,直接继续启动后续链路。 + --local-calibration-services + 底盘和运控阶段使用本地标定服务节点,便于验证四个算法服务的数据输入注入。 + --data-input-params-file PATH + 把真实采集/ingest 生成的 site_data_input.yaml 注入算法服务。 + --dataset-index-file PATH + 把真实采集/ingest 生成的 dataset_index.yaml 写入最终报告。 --isaac-wait-sec SEC 等待 Isaac topic 的最长时间,默认 180 秒。 --warmup-sec SEC 总控 demo 启动后等待采集真值历史的时间,默认 5 秒。 --tasks LIST 覆盖 smoke_test_workshop_orchestrator.py 的任务列表。 @@ -65,10 +76,13 @@ usage() { CONDA_ENV_NAME 同 --conda-env。 ROS_LOG_DIR ROS 日志目录,默认 /tmp/roslog。 RUN_LOG_ROOT 本脚本各后台进程日志目录。 + STOP_GRACE_SEC SIGINT 后等待后台进程优雅退出的秒数,默认 8。 + TERM_GRACE_SEC SIGTERM 后等待后台进程退出的秒数,默认 3。 示例: ./run_isaac_real_sim_test.sh --headless ./run_isaac_real_sim_test.sh --headless --keep-running + ./run_isaac_real_sim_test.sh --headless --local-calibration-services --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml ./run_isaac_real_sim_test.sh --no-smoke EOF } @@ -86,6 +100,18 @@ die() { exit 1 } +resolve_workspace_path() { + local path="$1" + case "${path}" in + /*) + echo "${path}" + ;; + *) + echo "${WORKSPACE_DIR}/${path}" + ;; + esac +} + while [[ $# -gt 0 ]]; do case "$1" in --headless) @@ -113,6 +139,20 @@ while [[ $# -gt 0 ]]; do WAIT_FOR_ISAAC_TOPICS=0 shift ;; + --local-calibration-services) + WORKSHOP_USE_GATEWAY=0 + shift + ;; + --data-input-params-file) + [[ $# -ge 2 ]] || die "--data-input-params-file 需要参数" + DATA_INPUT_PARAMS_FILE="$2" + shift 2 + ;; + --dataset-index-file) + [[ $# -ge 2 ]] || die "--dataset-index-file 需要参数" + DATASET_INDEX_FILE="$2" + shift 2 + ;; --isaac-wait-sec) [[ $# -ge 2 ]] || die "--isaac-wait-sec 需要参数" ISAAC_WAIT_SEC="$2" @@ -143,6 +183,26 @@ while [[ $# -gt 0 ]]; do esac done +if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then + DATA_INPUT_PARAMS_FILE="$(resolve_workspace_path "${DATA_INPUT_PARAMS_FILE}")" + [[ -f "${DATA_INPUT_PARAMS_FILE}" ]] || die "找不到数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}" + [[ "${DATA_INPUT_PARAMS_FILE}" != *"'"* ]] || die "data_input_params_file 路径不能包含单引号" + if [[ -z "${DATASET_INDEX_FILE}" ]]; then + DATASET_INDEX_FILE="$(dirname "${DATA_INPUT_PARAMS_FILE}")/dataset_index.yaml" + fi +fi + +if [[ -n "${DATASET_INDEX_FILE}" ]]; then + DATASET_INDEX_FILE="$(resolve_workspace_path "${DATASET_INDEX_FILE}")" + [[ "${DATASET_INDEX_FILE}" != *"'"* ]] || die "dataset_index_file 路径不能包含单引号" +fi + +if [[ "${WORKSHOP_USE_GATEWAY}" -eq 1 ]]; then + WORKSHOP_USE_GATEWAY_VALUE="true" +else + WORKSHOP_USE_GATEWAY_VALUE="false" +fi + resolve_conda_sh() { if [[ -n "${CONDA_SH:-}" && -f "${CONDA_SH}" ]]; then echo "${CONDA_SH}" @@ -182,6 +242,28 @@ source_ros_env() { set -u } +wait_for_background_exit() { + local timeout_sec="$1" + local deadline=$((SECONDS + timeout_sec)) + local pid + + while (( SECONDS < deadline )); do + local any_alive=0 + for pid in "${PIDS[@]}"; do + if kill -0 "${pid}" >/dev/null 2>&1; then + any_alive=1 + break + fi + done + if [[ "${any_alive}" -eq 0 ]]; then + return 0 + fi + sleep 1 + done + + return 1 +} + cleanup() { if [[ "${CLEANED}" -eq 1 ]]; then return @@ -203,17 +285,17 @@ cleanup() { fi done - sleep 3 + wait_for_background_exit "${STOP_GRACE_SEC}" || true for ((i=${#PIDS[@]}-1; i>=0; i--)); do pid="${PIDS[$i]}" name="${NAMES[$i]}" if kill -0 "${pid}" >/dev/null 2>&1; then - warn "${name} 未退出,发送 SIGTERM: pid=${pid}" + log "${name} 未在 ${STOP_GRACE_SEC}s 内退出,发送 SIGTERM: pid=${pid}" kill -TERM "${pid}" >/dev/null 2>&1 || true fi done - sleep 2 + wait_for_background_exit "${TERM_GRACE_SEC}" || true for ((i=${#PIDS[@]}-1; i>=0; i--)); do pid="${PIDS[$i]}" name="${NAMES[$i]}" @@ -321,6 +403,62 @@ wait_for_ros_service() { return 1 } +wait_for_ros_param() { + local node="$1" + local param="$2" + local timeout_sec="$3" + local deadline=$((SECONDS + timeout_sec)) + + log "等待 ROS 参数: ${node} ${param}" + while (( SECONDS < deadline )); do + if ros2 param get "${node}" "${param}" >/dev/null 2>&1; then + log "参数已就绪: ${node} ${param}" + return 0 + fi + sleep 1 + done + + warn "等待参数超时: ${node} ${param}" + return 1 +} + +require_ros_param_non_empty() { + local node="$1" + local param="$2" + local output + + output="$(ros2 param get "${node}" "${param}" 2>/dev/null || true)" + if [[ -z "${output}" || "${output}" == *"[]"* || "${output}" == *"not set"* ]]; then + warn "数据输入参数为空: ${node} ${param}; 输出: ${output}" + return 1 + fi + log "数据输入参数已注入: ${node} ${param}" + return 0 +} + +verify_data_input_params() { + if [[ -z "${DATA_INPUT_PARAMS_FILE}" ]]; then + return 0 + fi + + log "验证 data_input 参数注入: ${DATA_INPUT_PARAMS_FILE}" + wait_for_ros_param "/external_localization_service" "data_input.data_window_start_timestamp_us" 30 || return 1 + wait_for_ros_param "/sensor_calibration_service" "data_input.data_window_start_timestamp_us" 30 || return 1 + require_ros_param_non_empty "/external_localization_service" "data_input.external_observation_files" || return 1 + require_ros_param_non_empty "/sensor_calibration_service" "data_input.synchronized_dataset_files" || return 1 + wait_for_ros_param "/workshop_orchestrator_v2" "data_input_params_file" 30 || return 1 + require_ros_param_non_empty "/workshop_orchestrator_v2" "data_input_params_file" || return 1 + + if [[ "${WORKSHOP_USE_GATEWAY}" -eq 0 ]]; then + wait_for_ros_param "/chassis_calibration_service" "data_input.data_window_start_timestamp_us" 30 || return 1 + wait_for_ros_param "/control_calibration_service" "data_input.data_window_start_timestamp_us" 30 || return 1 + require_ros_param_non_empty "/chassis_calibration_service" "data_input.chassis_motion_data_files" || return 1 + require_ros_param_non_empty "/control_calibration_service" "data_input.control_evaluation_data_files" || return 1 + else + warn "当前使用 vehicle_agent_gateway,底盘/运控 data_input 由车端 agent 侧负责,本脚本只验证 external/sensor 注入。" + fi +} + check_no_existing_workshop_orchestrator() { local nodes nodes="$(timeout 5s ros2 node list 2>/dev/null || true)" @@ -361,6 +499,10 @@ log "工作空间: ${WORKSPACE_DIR}" log "ROS_LOG_DIR: ${ROS_LOG_DIR}" log "进程日志目录: ${RUN_LOG_ROOT}" log "Isaac conda 环境: ${CONDA_ENV_NAME}" +log "总控底盘/运控模式 use_gateway=${WORKSHOP_USE_GATEWAY_VALUE}" +if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then + log "数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}" +fi export PYTHONUNBUFFERED=1 check_no_existing_workshop_orchestrator @@ -382,6 +524,7 @@ start_bg "isaac" bash -lc " source '${WORKSPACE_DIR}/install/setup.bash' set -u export ROS_LOG_DIR='${ROS_LOG_DIR}' + export PYTHONNOUSERSITE=1 exec python3 src/simulation/tools/launch_sim_stack.py --component isaac ${ISAAC_HEADLESS_ARG} " @@ -447,13 +590,16 @@ start_bg "workshop-demo" bash -lc " set -u export ROS_LOG_DIR='${ROS_LOG_DIR}' exec ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \ - use_gateway:=true \ + 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 + 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}' " wait_for_ros_service "/workshop_v2/create_session" 60 || exit 1 +verify_data_input_params || exit 1 log "等待 ${WORKSHOP_WARMUP_SEC}s,让 external_localization_service 累积 Isaac 真值历史" sleep "${WORKSHOP_WARMUP_SEC}" @@ -464,6 +610,9 @@ if [[ "${RUN_SMOKE}" -eq 1 ]]; then --verbose-feedback --no-publish-fake-external-telemetry ) + if [[ "${WORKSHOP_USE_GATEWAY}" -eq 0 ]]; then + SMOKE_ARGS+=(--disable-wifi6-precheck) + fi if [[ -n "${SMOKE_TASKS}" ]]; then SMOKE_ARGS+=(--tasks "${SMOKE_TASKS}") fi @@ -481,6 +630,12 @@ if [[ "${RUN_SMOKE}" -eq 1 ]]; then set -e if [[ "${SMOKE_STATUS}" -eq 0 ]]; then + if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then + grep -q "site_data_input.yaml" "${SMOKE_LOG}" || die "报告中没有记录 site_data_input.yaml" + if [[ -f "${DATASET_INDEX_FILE}" ]]; then + grep -q "dataset_index.yaml" "${SMOKE_LOG}" || die "报告中没有记录 dataset_index.yaml" + fi + fi log "仿真闭环验收通过" else warn "仿真闭环验收失败,退出码: ${SMOKE_STATUS}" diff --git a/agv_calib_brain/run_local_data_input_smoke.sh b/agv_calib_brain/run_local_data_input_smoke.sh new file mode 100755 index 0000000..d769511 --- /dev/null +++ b/agv_calib_brain/run_local_data_input_smoke.sh @@ -0,0 +1,276 @@ +#!/usr/bin/env bash +# 本地数据输入回归测试。 +# +# 验证链路: +# dataset_index.yaml -> site_data_input.yaml -> external -> chassis -> control -> sensor_intrinsic -> report +# +# 这个脚本不启动 Isaac,不依赖 GPU;用于快速检查算法模板和数据输入注入合同。 + +set -Eeuo pipefail + +WORKSPACE_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}" +RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/local_data_input_smoke_$(date +%Y%m%d_%H%M%S)}" + +SITE_PROFILE="src/deployment/profiles/site_template.yaml" +SESSION_ID="session_001" +SESSION_DIR="/tmp/agv_calib_local_data_input_smoke/session_001" +DATASET_INDEX_FILE="" +DATA_INPUT_PARAMS_FILE="" +TASKS="external,chassis,control,sensor_intrinsic" +KEEP_RUNNING=0 +SKIP_GENERATE=0 +STRICT_INDEX=1 + +PIDS=() +NAMES=() +CLEANED=0 + +usage() { + cat <<'EOF' +用法: + ./run_local_data_input_smoke.sh [选项] + +用途: + 不启动 Isaac,只启动本地 ROS 标定服务和总控,验证数据输入参数注入与 report 链路。 + +选项: + --site-profile PATH 现场 profile,默认 src/deployment/profiles/site_template.yaml。 + --session-id ID 自动生成样例数据集时使用的 session_id,默认 session_001。 + --session-dir PATH 自动生成样例数据集时使用的会话目录。 + --data-input-params-file PATH + 使用已有 site_data_input.yaml,跳过自动生成。 + --dataset-index PATH 使用已有 site_data_input.yaml 时,对应的 dataset_index.yaml。 + --tasks LIST 逗号分隔任务列表,默认 external,chassis,control,sensor_intrinsic。 + --skip-generate 不生成 dataset_index/site_data_input,只使用 --data-input-params-file。 + --no-strict-index 关闭 dataset_index 严格任务输入校验。 + --keep-running smoke 结束后保持 launch 进程运行。 + -h, --help 显示帮助。 + +示例: + ./run_local_data_input_smoke.sh + ./run_local_data_input_smoke.sh --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml +EOF +} + +log() { + echo "[INFO] $*" +} + +warn() { + echo "[WARN] $*" >&2 +} + +die() { + echo "[ERROR] $*" >&2 + exit 1 +} + +resolve_workspace_path() { + local path="$1" + case "${path}" in + /*) + echo "${path}" + ;; + *) + echo "${WORKSPACE_DIR}/${path}" + ;; + esac +} + +source_ros_env() { + set +u + if [[ -f /opt/ros/humble/setup.bash ]]; then + source /opt/ros/humble/setup.bash + fi + source "${WORKSPACE_DIR}/install/setup.bash" + set -u +} + +cleanup() { + if [[ "${CLEANED}" -eq 1 ]]; then + return + fi + CLEANED=1 + + local i pid name + for ((i=${#PIDS[@]}-1; i>=0; i--)); do + pid="${PIDS[$i]}" + name="${NAMES[$i]}" + if kill -0 "${pid}" >/dev/null 2>&1; then + log "停止 ${name}: pid=${pid}" + kill -INT "${pid}" >/dev/null 2>&1 || true + fi + done + sleep 2 + for ((i=${#PIDS[@]}-1; i>=0; i--)); do + pid="${PIDS[$i]}" + name="${NAMES[$i]}" + if kill -0 "${pid}" >/dev/null 2>&1; then + warn "${name} 未退出,发送 SIGTERM: pid=${pid}" + kill -TERM "${pid}" >/dev/null 2>&1 || true + fi + done +} + +trap cleanup EXIT + +while [[ $# -gt 0 ]]; do + case "$1" in + --site-profile) + [[ $# -ge 2 ]] || die "--site-profile 需要参数" + SITE_PROFILE="$2" + shift 2 + ;; + --session-id) + [[ $# -ge 2 ]] || die "--session-id 需要参数" + SESSION_ID="$2" + shift 2 + ;; + --session-dir) + [[ $# -ge 2 ]] || die "--session-dir 需要参数" + SESSION_DIR="$2" + shift 2 + ;; + --data-input-params-file) + [[ $# -ge 2 ]] || die "--data-input-params-file 需要参数" + DATA_INPUT_PARAMS_FILE="$2" + SKIP_GENERATE=1 + shift 2 + ;; + --dataset-index) + [[ $# -ge 2 ]] || die "--dataset-index 需要参数" + DATASET_INDEX_FILE="$2" + shift 2 + ;; + --tasks) + [[ $# -ge 2 ]] || die "--tasks 需要参数" + TASKS="$2" + shift 2 + ;; + --skip-generate) + SKIP_GENERATE=1 + shift + ;; + --no-strict-index) + STRICT_INDEX=0 + shift + ;; + --keep-running) + KEEP_RUNNING=1 + shift + ;; + -h|--help) + usage + exit 0 + ;; + *) + die "未知参数: $1" + ;; + esac +done + +SITE_PROFILE="$(resolve_workspace_path "${SITE_PROFILE}")" +SESSION_DIR="$(resolve_workspace_path "${SESSION_DIR}")" +if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then + DATA_INPUT_PARAMS_FILE="$(resolve_workspace_path "${DATA_INPUT_PARAMS_FILE}")" +fi +if [[ -n "${DATASET_INDEX_FILE}" ]]; then + DATASET_INDEX_FILE="$(resolve_workspace_path "${DATASET_INDEX_FILE}")" +fi + +[[ -f "${SITE_PROFILE}" ]] || die "找不到 site profile: ${SITE_PROFILE}" +[[ -f "${WORKSPACE_DIR}/install/setup.bash" ]] || die "找不到 install/setup.bash,请先执行 colcon build" + +mkdir -p "${ROS_LOG_DIR}" "${RUN_LOG_ROOT}" + +source_ros_env + +if [[ "${SKIP_GENERATE}" -eq 0 ]]; then + log "生成本地样例数据集索引和数据输入参数文件" + python3 "${WORKSPACE_DIR}/src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py" \ + --site-profile "${SITE_PROFILE}" \ + --session-dir "${SESSION_DIR}" \ + --session-id "${SESSION_ID}" \ + --site-id local_smoke \ + --vehicle-id demo_agv_001 \ + --start-us 1770000000000000 \ + --end-us 1770000120000000 \ + --overwrite \ + --file external.external_observation_files=external/external_observations.csv \ + --file chassis.chassis_motion_data_files=chassis/chassis_motion.csv \ + --file control.control_evaluation_data_files=control/control_eval.csv \ + --file sensor.image_sample_files=sensor/front_camera/images.yaml \ + --file sensor.target_detection_files=sensor/front_camera/charuco_detections.json \ + --finalize + DATA_INPUT_PARAMS_FILE="${SESSION_DIR}/site_data_input.yaml" + DATASET_INDEX_FILE="${SESSION_DIR}/dataset_index.yaml" +elif [[ -z "${DATASET_INDEX_FILE}" ]]; then + DATASET_INDEX_FILE="$(dirname "${DATA_INPUT_PARAMS_FILE}")/dataset_index.yaml" +fi + +[[ -f "${DATA_INPUT_PARAMS_FILE}" ]] || die "找不到数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}" + +if [[ "${STRICT_INDEX}" -eq 1 ]]; then + if [[ -f "${DATASET_INDEX_FILE}" ]]; then + log "校验数据集索引: ${DATASET_INDEX_FILE}" + python3 "${WORKSPACE_DIR}/src/deployment/tools/validate_dataset_index.py" \ + "${DATASET_INDEX_FILE}" \ + --tasks "${TASKS}" + else + warn "未找到 dataset_index.yaml,跳过索引校验: ${DATASET_INDEX_FILE}" + fi +fi + +LAUNCH_LOG="${RUN_LOG_ROOT}/launch.log" +log "启动本地标定服务和总控,日志: ${LAUNCH_LOG}" +ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \ + use_gateway:=false \ + data_input_params_file:="${DATA_INPUT_PARAMS_FILE}" \ + dataset_index_file:="${DATASET_INDEX_FILE}" \ + sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu \ + >"${LAUNCH_LOG}" 2>&1 & +PIDS+=("$!") +NAMES+=("local-workshop-demo") + +log "等待 /workshop_v2/create_session" +for _ in $(seq 1 60); do + if ros2 service list 2>/dev/null | grep -qx /workshop_v2/create_session; then + break + fi + sleep 1 +done +ros2 service list 2>/dev/null | grep -qx /workshop_v2/create_session || { + tail -n 120 "${LAUNCH_LOG}" >&2 || true + die "等待 /workshop_v2/create_session 超时" +} + +log "确认四个算法服务已读取 data_input 参数" +ros2 param get /external_localization_service data_input.external_observation_files +ros2 param get /chassis_calibration_service data_input.chassis_motion_data_files +ros2 param get /control_calibration_service data_input.control_evaluation_data_files +ros2 param get /sensor_calibration_service data_input.synchronized_dataset_files +ros2 param get /workshop_orchestrator_v2 data_input_params_file +ros2 param get /workshop_orchestrator_v2 dataset_index_file + +SMOKE_LOG="${RUN_LOG_ROOT}/smoke.log" +log "执行本地四阶段 smoke,日志: ${SMOKE_LOG}" +python3 "${WORKSPACE_DIR}/src/simulation/tools/smoke_test_workshop_orchestrator.py" \ + --tasks "${TASKS}" \ + --verbose-feedback \ + --disable-wifi6-precheck \ + 2>&1 | tee "${SMOKE_LOG}" + +grep -q "site_data_input.yaml" "${SMOKE_LOG}" || die "报告中没有记录 site_data_input.yaml" +if [[ -f "${DATASET_INDEX_FILE}" ]]; then + grep -q "dataset_index.yaml" "${SMOKE_LOG}" || die "报告中没有记录 dataset_index.yaml" +fi + +if [[ "${KEEP_RUNNING}" -eq 1 ]]; then + log "本地服务保持运行中。按 Ctrl+C 停止。" + while true; do + sleep 3600 + done +fi + +log "本地数据输入回归测试通过" diff --git a/agv_calib_brain/src/README.md b/agv_calib_brain/src/README.md index 818ced4..a437ce6 100644 --- a/agv_calib_brain/src/README.md +++ b/agv_calib_brain/src/README.md @@ -17,7 +17,7 @@ 部署前仿真验证代码。Isaac 车间、仿真车辆、仿真车端 agent、仿真传感器、仿真标定靶和旧版仿真包都放在这里。 - `deployment/` - 部署 profile 和从仿真迁移到现场前的检查清单。这里放配置基准,不放算法实现。 + 部署 profile、数据集索引转换工具和从仿真迁移到现场前的检查清单。这里放配置基准,不放算法实现。 - `docs/` 源码树内的设计说明、边界说明和迁移规则。 @@ -34,8 +34,22 @@ - 部署 profile 放在 `deployment/`。 - 设计说明和迁移边界说明放在 `docs/`。 +## 文档入口 + +- `docs/sim_to_site_code_boundary.md` + 仿真代码、可复用核心代码和真实现场部署代码的边界。 + +- `docs/algorithm_template_contract.md` + 标定算法模板合同,定义模板维护者和算法实现者的责任边界、可修改文件、输入输出字段和 smoke 验证要求。 + ## 主要入口 +本地数据输入回归测试: + +```bash +./run_local_data_input_smoke.sh +``` + Isaac 车间仿真: ```bash diff --git a/agv_calib_brain/src/communication/win_ubuntu_bridge/vehicle_agent_gateway/launch/vehicle_agent_gateway.launch.py b/agv_calib_brain/src/communication/win_ubuntu_bridge/vehicle_agent_gateway/launch/vehicle_agent_gateway.launch.py index b9cadd6..a2095a8 100644 --- a/agv_calib_brain/src/communication/win_ubuntu_bridge/vehicle_agent_gateway/launch/vehicle_agent_gateway.launch.py +++ b/agv_calib_brain/src/communication/win_ubuntu_bridge/vehicle_agent_gateway/launch/vehicle_agent_gateway.launch.py @@ -16,7 +16,6 @@ def generate_launch_description(): Node( package="vehicle_agent_gateway", executable="vehicle_agent_gateway_node", - name="vehicle_agent_gateway", output="screen", parameters=[{ "chassis_host": LaunchConfiguration("chassis_host"), 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 b01f61f..91a659b 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 @@ -4,13 +4,15 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -# 最小 demo 拉起完整闭环:车辆画像 + 外部定位 + 传感器 + 底盘/运控(stub 或 WiFi 网关)+ 车间编排。 +# 最小演示启动完整闭环:车辆画像 + 外部定位 + 传感器 + 底盘/运控(桩节点或 WiFi 网关)+ 车间编排。 # 参数: -# use_gateway (bool, 默认 false):true 时用 vehicle_agent_gateway 替换本地底盘/运控 stub 节点。 +# use_gateway(布尔值,默认 false):true 时用 vehicle_agent_gateway 替换本地底盘/运控桩节点。 # chassis_host / control_host:Windows 车端 IP(仅 use_gateway=true 时生效)。 # profile_storage_path:vehicle_profile_manager 的画像持久化文件。 # sensor_storage_root / sensor_registry / capture_pipeline_name / telemetry_topics: # 传感器 readiness 的最小真实配置入口。 +# data_input_params_file:可选 ROS 参数文件,用于把真实采集/ingest 产物路径注入算法输入。 +# dataset_index_file:可选数据集索引文件;为空时主控会按 data_input_params_file 同目录自动查找。 def generate_launch_description(): use_gateway_arg = DeclareLaunchArgument( "use_gateway", @@ -60,6 +62,16 @@ def generate_launch_description(): default_value="true", description="当前机械臂是否就绪", ) + data_input_params_file_arg = DeclareLaunchArgument( + "data_input_params_file", + default_value="", + description="可选 ROS 参数文件,包含各 service 的 data_input.* 数据文件引用配置", + ) + dataset_index_file_arg = DeclareLaunchArgument( + "dataset_index_file", + default_value="", + description="可选数据集索引文件,用于最终报告追溯本次真实采集数据", + ) def make_nodes(context): use_gateway = LaunchConfiguration("use_gateway").perform(context).lower() == "true" @@ -72,6 +84,16 @@ def generate_launch_description(): telemetry_topics = LaunchConfiguration("telemetry_topics").perform(context) vehicle_safe_to_move = LaunchConfiguration("vehicle_safe_to_move").perform(context).lower() == "true" arm_ready = LaunchConfiguration("arm_ready").perform(context).lower() == "true" + data_input_params_file = LaunchConfiguration("data_input_params_file").perform(context).strip() + dataset_index_file = LaunchConfiguration("dataset_index_file").perform(context).strip() + + def with_data_input_params(params=None): + merged = [] + if data_input_params_file: + merged.append(data_input_params_file) + if params is not None: + merged.append(params) + return merged common_nodes = [ Node( @@ -88,26 +110,31 @@ def generate_launch_description(): executable="external_localization_service_node", name="external_localization_service", output="screen", + parameters=with_data_input_params(), ), Node( package="sensor_calibration_service", executable="sensor_calibration_service_node", name="sensor_calibration_service", output="screen", - parameters=[{ + parameters=with_data_input_params({ "sensor_storage_root": sensor_storage_root, "sensor_registry": sensor_registry, "capture_pipeline_name": capture_pipeline_name, "telemetry_topics": telemetry_topics, "vehicle_safe_to_move": vehicle_safe_to_move, "arm_ready": arm_ready, - }], + }), ), Node( package="workshop_orchestrator_v2", executable="workshop_orchestrator_v2_node", name="workshop_orchestrator_v2", output="screen", + parameters=[{ + "data_input_params_file": data_input_params_file, + "dataset_index_file": dataset_index_file, + }], ), ] @@ -117,7 +144,6 @@ def generate_launch_description(): Node( package="vehicle_agent_gateway", executable="vehicle_agent_gateway_node", - name="vehicle_agent_gateway", output="screen", parameters=[{ "chassis_host": chassis_host, @@ -130,19 +156,21 @@ def generate_launch_description(): ), ] else: - # 本地 stub 模式:底盘和运控使用本地 stub 节点,无需 Windows 连接。 + # 本地桩节点模式:底盘和运控使用本地桩节点,无需 Windows 连接。 chassis_control_nodes = [ Node( package="chassis_calibration_service", executable="chassis_calibration_service_node", name="chassis_calibration_service", output="screen", + parameters=with_data_input_params(), ), Node( package="control_calibration_service", executable="control_calibration_service_node", name="control_calibration_service", output="screen", + parameters=with_data_input_params(), ), ] @@ -159,5 +187,7 @@ def generate_launch_description(): telemetry_topics_arg, vehicle_safe_to_move_arg, arm_ready_arg, + data_input_params_file_arg, + dataset_index_file_arg, OpaqueFunction(function=make_nodes), ]) diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp index d21f127..c9bf6e4 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -9,6 +10,7 @@ #include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" #include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" #include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" #include "calibration_control_interfaces/msg/control_readiness_response.hpp" #include "calibration_control_interfaces/msg/control_telemetry.hpp" @@ -101,6 +103,20 @@ struct ChassisCalibrationInput // 历史外部定位遥测窗口:用于和车端里程计、控制误差、标定结果做对齐分析。 std::vector external_localization_telemetry_history; + // ===== 算法数据输入窗口 ===== + // 当前动作原语对应的数据时间窗口。0 表示上层尚未提供明确边界。 + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + + // 底盘原始运动数据文件:例如 wheel/steering/odom/driver_state CSV、JSON 或 rosbag。 + std::vector chassis_motion_data_files; + + // 控制命令与执行器反馈文件:例如 cmd_vel、Ackermann 命令、驱动器反馈、制动状态。 + std::vector actuator_command_files; + + // 外部真值轨迹文件:例如 external pose 轨迹、地面真值、测量轨迹,用于和底盘里程计对齐。 + std::vector truth_trajectory_files; + // 说明:如果后续算法还需要更多原始反馈,可以继续在这里扩展。 // 常见补充项包括:GNSS、SLAM 里程计、地面真值、控制器内部状态、制动状态、故障码、机械臂反馈等。 }; diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_service_node.hpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_service_node.hpp index bd3ff4a..409bdec 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_service_node.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_service_node.hpp @@ -2,10 +2,12 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_chassis_interfaces/action/execute_motion_primitive.hpp" #include "calibration_chassis_interfaces/srv/get_chassis_readiness.hpp" #include "calibration_vehicle_profile_interfaces/srv/get_vehicle_profile.hpp" @@ -30,12 +32,24 @@ private: using ExecuteTask = calibration_chassis_interfaces::action::ExecuteMotionPrimitive; using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle; + struct DataInputConfig + { + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + std::vector chassis_motion_data_files; + std::vector actuator_command_files; + std::vector truth_trajectory_files; + }; + // readiness 服务:让编排器确认当前底盘是否具备执行条件。 rclcpp::Service::SharedPtr readiness_service_; // 车辆画像客户端:从 vehicle_profile_manager 取当前车辆的底盘类型。 rclcpp::Client::SharedPtr get_vehicle_profile_client_; // action server:接收底盘动作原语任务,并在后台线程里执行。 rclcpp_action::Server::SharedPtr execute_task_action_server_; + DataInputConfig data_input_config_; + + DataInputConfig load_data_input_config(); // readiness 回调:把底盘运行状态翻译成统一的 readiness 响应。 void handle_readiness( diff --git a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_service_node.cpp b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_service_node.cpp index 7af0c27..cc6ee53 100644 --- a/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_service_node.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_service_node.cpp @@ -1,6 +1,7 @@ #include "chassis_calibration_service/chassis_calibration_service_node.hpp" #include +#include #include #include "rclcpp_components/register_node_macro.hpp" @@ -21,6 +22,33 @@ int64_t now_us() std::chrono::system_clock::now().time_since_epoch()) .count(); } + +calibration_common_interfaces::msg::FileReference make_file_reference( + const std::string & file_uri, + const std::string & description) +{ + calibration_common_interfaces::msg::FileReference ref; + ref.file_uri = file_uri; + const auto pos = file_uri.find_last_of("/\\"); + ref.file_name = pos == std::string::npos ? file_uri : file_uri.substr(pos + 1); + ref.size_bytes = 0; + ref.description = description; + return ref; +} + +std::vector make_file_references( + const std::vector & file_uris, + const std::string & description) +{ + std::vector refs; + refs.reserve(file_uris.size()); + for (const auto & file_uri : file_uris) { + if (!file_uri.empty()) { + refs.push_back(make_file_reference(file_uri, description)); + } + } + return refs; +} } // namespace using calibration_common_interfaces::msg::ErrorCode; @@ -29,6 +57,8 @@ using calibration_common_interfaces::msg::JobState; ChassisCalibrationServiceNode::ChassisCalibrationServiceNode(const rclcpp::NodeOptions & options) : Node("chassis_calibration_service", options) { + data_input_config_ = load_data_input_config(); + // readiness:供 orchestrator 在开跑前检查底盘是否具备执行条件。 readiness_service_ = create_service( "/chassis/get_readiness", @@ -46,6 +76,28 @@ ChassisCalibrationServiceNode::ChassisCalibrationServiceNode(const rclcpp::NodeO std::bind(&ChassisCalibrationServiceNode::handle_accepted, this, std::placeholders::_1)); } +ChassisCalibrationServiceNode::DataInputConfig ChassisCalibrationServiceNode::load_data_input_config() +{ + DataInputConfig config; + config.data_window_start_timestamp_us = + declare_parameter("data_input.data_window_start_timestamp_us", 0); + config.data_window_end_timestamp_us = + declare_parameter("data_input.data_window_end_timestamp_us", 0); + config.chassis_motion_data_files = make_file_references( + declare_parameter>( + "data_input.chassis_motion_data_files", std::vector{}), + "chassis motion data"); + config.actuator_command_files = make_file_references( + declare_parameter>( + "data_input.actuator_command_files", std::vector{}), + "actuator command and feedback data"); + config.truth_trajectory_files = make_file_references( + declare_parameter>( + "data_input.truth_trajectory_files", std::vector{}), + "external truth trajectory data"); + return config; +} + void ChassisCalibrationServiceNode::handle_readiness( const std::shared_ptr request, std::shared_ptr response) @@ -109,8 +161,9 @@ bool ChassisCalibrationServiceNode::fetch_chassis_type( auto request = std::make_shared(); request->request.vehicle_id = vehicle_id; auto future = get_vehicle_profile_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future, std::chrono::seconds(2)) != - rclcpp::FutureReturnCode::SUCCESS) { + + // 当前节点已经由 main/launch 的 executor spin,不能在执行线程里再次 spin 同一个节点。 + if (future.wait_for(std::chrono::seconds(2)) != std::future_status::ready) { failure_reason = "查询车辆画像超时。"; return false; } @@ -143,6 +196,11 @@ void ChassisCalibrationServiceNode::execute_goal( // chassis_type 不再靠 request_id 猜,而是从车辆画像服务中按 vehicle_id 查询。 ChassisCalibrationAlgorithmTemplate::Input input; input.request = *goal_handle->get_goal(); + input.data_window_start_timestamp_us = data_input_config_.data_window_start_timestamp_us; + input.data_window_end_timestamp_us = data_input_config_.data_window_end_timestamp_us; + input.chassis_motion_data_files = data_input_config_.chassis_motion_data_files; + input.actuator_command_files = data_input_config_.actuator_command_files; + input.truth_trajectory_files = data_input_config_.truth_trajectory_files; std::string failure_reason; if (!fetch_chassis_type( input.request.goal.header.vehicle_id, diff --git a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp index 66dafb9..ea8a118 100644 --- a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -7,6 +8,7 @@ #include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" #include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" #include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_control_interfaces/action/execute_controller_evaluation.hpp" #include "calibration_control_interfaces/msg/acceleration_deceleration_task.hpp" #include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" @@ -164,6 +166,23 @@ struct ControlCalibrationInput bool sensor_history_available{false}; bool truth_history_available{false}; + // ===== 算法数据输入窗口 ===== + // 当前控制评估任务对应的数据时间窗口。0 表示上层尚未提供明确边界。 + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + + // 控制评估原始数据文件:例如控制误差、控制输出、饱和状态、控制器内部状态 CSV/JSON/rosbag。 + std::vector control_evaluation_data_files; + + // 参考轨迹或参考速度曲线文件:当 reference_trajectory 不足以表达完整任务时使用。 + std::vector reference_signal_files; + + // 底盘响应文件:例如底盘里程计、轮速、舵角、执行器状态,用于分析车辆实际响应。 + std::vector chassis_response_files; + + // 外部真值轨迹文件:例如 external pose 轨迹、地面真值、定位质量日志。 + std::vector truth_trajectory_files; + // 说明:如果后续控制算法还需要更细反馈,可继续在这里补充。 // 常见补充项包括:规划轨迹、参考速度曲线、控制器内部状态、执行器饱和原因、故障码、时间同步诊断信息等。 } diff --git a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_service_node.hpp b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_service_node.hpp index e182bc3..fc27eed 100644 --- a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_service_node.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_service_node.hpp @@ -2,12 +2,14 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "control_calibration_service/control_calibration_algorithm_template.hpp" +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_control_interfaces/action/execute_controller_evaluation.hpp" #include "calibration_control_interfaces/srv/get_control_readiness.hpp" @@ -31,9 +33,22 @@ private: using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle; using ControllerAlgorithmType = calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType; + struct DataInputConfig + { + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + std::vector control_evaluation_data_files; + std::vector reference_signal_files; + std::vector chassis_response_files; + std::vector truth_trajectory_files; + }; + rclcpp::Service::SharedPtr readiness_service_; rclcpp_action::Server::SharedPtr execute_task_action_server_; ControlCalibrationAlgorithmTemplate algorithm_; + DataInputConfig data_input_config_; + + DataInputConfig load_data_input_config(); void handle_readiness( const std::shared_ptr request, diff --git a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/control_calibration_service_node.cpp b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/control_calibration_service_node.cpp index 5d8b4d8..1467cb7 100644 --- a/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/control_calibration_service_node.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/control_calibration_service/src/control_calibration_service_node.cpp @@ -22,6 +22,33 @@ int64_t now_us() std::chrono::system_clock::now().time_since_epoch()) .count(); } + +calibration_common_interfaces::msg::FileReference make_file_reference( + const std::string & file_uri, + const std::string & description) +{ + calibration_common_interfaces::msg::FileReference ref; + ref.file_uri = file_uri; + const auto pos = file_uri.find_last_of("/\\"); + ref.file_name = pos == std::string::npos ? file_uri : file_uri.substr(pos + 1); + ref.size_bytes = 0; + ref.description = description; + return ref; +} + +std::vector make_file_references( + const std::vector & file_uris, + const std::string & description) +{ + std::vector refs; + refs.reserve(file_uris.size()); + for (const auto & file_uri : file_uris) { + if (!file_uri.empty()) { + refs.push_back(make_file_reference(file_uri, description)); + } + } + return refs; +} } // namespace using calibration_common_interfaces::msg::ErrorCode; @@ -31,6 +58,8 @@ using calibration_control_interfaces::msg::ControllerEvaluationTaskType; ControlCalibrationServiceNode::ControlCalibrationServiceNode(const rclcpp::NodeOptions & options) : Node("control_calibration_service", options) { + data_input_config_ = load_data_input_config(); + readiness_service_ = create_service( "/control/get_readiness", std::bind(&ControlCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2)); @@ -43,6 +72,32 @@ ControlCalibrationServiceNode::ControlCalibrationServiceNode(const rclcpp::NodeO std::bind(&ControlCalibrationServiceNode::handle_accepted, this, std::placeholders::_1)); } +ControlCalibrationServiceNode::DataInputConfig ControlCalibrationServiceNode::load_data_input_config() +{ + DataInputConfig config; + config.data_window_start_timestamp_us = + declare_parameter("data_input.data_window_start_timestamp_us", 0); + config.data_window_end_timestamp_us = + declare_parameter("data_input.data_window_end_timestamp_us", 0); + config.control_evaluation_data_files = make_file_references( + declare_parameter>( + "data_input.control_evaluation_data_files", std::vector{}), + "control evaluation data"); + config.reference_signal_files = make_file_references( + declare_parameter>( + "data_input.reference_signal_files", std::vector{}), + "control reference signal data"); + config.chassis_response_files = make_file_references( + declare_parameter>( + "data_input.chassis_response_files", std::vector{}), + "chassis response data for control evaluation"); + config.truth_trajectory_files = make_file_references( + declare_parameter>( + "data_input.truth_trajectory_files", std::vector{}), + "external truth trajectory data"); + return config; +} + void ControlCalibrationServiceNode::handle_readiness( const std::shared_ptr request, std::shared_ptr response) @@ -116,6 +171,12 @@ void ControlCalibrationServiceNode::execute_goal( input.chassis_history_available = false; input.sensor_history_available = false; input.truth_history_available = false; + input.data_window_start_timestamp_us = data_input_config_.data_window_start_timestamp_us; + input.data_window_end_timestamp_us = data_input_config_.data_window_end_timestamp_us; + input.control_evaluation_data_files = data_input_config_.control_evaluation_data_files; + input.reference_signal_files = data_input_config_.reference_signal_files; + input.chassis_response_files = data_input_config_.chassis_response_files; + input.truth_trajectory_files = data_input_config_.truth_trajectory_files; ControlCalibrationAlgorithmTemplate::Output output; std::string failure_reason; diff --git a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp index 14f2c32..7be090d 100644 --- a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -7,6 +8,7 @@ #include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" #include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" #include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" #include "calibration_control_interfaces/msg/control_readiness_response.hpp" #include "calibration_control_interfaces/msg/control_telemetry.hpp" @@ -102,6 +104,20 @@ struct ExternalLocalizationInput // 数据采集与落盘诊断扩展:例如原始位姿采集窗口不足、采样频率不满足、落盘失败等。 std::vector acquisition_diagnostics; + // ===== 算法数据输入窗口 ===== + // 当前算法可使用的数据时间窗口。0 表示上层尚未提供明确边界。 + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + + // 外部定位原始观测文件:例如 CSV/JSON/rosbag,内容应至少包含时间戳、位姿、质量、协方差或标准差。 + std::vector external_observation_files; + + // 标靶观测或检测结果文件:例如 marker/AprilTag/反光柱观测日志、角点检测结果、对齐中间结果。 + std::vector marker_observation_files; + + // 时间同步诊断文件:例如 PTP/NTP 偏差统计、车端与车间主机时钟对齐日志。 + std::vector time_sync_diagnostic_files; + // ===== 底盘域反馈 ===== // 外部定位验证往往需要结合车辆真实运动状态,因此保留底盘域反馈接口。 calibration_chassis_interfaces::msg::ChassisReadinessResponse chassis_readiness; diff --git a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_service_node.hpp b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_service_node.hpp index f179cdb..f082d99 100644 --- a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_service_node.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_service_node.hpp @@ -59,6 +59,7 @@ private: }; ReadinessConfig load_readiness_config(); + ExternalReferenceExecutor::DataInputConfig load_data_input_config(); void handle_external_telemetry(const ExternalTelemetry::SharedPtr msg); void append_issue( calibration_external_localization_interfaces::msg::ExternalLocalizationValidationSummary & summary, @@ -71,6 +72,7 @@ private: calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse & readiness) const; ReadinessConfig readiness_config_; + ExternalReferenceExecutor::DataInputConfig data_input_config_; mutable std::mutex telemetry_mutex_; TelemetrySnapshot telemetry_snapshot_; rclcpp::Subscription::SharedPtr telemetry_subscription_; diff --git a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp index eb16d0c..7e734a5 100644 --- a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_reference_executor.hpp @@ -1,7 +1,10 @@ #pragma once +#include #include +#include +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_workshop_orchestration_interfaces/msg/stage_result_summary.hpp" #include "external_localization_service/external_localization_algorithm_template.hpp" @@ -26,11 +29,21 @@ public: using ExecuteTask = calibration_external_localization_interfaces::action::ExecuteExternalLocalizationTask; + struct DataInputConfig + { + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + std::vector external_observation_files; + std::vector marker_observation_files; + std::vector time_sync_diagnostic_files; + }; + bool validate_goal(const ExecuteTask::Goal & goal, std::string & reject_reason) const; bool build_result( const ExecuteTask::Goal & goal, const calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse & readiness, const std::vector & telemetry_history, + const DataInputConfig & data_input_config, ExecuteTask::Result & external_result, std::string & failure_reason) const; diff --git a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp index 192f367..ca429bb 100644 --- a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_localization_service_node.cpp @@ -19,6 +19,33 @@ int64_t now_us() std::chrono::system_clock::now().time_since_epoch()) .count(); } + +calibration_common_interfaces::msg::FileReference make_file_reference( + const std::string & file_uri, + const std::string & description) +{ + calibration_common_interfaces::msg::FileReference ref; + ref.file_uri = file_uri; + const auto pos = file_uri.find_last_of("/\\"); + ref.file_name = pos == std::string::npos ? file_uri : file_uri.substr(pos + 1); + ref.size_bytes = 0; + ref.description = description; + return ref; +} + +std::vector make_file_references( + const std::vector & file_uris, + const std::string & description) +{ + std::vector refs; + refs.reserve(file_uris.size()); + for (const auto & file_uri : file_uris) { + if (!file_uri.empty()) { + refs.push_back(make_file_reference(file_uri, description)); + } + } + return refs; +} } // namespace using calibration_common_interfaces::msg::ErrorCode; @@ -26,7 +53,8 @@ using calibration_common_interfaces::msg::JobState; ExternalLocalizationServiceNode::ExternalLocalizationServiceNode(const rclcpp::NodeOptions & options) : Node("external_localization_service", options), - readiness_config_(load_readiness_config()) + readiness_config_(load_readiness_config()), + data_input_config_(load_data_input_config()) { telemetry_subscription_ = create_subscription( readiness_config_.telemetry_topic, @@ -67,6 +95,28 @@ ExternalLocalizationServiceNode::ReadinessConfig ExternalLocalizationServiceNode return config; } +ExternalReferenceExecutor::DataInputConfig ExternalLocalizationServiceNode::load_data_input_config() +{ + ExternalReferenceExecutor::DataInputConfig config; + config.data_window_start_timestamp_us = + declare_parameter("data_input.data_window_start_timestamp_us", 0); + config.data_window_end_timestamp_us = + declare_parameter("data_input.data_window_end_timestamp_us", 0); + config.external_observation_files = make_file_references( + declare_parameter>( + "data_input.external_observation_files", std::vector{}), + "external localization observation data"); + config.marker_observation_files = make_file_references( + declare_parameter>( + "data_input.marker_observation_files", std::vector{}), + "external marker observation data"); + config.time_sync_diagnostic_files = make_file_references( + declare_parameter>( + "data_input.time_sync_diagnostic_files", std::vector{}), + "external time synchronization diagnostics"); + return config; +} + void ExternalLocalizationServiceNode::handle_external_telemetry(const ExternalTelemetry::SharedPtr msg) { std::lock_guard lock(telemetry_mutex_); @@ -345,7 +395,14 @@ void ExternalLocalizationServiceNode::execute_goal( telemetry_history = telemetry_snapshot_.history; } - if (!executor_.build_result(*goal_handle->get_goal(), readiness, telemetry_history, *result, failure_reason)) { + if (!executor_.build_result( + *goal_handle->get_goal(), + readiness, + telemetry_history, + data_input_config_, + *result, + failure_reason)) + { result->result.success = false; result->result.error_code.code = readiness.success ? ErrorCode::INVALID_ARGUMENT : readiness.error_code.code; diff --git a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_reference_executor.cpp b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_reference_executor.cpp index e9dcb33..732463e 100644 --- a/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_reference_executor.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/external_localization_service/src/external_reference_executor.cpp @@ -32,6 +32,7 @@ bool ExternalReferenceExecutor::build_result( const ExecuteTask::Goal & goal, const calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse & readiness, const std::vector & telemetry_history, + const DataInputConfig & data_input_config, ExecuteTask::Result & external_result, std::string & failure_reason) const { @@ -90,6 +91,13 @@ bool ExternalReferenceExecutor::build_result( input.chassis_history_available = !input.chassis_telemetry_history.empty(); input.control_history_available = !input.control_telemetry_history.empty(); input.sensor_history_available = !input.sensor_telemetry_history.empty(); + input.data_window_start_timestamp_us = data_input_config.data_window_start_timestamp_us; + input.data_window_end_timestamp_us = data_input_config.data_window_end_timestamp_us; + input.external_observation_files = data_input_config.external_observation_files; + input.marker_observation_files = data_input_config.marker_observation_files; + input.time_sync_diagnostic_files = data_input_config.time_sync_diagnostic_files; + input.acquisition_diagnostics.push_back( + "external_observation_files=" + std::to_string(input.external_observation_files.size())); ExternalLocalizationOutput output; if (!algorithm_.run(input, output, failure_reason)) { diff --git a/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp b/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp index 94fe881..8a979fa 100644 --- a/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -7,6 +8,7 @@ #include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp" #include "calibration_chassis_interfaces/msg/chassis_telemetry.hpp" #include "calibration_chassis_interfaces/msg/chassis_work_mode.hpp" +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_control_interfaces/msg/active_controller_parameters_response.hpp" #include "calibration_control_interfaces/msg/control_readiness_response.hpp" #include "calibration_control_interfaces/msg/control_telemetry.hpp" @@ -132,6 +134,33 @@ struct SensorCalibrationInput bool control_history_available{false}; bool truth_history_available{false}; + // ===== 算法数据输入窗口 ===== + // 当前采集任务对应的数据时间窗口。0 表示上层尚未提供明确边界。 + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + + // 图像样本文件:适用于前视/下视相机内参、相机外参、手眼标定。 + // 文件可为单帧图片、图片列表、rosbag、mcap 或算法约定的数据集索引。 + std::vector image_sample_files; + + // IMU 原始样本文件:适用于 IMU 内参、IMU 外参、运动段对齐。 + std::vector imu_sample_files; + + // 3D 点云样本文件:适用于 3D LiDAR 外参或点云配准类算法。 + std::vector pointcloud_sample_files; + + // 2D 激光扫描样本文件:适用于 2D LiDAR 外参、角落/平面靶标拟合。 + std::vector laser_scan_sample_files; + + // 标定目标检测结果文件:例如棋盘格/ChArUco/AprilTag 角点、PnP 初值、LiDAR 平面/角点提取结果。 + std::vector target_detection_files; + + // 机械臂或平台位姿文件:适用于手眼标定、传感器到 base_link 外参联合求解。 + std::vector robot_pose_sample_files; + + // 已同步的数据集索引:推荐作为真实算法的首选入口,内部可引用图像、IMU、点云、真值、检测结果。 + std::vector synchronized_dataset_files; + // 说明:如果后续某类传感器算法还需要更细原始数据入口,可继续在这里补充。 // 常见补充项包括:图像帧缓存、IMU 原始包、点云帧列表、机械臂位姿序列、标定板检测结果等。 }; diff --git a/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_service_node.hpp b/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_service_node.hpp index 9af050a..1d1e646 100644 --- a/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_service_node.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_service_node.hpp @@ -11,6 +11,7 @@ #include "sensor_calibration_service/sensor_calibration_algorithm_template.hpp" +#include "calibration_common_interfaces/msg/file_reference.hpp" #include "calibration_sensor_interfaces/action/execute_sensor_calibration_task.hpp" #include "calibration_sensor_interfaces/msg/sensor_calibration_task_type.hpp" #include "calibration_sensor_interfaces/srv/get_sensor_readiness.hpp" @@ -47,12 +48,27 @@ private: std::vector ready_sensor_ids; }; + struct DataInputConfig + { + int64_t data_window_start_timestamp_us{0}; + int64_t data_window_end_timestamp_us{0}; + std::vector image_sample_files; + std::vector imu_sample_files; + std::vector pointcloud_sample_files; + std::vector laser_scan_sample_files; + std::vector target_detection_files; + std::vector robot_pose_sample_files; + std::vector synchronized_dataset_files; + }; + rclcpp::Service::SharedPtr readiness_service_; rclcpp_action::Server::SharedPtr execute_task_action_server_; SensorCalibrationAlgorithmTemplate algorithm_; ReadinessConfig readiness_config_; + DataInputConfig data_input_config_; ReadinessConfig load_readiness_config(); + DataInputConfig load_data_input_config(); std::vector parse_csv_list(const std::string & raw) const; void append_issue( calibration_sensor_interfaces::msg::SensorReadinessResponse & response, diff --git a/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/src/sensor_calibration_service_node.cpp b/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/src/sensor_calibration_service_node.cpp index 20d0c21..15c2f89 100644 --- a/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/src/sensor_calibration_service_node.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/sensor_calibration_service/src/sensor_calibration_service_node.cpp @@ -24,6 +24,33 @@ int64_t now_us() std::chrono::system_clock::now().time_since_epoch()) .count(); } + +calibration_common_interfaces::msg::FileReference make_file_reference( + const std::string & file_uri, + const std::string & description) +{ + calibration_common_interfaces::msg::FileReference ref; + ref.file_uri = file_uri; + const auto pos = file_uri.find_last_of("/\\"); + ref.file_name = pos == std::string::npos ? file_uri : file_uri.substr(pos + 1); + ref.size_bytes = 0; + ref.description = description; + return ref; +} + +std::vector make_file_references( + const std::vector & file_uris, + const std::string & description) +{ + std::vector refs; + refs.reserve(file_uris.size()); + for (const auto & file_uri : file_uris) { + if (!file_uri.empty()) { + refs.push_back(make_file_reference(file_uri, description)); + } + } + return refs; +} } // namespace using calibration_common_interfaces::msg::ErrorCode; @@ -33,6 +60,7 @@ SensorCalibrationServiceNode::SensorCalibrationServiceNode(const rclcpp::NodeOpt : Node("sensor_calibration_service", options) { readiness_config_ = load_readiness_config(); + data_input_config_ = load_data_input_config(); readiness_service_ = create_service( "/sensor_calibration/get_readiness", @@ -141,6 +169,46 @@ SensorCalibrationServiceNode::ReadinessConfig SensorCalibrationServiceNode::load return config; } +SensorCalibrationServiceNode::DataInputConfig SensorCalibrationServiceNode::load_data_input_config() +{ + DataInputConfig config; + config.data_window_start_timestamp_us = + declare_parameter("data_input.data_window_start_timestamp_us", 0); + config.data_window_end_timestamp_us = + declare_parameter("data_input.data_window_end_timestamp_us", 0); + + config.image_sample_files = make_file_references( + declare_parameter>( + "data_input.image_sample_files", std::vector{}), + "sensor image samples"); + config.imu_sample_files = make_file_references( + declare_parameter>( + "data_input.imu_sample_files", std::vector{}), + "sensor imu samples"); + config.pointcloud_sample_files = make_file_references( + declare_parameter>( + "data_input.pointcloud_sample_files", std::vector{}), + "sensor pointcloud samples"); + config.laser_scan_sample_files = make_file_references( + declare_parameter>( + "data_input.laser_scan_sample_files", std::vector{}), + "sensor laser scan samples"); + config.target_detection_files = make_file_references( + declare_parameter>( + "data_input.target_detection_files", std::vector{}), + "sensor calibration target detection results"); + config.robot_pose_sample_files = make_file_references( + declare_parameter>( + "data_input.robot_pose_sample_files", std::vector{}), + "robot or platform pose samples"); + config.synchronized_dataset_files = make_file_references( + declare_parameter>( + "data_input.synchronized_dataset_files", std::vector{}), + "synchronized sensor calibration dataset index"); + + return config; +} + std::vector SensorCalibrationServiceNode::parse_csv_list(const std::string & raw) const { std::vector items; @@ -265,6 +333,17 @@ void SensorCalibrationServiceNode::execute_goal( input.chassis_history_available = false; input.control_history_available = false; input.truth_history_available = false; + input.data_window_start_timestamp_us = data_input_config_.data_window_start_timestamp_us; + input.data_window_end_timestamp_us = data_input_config_.data_window_end_timestamp_us; + input.image_sample_files = data_input_config_.image_sample_files; + input.imu_sample_files = data_input_config_.imu_sample_files; + input.pointcloud_sample_files = data_input_config_.pointcloud_sample_files; + input.laser_scan_sample_files = data_input_config_.laser_scan_sample_files; + input.target_detection_files = data_input_config_.target_detection_files; + input.robot_pose_sample_files = data_input_config_.robot_pose_sample_files; + input.synchronized_dataset_files = data_input_config_.synchronized_dataset_files; + input.storage_diagnostics.push_back( + "synchronized_dataset_files=" + std::to_string(input.synchronized_dataset_files.size())); SensorCalibrationAlgorithmTemplate::Output output; std::string failure_reason; diff --git a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp index ba0049a..4118496 100644 --- a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp +++ b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/include/workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp @@ -185,6 +185,7 @@ private: void finish_session_canceled( const std::shared_ptr goal_handle, const SessionExecutionContext & context); + void append_report_trace_files(WorkshopReport & report) const; // ─── 事件发布(同时发 topic + action feedback) ─── void publish_event( @@ -223,6 +224,8 @@ private: std::unique_ptr wifi6_link_client_; bool wifi6_precheck_enabled_{true}; bool wifi6_precheck_require_all_links_{true}; + std::string data_input_params_file_; + std::string dataset_index_file_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; // ─── ROS 接口 ─── diff --git a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp index b0e776f..7f3d499 100644 --- a/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp +++ b/agv_calib_brain/src/core/agv_calib_core/workshop_orchestrator/src/workshop_orchestrator_v2_node.cpp @@ -1,7 +1,10 @@ #include "workshop_orchestrator_v2/workshop_orchestrator_v2_node.hpp" // 对应头文件实现。 +#include // 检查报告文件是否已经存在。 #include // sleep_for 需要的时间单位。 +#include // 规范化现场数据输入和数据集索引路径。 #include // 拼接 session_id 时要用字符串流。 +#include // 文件系统查询失败时用 error_code 接住错误。 #include // 把整场执行放到后台线程时要用 std::thread。 #include "calibration_workshop_orchestration_interfaces/msg/approval_state.hpp" @@ -19,11 +22,106 @@ using calibration_workshop_orchestration_interfaces::srv::GetWorkshopSession; // using calibration_workshop_orchestration_interfaces::srv::PauseWorkshopSession; // 暂停接口短名。 using calibration_workshop_orchestration_interfaces::srv::ResumeWorkshopSession; // 恢复接口短名。 +namespace +{ +namespace fs = std::filesystem; + +std::string normalize_file_uri(const std::string & raw_path) +{ + if (raw_path.empty()) { + return ""; + } + + std::error_code ec; + const fs::path path(raw_path); + const fs::path absolute_path = path.is_absolute() ? path : fs::absolute(path, ec); + if (ec) { + return path.lexically_normal().string(); + } + return absolute_path.lexically_normal().string(); +} + +calibration_common_interfaces::msg::FileReference make_trace_file_reference( + const std::string & raw_path, + const std::string & description) +{ + calibration_common_interfaces::msg::FileReference ref; + ref.file_uri = normalize_file_uri(raw_path); + if (ref.file_uri.empty()) { + return ref; + } + + const fs::path path(ref.file_uri); + ref.file_name = path.filename().string(); + ref.description = description; + + std::error_code ec; + if (fs::exists(path, ec) && fs::is_regular_file(path, ec)) { + ref.size_bytes = static_cast(fs::file_size(path, ec)); + if (ec) { + ref.size_bytes = 0; + } + } + return ref; +} + +std::string infer_dataset_index_file(const std::string & data_input_params_file) +{ + const std::string normalized_data_input_file = normalize_file_uri(data_input_params_file); + if (normalized_data_input_file.empty()) { + return ""; + } + + const fs::path candidate = fs::path(normalized_data_input_file).parent_path() / "dataset_index.yaml"; + std::error_code ec; + if (!fs::exists(candidate, ec) || !fs::is_regular_file(candidate, ec)) { + return ""; + } + return candidate.lexically_normal().string(); +} + +void append_metadata_value( + WorkshopReport & report, + const std::string & key, + const std::string & value) +{ + if (value.empty()) { + return; + } + + calibration_common_interfaces::msg::KeyValuePair kv; + kv.key = key; + kv.value = value; + report.metadata.push_back(kv); +} + +void append_report_file( + WorkshopReport & report, + const calibration_common_interfaces::msg::FileReference & ref) +{ + if (ref.file_uri.empty()) { + return; + } + + const auto duplicate = std::find_if( + report.report_files.begin(), + report.report_files.end(), + [&ref](const auto & existing) { + return existing.file_uri == ref.file_uri; + }); + if (duplicate == report.report_files.end()) { + report.report_files.push_back(ref); + } +} +} // namespace + WorkshopOrchestratorV2Node::WorkshopOrchestratorV2Node(const rclcpp::NodeOptions & options) : Node("workshop_orchestrator_v2", options) // 创建一个名为 workshop_orchestrator_v2 的 ROS 节点。 { wifi6_precheck_enabled_ = declare_parameter("wifi6_precheck_enabled", true); wifi6_precheck_require_all_links_ = declare_parameter("wifi6_precheck_require_all_links", true); + data_input_params_file_ = declare_parameter("data_input_params_file", ""); + dataset_index_file_ = declare_parameter("dataset_index_file", ""); Wifi6LinkClient::Config wifi6_config; wifi6_config.vehicle_host = declare_parameter("wifi6_vehicle_host", "127.0.0.1"); wifi6_config.vehicle_port = static_cast(declare_parameter("wifi6_vehicle_port", 9000)); @@ -38,6 +136,10 @@ WorkshopOrchestratorV2Node::WorkshopOrchestratorV2Node(const rclcpp::NodeOptions wifi6_precheck_enabled_ = parameter.as_bool(); } else if (parameter.get_name() == "wifi6_precheck_require_all_links") { wifi6_precheck_require_all_links_ = parameter.as_bool(); + } else if (parameter.get_name() == "data_input_params_file") { + data_input_params_file_ = parameter.as_string(); + } else if (parameter.get_name() == "dataset_index_file") { + dataset_index_file_ = parameter.as_string(); } } return result; @@ -1019,6 +1121,23 @@ void WorkshopOrchestratorV2Node::record_stage_result( } } +void WorkshopOrchestratorV2Node::append_report_trace_files(WorkshopReport & report) const +{ + const auto data_input_ref = make_trace_file_reference( + data_input_params_file_, + "本次会话算法数据输入参数文件"); + append_report_file(report, data_input_ref); + append_metadata_value(report, "data_input_params_file", data_input_ref.file_uri); + + const std::string resolved_dataset_index_file = + dataset_index_file_.empty() ? infer_dataset_index_file(data_input_params_file_) : dataset_index_file_; + const auto dataset_index_ref = make_trace_file_reference( + resolved_dataset_index_file, + "本次会话真实采集数据集索引文件"); + append_report_file(report, dataset_index_ref); + append_metadata_value(report, "dataset_index_file", dataset_index_ref.file_uri); +} + void WorkshopOrchestratorV2Node::finish_session_precheck_failed( const std::shared_ptr goal_handle, const SessionExecutionContext & context, @@ -1038,6 +1157,7 @@ void WorkshopOrchestratorV2Node::finish_session_precheck_failed( context.started_timestamp_us, now_us(), record.last_precheck.message.empty() ? "Precheck failed." : record.last_precheck.message); + append_report_trace_files(record.report); record.executing = false; record.report_ready = true; publish_event(record, WorkshopEventType::REPORT_READY, "Report ready: precheck failed."); @@ -1069,6 +1189,7 @@ void WorkshopOrchestratorV2Node::finish_session_failed( context.started_timestamp_us, now_us(), failure_reason); + append_report_trace_files(record.report); record.executing = false; record.report_ready = true; publish_event(record, WorkshopEventType::REPORT_READY, "Report ready: session failed."); @@ -1099,6 +1220,7 @@ void WorkshopOrchestratorV2Node::finish_session_succeeded( context.started_timestamp_us, now_us(), "Workshop v2 minimal flow succeeded."); + append_report_trace_files(record.report); record.executing = false; record.report_ready = true; publish_event(record, WorkshopEventType::REPORT_READY, "Report ready."); @@ -1129,6 +1251,7 @@ void WorkshopOrchestratorV2Node::finish_session_canceled( context.started_timestamp_us, now_us(), "Session canceled."); + append_report_trace_files(record.report); record.executing = false; record.report_ready = true; publish_event(record, WorkshopEventType::SESSION_STATE_CHANGED, "Session canceled."); diff --git a/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md b/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md index ff19473..35d97fc 100644 --- a/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md +++ b/agv_calib_brain/src/deployment/checklists/sim_to_site_checklist.md @@ -1,18 +1,24 @@ -# Simulation To Site Checklist +# 从仿真迁移到现场的检查清单 -Before promoting a simulation-validated setup to site deployment, verify: - -- ROS 2 service/action/topic names match the deployment profile. -- `vehicle_id` is stable and checked by gateway and vehicle-side agent. -- `base_link` definition matches the real vehicle, especially rear axle center. -- Sensor frame IDs match the real URDF/TF tree. -- Calibration target dimensions are measured onsite and not copied from defaults. -- Vehicle-side agent owns low-level motion control. -- Workshop PC cannot publish directly to actuator topics. -- Emergency stop was tested from gateway and vehicle-side agent. -- Disconnect or timeout causes vehicle stop. -- WiFi or wired network timeout behavior was tested. -- Vehicle path is clear at the real site. -- First site run is low-speed and supervised. -- Calibration results include vehicle ID, config version, operator, and timestamp. +把已经通过仿真验证的配置迁移到真实现场前,需要确认: +- ROS 2 service、action、topic 名称与部署 profile 一致。 +- `vehicle_id` 稳定,并且 gateway 和车端 agent 都会校验。 +- `base_link` 定义与真实车辆一致,尤其是后轴中心位置。 +- 传感器 frame ID 与真实 URDF/TF 树一致。 +- 标定靶尺寸已在现场实测,不能直接沿用默认值。 +- 真实采集/ingest 已生成本次会话专用的 `dataset_index.yaml`。 +- 已使用 `validate_dataset_index.py` 校验本次 `dataset_index.yaml`。 +- 已在会话结束时调用收尾工具,从 `dataset_index.yaml` 生成 `site_data_input.yaml`。 +- 已使用 `run_local_data_input_smoke.sh` 验证数据输入注入和 report 链路。 +- 运行真实算法前,已通过 `data_input_params_file` 把数据输入参数文件传给现场 launch。 +- 最终 report 的 `report_files` 已包含本次 `site_data_input.yaml` 和 `dataset_index.yaml`。 +- 数据集索引只引用本次会话和本次数据窗口内的文件。 +- 车端 agent 负责底层运动控制。 +- 车间工控机不能直接向执行器 topic 发布控制指令。 +- 已从 gateway 和车端 agent 两侧测试急停。 +- 断连或超时会触发车辆停车。 +- 已测试 WiFi 或有线网络的超时行为。 +- 真实现场的车辆行驶路径已清空。 +- 第一次现场运行必须低速并有人监督。 +- 标定结果包含车辆 ID、配置版本、操作者和时间戳。 diff --git a/agv_calib_brain/src/deployment/profiles/example_dataset_index.yaml b/agv_calib_brain/src/deployment/profiles/example_dataset_index.yaml new file mode 100644 index 0000000..719547d --- /dev/null +++ b/agv_calib_brain/src/deployment/profiles/example_dataset_index.yaml @@ -0,0 +1,56 @@ +# 数据集索引样例:真实采集/ingest 每次标定会话应输出同类文件。 +# 具体转换命令见 src/docs/algorithm_template_contract.md。 + +schema_version: 1 + +session: + session_id: session_001 + site_id: site_a + vehicle_id: agv_001 + dataset_root: /data/agv_calib/site_a/session_001 + data_window_start_timestamp_us: 1770000000000000 + data_window_end_timestamp_us: 1770000120000000 + +data_inputs: + external: + external_observation_files: + - external/external_observations.csv + - external/truth_trajectory.csv + marker_observation_files: + - external/marker_observations.json + time_sync_diagnostic_files: + - external/time_sync_report.json + + chassis: + chassis_motion_data_files: + - chassis/chassis_motion.csv + actuator_command_files: + - chassis/actuator_commands.csv + truth_trajectory_files: + - external/truth_trajectory.csv + + control: + control_evaluation_data_files: + - control/control_eval.csv + reference_signal_files: + - control/reference_signal.csv + chassis_response_files: + - control/chassis_response.csv + truth_trajectory_files: + - external/truth_trajectory.csv + + sensor: + image_sample_files: + - sensor/front_camera/images.yaml + - sensor/down_camera/images.yaml + imu_sample_files: + - sensor/imu/imu_samples.mcap + pointcloud_sample_files: + - sensor/lidar_3d/pointclouds.mcap + laser_scan_sample_files: + - sensor/lidar_2d/scans.mcap + target_detection_files: + - sensor/front_camera/charuco_detections.json + - sensor/down_camera/charuco_detections.json + robot_pose_sample_files: + - sensor/hand_eye/robot_poses.csv diff --git a/agv_calib_brain/src/deployment/profiles/site_data_input_template.yaml b/agv_calib_brain/src/deployment/profiles/site_data_input_template.yaml new file mode 100644 index 0000000..a70fa01 --- /dev/null +++ b/agv_calib_brain/src/deployment/profiles/site_data_input_template.yaml @@ -0,0 +1,64 @@ +# ROS 2 参数模板:用于把真实标定采集数据注入算法输入。 +# +# 使用方式: +# 启动现场 launch 时通过 data_input_params_file 指向本文件副本。 +# +# 采集或数据接入程序应为每次标定会话生成一份本文件的副本, +# 并把数据集索引、rosbag/mcap、CSV/JSON 日志、图像、点云、 +# IMU 样本和检测结果等路径写入对应字段。 +# 推荐从 dataset_index.yaml 自动生成,转换工具为: +# src/deployment/tools/dataset_index_to_site_data_input.py + +external_localization_service: + ros__parameters: + data_input.data_window_start_timestamp_us: 0 + data_input.data_window_end_timestamp_us: 0 + # data_input.external_observation_files: + # - /data/agv_calib/session_001/external/external_observations.csv + # data_input.marker_observation_files: + # - /data/agv_calib/session_001/external/marker_observations.json + # data_input.time_sync_diagnostic_files: + # - /data/agv_calib/session_001/external/time_sync_report.json + +chassis_calibration_service: + ros__parameters: + data_input.data_window_start_timestamp_us: 0 + data_input.data_window_end_timestamp_us: 0 + # data_input.chassis_motion_data_files: + # - /data/agv_calib/session_001/chassis/chassis_motion.csv + # data_input.actuator_command_files: + # - /data/agv_calib/session_001/chassis/actuator_commands.csv + # data_input.truth_trajectory_files: + # - /data/agv_calib/session_001/external/truth_trajectory.csv + +control_calibration_service: + ros__parameters: + data_input.data_window_start_timestamp_us: 0 + data_input.data_window_end_timestamp_us: 0 + # data_input.control_evaluation_data_files: + # - /data/agv_calib/session_001/control/control_eval.csv + # data_input.reference_signal_files: + # - /data/agv_calib/session_001/control/reference_signal.csv + # data_input.chassis_response_files: + # - /data/agv_calib/session_001/control/chassis_response.csv + # data_input.truth_trajectory_files: + # - /data/agv_calib/session_001/external/truth_trajectory.csv + +sensor_calibration_service: + ros__parameters: + data_input.data_window_start_timestamp_us: 0 + data_input.data_window_end_timestamp_us: 0 + # data_input.image_sample_files: + # - /data/agv_calib/session_001/sensor/front_camera/images.yaml + # data_input.imu_sample_files: + # - /data/agv_calib/session_001/sensor/imu/imu_samples.mcap + # data_input.pointcloud_sample_files: + # - /data/agv_calib/session_001/sensor/lidar_3d/pointclouds.mcap + # data_input.laser_scan_sample_files: + # - /data/agv_calib/session_001/sensor/lidar_2d/scans.mcap + # data_input.target_detection_files: + # - /data/agv_calib/session_001/sensor/front_camera/charuco_detections.json + # data_input.robot_pose_sample_files: + # - /data/agv_calib/session_001/sensor/hand_eye/robot_poses.csv + # data_input.synchronized_dataset_files: + # - /data/agv_calib/session_001/dataset_index.yaml diff --git a/agv_calib_brain/src/deployment/profiles/site_template.yaml b/agv_calib_brain/src/deployment/profiles/site_template.yaml index f397a31..21ed8e1 100644 --- a/agv_calib_brain/src/deployment/profiles/site_template.yaml +++ b/agv_calib_brain/src/deployment/profiles/site_template.yaml @@ -36,10 +36,23 @@ external_pose_bridge: workshop_sensor_ingest: type: workshop_sensor_ingest + capture_session_tool: src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py publish_prefix: /workshop/vehicle_sensor poll_hz: 15.0 max_payload_bytes: 4194304 +algorithm_data_inputs: + # 每次标定会话对应的 ROS 参数文件,由 minimal_workshop_demo.launch.py + # 通过 data_input_params_file:=... 注入。 + ros_params_file: /data/agv_calib/replace_with_site_or_line_id/session_xxx/site_data_input.yaml + # 真实采集或数据接入程序应把每次会话的数据集写到这里。 + dataset_root: /data/agv_calib/replace_with_site_or_line_id + dataset_index_name: dataset_index.yaml + # 推荐由 dataset_index_to_site_data_input.py 从 dataset_index.yaml 自动生成。 + converter: src/deployment/tools/dataset_index_to_site_data_input.py + # 真实采集/ingest 会话结束时推荐调用这个收尾入口。 + session_finalizer: src/deployment/tools/finalize_site_session.py + frames: map: workshop base_link: rear_axle_center diff --git a/agv_calib_brain/src/deployment/tools/dataset_index_to_site_data_input.py b/agv_calib_brain/src/deployment/tools/dataset_index_to_site_data_input.py new file mode 100644 index 0000000..3c20b8b --- /dev/null +++ b/agv_calib_brain/src/deployment/tools/dataset_index_to_site_data_input.py @@ -0,0 +1,268 @@ +#!/usr/bin/env python3 +"""把真实采集数据集索引转换为算法服务可读取的 ROS 2 参数文件。""" + +from __future__ import annotations + +import argparse +import re +import sys +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as exc: + raise SystemExit("缺少 PyYAML,请先在当前 Python 环境安装 yaml 模块。") from exc + + +URI_SCHEME_RE = re.compile(r"^[A-Za-z][A-Za-z0-9+.-]*://") + +SERVICE_INPUT_FIELDS = { + "external_localization_service": { + "index_section": "external", + "fields": [ + "external_observation_files", + "marker_observation_files", + "time_sync_diagnostic_files", + ], + }, + "chassis_calibration_service": { + "index_section": "chassis", + "fields": [ + "chassis_motion_data_files", + "actuator_command_files", + "truth_trajectory_files", + ], + }, + "control_calibration_service": { + "index_section": "control", + "fields": [ + "control_evaluation_data_files", + "reference_signal_files", + "chassis_response_files", + "truth_trajectory_files", + ], + }, + "sensor_calibration_service": { + "index_section": "sensor", + "fields": [ + "image_sample_files", + "imu_sample_files", + "pointcloud_sample_files", + "laser_scan_sample_files", + "target_detection_files", + "robot_pose_sample_files", + "synchronized_dataset_files", + ], + }, +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="根据真实采集 dataset_index.yaml 生成 site_data_input.yaml。" + ) + parser.add_argument( + "dataset_index", + help="真实采集或 ingest 生成的数据集索引文件。", + ) + parser.add_argument( + "-o", + "--output", + help="输出的 ROS 2 参数文件;不填写时输出到标准输出。", + ) + parser.add_argument( + "--dataset-root", + help="覆盖索引文件中的 session.dataset_root,用于现场挂载路径重定向。", + ) + parser.add_argument( + "--no-index-reference", + action="store_true", + help="不自动把 dataset_index.yaml 写入 sensor 的 synchronized_dataset_files。", + ) + parser.add_argument( + "--strict", + action="store_true", + help="严格检查:要求时间窗口有效,并且至少存在一个数据文件引用。", + ) + return parser.parse_args() + + +def load_yaml(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + if data is None: + return {} + if not isinstance(data, dict): + raise ValueError(f"{path} 的顶层结构必须是 YAML map。") + return data + + +def is_uri(value: str) -> bool: + return bool(URI_SCHEME_RE.match(value)) + + +def normalize_dataset_root(index_path: Path, raw_root: str | None) -> Path: + if not raw_root: + return index_path.parent + root = Path(raw_root).expanduser() + if root.is_absolute(): + return root + return (index_path.parent / root).resolve(strict=False) + + +def resolve_file_uri(raw_uri: str, dataset_root: Path) -> str: + uri = raw_uri.strip() + if not uri: + return "" + if is_uri(uri): + return uri + path = Path(uri).expanduser() + if path.is_absolute(): + return str(path) + return str((dataset_root / path).resolve(strict=False)) + + +def extract_file_uri(entry: Any) -> str: + if isinstance(entry, str): + return entry + if isinstance(entry, dict): + for key in ("file_uri", "uri", "path", "file"): + value = entry.get(key) + if isinstance(value, str): + return value + raise ValueError(f"文件引用条目必须是字符串,或包含 file_uri/uri/path/file 的 map:{entry!r}") + + +def read_file_list(value: Any, dataset_root: Path) -> list[str]: + if value is None: + return [] + entries = value if isinstance(value, list) else [value] + result: list[str] = [] + for entry in entries: + file_uri = resolve_file_uri(extract_file_uri(entry), dataset_root) + if file_uri and file_uri not in result: + result.append(file_uri) + return result + + +def read_int(value: Any, field_name: str) -> int: + if value in (None, ""): + return 0 + try: + return int(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是整数微秒时间戳,当前值为 {value!r}。") from exc + + +def module_data(index: dict[str, Any], section_name: str) -> dict[str, Any]: + data_inputs = index.get("data_inputs") + if data_inputs is None: + data_inputs = index.get("files", {}) + if not isinstance(data_inputs, dict): + raise ValueError("data_inputs 必须是 YAML map。") + section = data_inputs.get(section_name, {}) + if section is None: + return {} + if not isinstance(section, dict): + raise ValueError(f"data_inputs.{section_name} 必须是 YAML map。") + return section + + +def window_value(session: dict[str, Any], section: dict[str, Any], key: str) -> int: + return read_int(section.get(key, session.get(key, 0)), key) + + +def build_ros_params( + index_path: Path, + index: dict[str, Any], + dataset_root_override: str | None, + add_index_reference: bool, +) -> dict[str, Any]: + session = index.get("session", {}) + if session is None: + session = {} + if not isinstance(session, dict): + raise ValueError("session 必须是 YAML map。") + + dataset_root = normalize_dataset_root( + index_path, + dataset_root_override or session.get("dataset_root"), + ) + + output: dict[str, Any] = {} + for service_name, config in SERVICE_INPUT_FIELDS.items(): + section_name = str(config["index_section"]) + section = module_data(index, section_name) + params: dict[str, Any] = { + "data_input.data_window_start_timestamp_us": window_value( + session, section, "data_window_start_timestamp_us" + ), + "data_input.data_window_end_timestamp_us": window_value( + session, section, "data_window_end_timestamp_us" + ), + } + for field_name in config["fields"]: + files = read_file_list(section.get(field_name), dataset_root) + if ( + add_index_reference + and section_name == "sensor" + and field_name == "synchronized_dataset_files" + ): + index_uri = str(index_path.resolve(strict=False)) + if index_uri not in files: + files.append(index_uri) + if files: + params[f"data_input.{field_name}"] = files + output[service_name] = {"ros__parameters": params} + return output + + +def validate_output(output: dict[str, Any], strict: bool) -> None: + if not strict: + return + file_count = 0 + for service_name, node_config in output.items(): + params = node_config["ros__parameters"] + start_us = params["data_input.data_window_start_timestamp_us"] + end_us = params["data_input.data_window_end_timestamp_us"] + if start_us <= 0 or end_us <= 0 or end_us <= start_us: + raise ValueError(f"{service_name} 的数据时间窗口无效。") + file_count += sum( + len(value) + for key, value in params.items() + if key.startswith("data_input.") and isinstance(value, list) + ) + if file_count == 0: + raise ValueError("严格检查要求至少存在一个数据文件引用。") + + +def main() -> int: + args = parse_args() + index_path = Path(args.dataset_index).expanduser().resolve(strict=False) + index = load_yaml(index_path) + output = build_ros_params( + index_path, + index, + args.dataset_root, + not args.no_index_reference, + ) + validate_output(output, args.strict) + rendered = yaml.safe_dump(output, sort_keys=False, allow_unicode=True) + + if args.output: + output_path = Path(args.output).expanduser() + output_path.parent.mkdir(parents=True, exist_ok=True) + output_path.write_text(rendered, encoding="utf-8") + print(f"已生成 ROS 2 数据输入参数文件:{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/deployment/tools/finalize_site_session.py b/agv_calib_brain/src/deployment/tools/finalize_site_session.py new file mode 100644 index 0000000..14cde0d --- /dev/null +++ b/agv_calib_brain/src/deployment/tools/finalize_site_session.py @@ -0,0 +1,171 @@ +#!/usr/bin/env python3 +"""收尾真实标定会话,并生成算法服务数据输入参数文件。""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path +from typing import Any + +import yaml + +from dataset_index_to_site_data_input import ( + build_ros_params, + load_yaml, + validate_output, +) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="根据现场 profile 和本次会话数据集索引生成 site_data_input.yaml。" + ) + parser.add_argument( + "--site-profile", + default="src/deployment/profiles/site_template.yaml", + help="现场部署 profile 路径。", + ) + parser.add_argument( + "--session-id", + help="本次标定会话 ID;未提供 --session-dir 时,会拼到 dataset_root 后面。", + ) + parser.add_argument( + "--session-dir", + help="本次标定会话目录;优先级高于 --session-id。", + ) + parser.add_argument( + "--dataset-index", + help="本次会话 dataset_index.yaml 路径;优先级高于 profile 中的约定。", + ) + parser.add_argument( + "-o", + "--output", + help="输出的 site_data_input.yaml 路径;默认写入会话目录。", + ) + parser.add_argument( + "--no-strict", + action="store_true", + help="关闭严格检查;默认要求时间窗口有效并且存在数据文件引用。", + ) + parser.add_argument( + "--no-index-reference", + action="store_true", + help="不自动把 dataset_index.yaml 写入 sensor 的 synchronized_dataset_files。", + ) + return parser.parse_args() + + +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 resolve_relative(path_value: str, base_dir: Path) -> Path: + path = Path(path_value).expanduser() + if path.is_absolute(): + return path + return (base_dir / path).resolve(strict=False) + + +def load_site_profile(path: Path) -> dict[str, Any]: + profile = load_yaml(path) + return require_map(profile, "site_profile") + + +def resolve_session_dir( + profile: dict[str, Any], + profile_path: Path, + session_id: str | None, + session_dir_arg: str | None, +) -> Path: + if session_dir_arg: + return resolve_relative(session_dir_arg, Path.cwd()) + + data_inputs = require_map(profile.get("algorithm_data_inputs", {}), "algorithm_data_inputs") + dataset_root_raw = data_inputs.get("dataset_root") + if not isinstance(dataset_root_raw, str) or not dataset_root_raw: + raise ValueError("缺少 algorithm_data_inputs.dataset_root。") + + dataset_root = resolve_relative(dataset_root_raw, profile_path.parent) + if session_id: + return dataset_root / session_id + return dataset_root + + +def resolve_dataset_index( + profile: dict[str, Any], + profile_path: Path, + session_dir: Path, + dataset_index_arg: str | None, +) -> Path: + if dataset_index_arg: + return resolve_relative(dataset_index_arg, Path.cwd()) + + data_inputs = require_map(profile.get("algorithm_data_inputs", {}), "algorithm_data_inputs") + index_name = data_inputs.get("dataset_index_name", "dataset_index.yaml") + if not isinstance(index_name, str) or not index_name: + raise ValueError("algorithm_data_inputs.dataset_index_name 必须是非空字符串。") + + index_path = Path(index_name).expanduser() + if index_path.is_absolute(): + return index_path + return (session_dir / index_path).resolve(strict=False) + + +def resolve_output_path( + profile: dict[str, Any], + profile_path: Path, + session_dir: Path, + session_id: str | None, + output_arg: str | None, +) -> Path: + if output_arg: + return resolve_relative(output_arg, Path.cwd()) + data_inputs = require_map(profile.get("algorithm_data_inputs", {}), "algorithm_data_inputs") + ros_params_file = data_inputs.get("ros_params_file") + if isinstance(ros_params_file, str) and ros_params_file: + output_value = ros_params_file + if session_id: + output_value = output_value.replace("{session_id}", session_id) + output_value = output_value.replace("session_xxx", session_id) + if "replace_with" not in output_value and "session_xxx" not in output_value: + return resolve_relative(output_value, profile_path.parent) + return (session_dir / "site_data_input.yaml").resolve(strict=False) + + +def main() -> int: + args = parse_args() + profile_path = Path(args.site_profile).expanduser().resolve(strict=False) + profile = load_site_profile(profile_path) + session_dir = resolve_session_dir(profile, profile_path, args.session_id, args.session_dir) + dataset_index_path = resolve_dataset_index(profile, profile_path, session_dir, args.dataset_index) + output_path = resolve_output_path(profile, profile_path, session_dir, args.session_id, args.output) + + index = load_yaml(dataset_index_path) + output = build_ros_params( + dataset_index_path, + index, + str(session_dir), + not args.no_index_reference, + ) + validate_output(output, strict=not args.no_strict) + + output_path.parent.mkdir(parents=True, exist_ok=True) + output_path.write_text( + yaml.safe_dump(output, sort_keys=False, allow_unicode=True), + encoding="utf-8", + ) + + print(f"已完成会话数据输入收尾:{output_path}") + print(f"启动参数:data_input_params_file:={output_path}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/deployment/tools/validate_dataset_index.py b/agv_calib_brain/src/deployment/tools/validate_dataset_index.py new file mode 100755 index 0000000..73f946b --- /dev/null +++ b/agv_calib_brain/src/deployment/tools/validate_dataset_index.py @@ -0,0 +1,252 @@ +#!/usr/bin/env python3 +"""校验真实采集数据集索引是否满足算法输入合同。""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path +from typing import Any + +from dataset_index_to_site_data_input import ( + SERVICE_INPUT_FIELDS, + extract_file_uri, + is_uri, + load_yaml, + normalize_dataset_root, + resolve_file_uri, +) + + +TASK_REQUIREMENTS = { + "external": { + "section": "external", + "any_of": ["external_observation_files", "marker_observation_files"], + }, + "chassis": { + "section": "chassis", + "all_of": ["chassis_motion_data_files"], + }, + "control": { + "section": "control", + "all_of": ["control_evaluation_data_files"], + }, + "sensor_intrinsic": { + "section": "sensor", + "all_of": ["image_sample_files"], + "warn_if_missing": ["target_detection_files"], + }, + "sensor_extrinsic": { + "section": "sensor", + "all_of": ["image_sample_files", "robot_pose_sample_files"], + "warn_if_missing": ["target_detection_files"], + }, + "hand_eye": { + "section": "sensor", + "all_of": ["image_sample_files", "robot_pose_sample_files"], + }, +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="校验 dataset_index.yaml。") + parser.add_argument("dataset_index", help="待校验的数据集索引文件。") + parser.add_argument( + "--tasks", + default="external,chassis,control,sensor_intrinsic", + help="逗号分隔的任务列表,用于检查每个任务的最小数据输入。", + ) + parser.add_argument( + "--dataset-root", + help="覆盖索引文件中的 session.dataset_root,用于现场挂载路径重定向。", + ) + parser.add_argument( + "--require-existing-files", + action="store_true", + help="要求所有本地文件引用都真实存在;URI 引用只检查格式。", + ) + parser.add_argument( + "--no-strict-window", + action="store_true", + help="不强制要求数据窗口大于 0 且结束时间晚于开始时间。", + ) + return parser.parse_args() + + +def parse_tasks(raw_tasks: str) -> list[str]: + tasks: list[str] = [] + for item in raw_tasks.split(","): + task = item.strip() + if not task: + continue + if task not in TASK_REQUIREMENTS: + raise ValueError(f"未知任务 {task!r},可选任务:{', '.join(TASK_REQUIREMENTS)}") + tasks.append(task) + if not tasks: + raise ValueError("至少需要指定一个任务。") + return tasks + + +def known_sections() -> set[str]: + return {str(config["index_section"]) for config in SERVICE_INPUT_FIELDS.values()} + + +def known_fields_by_section() -> dict[str, set[str]]: + result: dict[str, set[str]] = {} + for config in SERVICE_INPUT_FIELDS.values(): + section = str(config["index_section"]) + result.setdefault(section, set()).update(str(field) for field in config["fields"]) + return result + + +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 read_int(value: Any, field_name: str) -> int: + try: + return int(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{field_name} 必须是整数,当前值为 {value!r}。") from exc + + +def file_entries(value: Any) -> list[Any]: + if value is None: + return [] + return value if isinstance(value, list) else [value] + + +def resolve_entries(value: Any, dataset_root: Path) -> list[str]: + result: list[str] = [] + for entry in file_entries(value): + uri = resolve_file_uri(extract_file_uri(entry), dataset_root) + if uri and uri not in result: + result.append(uri) + return result + + +def field_has_files(section: dict[str, Any], field_name: str) -> bool: + return bool(file_entries(section.get(field_name))) + + +def validate_session( + index: dict[str, Any], + strict_window: bool, +) -> dict[str, Any]: + if read_int(index.get("schema_version"), "schema_version") != 1: + raise ValueError("schema_version 必须为 1。") + session = require_map(index.get("session"), "session") + for field_name in ("session_id", "vehicle_id", "dataset_root"): + value = session.get(field_name) + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"session.{field_name} 必须是非空字符串。") + + start_us = read_int( + session.get("data_window_start_timestamp_us"), + "session.data_window_start_timestamp_us", + ) + end_us = read_int( + session.get("data_window_end_timestamp_us"), + "session.data_window_end_timestamp_us", + ) + if strict_window and (start_us <= 0 or end_us <= 0 or end_us <= start_us): + raise ValueError("数据窗口无效,要求 start_us > 0 且 end_us > start_us。") + return session + + +def validate_sections(index: dict[str, Any]) -> dict[str, dict[str, Any]]: + data_inputs = require_map(index.get("data_inputs"), "data_inputs") + valid_sections = known_sections() + valid_fields = known_fields_by_section() + result: dict[str, dict[str, Any]] = {} + + for section_name, section_value in data_inputs.items(): + if section_name not in valid_sections: + raise ValueError(f"未知数据输入模块 data_inputs.{section_name}。") + section = require_map(section_value, f"data_inputs.{section_name}") + for field_name in section: + if field_name not in valid_fields[section_name]: + raise ValueError(f"未知数据输入字段 data_inputs.{section_name}.{field_name}。") + result[section_name] = section + + for section_name in valid_sections: + result.setdefault(section_name, {}) + return result + + +def validate_task_requirements( + sections: dict[str, dict[str, Any]], + tasks: list[str], +) -> list[str]: + warnings: list[str] = [] + for task in tasks: + requirement = TASK_REQUIREMENTS[task] + section_name = str(requirement["section"]) + section = sections.get(section_name, {}) + + for field_name in requirement.get("all_of", []): + if not field_has_files(section, str(field_name)): + raise ValueError(f"任务 {task} 缺少 data_inputs.{section_name}.{field_name}。") + + any_of = [str(field) for field in requirement.get("any_of", [])] + if any_of and not any(field_has_files(section, field_name) for field_name in any_of): + fields = ", ".join(f"data_inputs.{section_name}.{field_name}" for field_name in any_of) + raise ValueError(f"任务 {task} 至少需要以下字段之一:{fields}。") + + for field_name in requirement.get("warn_if_missing", []): + if not field_has_files(section, str(field_name)): + warnings.append(f"任务 {task} 未提供 data_inputs.{section_name}.{field_name}。") + return warnings + + +def validate_file_references( + sections: dict[str, dict[str, Any]], + dataset_root: Path, + require_existing_files: bool, +) -> list[str]: + warnings: list[str] = [] + for section_name, section in sections.items(): + for field_name, value in section.items(): + files = resolve_entries(value, dataset_root) + if not files: + warnings.append(f"data_inputs.{section_name}.{field_name} 为空。") + continue + if not require_existing_files: + continue + for file_uri in files: + if is_uri(file_uri): + continue + if not Path(file_uri).exists(): + raise ValueError(f"文件不存在:{file_uri}") + return warnings + + +def main() -> int: + args = parse_args() + index_path = Path(args.dataset_index).expanduser().resolve(strict=False) + index = load_yaml(index_path) + tasks = parse_tasks(args.tasks) + session = validate_session(index, strict_window=not args.no_strict_window) + sections = validate_sections(index) + dataset_root = normalize_dataset_root(index_path, args.dataset_root or session.get("dataset_root")) + + warnings = [] + warnings.extend(validate_task_requirements(sections, tasks)) + warnings.extend(validate_file_references(sections, dataset_root, args.require_existing_files)) + + print(f"[OK] dataset_index 校验通过: {index_path}") + print(f"[OK] 数据集根目录: {dataset_root}") + print(f"[OK] 任务列表: {', '.join(tasks)}") + for warning in warnings: + print(f"[WARN] {warning}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/agv_calib_brain/src/docs/algorithm_template_contract.md b/agv_calib_brain/src/docs/algorithm_template_contract.md new file mode 100644 index 0000000..31ce4a8 --- /dev/null +++ b/agv_calib_brain/src/docs/algorithm_template_contract.md @@ -0,0 +1,653 @@ +# 算法模板合同 + +本文档定义自动化标定车间的算法接入模板边界。它面向两类角色: + +- 模板维护者:维护 ROS 2 接口、service/action 节点、算法模板分发、报告汇总和 smoke test。 +- 算法实现者:只在指定算法文件中补充具体标定算法、质量评估和产物保存逻辑。 + +当前模板状态按 `v0.9` 处理:调度框架、输入输出结构和结果回填路径已经可以作为算法开发基线;真实现场数据接入后,如果发现原始数据窗口或产物字段不够,可以由模板维护者小范围扩展 `Input` 结构和接口文档。 + +## 总原则 + +算法实现者默认只改各模块 `src/*_algorithm.cpp` 文件,不改 node、launch、gateway、orchestrator、report 和 smoke 主流程。 + +算法文件必须满足这些约束: + +- 保留 `run(const Input &, Output &, std::string & failure_reason)` 入口签名。 +- 失败时返回 `false`,同时写清楚 `failure_reason`。 +- 成功时返回 `true`,并完整填写 `output.response.result`。 +- 不直接发布 ROS topic,不直接创建 ROS service/client,不绕过模板写报告。 +- 不把 Isaac、仿真车辆、真实车辆 SDK、PLC/CAN 代码写进算法文件。 +- 需要新增输入字段时,先由模板维护者扩展对应 `Input` 结构,再由 node 层统一填充。 +- 需要新增输出字段时,先评估是否已有 `validation_summary`、`estimated_*`、`artifacts` 可承载;确实不够时再改接口包。 + +## 统一执行链路 + +四个算法域都遵循同一个形态: + +```text +orchestrator / gateway + -> service node 组装 Input + -> AlgorithmTemplate 做通用校验和分发 + -> 具体 Algorithm::run(...) 执行 + -> Output.response.result 回填 + -> orchestrator 汇总 stage result + -> report 输出 +``` + +通用校验只检查请求是否完整、任务类型是否明确、必要 ID 是否存在。业务层更细的判断,例如采样数量、图像质量、轨迹长度、时间同步、真值源质量、目标板检测质量,应写在具体算法文件里。 + +## 稳定边界 + +这些部分视为模板边界,算法实现者不应修改: + +- `src/core/agv_calib_core/workshop_orchestrator/` +- `src/communication/win_ubuntu_bridge/` +- `src/core/agv_calib_core/*_service/src/*_service_node.cpp` +- `src/core/agv_calib_core/*_service/src/*_algorithm_template.cpp` +- `src/core/agv_calib_core/*_service/include/*/*_algorithm_template.hpp` +- `run_isaac_real_sim_test.sh` +- `src/simulation/tools/smoke_test_workshop_orchestrator.py` + +这些部分是算法实现者的主要填充位置: + +- `src/core/agv_calib_core/external_localization_service/src/reference_pose_collection_algorithm.cpp` +- `src/core/agv_calib_core/external_localization_service/src/marker_alignment_algorithm.cpp` +- `src/core/agv_calib_core/external_localization_service/src/truth_source_validation_algorithm.cpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/ackermann_chassis_algorithm.cpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/differential_chassis_algorithm.cpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/single_steer_wheel_chassis_algorithm.cpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/multi_steer_wheel_chassis_algorithm.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/pid_control_calibration_algorithm.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/mpc_control_calibration_algorithm.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/lqr_control_calibration_algorithm.cpp` +- `src/core/agv_calib_core/control_calibration_service/src/pure_pursuit_control_calibration_algorithm.cpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/camera_intrinsic_calibration_algorithm.cpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/imu_intrinsic_calibration_algorithm.cpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/sensor_to_base_extrinsic_calibration_algorithm.cpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/hand_eye_calibration_algorithm.cpp` + +## 统一输出要求 + +每个算法最终都要写入对应 action result。通用字段含义如下: + +- `success`:算法任务是否执行成功。 +- `error_code`:失败时必须写非 OK 错误码;成功时写 OK。 +- `message`:面向总控、日志和报告的简短结果说明。 +- `job_id`:通常沿用请求里的 `request_id`。 +- `data_quality_passed`:采集数据或执行数据是否满足算法输入质量。 +- `suitable_for_commit`:是否建议把本轮结果进入参数提交或人工确认环节。 +- `recommended_parameter_version`:本轮推荐结果版本号,必须能用于追踪。 +- `validation_summary`:自动验收摘要,是 report 和 stage result 的核心依据。 +- `estimated_params` / `estimated_parameter_set` / `result`:本轮估计出的参数或外部定位结果。 +- `artifacts`:算法产物引用,例如 YAML、JSON、CSV、图片、波形图、拟合日志和报告片段。 + +成功但不建议提交时,允许 `success=true`、`data_quality_passed=true`、`suitable_for_commit=false`。例如算法完成了评估,但质量指标未达到自动写入阈值。 + +采集数据不足、真值源不可用、车辆状态不满足、安全条件不满足时,应返回 `false` 并写明 `failure_reason`。 + +## 统一数据输入合同 + +算法模板的数据输入分为五类。算法实现者只能从这些入口读取数据,不应自己订阅 ROS topic、连接 TCP、读取未声明的临时目录,或绕过 service node 访问仿真/真实设备。 + +- 任务请求:`input.request` 以及已经展开的 `task_type`、`task_subtype`、`required_*`、`reference_*` 字段,表达“本次要做什么”。 +- 静态上下文:`input.workshop_session`、`input.vehicle_profile`、目标传感器 ID、底盘类型、控制器类型、frame ID,表达“在哪台车、哪个工位、哪个坐标系下做”。 +- 轻量在线数据:各模块的 `latest_*_telemetry` 和 `*_telemetry_history`,表达“最近一段时间的状态、质量和趋势”。 +- 大文件数据:各模块新增的 `*data_files`、`*sample_files`、`*trajectory_files`、`synchronized_dataset_files`,通过 `calibration_common_interfaces/FileReference` 指向图片、点云、IMU 包、CSV、JSON、rosbag、mcap 或数据集索引。 +- 诊断与有效性:`*_diagnostics`、`*_history_available`、`data_window_start_timestamp_us`、`data_window_end_timestamp_us`,表达“这些数据是否可信、采样窗口是什么、缺什么”。 + +大文件数据不直接塞进 action result 或 `Input` 内存对象。模板统一使用 `FileReference` 传递文件引用: + +- `file_name`:文件名或数据集逻辑名。 +- `file_uri`:本机绝对路径、挂载路径或约定 URI。 +- `size_bytes`:可选,未知时填 0。 +- `description`:说明数据来源、传感器 ID、时间窗口、格式。 +- `digest`:可选,现场归档或验收时建议填写。 + +算法实现者应按这个顺序选择输入: + +1. 优先使用 `synchronized_dataset_files` 或对应模块的数据集索引文件,因为它能表达多传感器、真值、检测结果和时间同步关系。 +2. 没有数据集索引时,使用领域内专用文件列表,例如 `image_sample_files`、`imu_sample_files`、`chassis_motion_data_files`、`control_evaluation_data_files`。 +3. 只做 readiness、链路、轻量统计或 smoke 时,可以使用 `*_telemetry_history`。 +4. 如果本算法必须使用原始数据,而对应文件列表为空,应返回 `false`,并把缺失项写入 `failure_reason`。 + +数据窗口约定: + +- `data_window_start_timestamp_us` 和 `data_window_end_timestamp_us` 使用微秒 Unix 时间戳。 +- 两者都为 0 表示上层尚未提供明确窗口,算法可以从文件或 telemetry 自行推断。 +- 如果只给了 telemetry history,没有文件引用,算法只能做轻量统计或模板级验证,不能宣称完成真实高精度标定。 +- 如果文件引用和 telemetry history 同时存在,算法应优先用文件数据求解,用 telemetry history 做质量和安全状态交叉检查。 + +当前数据填充责任: + +- `external_localization_service` 已经缓存外部定位 telemetry history,并将其填入 `ExternalLocalizationInput`。 +- `external_localization_service`、`chassis_calibration_service`、`control_calibration_service`、`sensor_calibration_service` 都支持通过 ROS 参数 `data_input.*` 传入数据窗口和数据文件路径,并在 node 层转换成 `FileReference` 填入 `Input`。 +- 真实采集链路接入后,应由采集/ingest 节点负责落盘并把这些参数指向本轮任务的数据集;算法实现者不应自己寻找临时目录。 +- gateway 模式下,chassis/control 的真实执行可能在车端 agent 完成;这种情况下车端返回的 `ChassisJobResult` / `ControlJobResult` 也必须按同一套数据输入和产物追溯规则生成。 + +当前已支持的 ROS 参数如下。所有文件列表参数类型都是 `string[]`,窗口参数类型是 `int64`。 + +External: + +- `data_input.data_window_start_timestamp_us` +- `data_input.data_window_end_timestamp_us` +- `data_input.external_observation_files` +- `data_input.marker_observation_files` +- `data_input.time_sync_diagnostic_files` + +Chassis: + +- `data_input.data_window_start_timestamp_us` +- `data_input.data_window_end_timestamp_us` +- `data_input.chassis_motion_data_files` +- `data_input.actuator_command_files` +- `data_input.truth_trajectory_files` + +Control: + +- `data_input.data_window_start_timestamp_us` +- `data_input.data_window_end_timestamp_us` +- `data_input.control_evaluation_data_files` +- `data_input.reference_signal_files` +- `data_input.chassis_response_files` +- `data_input.truth_trajectory_files` + +Sensor: + +- `data_input.data_window_start_timestamp_us` +- `data_input.data_window_end_timestamp_us` +- `data_input.image_sample_files` +- `data_input.imu_sample_files` +- `data_input.pointcloud_sample_files` +- `data_input.laser_scan_sample_files` +- `data_input.target_detection_files` +- `data_input.robot_pose_sample_files` +- `data_input.synchronized_dataset_files` + +配置示例: + +```yaml +sensor_calibration_service: + ros__parameters: + data_input.data_window_start_timestamp_us: 0 + data_input.data_window_end_timestamp_us: 0 + data_input.image_sample_files: + - /data/calib/session_001/front_camera/images.yaml + data_input.target_detection_files: + - /data/calib/session_001/front_camera/charuco_detections.json + data_input.synchronized_dataset_files: + - /data/calib/session_001/dataset_index.yaml +``` + +现场启动时,推荐把这些参数放到一个 session-specific ROS 参数文件里,然后通过 launch 注入: + +```bash +ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \ + use_gateway:=true \ + chassis_host:=192.168.10.42 \ + control_host:=192.168.10.42 \ + data_input_params_file:=/data/agv_calib/session_001/site_data_input.yaml \ + dataset_index_file:=/data/agv_calib/session_001/dataset_index.yaml +``` + +仓库提供了模板文件: + +- `src/deployment/profiles/site_data_input_template.yaml` + +真实采集/ingest 程序应为每次 session 生成一份副本,并把本轮采集产物路径写入其中。 + +### 数据集索引到算法输入参数 + +真实采集/ingest 的推荐输出是 `dataset_index.yaml`,再由工具转换成 +`site_data_input.yaml`。这样采集侧只维护一份数据索引,算法服务仍然只读取 +稳定的 `data_input.*` ROS 参数。 + +数据集索引样例: + +- `src/deployment/profiles/example_dataset_index.yaml` + +转换工具: + +- `src/deployment/tools/dataset_index_to_site_data_input.py` +- `src/deployment/tools/finalize_site_session.py` + +真实采集侧脚手架: + +- `src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py` + +校验与本地回归: + +- `src/deployment/tools/validate_dataset_index.py` +- `run_local_data_input_smoke.sh` + +真实采集/ingest 在本次会话结束时推荐调用收尾工具。它会读取现场 +`site_template.yaml`,按 `algorithm_data_inputs.dataset_root`、 +`dataset_index_name` 和 `session_id` 找到数据集索引,并生成 +`site_data_input.yaml`: + +```bash +python3 src/deployment/tools/finalize_site_session.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --session-id session_001 +``` + +底层转换命令也可以直接使用: + +```bash +python3 src/deployment/tools/dataset_index_to_site_data_input.py \ + /data/agv_calib/session_001/dataset_index.yaml \ + -o /data/agv_calib/session_001/site_data_input.yaml \ + --strict +``` + +数据集索引校验命令: + +```bash +python3 src/deployment/tools/validate_dataset_index.py \ + /data/agv_calib/session_001/dataset_index.yaml \ + --tasks external,chassis,control,sensor_intrinsic +``` + +生成参数文件后,可以用现有 smoke 脚本验证注入链路。若要同时验证 +external、chassis、control 和 sensor 四个本地算法服务都读取了同一份 +`site_data_input.yaml`,并验证最终 report 记录本次 `site_data_input.yaml` +和 `dataset_index.yaml`,需要使用本地标定服务模式: + +```bash +./run_isaac_real_sim_test.sh \ + --headless \ + --local-calibration-services \ + --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml \ + --dataset-index-file /data/agv_calib/session_001/dataset_index.yaml \ + --tasks external,chassis,control,sensor_intrinsic +``` + +不依赖 Isaac 的推荐回归命令: + +```bash +./run_local_data_input_smoke.sh \ + --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml +``` + +`dataset_index.yaml` 顶层字段约定如下: + +- `schema_version`:索引格式版本,当前为 `1`。 +- `session.session_id`:本次标定会话 ID。 +- `session.vehicle_id`:本次会话对应车辆 ID。 +- `session.dataset_root`:本次数据集根目录;相对路径都会基于它解析。 +- `session.data_window_start_timestamp_us`:本次数据窗口开始时间,微秒 Unix 时间戳。 +- `session.data_window_end_timestamp_us`:本次数据窗口结束时间,微秒 Unix 时间戳。 +- `data_inputs.external`:外部定位相关数据。 +- `data_inputs.chassis`:底盘标定相关数据。 +- `data_inputs.control`:运控标定相关数据。 +- `data_inputs.sensor`:传感器标定相关数据。 + +文件引用字段可以写字符串路径,也可以写成包含 `path`、`uri` 或 `file_uri` +的 map。转换工具会保留 URI,保留绝对路径,并把相对路径解析到 +`session.dataset_root` 下。默认情况下,工具还会把 `dataset_index.yaml` +自身写入 `sensor_calibration_service` 的 +`data_input.synchronized_dataset_files`,便于算法实现者追溯完整数据关系。 + +## 外部定位模板 + +模板入口: + +- `src/core/agv_calib_core/external_localization_service/include/external_localization_service/external_localization_algorithms.hpp` +- `src/core/agv_calib_core/external_localization_service/src/external_localization_algorithm_template.cpp` + +分发关系: + +- `REFERENCE_POSE_COLLECTION` -> `ReferencePoseCollectionAlgorithm` +- `MARKER_ALIGNMENT` -> `MarkerAlignmentAlgorithm` +- `TRUTH_SOURCE_VALIDATION` -> `TruthSourceValidationAlgorithm` + +算法实现者优先读取: + +- `input.request` +- `input.task_type` +- `input.reference_pose_collection_task` +- `input.marker_alignment_task` +- `input.truth_source_validation_task` +- `input.localization_source_id` +- `input.workcell_zone_id` +- `input.external_localization_readiness` +- `input.latest_external_localization_telemetry` +- `input.external_localization_telemetry_history` +- `input.chassis_telemetry_history` +- `input.control_telemetry_history` +- `input.sensor_telemetry_history` +- `input.data_window_start_timestamp_us` +- `input.data_window_end_timestamp_us` +- `input.external_observation_files` +- `input.marker_observation_files` +- `input.time_sync_diagnostic_files` + +数据输入要求: + +- `TRUTH_SOURCE_VALIDATION` 至少需要 `external_localization_telemetry_history` 或 `external_observation_files`。 +- `REFERENCE_POSE_COLLECTION` 至少需要满足采样数量的外部定位观测窗口。 +- `MARKER_ALIGNMENT` 至少需要 `marker_observation_files`,或 telemetry 中已经包含可用于对齐的标靶观测结果。 +- 如果任务要求时间同步验收,应使用 telemetry 中的 `time_sync_offset_ms`,或使用 `time_sync_diagnostic_files` 中的统计结果。 + +必须回填: + +- `output.response.result.validation_summary.time_sync_ok` +- `output.response.result.validation_summary.coverage_ok` +- `output.response.result.validation_summary.quality_ok` +- `output.response.result.validation_summary.tracking_stable` +- `output.response.result.validation_summary.recommended_as_truth_source` +- `output.response.result.validation_summary.position_stddev_m` +- `output.response.result.validation_summary.yaw_stddev_rad` +- `output.response.result.validation_summary.tracking_loss_ratio` +- `output.response.result.validation_summary.time_sync_offset_ms` +- `output.response.result.result` +- `output.response.result.suitable_for_commit` +- `output.response.result.recommended_parameter_version` + +典型失败原因: + +- 没有外部定位观测窗口。 +- `localization_source_id` 或 `workcell_zone_id` 不匹配。 +- 时间同步偏差超阈值。 +- 位置或航向标准差超阈值。 +- 跟踪丢失率过高。 +- 标靶观测数量不足。 + +## 底盘标定模板 + +模板入口: + +- `src/core/agv_calib_core/chassis_calibration_service/include/chassis_calibration_service/chassis_calibration_algorithms.hpp` +- `src/core/agv_calib_core/chassis_calibration_service/src/chassis_calibration_algorithm_template.cpp` + +分发关系: + +- `ACKERMANN` -> `AckermannChassisAlgorithm` +- `DIFFERENTIAL` -> `DifferentialChassisAlgorithm` +- `SINGLE_STEER_WHEEL` -> `SingleSteerWheelChassisAlgorithm` +- `MULTI_STEER_WHEEL` -> `MultiSteerWheelChassisAlgorithm` + +当前本地模板的通用校验只演示 `STRAIGHT_LINE` 动作原语。后续要支持转弯、原地旋转、横移、舵角扫描等原语时,应由模板维护者扩展分发规则和输入展开字段。 + +算法实现者优先读取: + +- `input.request` +- `input.vehicle_profile` +- `input.chassis_type` +- `input.chassis_capability` +- `input.chassis_readiness` +- `input.applied_chassis_parameters` +- `input.latest_chassis_telemetry` +- `input.chassis_telemetry_history` +- `input.latest_external_localization_telemetry` +- `input.external_localization_telemetry_history` +- `input.latest_control_telemetry` +- `input.control_telemetry_history` +- `input.data_window_start_timestamp_us` +- `input.data_window_end_timestamp_us` +- `input.chassis_motion_data_files` +- `input.actuator_command_files` +- `input.truth_trajectory_files` + +数据输入要求: + +- 底盘真实标定至少需要 `chassis_telemetry_history` 或 `chassis_motion_data_files`。 +- 需要计算真实跑偏、曲率、轮径、轴距、舵角零位时,应同时提供 `truth_trajectory_files` 或 `external_localization_telemetry_history`。 +- 需要分析执行器响应时,应提供 `actuator_command_files` 或控制/驱动遥测历史。 +- 如果只有动作原语请求,没有任何运动反馈数据,只能返回模板 smoke 结果,不能作为真实标定结果。 + +必须回填: + +- `output.response.result.estimated_straight_line_bias` +- `output.response.result.validation_summary.max_lateral_error_m` +- `output.response.result.validation_summary.max_yaw_error_rad` +- `output.response.result.validation_summary.rms_lateral_error_m` +- `output.response.result.validation_summary.rms_yaw_error_rad` +- `output.response.result.validation_summary.repeatability_error_m` +- `output.response.result.validation_summary.curvature_error` +- `output.response.result.validation_summary.module_consistency_error` +- `output.response.result.validation_summary.auto_acceptance_passed` +- `output.response.result.estimated_params` +- `output.response.result.suitable_for_commit` +- `output.response.result.recommended_parameter_version` + +典型失败原因: + +- 底盘未就绪或急停未释放。 +- 动作原语不被当前底盘支持。 +- 有效运动距离或有效轨迹时间不足。 +- 轮速、舵角、里程计、驱动状态缺失。 +- 真值源不可用或时间同步不达标。 +- 执行过程中触发安全限幅或人工中断。 + +注意:在当前完整 smoke 的 gateway 模式下,`chassis` 阶段主要验证 `orchestrator -> vehicle_agent_gateway -> 车端 agent -> result` 链路,不等价于已经运行本地 `chassis_calibration_service` 内的算法模板。若现场设计为车端执行底盘算法,应保证车端返回的结果语义与这里的 `ChassisJobResult` 一致。 + +## 运控标定模板 + +模板入口: + +- `src/core/agv_calib_core/control_calibration_service/include/control_calibration_service/control_calibration_algorithms.hpp` +- `src/core/agv_calib_core/control_calibration_service/src/control_calibration_algorithm_template.cpp` + +分发关系: + +- `PID` -> `PidControlCalibrationAlgorithm` +- `MPC` -> `MpcControlCalibrationAlgorithm` +- `LQR` -> `LqrControlCalibrationAlgorithm` +- `PURE_PURSUIT` -> `PurePursuitControlCalibrationAlgorithm` + +算法实现者优先读取: + +- `input.request` +- `input.controller_algorithm` +- `input.task_type` +- `input.trajectory_tracking_task` +- `input.velocity_step_task` +- `input.acceleration_deceleration_task` +- `input.stop_accuracy_task` +- `input.reference_trajectory` +- `input.reference_target_velocity_ms` +- `input.reference_hold_time_sec` +- `input.active_controller_parameters` +- `input.latest_control_telemetry` +- `input.control_telemetry_history` +- `input.latest_chassis_telemetry` +- `input.chassis_telemetry_history` +- `input.latest_external_localization_telemetry` +- `input.external_localization_telemetry_history` +- `input.data_window_start_timestamp_us` +- `input.data_window_end_timestamp_us` +- `input.control_evaluation_data_files` +- `input.reference_signal_files` +- `input.chassis_response_files` +- `input.truth_trajectory_files` + +数据输入要求: + +- 控制真实评估至少需要 `control_telemetry_history` 或 `control_evaluation_data_files`。 +- 轨迹跟踪类任务必须有 `reference_trajectory` 或 `reference_signal_files`。 +- 速度阶跃、加减速、停车精度任务必须能从请求 payload 或 `reference_signal_files` 还原参考信号。 +- 如果要计算横向/航向/速度误差,应提供底盘响应和真值轨迹;没有真值时只能做车端反馈一致性评估。 + +必须回填: + +- `output.response.result.validation_summary.rms_lateral_error_m` +- `output.response.result.validation_summary.rms_heading_error_rad` +- `output.response.result.validation_summary.rms_speed_error_ms` +- `output.response.result.validation_summary.overshoot_ratio` +- `output.response.result.validation_summary.settle_time_sec` +- `output.response.result.validation_summary.stop_position_error_m` +- `output.response.result.validation_summary.max_jerk` +- `output.response.result.validation_summary.saturation_ratio` +- `output.response.result.validation_summary.auto_acceptance_passed` +- `output.response.result.estimated_parameter_set` +- `output.response.result.suitable_for_commit` +- `output.response.result.recommended_parameter_version` + +典型失败原因: + +- 参考轨迹或速度阶跃任务参数缺失。 +- 控制历史窗口不足。 +- 底盘执行器受限或长时间饱和。 +- 车辆实际响应和控制输出无法对齐。 +- 真值定位质量不足,无法计算可靠误差。 +- 安全状态不允许继续执行。 + +注意:在当前完整 smoke 的 gateway 模式下,`control` 阶段主要验证 `orchestrator -> vehicle_agent_gateway -> 车端 agent -> result` 链路,不等价于已经运行本地 `control_calibration_service` 内的算法模板。若现场设计为车端执行控制评估,应保证车端返回的结果语义与这里的 `ControlJobResult` 一致。 + +## 传感器标定模板 + +模板入口: + +- `src/core/agv_calib_core/sensor_calibration_service/include/sensor_calibration_service/sensor_calibration_algorithms.hpp` +- `src/core/agv_calib_core/sensor_calibration_service/src/sensor_calibration_algorithm_template.cpp` + +分发关系: + +- `CAMERA_INTRINSIC + FRONT_CAMERA_INTRINSIC` -> `FrontCameraIntrinsicCalibrationAlgorithm` +- `CAMERA_INTRINSIC + DOWNWARD_CAMERA_INTRINSIC` -> `DownwardCameraIntrinsicCalibrationAlgorithm` +- `IMU_INTRINSIC + IMU_INTRINSIC` -> `ImuIntrinsicCalibrationAlgorithm` +- `SENSOR_TO_BASE_EXTRINSIC + FRONT_CAMERA_EXTRINSIC` -> `FrontCameraExtrinsicCalibrationAlgorithm` +- `SENSOR_TO_BASE_EXTRINSIC + DOWNWARD_CAMERA_EXTRINSIC` -> `DownwardCameraExtrinsicCalibrationAlgorithm` +- `SENSOR_TO_BASE_EXTRINSIC + IMU_EXTRINSIC` -> `ImuExtrinsicCalibrationAlgorithm` +- `SENSOR_TO_BASE_EXTRINSIC + LIDAR_2D_EXTRINSIC` -> `Lidar2DExtrinsicCalibrationAlgorithm` +- `SENSOR_TO_BASE_EXTRINSIC + LIDAR_3D_EXTRINSIC` -> `Lidar3DExtrinsicCalibrationAlgorithm` +- `HAND_EYE + EYE_IN_HAND` -> `EyeInHandCalibrationAlgorithm` +- `HAND_EYE + EYE_TO_HAND` -> `EyeToHandCalibrationAlgorithm` + +算法实现者优先读取: + +- `input.request` +- `input.task_type` +- `input.task_subtype` +- `input.target_sensor_id` +- `input.target_sensor_type` +- `input.camera_intrinsic_task` +- `input.imu_intrinsic_task` +- `input.sensor_to_base_extrinsic_task` +- `input.hand_eye_task` +- `input.required_image_count` +- `input.required_static_segment_count` +- `input.required_motion_segment_count` +- `input.required_sample_count` +- `input.required_pose_count` +- `input.reference_board_id` +- `input.reference_base_frame_id` +- `input.latest_sensor_telemetry` +- `input.sensor_telemetry_history` +- `input.capture_quality_diagnostics` +- `input.storage_diagnostics` +- `input.latest_chassis_telemetry` +- `input.latest_control_telemetry` +- `input.latest_external_localization_telemetry` +- `input.data_window_start_timestamp_us` +- `input.data_window_end_timestamp_us` +- `input.image_sample_files` +- `input.imu_sample_files` +- `input.pointcloud_sample_files` +- `input.laser_scan_sample_files` +- `input.target_detection_files` +- `input.robot_pose_sample_files` +- `input.synchronized_dataset_files` + +数据输入要求: + +- `front_camera_intrinsic` / `downward_camera_intrinsic` 至少需要 `image_sample_files` 或包含图像列表的 `synchronized_dataset_files`。 +- `imu_intrinsic` 至少需要 `imu_sample_files`,并能区分静止段和运动段。 +- `front_camera_extrinsic` / `downward_camera_extrinsic` 至少需要图像样本和标定目标检测结果;检测结果可由算法内部生成,也可由 `target_detection_files` 提供。 +- `lidar_2d_extrinsic` 至少需要 `laser_scan_sample_files` 和对应靶标/角点/平面检测结果。 +- `lidar_3d_extrinsic` 至少需要 `pointcloud_sample_files` 和对应靶标/平面/点云配准输入。 +- `imu_extrinsic` 至少需要 `imu_sample_files`,并通常需要底盘运动或外部真值轨迹辅助。 +- `hand_eye` 至少需要图像/检测结果和 `robot_pose_sample_files`。 +- 如果只存在 `sensor_telemetry_history`,但没有原始图像/IMU/点云/位姿文件,算法只能判断采集状态,不能完成真实传感器标定。 + +必须回填: + +- `output.response.result.validation_summary.reprojection_error_px` +- `output.response.result.validation_summary.translation_residual_m` +- `output.response.result.validation_summary.rotation_residual_rad` +- `output.response.result.validation_summary.plane_residual_m` +- `output.response.result.validation_summary.repeatability_error_m` +- `output.response.result.validation_summary.auto_acceptance_passed` +- `output.response.result.estimated_params` +- `output.response.result.suitable_for_commit` +- `output.response.result.recommended_parameter_version` + +典型失败原因: + +- 图像帧、IMU 原始包、点云或机械臂位姿数量不足。 +- 标定板 ID 不匹配或检测质量不足。 +- 图像模糊、过曝、角点不足、点云稀疏、IMU 饱和。 +- 采样窗口时间同步不满足。 +- 车辆静止条件或运动激励条件不满足。 +- 产物目录不可写或中间数据落盘失败。 + +当前完整 smoke 只跑 `front_camera_intrinsic` 作为 sensor intrinsic 的最小代表链路。它验证的是任务分发、结果回填和 report 汇总,不代表所有传感器子类型都已经完成真实算法验证。 + +## 报告和冒烟测试要求 + +模板维护者需要保证每个算法接入后,最终 report 能读到这些信息: + +- 阶段是否成功。 +- 自动验收是否通过。 +- 推荐参数版本。 +- 验证摘要关键指标。 +- 产物引用。 +- 失败原因或阻塞原因。 + +当前完整链路 smoke 命令: + +```bash +./run_isaac_real_sim_test.sh --headless --tasks external,chassis,control,sensor_intrinsic +``` + +该 smoke 的定位是链路和合同验证: + +- `external`:验证外部定位遥测、readiness、truth validation、stage result 和 report 汇总。 +- `chassis`:gateway 模式下验证底盘任务下发、车端 agent 执行结果返回和 stage result。 +- `control`:gateway 模式下验证控制评估任务下发、车端 agent 执行结果返回和 stage result。 +- `sensor_intrinsic`:验证 sensor service 的 `front_camera_intrinsic` 模板分发和结果回填。 +- `report`:验证 `overall_success`、各阶段结果、自动验收字段和 `/workshop_v2/get_report`。 + +smoke 通过不代表真实算法精度达标;真实算法合入后还要补对应模块的单元测试、数据回放测试和现场低速验收。 + +## 新增算法子类型流程 + +如果要新增算法类型或子类型,按这个顺序修改: + +1. 在对应接口包里新增 task type / subtype 枚举。 +2. 在对应 `Input` 结构里补充任务 payload 或展开字段。 +3. 在 service node 中把 request payload 展开到 `Input`。 +4. 在对应 `Input` 结构里补充数据文件引用字段,或复用已有 `FileReference` 字段。 +5. 在 service node 中把采集产物、数据窗口和 telemetry history 填入 `Input`。 +6. 在 `*_algorithm_template.cpp` 中新增分发规则。 +7. 新增具体 `*_algorithm.cpp` 文件和类声明。 +8. 在 `CMakeLists.txt` 中加入新源文件。 +9. 在 smoke 中新增最小任务用例。 +10. 在本文档中补充分发关系、数据输入要求和输出要求。 + +## 交付检查清单 + +算法实现者提交前至少自查: + +- 算法只改了约定的算法文件,或已说明必须改接口/模板的原因。 +- `run(...)` 的成功和失败路径都能稳定返回。 +- 失败路径写了可定位的 `failure_reason`。 +- 成功路径填了 `message`、`data_quality_passed`、`suitable_for_commit`、`recommended_parameter_version`。 +- `validation_summary` 中和本算法相关的字段不是模板默认值。 +- 参数结果写到了对应的 `estimated_*` 或 `result` 字段。 +- 产物路径写入 `artifacts`,路径可追溯。 +- 所需输入数据来自 `Input` 中的 telemetry history 或 `FileReference`,没有私自读取未声明路径。 +- 必需数据为空时返回 `false`,并在 `failure_reason` 中指出缺少哪个输入。 +- 不依赖 Isaac 专有 topic 或真实车辆 SDK 专有代码。 +- 本地 build 通过。 +- 相关 smoke 或回放测试通过。 + +模板维护者提交前至少自查: + +- 新增字段在 node 层被统一填充。 +- 所有新增枚举都有模板分发规则。 +- 数据窗口、telemetry history 和 `FileReference` 产物引用有明确来源。 +- 旧 smoke 不被破坏。 +- report 能看到新增算法的关键结果。 +- 文档中的文件路径、字段名和代码一致。 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 23deb62..5cd0cc9 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 @@ -2,6 +2,7 @@ import argparse import json import math import os +import signal import time import xml.etree.ElementTree as ET from pathlib import Path @@ -32,6 +33,7 @@ except ImportError: def parse_args(): parser = argparse.ArgumentParser(description="Isaac 标定车间构建脚本") parser.add_argument("--headless", action="store_true", help="以无界面模式启动 Isaac Sim") + parser.add_argument("--skip-final-close", action="store_true", help="退出时不调用 SimulationApp.close(),用于定位 Isaac 关闭阶段崩溃") parser.add_argument("--room-length", type=float, default=10.0, help="车间长度(米)") parser.add_argument("--room-width", type=float, default=6.0, help="车间宽度(米)") parser.add_argument("--room-height", type=float, default=3.5, help="车间高度(米)") @@ -161,6 +163,52 @@ def parse_args(): ARGS = parse_args() SCRIPT_PATH = Path(__file__).resolve() +SHUTDOWN_SIGNAL = None +SHUTDOWN_REQUESTED = False + + +def _record_shutdown_signal(signum, _frame): + global SHUTDOWN_SIGNAL, SHUTDOWN_REQUESTED + SHUTDOWN_SIGNAL = signum + SHUTDOWN_REQUESTED = True + print(f"\n[INFO] 收到退出信号: {signal.Signals(signum).name}({signum})", flush=True) + + +def install_shutdown_signal_handlers(): + signal.signal(signal.SIGINT, _record_shutdown_signal) + signal.signal(signal.SIGTERM, _record_shutdown_signal) + + +def ros_context_is_shutdown(): + return rclpy is not None and not rclpy.ok() + + +def is_ros_context_invalid_error(exc): + text = str(exc) + return "publisher's context is invalid" in text or "context is invalid" in text + + +install_shutdown_signal_handlers() + + +def patch_pillow_compat_for_isaac_extensions(): + try: + import PIL._util as pil_util + except Exception as exc: + print(f"[WARN] Pillow 兼容补丁跳过: {exc}") + return + + if hasattr(pil_util, "is_directory"): + return + + def is_directory(path): + return isinstance(path, (bytes, str, os.PathLike)) and os.path.isdir(path) + + pil_util.is_directory = is_directory + print("[*] 已补齐 PIL._util.is_directory 兼容接口,用于 Isaac UI 测试扩展加载。") + + +patch_pillow_compat_for_isaac_extensions() def find_project_root(script_path): @@ -566,6 +614,12 @@ class VehicleTfPublisher: t.header.stamp = timestamp t.header.frame_id = frame_id t.child_frame_id = child_frame_id + x = float(x) + y = float(y) + z = float(z) + roll = float(roll) + pitch = float(pitch) + yaw = float(yaw) t.transform.translation.x = x t.transform.translation.y = y t.transform.translation.z = z @@ -578,10 +632,10 @@ class VehicleTfPublisher: cr = math.cos(roll * 0.5) sr = math.sin(roll * 0.5) - t.transform.rotation.w = cr * cp * cy + sr * sp * sy - t.transform.rotation.x = sr * cp * cy - cr * sp * sy - t.transform.rotation.y = cr * sp * cy + sr * cp * sy - t.transform.rotation.z = cr * cp * sy - sr * sp * cy + t.transform.rotation.w = float(cr * cp * cy + sr * sp * sy) + t.transform.rotation.x = float(sr * cp * cy - cr * sp * sy) + t.transform.rotation.y = float(cr * sp * cy + sr * cp * sy) + t.transform.rotation.z = float(cr * cp * sy - sr * sp * cy) return t @@ -627,7 +681,7 @@ class VehicleTfPublisher: position, quat = agv.get_world_pose() # 四元数 (w, x, y, z) 转欧拉角(用于调试) - qw, qx, qy, qz = quat + qw, qx, qy, qz = (float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3])) yaw = math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz)) # 发布 world -> base_link 动态 TF @@ -639,10 +693,10 @@ class VehicleTfPublisher: timestamp ) # 使用原始四元数覆盖(更精确) - t.transform.rotation.w = qw - t.transform.rotation.x = qx - t.transform.rotation.y = qy - t.transform.rotation.z = qz + t.transform.rotation.w = float(qw) + t.transform.rotation.x = float(qx) + t.transform.rotation.y = float(qy) + t.transform.rotation.z = float(qz) self.tf_broadcaster.sendTransform(t) self.last_publish_time = now @@ -991,14 +1045,14 @@ class ChassisCalibrationTelemetryPublisher: msg.odom_x_m = float(position[0] + self.rng.normal(0.0, self.odom_noise_stddev_m)) msg.odom_y_m = float(position[1] + self.rng.normal(0.0, self.odom_noise_stddev_m)) msg.odom_yaw_rad = float(normalize_angle(yaw_rad + self.rng.normal(0.0, self.yaw_noise_stddev_rad))) - msg.linear_velocity_ms = forward_velocity - msg.angular_velocity_rads = yaw_rate + msg.linear_velocity_ms = float(forward_velocity) + msg.angular_velocity_rads = float(yaw_rate) msg.modules = self.build_modules(forward_velocity, yaw_rate, dt) msg.estop_engaged = False msg.driver_error_code = 0 msg.active_job_id = "isaac_chassis_calibration" - msg.lateral_slip_estimate = lateral_velocity - msg.curvature_estimate = yaw_rate / forward_velocity if abs(forward_velocity) > 0.02 else 0.0 + msg.lateral_slip_estimate = float(lateral_velocity) + msg.curvature_estimate = float(yaw_rate / forward_velocity if abs(forward_velocity) > 0.02 else 0.0) self.publisher.publish(msg) self.last_publish_wall_time = now @@ -1049,25 +1103,25 @@ class ControlCalibrationTelemetryPublisher: 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 = normalize_angle(yaw_rad) - speed_error = forward_velocity - self.reference_speed_ms - steering_output = clamp(-0.8 * lateral_error - 1.2 * heading_error, -1.0, 1.0) - throttle_output = clamp(-2.0 * speed_error, 0.0, 1.0) - brake_output = clamp(2.0 * speed_error, 0.0, 1.0) + 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)) msg = ControlTelemetry() msg.hardware_timestamp_us = time.time_ns() // 1000 msg.odom_x_m = float(position[0]) msg.odom_y_m = float(position[1]) msg.odom_yaw_rad = float(yaw_rad) - msg.linear_velocity_ms = forward_velocity + msg.linear_velocity_ms = float(forward_velocity) msg.angular_velocity_rads = float(angular_velocity[2]) - msg.lateral_error_m = lateral_error - msg.heading_error_rad = heading_error - msg.speed_error_ms = speed_error - msg.steering_output = steering_output - msg.throttle_output = throttle_output - msg.brake_output = brake_output + msg.lateral_error_m = float(lateral_error) + msg.heading_error_rad = float(heading_error) + msg.speed_error_ms = float(speed_error) + msg.steering_output = float(steering_output) + msg.throttle_output = float(throttle_output) + msg.brake_output = float(brake_output) msg.saturation_flag = abs(steering_output) > 0.98 or throttle_output > 0.98 or brake_output > 0.98 msg.active_job_id = "isaac_control_calibration" msg.parameter_version = self.parameter_version @@ -1674,7 +1728,7 @@ class IsaacWorkshopRuntime: if not self.args.disable_vehicle_imu: imu_parent_path = self.find_vehicle_rigid_body_prim_path(["base_link", "imu_link"]) if not imu_parent_path: - print("[WARN] 未找到可挂载 IMU 的车辆刚体 link,跳过 Isaac IMU sensor prim 创建。") + print("[INFO] 未找到可挂载 IMU 的车辆刚体 link,跳过 Isaac IMU sensor prim;IMU ROS topic 继续由车体运动学发布器输出。") imu_parent_path = "" imu_path = f"{imu_parent_path}/SimImu" if imu_parent_path else "" if imu_parent_path: @@ -1893,6 +1947,8 @@ def main(): sensor_telemetry_publisher = SensorCalibrationTelemetryPublisher(ARGS) world = runtime.build_workshop() vehicle_laser_scan_publisher = VehicleLaserScanTopicPublisher(ARGS, runtime.lidar_2d_target_specs) + # rclpy.init() may install its own signal handlers; restore ours before entering the long-running loop. + install_shutdown_signal_handlers() runtime.write_scene_manifest() world.reset() @@ -1934,8 +1990,32 @@ def main(): if ARGS.headless and render_sensor_outputs: print("[*] headless 模式下启用离屏渲染,用于相机和 RTX LiDAR render product 发布。") + frame_count = 0 + loop_started_wall_time = time.time() + exit_reason = "unknown" + ros_context_shutdown_seen = False + ros_publishers = ( + (telemetry_publisher, (agv,)), + (vehicle_tf_publisher, (agv,)), + (vehicle_imu_publisher, (agv,)), + (vehicle_laser_scan_publisher, (agv,)), + (chassis_telemetry_publisher, (agv,)), + (control_telemetry_publisher, (agv,)), + (sensor_telemetry_publisher, ()), + ) + ros_context_required = any(getattr(publisher, "node", None) is not None for publisher, _ in ros_publishers) try: - while simulation_app.is_running(): + while True: + if SHUTDOWN_REQUESTED: + exit_reason = f"收到退出信号 {signal.Signals(SHUTDOWN_SIGNAL).name if SHUTDOWN_SIGNAL else 'UNKNOWN'}" + break + if ros_context_required and ros_context_is_shutdown(): + ros_context_shutdown_seen = True + exit_reason = "ROS context 已关闭" + break + if not simulation_app.is_running(): + 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")) @@ -1961,14 +2041,37 @@ def main(): print(f"\n[ERROR] 运动学控制循环异常: {exc}") world.step(render=render_frame) - telemetry_publisher.publish(agv) - vehicle_tf_publisher.publish(agv) - vehicle_imu_publisher.publish(agv) - vehicle_laser_scan_publisher.publish(agv) - chassis_telemetry_publisher.publish(agv) - control_telemetry_publisher.publish(agv) - sensor_telemetry_publisher.publish() + try: + for publisher, publish_args in ros_publishers: + if SHUTDOWN_REQUESTED: + exit_reason = f"收到退出信号 {signal.Signals(SHUTDOWN_SIGNAL).name if SHUTDOWN_SIGNAL else 'UNKNOWN'}" + break + if ros_context_required and ros_context_is_shutdown(): + ros_context_shutdown_seen = True + exit_reason = "ROS context 已关闭" + break + publisher.publish(*publish_args) + else: + frame_count += 1 + continue + break + except Exception as exc: + if is_ros_context_invalid_error(exc): + ros_context_shutdown_seen = True + exit_reason = "ROS context 已关闭,停止发布" + break + raise + except Exception as exc: + exit_reason = f"未捕获异常: {type(exc).__name__}: {exc}" + print(f"\n[ERROR] Isaac 主循环异常退出: {exit_reason}", flush=True) + raise finally: + elapsed_sec = time.time() - loop_started_wall_time + print( + f"\n[*] Isaac 主循环退出: reason={exit_reason}, " + f"frames={frame_count}, elapsed_sec={elapsed_sec:.3f}", + flush=True, + ) telemetry_publisher.shutdown() vehicle_tf_publisher.shutdown() vehicle_imu_publisher.shutdown() @@ -1978,7 +2081,14 @@ def main(): sensor_telemetry_publisher.shutdown() if rclpy is not None and rclpy.ok(): rclpy.shutdown() - simulation_app.close() + if SHUTDOWN_REQUESTED or ros_context_shutdown_seen: + print("[*] 停止信号/ROS context 关闭期间跳过 SimulationApp.close(),避免 Isaac 退出阶段崩溃。", flush=True) + os._exit(0) + elif ARGS.skip_final_close: + print("[*] 已按 --skip-final-close 跳过 SimulationApp.close()。", flush=True) + else: + print("[*] 正在调用 SimulationApp.close()。如这里发生 segfault,请加 --skip-final-close 复测。", flush=True) + simulation_app.close() if __name__ == "__main__": diff --git a/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py b/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py index 0f939e5..e9a819f 100644 --- a/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py +++ b/agv_calib_brain/src/simulation/tools/external_pose_wifi6_bridge.py @@ -11,6 +11,7 @@ import time from typing import Any import rclpy +from rclpy.executors import ExternalShutdownException try: from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry @@ -127,7 +128,7 @@ class ExternalPoseWifi6Bridge: except Exception as exc: self.fail_count += 1 now = time.monotonic() - if now - self.last_error_log_monotonic >= self.args.error_log_interval_sec: + if rclpy.ok() and now - self.last_error_log_monotonic >= self.args.error_log_interval_sec: self.last_error_log_monotonic = now self.node.get_logger().warning( f"failed to push external pose over wifi6_sim_tcp: {exc}; fail_count={self.fail_count}" @@ -153,7 +154,7 @@ def main() -> int: try: bridge = ExternalPoseWifi6Bridge(args) rclpy.spin(bridge.node) - except KeyboardInterrupt: + except (KeyboardInterrupt, ExternalShutdownException): pass finally: if bridge is not None: 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 9275abd..2f8ae9c 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 @@ -556,6 +556,14 @@ class OrchestratorSmoke: def print_report(report) -> None: print(f"[REPORT] session_id={report.session_id} overall_success={report.overall_success}") print(f"[REPORT] summary={report.summary}") + for file_ref in report.report_files: + print( + "[REPORT_FILE] " + f"name={file_ref.file_name} " + f"uri={file_ref.file_uri} " + f"size={file_ref.size_bytes} " + f"description={file_ref.description}" + ) for result in report.stage_results: print( "[STAGE] " diff --git a/agv_calib_brain/src/simulation/tools/workshop_sensor_ingest_sim.py b/agv_calib_brain/src/simulation/tools/workshop_sensor_ingest_sim.py index 3f70684..71cd100 100644 --- a/agv_calib_brain/src/simulation/tools/workshop_sensor_ingest_sim.py +++ b/agv_calib_brain/src/simulation/tools/workshop_sensor_ingest_sim.py @@ -12,6 +12,7 @@ import time from typing import Any import rclpy +from rclpy.executors import ExternalShutdownException from sensor_msgs.msg import Image, Imu, LaserScan, PointCloud2, PointField @@ -26,6 +27,11 @@ PAYLOAD_LASER_SCAN = 3 PAYLOAD_IMU = 4 +def is_shutdown_context_error(exc: Exception) -> bool: + text = str(exc) + return "publisher's context is invalid" in text or "context is invalid" in text + + def read_exactly(conn: socket.socket, size: int) -> bytes: chunks: list[bytes] = [] remaining = size @@ -107,6 +113,8 @@ class WorkshopSensorIngestSim: ) def log_error(self, message: str) -> None: + if not rclpy.ok(): + return self.fail_count += 1 now = time.monotonic() if now - self.last_error_log_monotonic >= self.args.error_log_interval_sec: @@ -161,12 +169,18 @@ class WorkshopSensorIngestSim: self.node.get_logger().info(f"publish {sensor_id} frames to {topic}") def poll_once(self) -> None: + if not rclpy.ok(): + return try: self.refresh_sensors() for sensor_id in list(self.sensors): + if not rclpy.ok(): + return frame = self.fetch_latest_frame(sensor_id) self.publish_frame(frame) except Exception as exc: + if not rclpy.ok() or is_shutdown_context_error(exc): + return self.log_error(str(exc)) def fetch_latest_frame(self, sensor_id: str) -> dict[str, Any]: @@ -342,13 +356,15 @@ def parse_args() -> argparse.Namespace: def main() -> int: args = parse_args() rclpy.init(args=None) - ingest = WorkshopSensorIngestSim(args) + ingest = None try: + ingest = WorkshopSensorIngestSim(args) rclpy.spin(ingest.node) - except KeyboardInterrupt: + except (KeyboardInterrupt, ExternalShutdownException): pass finally: - ingest.node.destroy_node() + if ingest is not None: + ingest.node.destroy_node() if rclpy.ok(): rclpy.shutdown() return 0 diff --git a/agv_calib_brain/src/site_deployment/README.md b/agv_calib_brain/src/site_deployment/README.md index f65a0af..f11f7f9 100644 --- a/agv_calib_brain/src/site_deployment/README.md +++ b/agv_calib_brain/src/site_deployment/README.md @@ -4,6 +4,31 @@ 现场部署代码应该复用仿真阶段已经验证过的外部接口语义,但需要把仿真适配器替换成真实适配器。 +真实采集链路还需要为每次标定 session 生成数据集索引文件。推荐先输出 +`dataset_index.yaml`,然后在会话结束时调用 +`src/deployment/tools/finalize_site_session.py` 生成 `site_data_input.yaml`,最后通过 +`minimal_workshop_demo.launch.py data_input_params_file:=... dataset_index_file:=...` +注入到算法服务和最终报告。 +索引格式样例见 `src/deployment/profiles/example_dataset_index.yaml`。 +如果真实采集程序还没有自己的索引写入逻辑,可以先使用 +`src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py` 登记采集文件并触发收尾。 + +收尾命令示例: + +```bash +python3 src/deployment/tools/finalize_site_session.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --session-id session_001 +``` + +本地回归命令: + +```bash +./run_local_data_input_smoke.sh \ + --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml \ + --dataset-index /data/agv_calib/session_001/dataset_index.yaml +``` + 当前边界: - `vehicle_agent_real`:真实车端电脑适配器。 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_ingest_real/README.md b/agv_calib_brain/src/site_deployment/workshop_sensor_ingest_real/README.md new file mode 100644 index 0000000..5214ff1 --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_ingest_real/README.md @@ -0,0 +1,63 @@ +# 真实采集会话接入 + +这个目录放真实车间侧采集/ingest 的接入脚手架。它不直接绑定具体相机、雷达、 +底盘或控制器 SDK,只固定一件事:真实采集程序在采到文件后,把文件路径登记到 +`dataset_index.yaml`,并在会话结束时生成 `site_data_input.yaml`。 + +最小使用示例: + +```bash +python3 src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py \ + --site-profile src/deployment/profiles/site_template.yaml \ + --session-dir /data/agv_calib/site_a/session_001 \ + --site-id site_a \ + --vehicle-id agv_001 \ + --file external.external_observation_files=external/external_observations.csv \ + --file chassis.chassis_motion_data_files=chassis/chassis_motion.csv \ + --file control.control_evaluation_data_files=control/control_eval.csv \ + --file sensor.image_sample_files=sensor/front_camera/images.yaml \ + --file sensor.target_detection_files=sensor/front_camera/charuco_detections.json \ + --finalize +``` + +执行后会生成: + +- `dataset_index.yaml` +- `site_data_input.yaml` + +校验 `dataset_index.yaml`: + +```bash +python3 src/deployment/tools/validate_dataset_index.py \ + /data/agv_calib/site_a/session_001/dataset_index.yaml \ + --tasks external,chassis,control,sensor_intrinsic +``` + +现场 launch 使用 `site_data_input.yaml`: + +```bash +ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \ + use_gateway:=true \ + data_input_params_file:=/data/agv_calib/site_a/session_001/site_data_input.yaml +``` + +使用现有 Isaac smoke 验证四个算法服务的数据输入注入: + +```bash +./run_isaac_real_sim_test.sh \ + --headless \ + --local-calibration-services \ + --data-input-params-file /data/agv_calib/site_a/session_001/site_data_input.yaml \ + --tasks external,chassis,control,sensor_intrinsic +``` + +更快的本地回归命令: + +```bash +./run_local_data_input_smoke.sh \ + --data-input-params-file /data/agv_calib/site_a/session_001/site_data_input.yaml +``` + +真实采集程序后续只需要在对应设备数据落盘后调用本脚本,或者直接复用本脚本里的 +索引写入逻辑。具体标定算法不需要知道采集程序如何连接设备,只读取算法模板里的 +`data_input.*` 文件引用。 diff --git a/agv_calib_brain/src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py b/agv_calib_brain/src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py new file mode 100644 index 0000000..c80204d --- /dev/null +++ b/agv_calib_brain/src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python3 +"""真实采集会话索引写入入口。""" + +from __future__ import annotations + +import argparse +import sys +from datetime import datetime, timezone +from pathlib import Path +from typing import Any + +import yaml + + +PROJECT_ROOT = Path(__file__).resolve().parents[3] +DEPLOYMENT_TOOLS = PROJECT_ROOT / "src" / "deployment" / "tools" +if str(DEPLOYMENT_TOOLS) not in sys.path: + sys.path.insert(0, str(DEPLOYMENT_TOOLS)) + +from dataset_index_to_site_data_input import SERVICE_INPUT_FIELDS, load_yaml +from finalize_site_session import ( + load_site_profile, + resolve_dataset_index, + resolve_output_path, + resolve_session_dir, +) + + +def now_us() -> int: + return int(datetime.now(timezone.utc).timestamp() * 1_000_000) + + +def default_session_id() -> str: + return datetime.now().strftime("session_%Y%m%d_%H%M%S") + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="把真实采集产物登记到 dataset_index.yaml,并可选生成 site_data_input.yaml。" + ) + parser.add_argument( + "--site-profile", + default="src/deployment/profiles/site_template.yaml", + help="现场部署 profile 路径。", + ) + parser.add_argument( + "--session-id", + default="", + help="本次标定会话 ID;不填写时自动生成。", + ) + parser.add_argument( + "--session-dir", + help="本次标定会话目录;优先级高于 --session-id。", + ) + parser.add_argument( + "--site-id", + default="", + help="现场或产线 ID。", + ) + parser.add_argument( + "--vehicle-id", + default="", + help="车辆 ID;默认读取 site profile 的 vehicle_id。", + ) + parser.add_argument( + "--start-us", + type=int, + default=0, + help="采集窗口开始时间,微秒 Unix 时间戳;不填写时使用当前时间。", + ) + parser.add_argument( + "--end-us", + type=int, + default=0, + help="采集窗口结束时间,微秒 Unix 时间戳;--finalize 时不填写则使用当前时间。", + ) + parser.add_argument( + "--file", + action="append", + default=[], + metavar="模块.字段=路径", + help="登记采集文件,可重复,例如 sensor.image_sample_files=sensor/front_camera/images.yaml。", + ) + parser.add_argument( + "--overwrite", + action="store_true", + help="覆盖已有 dataset_index.yaml;默认合并已有索引。", + ) + parser.add_argument( + "--finalize", + action="store_true", + help="写完 dataset_index.yaml 后立即生成 site_data_input.yaml。", + ) + parser.add_argument( + "--site-data-output", + help="生成的 site_data_input.yaml 路径;仅 --finalize 时生效。", + ) + parser.add_argument( + "--no-strict", + action="store_true", + help="生成 site_data_input.yaml 时关闭严格检查。", + ) + return parser.parse_args() + + +def known_input_fields() -> set[str]: + result: set[str] = set() + for config in SERVICE_INPUT_FIELDS.values(): + section_name = str(config["index_section"]) + for field_name in config["fields"]: + result.add(f"{section_name}.{field_name}") + return result + + +def parse_file_reference(raw_value: str, allowed_fields: set[str]) -> tuple[str, str, str]: + if "=" not in raw_value: + raise ValueError(f"--file 必须使用 模块.字段=路径 格式,当前值为 {raw_value!r}。") + key, path = raw_value.split("=", 1) + key = key.strip() + path = path.strip() + if "." not in key: + raise ValueError(f"--file 左侧必须是 模块.字段,当前值为 {key!r}。") + section_name, field_name = key.split(".", 1) + if key not in allowed_fields: + raise ValueError(f"未知数据输入字段 {key!r}。") + if not path: + raise ValueError(f"{key} 的路径不能为空。") + return section_name, field_name, path + + +def empty_data_inputs() -> dict[str, Any]: + return { + str(config["index_section"]): {} + for config in SERVICE_INPUT_FIELDS.values() + } + + +def merge_file_reference(index: dict[str, Any], section_name: str, field_name: str, path: str) -> None: + data_inputs = index.setdefault("data_inputs", empty_data_inputs()) + section = data_inputs.setdefault(section_name, {}) + files = section.setdefault(field_name, []) + if isinstance(files, str): + files = [files] + section[field_name] = files + if not isinstance(files, list): + raise ValueError(f"data_inputs.{section_name}.{field_name} 必须是列表或字符串。") + if path not in files: + files.append(path) + + +def make_base_index( + session_id: str, + site_id: str, + vehicle_id: str, + session_dir: Path, + start_us: int, + end_us: int, +) -> dict[str, Any]: + return { + "schema_version": 1, + "session": { + "session_id": session_id, + "site_id": site_id, + "vehicle_id": vehicle_id, + "dataset_root": str(session_dir), + "data_window_start_timestamp_us": start_us, + "data_window_end_timestamp_us": end_us, + }, + "data_inputs": empty_data_inputs(), + } + + +def update_session_metadata( + index: dict[str, Any], + session_id: str, + site_id: str, + vehicle_id: str, + session_dir: Path, + start_us: int, + end_us: int, +) -> None: + session = index.setdefault("session", {}) + if not isinstance(session, dict): + raise ValueError("dataset_index.yaml 中的 session 必须是 YAML map。") + session["session_id"] = session_id + session["site_id"] = site_id + session["vehicle_id"] = vehicle_id + session["dataset_root"] = str(session_dir) + session["data_window_start_timestamp_us"] = start_us + session["data_window_end_timestamp_us"] = end_us + index.setdefault("schema_version", 1) + index.setdefault("data_inputs", empty_data_inputs()) + + +def write_yaml(path: Path, data: dict[str, Any]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text( + yaml.safe_dump(data, sort_keys=False, allow_unicode=True), + encoding="utf-8", + ) + + +def main() -> int: + args = parse_args() + profile_path = Path(args.site_profile).expanduser().resolve(strict=False) + profile = load_site_profile(profile_path) + session_id = args.session_id.strip() or default_session_id() + session_dir = resolve_session_dir(profile, profile_path, session_id, args.session_dir) + dataset_index_path = resolve_dataset_index(profile, profile_path, session_dir, None) + site_id = args.site_id.strip() + vehicle_id = args.vehicle_id.strip() or str(profile.get("vehicle_id", "")).strip() + start_us = args.start_us or now_us() + end_us = args.end_us + if args.finalize and end_us <= 0: + end_us = now_us() + if end_us > 0 and end_us <= start_us: + end_us = start_us + 1 + + if dataset_index_path.exists() and not args.overwrite: + index = load_yaml(dataset_index_path) + else: + index = make_base_index(session_id, site_id, vehicle_id, session_dir, start_us, end_us) + + update_session_metadata(index, session_id, site_id, vehicle_id, session_dir, start_us, end_us) + + allowed_fields = known_input_fields() + for raw_file in args.file: + section_name, field_name, file_path = parse_file_reference(raw_file, allowed_fields) + merge_file_reference(index, section_name, field_name, file_path) + + write_yaml(dataset_index_path, index) + print(f"已写入数据集索引:{dataset_index_path}") + + if args.finalize: + from dataset_index_to_site_data_input import build_ros_params, validate_output + + output_path = resolve_output_path( + profile, + profile_path, + session_dir, + session_id, + args.site_data_output, + ) + output = build_ros_params( + dataset_index_path, + index, + str(session_dir), + True, + ) + validate_output(output, strict=not args.no_strict) + write_yaml(output_path, output) + print(f"已生成数据输入参数文件:{output_path}") + print(f"启动参数:data_input_params_file:={output_path}") + + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as exc: + print(f"[错误] {exc}", file=sys.stderr) + raise SystemExit(1) diff --git a/upload_to_github.sh b/upload_to_github.sh new file mode 100755 index 0000000..698d80f --- /dev/null +++ b/upload_to_github.sh @@ -0,0 +1,135 @@ +#!/usr/bin/env bash +set -Eeuo pipefail + +usage() { + cat <<'USAGE' +Usage: + ./upload_to_github.sh [options] + +Options: + -m, --message MESSAGE Commit message. Default: "Update AutoCalib-Workshop" + -r, --repo-url URL GitHub repository URL. Uses existing origin if omitted. + -b, --branch BRANCH Branch to push. Default: current branch. + --remote NAME Git remote name. Default: origin. + --dry-run Print commands without changing anything. + -h, --help Show this help. + +Examples: + ./upload_to_github.sh -m "Initial upload" + ./upload_to_github.sh -r https://github.com/USER/REPO.git -m "Upload project" + GITHUB_REPO_URL=git@github.com:USER/REPO.git ./upload_to_github.sh +USAGE +} + +die() { + echo "Error: $*" >&2 + exit 1 +} + +run() { + echo "+ $*" + if [[ "${DRY_RUN}" != "1" ]]; then + "$@" + fi +} + +PROJECT_DIR="${PROJECT_DIR:-$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)}" +REMOTE_NAME="${REMOTE_NAME:-origin}" +REPO_URL="${GITHUB_REPO_URL:-}" +BRANCH="${BRANCH:-}" +COMMIT_MESSAGE="${COMMIT_MESSAGE:-Update AutoCalib-Workshop}" +DRY_RUN=0 + +while [[ $# -gt 0 ]]; do + case "$1" in + -m|--message) + [[ $# -ge 2 ]] || die "missing value for $1" + COMMIT_MESSAGE="$2" + shift 2 + ;; + -r|--repo-url) + [[ $# -ge 2 ]] || die "missing value for $1" + REPO_URL="$2" + shift 2 + ;; + -b|--branch) + [[ $# -ge 2 ]] || die "missing value for $1" + BRANCH="$2" + shift 2 + ;; + --remote) + [[ $# -ge 2 ]] || die "missing value for $1" + REMOTE_NAME="$2" + shift 2 + ;; + --dry-run) + DRY_RUN=1 + shift + ;; + -h|--help) + usage + exit 0 + ;; + *) + die "unknown option: $1" + ;; + esac +done + +command -v git >/dev/null 2>&1 || die "git is not installed" +[[ -d "${PROJECT_DIR}" ]] || die "project directory does not exist: ${PROJECT_DIR}" + +cd "${PROJECT_DIR}" + +if ! git rev-parse --is-inside-work-tree >/dev/null 2>&1; then + run git init +fi + +CURRENT_BRANCH="$(git branch --show-current 2>/dev/null || true)" +if [[ -z "${BRANCH}" ]]; then + if [[ -n "${CURRENT_BRANCH}" ]]; then + BRANCH="${CURRENT_BRANCH}" + elif git rev-parse --verify HEAD >/dev/null 2>&1; then + die "detached HEAD; provide a branch with -b or BRANCH=..." + else + BRANCH="main" + fi +fi + +if [[ -n "${CURRENT_BRANCH}" && "${CURRENT_BRANCH}" != "${BRANCH}" ]]; then + if git show-ref --verify --quiet "refs/heads/${BRANCH}"; then + run git switch "${BRANCH}" + else + run git switch -c "${BRANCH}" + fi +fi + +if git remote get-url "${REMOTE_NAME}" >/dev/null 2>&1; then + if [[ -n "${REPO_URL}" ]]; then + run git remote set-url "${REMOTE_NAME}" "${REPO_URL}" + fi +else + [[ -n "${REPO_URL}" ]] || die "remote '${REMOTE_NAME}' does not exist; provide --repo-url URL" + run git remote add "${REMOTE_NAME}" "${REPO_URL}" +fi + +HAS_WORKTREE_CHANGES=0 +if ! git diff --quiet || ! git diff --cached --quiet || [[ -n "$(git ls-files --others --exclude-standard)" ]]; then + HAS_WORKTREE_CHANGES=1 +fi + +if [[ "${HAS_WORKTREE_CHANGES}" == "1" && "${DRY_RUN}" != "1" ]]; then + [[ -n "$(git config --get user.name || true)" ]] || die "git user.name is not set" + [[ -n "$(git config --get user.email || true)" ]] || die "git user.email is not set" +fi + +run git add -A + +if git diff --cached --quiet; then + echo "No staged changes to commit." +else + run git commit -m "${COMMIT_MESSAGE}" +fi + +run git push -u "${REMOTE_NAME}" "${BRANCH}" +echo "Done."