#!/usr/bin/env bash
# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。
# 可以和 run_camera_d405.sh 同时开：只杀 camera_name:=d435，不动腕部那台。
#
#   bash ~/ros2/scripts/run_camera_d435.sh --preview
#
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=1
while [[ $# -gt 0 ]]; do
  case "$1" in
    --preview) PREVIEW=1; shift ;;
    --no-preview) PREVIEW=0; shift ;;
    -h|--help) sed -n '2,8p' "$0"; exit 0 ;;
    *) echo "Unknown: $1" >&2; exit 1 ;;
  esac
done
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
SN="${SITE_D435_SERIAL:-}"
if [[ -z "${SN}" ]]; then
  echo "ERROR: cameras.d435.serial empty in site.yaml" >&2
  exit 2
fi
echo "=== D435 overhead serial=${SN}  ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ==="
# 只杀顶视这一台。不要 pkill 全部 realsense_view —— 那会把正在看的 D405 预览窗干掉，
# 绿框叠在 D405 窗口上，窗没了就像"伺服没画面、也没工件框"。
pkill -f "camera_name:=d435( |$)" 2>/dev/null || true
CAM_ARGS=(
  --ros-args
  -p camera_name:=d435
  -p camera_serial:="${SN}"
  -p width:="${SITE_D435_WIDTH:-1280}"
  -p height:="${SITE_D435_HEIGHT:-720}"
  -p fps:="${SITE_D435_FPS:-30}"
  -p enable_depth:=true
  -p enable_infra:=false
  -p preview_stereo:=false
  -p auto_exposure:=true
  -p ae_limit_us:=8000
  -p emitter_enabled:=true
)
if [[ "${PREVIEW}" == "1" ]]; then
  ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
  CAM_PID=$!
  trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
  # 绿框来自 workpiece/detection_overhead（D435 像素）。不要订 workpiece/detection。
  ros2 run arm_application realsense_view --ros-args \
    -p camera_name:=d435 \
    -p overlay_detection:=true \
    -p detection_topic:=workpiece/detection_overhead \
    -p overlay_hold_sec:=1.5 \
    -p hud_label:="D435 顶视"
else
  exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi