RTAB-Map

#!/usr/bin/env bash

set -euo pipefail

# 目录与环境相关配置
SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
WORKSPACE_PATH="$(cd -- "$SCRIPT_DIR/../../.." && pwd)"
WORKSPACE_SETUP="$WORKSPACE_PATH/install/setup.bash"
# 是否在新终端拉起节点（1: gnome-terminal, 0: 后台 nohup）
LAUNCH_IN_NEW_TERMINAL="${LAUNCH_IN_NEW_TERMINAL:-1}"
# 话题就绪等待超时（秒）
TOPIC_WAIT_TIMEOUT_SEC="${TOPIC_WAIT_TIMEOUT_SEC:-25}"
# RTAB-Map 机体坐标系（按你的 TF 习惯可改）
RTABMAP_FRAME_ID="${RTABMAP_FRAME_ID:-camera_link}"
LOG_DIR="$WORKSPACE_PATH/logs"
# 是否加载当前工作区 overlay（install/setup.bash）
USE_WORKSPACE_OVERLAY="${USE_WORKSPACE_OVERLAY:-0}"
RTABMAP_IMU_TOPIC="${RTABMAP_IMU_TOPIC:-}"
CLEANUP_PREVIOUS_DEMO="${CLEANUP_PREVIOUS_DEMO:-1}"
# 默认关闭 IMU，先保证纯视觉链路稳定
RTABMAP_USE_IMU="${RTABMAP_USE_IMU:-0}"
RTABMAP_DATABASE_PATH="${RTABMAP_DATABASE_PATH:-$WORKSPACE_PATH/maps/learning_depth_demo.db}"
RTABMAP_DELETE_DB_ON_START="${RTABMAP_DELETE_DB_ON_START:-true}"
RTABMAP_LINEAR_UPDATE="${RTABMAP_LINEAR_UPDATE:-0.10}"
RTABMAP_ANGULAR_UPDATE="${RTABMAP_ANGULAR_UPDATE:-0.10}"
# Orbbec UVC 后端（当前默认 v4l2）
CAMERA_UVC_BACKEND="${CAMERA_UVC_BACKEND:-v4l2}"

# 安全 source：规避 set -u 导致的未定义变量报错
source_with_nounset_guard() {
  set +u
  # ROS setup scripts may access optional variables directly.
  source "$1"
  set -u
}

# 统一加载 ROS 环境，可选叠加工作区 overlay
source_demo_environment() {
  source_with_nounset_guard /opt/ros/humble/setup.bash
  if [[ "$USE_WORKSPACE_OVERLAY" == "1" ]]; then
    if [[ ! -f "$WORKSPACE_SETUP" ]]; then
      echo "未找到 $WORKSPACE_SETUP，无法启用工作区 overlay。"
      exit 1
    fi
    source_with_nounset_guard "$WORKSPACE_SETUP"
  fi
}

# 启动命令包装：优先新终端，回退到后台 nohup
launch_command() {
  local title="$1"
  local command="$2"

  if [[ "$LAUNCH_IN_NEW_TERMINAL" == "1" ]]; then
    if ! command -v gnome-terminal >/dev/null 2>&1; then
      echo "未找到 gnome-terminal，自动改为后台启动模式。"
      LAUNCH_IN_NEW_TERMINAL="0"
    fi
  fi

  if [[ "$LAUNCH_IN_NEW_TERMINAL" == "1" ]]; then
    gnome-terminal --title "$title" -- bash -lc "
      set +u
      source /opt/ros/humble/setup.bash
      if [ '$USE_WORKSPACE_OVERLAY' = '1' ]; then
        source '$WORKSPACE_SETUP'
      fi
      set -u
      $command
      exec bash
    "
  else
    echo "后台启动: $title"
    nohup bash -lc "
      set +u
      source /opt/ros/humble/setup.bash
      if [ '$USE_WORKSPACE_OVERLAY' = '1' ]; then
        source '$WORKSPACE_SETUP'
      fi
      set -u
      $command
    " >"$LOG_DIR/${title// /_}.log" 2>&1 &
  fi
}

# 等待话题出现（存在即可）
wait_for_topic() {
  local topic="$1"
  local timeout_sec="$2"
  local start_ts elapsed

  start_ts="$(date +%s)"
  while true; do
    if ros2 topic info "$topic" >/dev/null 2>&1; then
      return 0
    fi
    elapsed=$(( $(date +%s) - start_ts ))
    if [[ "$elapsed" -ge "$timeout_sec" ]]; then
      return 1
    fi
    sleep 0.5
  done
}

# 等待话题有发布者（防止只有话题名但无数据源）
wait_for_topic_publisher() {
  local topic="$1"
  local timeout_sec="$2"
  local start_ts elapsed info

  start_ts="$(date +%s)"
  while true; do
    info="$(ros2 topic info "$topic" 2>/dev/null || true)"
    if printf '%s\n' "$info" | grep -q "Publisher count: [1-9]"; then
      return 0
    fi
    elapsed=$(( $(date +%s) - start_ts ))
    if [[ "$elapsed" -ge "$timeout_sec" ]]; then
      return 1
    fi
    sleep 0.5
  done
}

# 自动探测常见 IMU 话题
detect_imu_topic() {
  local timeout_sec="$1"
  local candidate

  if [[ -n "$RTABMAP_IMU_TOPIC" ]] && wait_for_topic "$RTABMAP_IMU_TOPIC" "$timeout_sec"; then
    return 0
  fi

  for candidate in /imu/data /camera/gyro_accel/sample /camera/imu; do
    if wait_for_topic "$candidate" "$timeout_sec"; then
      RTABMAP_IMU_TOPIC="$candidate"
      return 0
    fi
  done

  return 1
}

# 清理上一次学习流程残留进程，避免重复节点/TF 冲突
cleanup_previous_demo_processes() {
  if [[ "$CLEANUP_PREVIOUS_DEMO" != "1" ]]; then
    return 0
  fi

  # Keep cleanup narrow to this learning demo's usual processes.
  pkill -f 'gemini_330_series.launch.py' >/dev/null 2>&1 || true
  pkill -f 'rtabmap_launch rtabmap.launch.py' >/dev/null 2>&1 || true
  pkill -f 'rtabmap_slam/rtabmap' >/dev/null 2>&1 || true
  pkill -f 'rtabmap_odom/rgbd_odometry' >/dev/null 2>&1 || true
  pkill -f 'rtabmap_util/point_cloud_xyzrgb' >/dev/null 2>&1 || true
  sleep 1
}

# 准备 RTAB-Map 数据库：按需删除旧库，避免历史状态干扰教学
prepare_demo_database() {
  mkdir -p "$(dirname "$RTABMAP_DATABASE_PATH")"

  if [[ "$RTABMAP_DELETE_DB_ON_START" == "true" ]]; then
    rm -f "$RTABMAP_DATABASE_PATH"
    echo "已重置学习数据库: $RTABMAP_DATABASE_PATH"
  else
    echo "复用学习数据库: $RTABMAP_DATABASE_PATH"
  fi
}

mkdir -p "$LOG_DIR"
cleanup_previous_demo_processes
prepare_demo_database

# 步骤 1：加载运行环境
echo "[1/4] 加载 ROS 环境"
source_demo_environment

# 步骤 2：启动深度相机节点
echo "[2/4] 启动 Orbbec 深度相机"
launch_command "[learn] depth camera" "
  ros2 launch orbbec_camera gemini_330_series.launch.py \
    camera_name:=camera \
    uvc_backend:=$CAMERA_UVC_BACKEND \
    publish_tf:=true \
    enable_color:=true \
    enable_depth:=true \
    color_width:=640 \
    color_height:=480 \
    color_fps:=30 \
    depth_width:=640 \
    depth_height:=480 \
    depth_fps:=30
"

# 步骤 3：确认关键输入话题已就绪
echo "[3/4] 采样确认关键话题"
for topic in \
  /camera/color/image_raw \
  /camera/depth/image_raw \
  /camera/color/camera_info
 do
  echo "检查话题: $topic"
  if ! wait_for_topic "$topic" "$TOPIC_WAIT_TIMEOUT_SEC"; then
    echo "[错误] 话题未就绪: $topic"
    echo "请确认相机终端是否正常启动，或增大 TOPIC_WAIT_TIMEOUT_SEC 后重试。"
    exit 1
  fi
  if ! wait_for_topic_publisher "$topic" "$TOPIC_WAIT_TIMEOUT_SEC"; then
    echo "[错误] 话题未就绪: $topic"
    echo "该话题存在但没有发布者，请检查相机是否真正完成初始化。"
    exit 1
  fi
 done

# 根据配置决定是否启用 IMU 融合
imu_launch_args="wait_imu_to_init:=false"
if [[ "$RTABMAP_USE_IMU" == "1" ]]; then
  echo "检查 IMU 话题"
  if ! detect_imu_topic "$TOPIC_WAIT_TIMEOUT_SEC"; then
    echo "[错误] 未找到可用 IMU 话题（尝试了 /imu/data, /camera/gyro_accel/sample, /camera/imu）。"
    exit 1
  fi
  echo "检测到 IMU 话题: $RTABMAP_IMU_TOPIC"
  imu_launch_args="imu_topic:=$RTABMAP_IMU_TOPIC wait_imu_to_init:=true"
else
  echo "跳过 IMU 融合（RTABMAP_USE_IMU=$RTABMAP_USE_IMU），使用纯视觉里程计以减少静止抖动。"
fi

# 步骤 4：启动 RTAB-Map 建图与 RViz
echo "[4/4] 启动 RTAB-Map（教学最小配置）"
launch_command "[learn] rtabmap" "
  ros2 launch rtabmap_launch rtabmap.launch.py \
    database_path:=$RTABMAP_DATABASE_PATH \
    rtabmap_viz:=false \
    rviz:=true \
    frame_id:=$RTABMAP_FRAME_ID \
    visual_odometry:=true \
    vo_frame_id:=odom \
    odom_topic:=/odom \
    publish_tf_map:=true \
    publish_tf_odom:=true \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    $imu_launch_args \
    approx_sync:=true \
    approx_sync_max_interval:=0.02 \
    odom_args:='--Vis/MinInliers 20 --OdomF2M/BundleAdjustment true --Odom/ResetCountdown 0' \
    args:='--Reg/Force3DoF true --RGBD/LinearUpdate $RTABMAP_LINEAR_UPDATE --RGBD/AngularUpdate $RTABMAP_ANGULAR_UPDATE'
"

echo
echo "学习目录已准备完成。"
echo "请打开检查清单: docs/learning/01-depth-camera-to-rtabmap/checklist.md"