smoke.sh

#!/usr/bin/env bash
# End-to-end smoke test. Converts moving_short_bag2, runs slam_offline to count
# replayed frames, then runs slam_live against a local fake VN-300 server.
#
# Expected runtime: <60 s on x86_64.

set -euo pipefail

REPO="$(cd "$(dirname "$0")/.." && pwd)"
BAG="${BAG:-$HOME/bags/moving_short_bag2}"
OUT="${OUT:-/tmp/rove_slam_smoke}"
BUILD="${BUILD:-$REPO/build}"
VN_PORT="${VN_PORT:-15000}"   # not :5000 to avoid clashing with a real api

echo "[smoke] repo:  $REPO"
echo "[smoke] bag:   $BAG"
echo "[smoke] out:   $OUT"
echo "[smoke] build: $BUILD"

# ─── Build ──────────────────────────────────────────────────────────
echo
echo "── build ─────────────────────────────────────────────────"
cmake -S "$REPO" -B "$BUILD" -DCMAKE_BUILD_TYPE=Release > /tmp/smoke_cmake.log
cmake --build "$BUILD" -j8 > /tmp/smoke_build.log
ctest --test-dir "$BUILD" --output-on-failure > /tmp/smoke_ctest.log
echo "  unit tests: OK"

# ─── Convert ────────────────────────────────────────────────────────
echo
echo "── convert ──────────────────────────────────────────────"
rm -rf "$OUT"
"$REPO/.venv/bin/python" "$REPO/tools/bag2recording.py" "$BAG" "$OUT"

# Compare against rosbag2 metadata
get_count() {
  # extract message_count for a given topic from metadata.yaml
  awk -v topic="$1" '
    $0 ~ "name: " topic "$"    { in_block = 1; next }
    in_block && /message_count:/ { print $2; exit }
  ' "$BAG/metadata.yaml"
}
ROSBAG_LIDAR=$(get_count "/livox/lidar")
ROSBAG_IMU=$(get_count "/imu/data")
ROSBAG_GPS=$(get_count "/fix")
ROSBAG_LIVOX_IMU=$(get_count "/livox/imu")

# Replay counts (per slam_offline)
LIDAR=$("$BUILD/slam_offline" "$OUT" 2>/dev/null | awk '$1=="LIDAR"' | wc -l)
IMU=$("$BUILD/slam_offline" "$OUT" 2>/dev/null | awk '$1=="IMU"' | wc -l)
GPS=$("$BUILD/slam_offline" "$OUT" 2>/dev/null | awk '$1=="GPS"' | wc -l)

echo
printf "  %-10s  %s vs rosbag %s\n" "LIDAR"     "$LIDAR" "$ROSBAG_LIDAR"
printf "  %-10s  %s vs rosbag %s\n" "IMU"       "$IMU" "$ROSBAG_IMU"
printf "  %-10s  %s vs rosbag %s\n" "GPS"       "$GPS" "$ROSBAG_GPS"
printf "  %-10s  not dispatched (msg_type 0x82, rosbag had %s)\n" "LIVOX_IMU" "$ROSBAG_LIVOX_IMU"

[ "$LIDAR" = "$ROSBAG_LIDAR" ] || { echo "[smoke] LIDAR count mismatch"; exit 1; }
[ "$IMU"   = "$ROSBAG_IMU"   ] || { echo "[smoke] IMU count mismatch"; exit 1; }
[ "$GPS"   = "$ROSBAG_GPS"   ] || { echo "[smoke] GPS count mismatch"; exit 1; }

# ─── Live (against fake VN-300) ─────────────────────────────────────
echo
echo "── live (fake VN-300) ──────────────────────────────────────"
"$REPO/.venv/bin/python" "$REPO/tools/fake_vectornav.py" \
    --port "$VN_PORT" --duration 4 &
FAKE_PID=$!
sleep 0.3
# Keep stderr separate so the stats line is greppable.
"$BUILD/slam_live" --vn "127.0.0.1:$VN_PORT" \
    --no-livox --duration 3 > /tmp/smoke_live_out.log 2> /tmp/smoke_live_err.log
wait "$FAKE_PID" 2>/dev/null || true

STATS_LINE=$(grep '^slam_live:' /tmp/smoke_live_err.log || true)
echo "  $STATS_LINE"

# Confirm at least some packets were received.
EMITTED=$(echo "$STATS_LINE" | grep -oE 'imu_emitted=[0-9]+' | cut -d= -f2)
if [ -z "$EMITTED" ] || [ "$EMITTED" -lt 10 ]; then
  echo "[smoke] expected >=10 IMU frames from fake server, got: ${EMITTED:-?}"
  exit 1
fi

# ─── SLAM (Phase 1) ──────────────────────────────────────────────────
if [ -x "$BUILD/slam_offline" ] && grep -q ROVE_SLAM_HAVE_KISS_ICP "$BUILD/compile_commands.json" 2>/dev/null; then
  echo
  echo "── slam (KISS-ICP) ───────────────────────────────────────"
  SLAM_OUT="${SLAM_OUT:-/tmp/rove_slam_traj}"
  rm -rf "$SLAM_OUT"
  "$BUILD/slam_offline" "$OUT" --slam --out "$SLAM_OUT" --map > /tmp/smoke_slam.log 2>&1
  TUM="$SLAM_OUT/trajectory.tum"
  [ -f "$TUM" ] || { echo "[smoke] no trajectory.tum"; exit 1; }
  POSES=$(wc -l < "$TUM")
  printf "  %-10s  %s vs rosbag %s lidar frames\n" "poses" "$POSES" "$ROSBAG_LIDAR"
  [ "$POSES" = "$ROSBAG_LIDAR" ] || { echo "[smoke] pose count != lidar count"; exit 1; }

  # End-to-start drift: a "moving_short" loop bag, should close to <1 m.
  DRIFT=$("$REPO/.venv/bin/python" - "$TUM" <<'PYEOF'
import sys
import numpy as np
xyz = np.loadtxt(sys.argv[1])[:, 1:4]
print(f"{np.linalg.norm(xyz[-1] - xyz[0]):.3f}")
PYEOF
)
  printf "  %-10s  %s m (expect <1 m on a clean loop)\n" "drift" "$DRIFT"
  if [ "$(echo "$DRIFT < 1.0" | bc -l)" != "1" ]; then
    echo "[smoke] end-to-start drift too large"
    exit 1
  fi
fi

# ─── SLAM (Phase 3.1: IMU factor + auto-calibrated bias) ──────────
# Optional: only run when GTSAM is compiled in. Verifies the IMU path
# doesn't crash and the resulting drift stays well below 5 m on the
# moving_short_bag2 loop. Don't compare against Phase 1 here — the
# IMU factor pulls the trajectory off the lidar-only solution
# until extrinsic calibration lands (Phase 3.1.2+).
if [ -x "$BUILD/slam_offline" ] && grep -q ROVE_SLAM_HAVE_GTSAM "$BUILD/compile_commands.json" 2>/dev/null; then
  echo
  echo "── slam (Phase 3.1: --imu-factor + bias) ─────────────────"
  P31_OUT="${P31_OUT:-/tmp/rove_slam_p31}"
  rm -rf "$P31_OUT"
  CAL=$("$REPO/.venv/bin/python" "$REPO/tools/calibrate_imu_bias.py" "$OUT" \
        | grep -v '^#' | tr '\n' ' ')
  echo "  calibration: $CAL"
  "$BUILD/slam_offline" "$OUT" --slam --imu-factor $CAL --out "$P31_OUT" \
      > /tmp/smoke_p31.log 2>&1
  P31_TUM="$P31_OUT/trajectory.tum"
  [ -f "$P31_TUM" ] || { echo "[smoke] Phase 3.1: no trajectory.tum"; exit 1; }
  P31_DRIFT=$("$REPO/.venv/bin/python" - "$P31_TUM" <<'PYEOF'
import sys, numpy as np
xyz = np.loadtxt(sys.argv[1])[:, 1:4]
print(f"{np.linalg.norm(xyz[-1] - xyz[0]):.3f}")
PYEOF
)
  printf "  %-10s  %s m (expect <5 m; phase-3.1 with uncalibrated extrinsic)\n" \
         "drift" "$P31_DRIFT"
  if [ "$(echo "$P31_DRIFT < 5.0" | bc -l)" != "1" ]; then
    echo "[smoke] Phase 3.1 drift too large — IMU is diverging"
    exit 1
  fi
fi

echo
echo "[smoke] PASS"