ros_contorl/scripts/run_camera_d405.sh

#!/usr/bin/env bash
# Terminal: D405 via official pyrealsense2 SDK — color + depth + stereo IR topics.
# With --preview the Qt window runs in a SEPARATE viewer process (realsense_view)
# so painting never slows the camera stream. Closing the window ≠ stop camera.
# Do NOT start view_realsense_depth.sh at the same time (USB is exclusive).
#
# Usage:
#   bash ~/ros2/scripts/run_camera_d405.sh
#   bash ~/ros2/scripts/run_camera_d405.sh --preview
#   bash ~/ros2/scripts/run_camera_d405.sh --arm 91 --preview

set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=-1   # -1 = use site; 0/1 = force
HIGH_FPS=0

# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"

while [[ $# -gt 0 ]]; do
  case "$1" in
    --preview) PREVIEW=1; shift ;;
    --no-preview) PREVIEW=0; shift ;;
    --30fps) HIGH_FPS=1; shift ;;
    -h|--help) sed -n '2,14p' "$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"

pkill -f 'lib/arm_application/realsense_color_pub' 2>/dev/null || true
sleep 2

USB_INFO="$(python3 - <<PY 2>/dev/null || true
import pyrealsense2 as rs
want_sn = "${SITE_CAM_SERIAL:-}"
ds = list(rs.context().devices)
if not ds:
    print("")
    raise SystemExit
pick = None
for d in ds:
    sn = d.get_info(rs.camera_info.serial_number)
    if want_sn and sn == want_sn:
        pick = d
        break
if pick is None:
    for d in ds:
        name = (d.get_info(rs.camera_info.name) or "").lower()
        if "405" in name:
            pick = d
            break
pick = pick or ds[0]
usb = pick.get_info(rs.camera_info.usb_type_descriptor)
sn = pick.get_info(rs.camera_info.serial_number)
name = pick.get_info(rs.camera_info.name)
print(f"{usb}|{sn}|{name}")
PY
)"
USB_TYPE="${USB_INFO%%|*}"
echo "=== RealSense USB=${USB_TYPE:-unknown}  ${USB_INFO} ==="
echo "=== site: ${SITE_CONFIG:-?}  weights=${SITE_YOLO_WEIGHTS:-?} ==="

if [[ "${USB_TYPE}" == 2* ]]; then
  CAM_W=640; CAM_H=480; CAM_FPS=15
  echo "=== camera 640x480@15 (USB2: depth+IR still ON) ==="
  echo "=== WARN: D405 is on USB2. Plug it into a USB3 (blue) port for 848x480@30 ==="
elif [[ "${SITE_CAM_USE_SITE_RES:-1}" == "1" ]]; then
  # site.yaml camera.width/height/fps 优先；--30fps 只抬帧率，不再写死 848x480
  CAM_W="${SITE_CAM_WIDTH:-848}"
  CAM_H="${SITE_CAM_HEIGHT:-480}"
  CAM_FPS="${SITE_CAM_FPS:-30}"
  if [[ "${HIGH_FPS}" -eq 1 && "${CAM_FPS}" -lt 30 ]]; then
    CAM_FPS=30
  fi
  echo "=== camera ${CAM_W}x${CAM_H}@${CAM_FPS} serial=${SITE_CAM_SERIAL:-} arm=${SITE_ROBOT_SN:-} ==="
elif [[ "${HIGH_FPS}" -eq 1 ]]; then
  CAM_W=848; CAM_H=480; CAM_FPS=30
  echo "=== camera 848x480@30 (--30fps, site use_site_resolution=false) ==="
else
  CAM_W=1280; CAM_H=720; CAM_FPS=15
  echo "=== camera 1280x720@15 ==="
fi

CAM_NAME="${SITE_CAM_NAME:-d405}"
CAM_ARGS=(
  --ros-args
  -p camera_name:="${CAM_NAME}"
  -p width:="${CAM_W}"
  -p height:="${CAM_H}"
  -p fps:="${CAM_FPS}"
)
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
  CAM_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
fi
CAM_ARGS+=(
  -p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
  -p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-4000}"
  -p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
  -p laser_power:="${SITE_CAM_LASER_POWER:-0}"
)
if [[ -n "${SITE_CAM_EXPOSURE_US:-}" && "${SITE_CAM_EXPOSURE_US}" != "0" && "${SITE_CAM_EXPOSURE_US}" != "0.0" ]]; then
  CAM_ARGS+=(-p exposure_us:="${SITE_CAM_EXPOSURE_US}")
fi
if [[ -n "${SITE_CAM_GAIN:-}" && "${SITE_CAM_GAIN}" != "0" && "${SITE_CAM_GAIN}" != "0.0" ]]; then
  CAM_ARGS+=(-p gain:="${SITE_CAM_GAIN}")
fi

if [[ "${SITE_CAM_ENABLE_DEPTH:-1}" == "1" ]]; then
  CAM_ARGS+=(-p enable_depth:=true)
  echo "=== depth ON ==="
else
  CAM_ARGS+=(-p enable_depth:=false)
  echo "=== depth OFF (site.yaml) ==="
fi

DO_PREVIEW="${SITE_CAM_PREVIEW:-1}"
if [[ "${PREVIEW}" -ge 0 ]]; then
  DO_PREVIEW="${PREVIEW}"
fi
if [[ "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
  CAM_ARGS+=(-p enable_infra:=true)
else
  CAM_ARGS+=(-p enable_infra:=false)
fi
if [[ "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
  CAM_ARGS+=(-p preview_stereo:=true)
else
  CAM_ARGS+=(-p preview_stereo:=false)
fi

if [[ "${DO_PREVIEW}" == "1" ]]; then
  echo "=== SDK preview ON (separate viewer process, hold ${SITE_CAM_OVERLAY_HOLD:-0.1}s) ==="
  echo "=== close window = hide preview only; camera keeps publishing ==="
fi

echo "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ==="

if [[ "${DO_PREVIEW}" == "1" ]]; then
  # Camera publishes only; the Qt viewer runs in its own process so painting
  # can never starve the capture loop (shared GIL cost ~70 ms/frame before).
  pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
  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
  ros2 run arm_application realsense_view --ros-args \
    -p camera_name:="${CAM_NAME}" \
    -p overlay_hold_sec:="${SITE_CAM_OVERLAY_HOLD:-0.1}" \
    -p hud_label:="${SITE_ROBOT_LABEL:-} ${SITE_ROBOT_SN:-} ${SITE_CAM_SERIAL:-}"
else
  exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi