ros_contorl/scripts/run_all.sh

#!/usr/bin/env bash
# 三阶段装配流程的完整入口：一阶段跟踪 → 二阶段对孔 → 三阶段搜孔轻压。
#
# 为什么要单独有这个脚本：
#   pipeline.launch.py 只管两件事 —— 发布手眼静态 TF、起 assembly_pipeline 串联节点。
#   它**不**拉相机、也**不**拉触觉（见它文件头的注释）。前置的相机与触感服务
#   必须由本脚本拉起一次，否则二/三阶段会因为拿不到图像或力数据而空转。
#
# 本脚本做的事（顺序固定）：
#   1) 确认机械臂控制器已 ACTIVE（不在就直接报错退出，不假装能跑）
#   2) 拉起 D405 相机（已在发图就跳过）
#   3) 拉起触觉服务（只有左臂装了传感器；右臂自动跳过）
#   4) exec pipeline.launch.py，把下面的 CLI 参数映射成它的 launch 参数
#
# 用法：
#   bash ~/ros2/scripts/run_all.sh --arm 89                      # 全流程
#   bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2            # 指定对 k2 孔
#   bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1        # 臂已在工件上方，跳过跟踪
#   bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
#   bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview     # 相机带 Qt 预览窗
#   bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera          # 相机已在别处拉起了
#   bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile         # 不起触觉服务
#
# 前置（另开一个终端，先跑起来）：
#   bash ~/ros2/scripts/run_real.sh --arm 89
#
# 注意：run_pipeline.sh 是本脚本的别名（历史名字），两者等价。

set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"

# ---- 默认值：全部沿用 site.yaml / launch 默认，CLI 只做覆盖 ----
SKIP_STAGE1=false
SKIP_STAGE2=false
SKIP_STAGE3=false
HOLE=""
DEPTH=""
PITCH=""
PIPELINE_PREVIEW=true     # 传给 pipeline 节点：是否显示推理预览
RESNAP=false
WITH_CAMERA=true
WITH_TACTILE=true
CAMERA_PREVIEW=false

# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
# 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
    --skip-stage1)    SKIP_STAGE1=true; shift ;;
    --skip-stage2)    SKIP_STAGE2=true; shift ;;
    --skip-stage3)    SKIP_STAGE3=true; shift ;;
    --hole)           HOLE="${2:?--hole 需要一个孔名（k1 或 k2）}"; shift 2 ;;
    --depth)          DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
    --pitch)          PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
    --resnap)         RESNAP=true; shift ;;
    --no-preview)     PIPELINE_PREVIEW=false; shift ;;
    --camera-preview) CAMERA_PREVIEW=true; shift ;;
    --no-camera)      WITH_CAMERA=false; shift ;;
    --no-tactile)     WITH_TACTILE=false; shift ;;
    -h|--help)        sed -n '2,30p' "$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"

if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
  export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
  export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
fi

echo "=== run_all（三阶段串联） arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="

# ---- 1) 控制器必须在。判活方式与 run_stage2_locate.sh 保持一致 ----
echo "=== 等待 rizon_arm_controller ACTIVE（T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}）==="
# 先看服务在不在，再 call —— 顺序反过来的话，服务不存在时每次 call 都要耗满
# 超时才返回，30 次循环能白等两三分钟（实测 63s），用户会以为脚本卡死了。
ready=false
if timeout 3 ros2 service list 2>/dev/null | grep -q "^/controller_manager/list_controllers$"; then
  for _i in $(seq 1 20); do
    if timeout 3 ros2 service call /controller_manager/list_controllers \
        controller_manager_msgs/srv/ListControllers 2>/dev/null \
        | grep -q "name='rizon_arm_controller', state='active'"; then
      ready=true
      break
    fi
    sleep 0.5
  done
fi
if [[ "${ready}" != true ]]; then
  echo "ERROR: 机械臂控制器未激活。请先在另一终端跑：" >&2
  echo "  bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
  echo "（若该终端已在跑，先确认它没报错退出、且 Rizon 上电使能成功。）" >&2
  exit 1
fi
echo "    rizon_arm_controller ACTIVE"

# ---- 2) 相机。已在发图就跳过（避免重复起导致 USB 独占失败）----
CAM_NS="${SITE_CAM_NAME:-d405_left}"
IMG_TOPIC="/${CAM_NS}/color/image_raw"

_camera_ok() {
  # 以真实消息为准：topic info 的 Publisher count 依赖 DDS 发现，
  # 进程频繁重启后会出现假阴性（run_stage2_locate.sh 里踩过这个坑）。
  # 但先查 topic 列表，避免话题根本不存在时 echo 白等到超时。
  timeout 2 ros2 topic list 2>/dev/null | grep -qx "${IMG_TOPIC}" || return 1
  timeout 3 ros2 topic echo --once "${IMG_TOPIC}" 2>/dev/null | grep -q "^header:"
}

if [[ "${WITH_CAMERA}" == true ]]; then
  if _camera_ok; then
    echo "=== 相机 ${IMG_TOPIC} 已在发图，跳过拉起 ==="
  else
    echo "=== 拉起相机 ${CAM_NS} ==="
    # 后台拉起；默认不带预览窗（nohup 下开 Qt 窗口没有意义）。
    # 想实时看画面就用 --camera-preview，或干脆另开终端跑 run_camera_d405.sh --preview。
    CAM_ARGS=()
    [[ "${CAMERA_PREVIEW}" == true ]] && CAM_ARGS+=(--preview) || CAM_ARGS+=(--no-preview)
    nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \
      "${CAM_ARGS[@]}" > /tmp/run_all_camera.log 2>&1 &
    echo "    camera pid=$! log=/tmp/run_all_camera.log"
    cam_ok=false
    for _i in $(seq 1 24); do   # 最多等 ~12s；RealSense 枚举 + 首帧比想象中慢
      if _camera_ok; then cam_ok=true; break; fi
      sleep 0.5
    done
    if [[ "${cam_ok}" != true ]]; then
      echo "ERROR: ${IMG_TOPIC} 仍无图像。看日志：/tmp/run_all_camera.log" >&2
      echo "  常见原因：USB 没插好；或另一个进程占着相机（USB 是独占的）。" >&2
      exit 1
    fi
    echo "    相机 ${CAM_NS} 出图 OK"
  fi
else
  echo "=== --no-camera：跳过相机检查 ==="
fi

# ---- 3) 触觉服务。只有左臂装了传感器 ----
# 同样先查 topic 列表再 echo，避免服务没起时反复撞超时。
TACTILE_TOPIC="/tactile/left/status"
_tactile_ok() {
  timeout 2 ros2 topic list 2>/dev/null | grep -qx "${TACTILE_TOPIC}" || return 1
  timeout 3 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
}

if [[ "${WITH_TACTILE}" == true ]]; then
  # 本臂有没有触觉由 site.yaml 的 robots[].tactile 说了算。
  # 以前这里写的是 `SN == Rizon4-063989`：换一台机器人就再也拉不起触觉服务，
  # 而且失败现象是"搜孔时力阈值全是 0"，离真正原因很远。
  if [[ "${SITE_ROBOT_HAS_TACTILE:-0}" == "1" ]]; then
    if _tactile_ok; then
      echo "=== 触觉 ${TACTILE_TOPIC} 已在发，跳过拉起 ==="
    else
      echo "=== 拉起触觉服务（左臂）==="
      # 先清干净：上一次异常退出留下的 worker 会占死 /dev/video*，
      # 新进程会以"打不开相机"的形式失败，报错信息看不出根因。
      pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
      pkill orisys_tac_w 2>/dev/null || true
      pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
      sleep 1.0
      nohup ros2 launch arm_application tactile.launch.py > /tmp/run_all_tactile.log 2>&1 &
      echo "    tactile pid=$! log=/tmp/run_all_tactile.log"
      t_ok=false
      for _i in $(seq 1 60); do   # 最多等 ~30s；传感器要在 Python 侧加载模型
        if _tactile_ok; then
          t_ok=true
          break
        fi
        sleep 0.5
      done
      if [[ "${t_ok}" != true ]]; then
        # 触觉不通不阻断主流程：三阶段的搜孔轻压需要力数据，但一/二阶段不需要。
        # 让用户自己判断是先把传感器弄好，还是只跑到对孔就停。
        echo "WARN: 触觉话题仍未就绪（传感器没接？）。流程继续，三阶段搜孔会受影响。" >&2
        echo "      日志：/tmp/run_all_tactile.log" >&2
      else
        echo "    触觉就绪 OK"
      fi
    fi
  else
    echo "=== 触觉只有左臂装了；当前 ${SITE_ROBOT_ARM:-?}，跳过 ==="
  fi
else
  echo "=== --no-tactile：跳过触觉检查 ==="
fi

# ---- 4) 清掉可能打架的旧进程 ----
echo "=== 清理旧的一阶段伺服/感知进程 ==="
pkill -f 'visual_servo.launch' 2>/dev/null || true
pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
pkill -f 'lib/arm_application/assembly_pipeline' 2>/dev/null || true
sleep 1

# ---- 5) exec pipeline.launch.py（把 CLI 映射成它的 launch 参数）----
ARGS=(
  "skip_stage1:=${SKIP_STAGE1}"
  "skip_stage2:=${SKIP_STAGE2}"
  "skip_stage3:=${SKIP_STAGE3}"
  "preview:=${PIPELINE_PREVIEW}"
  "resnap:=${RESNAP}"
)
[[ -n "${HOLE}" ]]  && ARGS+=("target_hole:=${HOLE}")
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")

echo "=== launch pipeline  hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} ==="
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"