Robot record test

align_recording.py

@@ -0,0 +1,169 @@
+#!/usr/bin/env python3
+"""
+align_recording.py — align all streams of a recording to a common timeline.
+
+Strategy: for each "master" timestamp (default: lidar @ 10 Hz),
+find the nearest sample in every other stream.  This yields a single
+table where each row is a synchronized multimodal observation.
+
+Output: a pandas DataFrame (saved as parquet for fast loading).
+
+Usage:
+    python3 align_recording.py /path/to/session
+    python3 align_recording.py /path/to/session --master lowstate --rate 20
+"""
+
+import argparse
+from pathlib import Path
+import numpy as np
+import pandas as pd
+
+
+# Per-stream timestamp column names + bin file (None = no bin, just timestamps)
+STREAMS = {
+    "lowstate":   ("lowstate_timestamps.csv",   "lowstate_frames.bin",   "unix_ns"),
+    "lidar":      ("lidar_timestamps.csv",      "lidar_scans.bin",       "unix_ns"),
+    "depth":      ("depth_timestamps.csv",      "depth_frames.bin",      "unix_ns"),
+    "odom":       ("odom_timestamps.csv",       "odom_frames.bin",       "unix_ns"),
+    "audio":      ("audio_packets.csv",         None,                    "unix_ns"),
+    "top_cam":    ("top_camera_frames.csv",     None,                    "wallclock_unix_ns"),
+    "front_cam":  ("front_camera_frames.csv",   None,                    "wallclock_unix_ns"),
+    "down_cam":   ("down_camera_frames.csv",    None,                    "wallclock_unix_ns"),
+}
+
+
+def load_stream(session: Path, csv_name: str, time_col: str):
+    """Load a timestamp CSV.  Returns (timestamps_ns, row_indices, df)."""
+    path = session / csv_name
+    if not path.exists():
+        return None, None, None
+    df = pd.read_csv(path)
+    if time_col not in df.columns:
+        # fall through to alternative column names
+        for alt in ("wallclock_unix_ns", "unix_ns"):
+            if alt in df.columns:
+                time_col = alt
+                break
+        else:
+            return None, None, None
+    ts = df[time_col].astype(np.int64).values
+    idx = np.arange(len(ts))
+    return ts, idx, df
+
+
+def align_to_master(master_ts: np.ndarray, slave_ts: np.ndarray,
+                    slave_idx: np.ndarray, max_dt_ms: float = 100):
+    """For each master timestamp, return the slave index of the nearest
+    sample, or -1 if no slave sample within max_dt_ms.
+    """
+    # searchsorted gives the insertion point; we check both neighbors.
+    pos = np.searchsorted(slave_ts, master_ts)
+    pos_left = np.clip(pos - 1, 0, len(slave_ts) - 1)
+    pos_right = np.clip(pos, 0, len(slave_ts) - 1)
+
+    d_left = np.abs(master_ts - slave_ts[pos_left])
+    d_right = np.abs(master_ts - slave_ts[pos_right])
+
+    pick_right = d_right < d_left
+    best_pos = np.where(pick_right, pos_right, pos_left)
+    best_dt = np.where(pick_right, d_right, d_left)
+
+    # Mask out samples beyond max_dt
+    too_far = best_dt > int(max_dt_ms * 1e6)  # ms → ns
+    aligned_idx = np.where(too_far, -1, slave_idx[best_pos])
+    aligned_dt_ms = best_dt / 1e6
+    aligned_dt_ms[too_far] = np.nan
+
+    return aligned_idx, aligned_dt_ms
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument("session", help="Path to a recording session directory")
+    p.add_argument("--master", default="lidar",
+                   help="Stream to use as the master timeline (default: lidar)")
+    p.add_argument("--max-dt-ms", type=float, default=100,
+                   help="Max alignment distance to accept; samples further than "
+                        "this from master become -1/NaN (default: 100ms)")
+    p.add_argument("--out", default="aligned.parquet",
+                   help="Output filename (parquet) relative to session dir")
+    p.add_argument("--out-csv", default=None,
+                   help="Also write a CSV (slower to load but human-readable)")
+    args = p.parse_args()
+
+    session = Path(args.session)
+    if not session.exists():
+        print(f"❌ session not found: {session}")
+        return 1
+
+    # ── Load master ──────────────────────────────────────────────────────
+    if args.master not in STREAMS:
+        print(f"❌ unknown master '{args.master}'. choices: {list(STREAMS)}")
+        return 1
+    master_csv, _, master_time_col = STREAMS[args.master]
+    master_ts, master_idx, master_df = load_stream(session, master_csv, master_time_col)
+    if master_ts is None:
+        print(f"❌ master stream '{args.master}' not found in session")
+        return 1
+    print(f"Master: {args.master}  ({len(master_ts)} samples)")
+
+    # Build the master table: timestamp + frame index in master file
+    aligned = pd.DataFrame({
+        "unix_ns": master_ts,
+        f"{args.master}_idx": master_idx,
+    })
+
+    # ── For each other stream, find nearest neighbor ─────────────────────
+    for name, (csv_name, _, time_col) in STREAMS.items():
+        if name == args.master:
+            continue
+        slave_ts, slave_idx, slave_df = load_stream(session, csv_name, time_col)
+        if slave_ts is None:
+            print(f"  - {name}: not found, skipping")
+            continue
+
+        aligned_idx, aligned_dt = align_to_master(
+            master_ts, slave_ts, slave_idx, args.max_dt_ms
+        )
+
+        aligned[f"{name}_idx"] = aligned_idx
+        aligned[f"{name}_dt_ms"] = aligned_dt
+
+        n_matched = (aligned_idx >= 0).sum()
+        coverage = 100 * n_matched / len(master_ts)
+        median_dt = np.nanmedian(aligned_dt)
+        print(f"  + {name}: {n_matched}/{len(master_ts)} matched "
+              f"({coverage:.1f}% coverage, median Δt={median_dt:.1f}ms)")
+
+    # ── Save ─────────────────────────────────────────────────────────────
+    out_path = session / args.out
+    aligned.to_parquet(out_path, index=False)
+    print(f"\n✅ aligned table saved to: {out_path}")
+    print(f"   shape: {aligned.shape}")
+
+    if args.out_csv:
+        csv_path = session / args.out_csv
+        aligned.to_csv(csv_path, index=False)
+        print(f"✅ also saved CSV to: {csv_path}")
+
+    print("\nFirst 3 rows:")
+    print(aligned.head(3).to_string())
+
+    print("\nUsage example for training data loader:")
+    print("""
+    import pandas as pd
+    aligned = pd.read_parquet("aligned.parquet")
+
+    for _, row in aligned.iterrows():
+        t_ns = row["unix_ns"]
+        lidar_frame_idx = int(row["lidar_idx"])     # which frame in lidar_scans.bin
+        lowstate_idx = int(row["lowstate_idx"])     # which row in lowstate_frames.bin
+        depth_idx = int(row["depth_idx"])
+        top_cam_idx = int(row["top_cam_idx"])       # which frame in top_camera.mp4
+        # ... use these indices to fetch the actual sensor data
+    """)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

cmd/main/main.go

@@ -1,23 +1,39 @@
 package main
 
 import (
+	"context"
 	"encoding/json"
 	"log/slog"
 	"os"
 	"os/signal"
 	"path/filepath"
 	"syscall"
+	"time"
 
 	"om1-telemetry/config"
 	"om1-telemetry/internal/audio"
 	"om1-telemetry/internal/depth"
+	"om1-telemetry/internal/heartbeat"
 	"om1-telemetry/internal/lidar"
+	"om1-telemetry/internal/lowstate"
 	"om1-telemetry/internal/network"
 	"om1-telemetry/internal/odom"
 	"om1-telemetry/internal/pointcloud"
 	"om1-telemetry/internal/video"
 )
 
+// envBool reads a boolean env var.  Defaults true if unset.
+// Accepts: false/0/no → false; true/1/yes (or unset) → true.
+func envBool(key string, defaultValue bool) bool {
+	switch os.Getenv(key) {
+	case "false", "0", "no":
+		return false
+	case "true", "1", "yes":
+		return true
+	}
+	return defaultValue
+}
+
 func main() {
 	cfg := config.Load()
 
@@ -46,25 +62,118 @@ func main() {
 		os.Exit(1)
 	}
 
+	// ── PER-ROBOT RECORDER SELECTION ─────────────────────────────────────
+	// Go2 typically wants /scan (2D) recorded; the 3D pointcloud topic
+	// "cloud_livox_mid360" doesn't exist on Go2 anyway.
+	// G1 typically wants the 3D Livox Mid-360 point cloud; /scan exists but
+	// is less informative than the 3D source.
+	//
+	// Set in env.go2:  ENABLE_LIDAR=true   ENABLE_POINTCLOUD=false
+	// Set in env.g1:   ENABLE_LIDAR=false  ENABLE_POINTCLOUD=true
+	enableLidar := envBool("ENABLE_LIDAR", true)
+	enablePointcloud := envBool("ENABLE_POINTCLOUD", true)
+	// ─────────────────────────────────────────────────────────────────────
+
+	// ── HEARTBEAT MONITOR ────────────────────────────────────────────────
+	// SILENT during normal operation; warns once when a stream stops
+	// receiving or its rate falls below half of expected.
+	mon := heartbeat.NewMonitor(30 * time.Second)
+
+	// Register only the recorders that will actually run, so disabled ones
+	// don't generate spurious "never received any message" warnings.
+	if enableLidar {
+		mon.Register(lidar.HeartbeatName, 10) // ~10 Hz nominal
+	}
+	if enablePointcloud {
+		mon.Register(pointcloud.HeartbeatName, 10) // ~10 Hz nominal
+	}
+	mon.Register(depth.HeartbeatName, 10)     // measured 15 Hz; floor at 10
+	mon.Register(odom.HeartbeatName, 30)      // 30-50 Hz typical
+	mon.Register(lowstate.HeartbeatName, 400) // Go2 ~500 / G1 ~1053; 400 = safe floor
+	mon.Register(network.HeartbeatName, 0)    // 0 = liveness check only
+	mon.Register(audio.HeartbeatName, 0)      // 0 = liveness check only
+
+	// Video has multiple cameras; give each a unique heartbeat name.
+	videoHeartbeatNames := make([]string, len(cfg.Video))
+	for i, vc := range cfg.Video {
+		videoHeartbeatNames[i] = "video_" + vc.Name
+		mon.Register(videoHeartbeatNames[i], 0)
+	}
+
+	hbCtx, hbCancel := context.WithCancel(context.Background())
+	go mon.Run(hbCtx)
+	// ─────────────────────────────────────────────────────────────────────
+
+	// Build each video recorder with its unique heartbeat name.
 	videoStreams := make([]*video.VideoRTSPStream, 0, len(cfg.Video))
-	for _, vc := range cfg.Video {
-		videoStreams = append(videoStreams, video.New(vc.VideoStreamConfig()))
+	for i, vc := range cfg.Video {
+		vcfg := vc.VideoStreamConfig()
+		vcfg.Monitor = mon
+		vcfg.HeartbeatName = videoHeartbeatNames[i]
+		if vcfg.FramesFile == "" {
+			ext := filepath.Ext(vcfg.OutputFile)
+			stem := vcfg.OutputFile[:len(vcfg.OutputFile)-len(ext)]
+			vcfg.FramesFile = stem + "_frames.csv"
+		}
+		videoStreams = append(videoStreams, video.New(vcfg))
+	}
+
+	// Audio: attach monitor and ensure frames CSV path is set.
+	audioCfg := cfg.Audio.AudioStreamConfig()
+	audioCfg.Monitor = mon
+	if audioCfg.FramesFile == "" {
+		ext := filepath.Ext(audioCfg.OutputFile)
+		stem := audioCfg.OutputFile[:len(audioCfg.OutputFile)-len(ext)]
+		audioCfg.FramesFile = stem + "_packets.csv"
 	}
-	audioStream := audio.New(cfg.Audio.AudioStreamConfig())
-	lidarStream := lidar.New(cfg.Lidar.LidarStreamConfig())
-	pointCloudStream := pointcloud.New(cfg.PointCloud.PointCloudStreamConfig())
-	depthStream := depth.New(cfg.Depth.DepthStreamConfig())
-	odomStream := odom.New(cfg.Odom.OdomStreamConfig())
-	networkStream := network.New(cfg.Network.NetworkStreamConfig())
+	audioStream := audio.New(audioCfg)
 
+	// ── ZENOH RECORDERS ──────────────────────────────────────────────────
+	// Lidar and Pointcloud are conditionally created based on env vars.
+	var lidarStream *lidar.LidarStream
+	if enableLidar {
+		lidarCfg := cfg.Lidar.LidarStreamConfig()
+		lidarCfg.Monitor = mon
+		lidarStream = lidar.New(lidarCfg)
+	}
+
+	var pointCloudStream *pointcloud.PointCloudStream
+	if enablePointcloud {
+		pointCloudCfg := cfg.PointCloud.PointCloudStreamConfig()
+		pointCloudCfg.Monitor = mon
+		pointCloudStream = pointcloud.New(pointCloudCfg)
+	}
+
+	depthCfg := cfg.Depth.DepthStreamConfig()
+	depthCfg.Monitor = mon
+	depthStream := depth.New(depthCfg)
+
+	odomCfg := cfg.Odom.OdomStreamConfig()
+	odomCfg.Monitor = mon
+	odomStream := odom.New(odomCfg)
+
+	lowstateCfg := cfg.Lowstate.LowstateStreamConfig()
+	lowstateCfg.Monitor = mon
+	lowstateStream := lowstate.New(lowstateCfg)
+
+	networkCfg := cfg.Network.NetworkStreamConfig()
+	networkCfg.Monitor = mon
+	networkStream := network.New(networkCfg)
+
+	// Start every recorder (nil-safe for the conditional ones).
 	for _, vs := range videoStreams {
 		vs.Start()
 	}
 	audioStream.Start()
-	lidarStream.Start()
-	pointCloudStream.Start()
+	if lidarStream != nil {
+		lidarStream.Start()
+	}
+	if pointCloudStream != nil {
+		pointCloudStream.Start()
+	}
 	depthStream.Start()
 	odomStream.Start()
+	lowstateStream.Start()
 	networkStream.Start()
 
 	videoURLs := make([]string, 0, len(cfg.Video))
@@ -76,11 +185,15 @@ func main() {
 		"session", cfg.SessionDir,
 		"video-cameras", videoURLs,
 		"audio-url", cfg.Audio.RTSPURL,
+		"lidar-enabled", enableLidar,
 		"lidar-topic", cfg.Lidar.ZenohTopic,
+		"pointcloud-enabled", enablePointcloud,
 		"pointcloud-topic", cfg.PointCloud.ZenohTopic,
 		"depth-topic", cfg.Depth.ZenohTopic,
 		"odom-topic", cfg.Odom.ZenohTopic,
+		"lowstate-topic", cfg.Lowstate.ZenohTopic,
 		"net-ping-host", cfg.Network.PingHost,
+		"heartbeat", "30s interval; logs only when broken/recovered",
 	)
 	slog.Info("press Ctrl-C to stop")
 
@@ -89,13 +202,23 @@ func main() {
 	<-shutdownSignal
 
 	slog.Info("shutting down…")
+
+	// Stop the heartbeat monitor BEFORE recorders so it doesn't warn
+	// "stopped receiving" during shutdown.
+	hbCancel()
+
 	for _, vs := range videoStreams {
 		vs.Stop()
 	}
 	audioStream.Stop()
-	lidarStream.Stop()
-	pointCloudStream.Stop()
+	if lidarStream != nil {
+		lidarStream.Stop()
+	}
+	if pointCloudStream != nil {
+		pointCloudStream.Stop()
+	}
 	depthStream.Stop()
 	odomStream.Stop()
+	lowstateStream.Stop()
 	networkStream.Stop()
 }