diff --git a/align_recording.py b/align_recording.py new file mode 100644 index 0000000..42688c3 --- /dev/null +++ b/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()) diff --git a/cmd/main/main.go b/cmd/main/main.go index dd02eb3..f984a7a 100644 --- a/cmd/main/main.go +++ b/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() } diff --git a/config/config.go b/config/config.go index 7f46bb7..f0f6c40 100644 --- a/config/config.go +++ b/config/config.go @@ -9,6 +9,7 @@ import ( "om1-telemetry/internal/audio" "om1-telemetry/internal/depth" "om1-telemetry/internal/lidar" + "om1-telemetry/internal/lowstate" "om1-telemetry/internal/network" "om1-telemetry/internal/odom" "om1-telemetry/internal/pointcloud" @@ -25,6 +26,7 @@ type Config struct { PointCloud PointCloudConfig Depth DepthConfig Odom OdomConfig + Lowstate LowstateConfig Network NetworkConfig } @@ -69,6 +71,18 @@ type OdomConfig struct { DataFile string } +// LowstateConfig: catch-all robot state (IMU + joints + battery + foot +// forces + remote). Same config & recorder for Go2 and G1 — the message +// schema differs (unitree_go/LowState vs unitree_hg/LowState) but the +// recorder stores raw bytes, so it doesn't need to know. Decode the +// recorded data offline with the appropriate ROS package. +type LowstateConfig struct { + ZenohEndpoint string + ZenohTopic string + TimestampsFile string + DataFile string +} + type NetworkConfig struct { PingHost string PingTimeout time.Duration @@ -95,12 +109,21 @@ func Load() Config { OutputFile: filepath.Join(sessionDir, "audio.ogg"), TimestampsFile: filepath.Join(sessionDir, "audio_timestamps.csv"), }, + // Lidar (2D scan): /scan is the 2D laser scan converted from 3D + // point cloud by pointcloud_to_laserscan_node. Smaller than the + // 3D cloud (~10 KB vs ~440 KB per scan) but loses height info. + // On G1 the converter outputs /scan_raw (cloud_in = utlidar/cloud_livox_mid360). + // Override with LIDAR_ZENOH_TOPIC env var (e.g. "rt/scan_raw" on G1) + // or set ENABLE_COLLECTION=false on this recorder if you only care + // about the 3D point cloud (recorded by the pointcloud recorder). Lidar: LidarConfig{ ZenohEndpoint: envStr("LIDAR_ZENOH_ENDPOINT", "tcp/127.0.0.1:7447"), ZenohTopic: envStr("LIDAR_ZENOH_TOPIC", "scan"), TimestampsFile: filepath.Join(sessionDir, "lidar_timestamps.csv"), DataFile: filepath.Join(sessionDir, "lidar_scans.bin"), }, + // PointCloud (3D): the main 3D LiDAR data source. Default is + // the G1 Livox Mid-360 topic; on Go2 override to "rt/utlidar/cloud_deskewed". PointCloud: PointCloudConfig{ ZenohEndpoint: envStr("POINTCLOUD_ZENOH_ENDPOINT", "tcp/127.0.0.1:7447"), ZenohTopic: envStr("POINTCLOUD_ZENOH_TOPIC", "rt/utlidar/cloud_livox_mid360"), @@ -113,12 +136,27 @@ func Load() Config { TimestampsFile: filepath.Join(sessionDir, "depth_timestamps.csv"), DataFile: filepath.Join(sessionDir, "depth_frames.bin"), }, + // Odom: basic foot-odometry by default ("odom"). Could be + // overridden to a higher-quality LIO topic if available: + // Go2: "/lio_sam_ros2/mapping/odometry" + // G1: "/unitree/slam_mapping/odom" + // The /lowstate recording already contains raw IMU, so post-hoc + // SLAM-quality odom can be computed offline if needed. Odom: OdomConfig{ ZenohEndpoint: envStr("ODOM_ZENOH_ENDPOINT", "tcp/127.0.0.1:7447"), ZenohTopic: envStr("ODOM_ZENOH_TOPIC", "odom"), TimestampsFile: filepath.Join(sessionDir, "odom_timestamps.csv"), DataFile: filepath.Join(sessionDir, "odom_frames.bin"), }, + // Lowstate: Same default topic ("rt/lowstate") for Go2 and G1. + // The message schema differs internally but the recorder is robot-agnostic + // (stores raw bytes). Measured rates: Go2 ~500 Hz, G1 ~1053 Hz. + Lowstate: LowstateConfig{ + ZenohEndpoint: envStr("LOWSTATE_ZENOH_ENDPOINT", "tcp/127.0.0.1:7447"), + ZenohTopic: envStr("LOWSTATE_ZENOH_TOPIC", "rt/lowstate"), + TimestampsFile: filepath.Join(sessionDir, "lowstate_timestamps.csv"), + DataFile: filepath.Join(sessionDir, "lowstate_frames.bin"), + }, Network: NetworkConfig{ PingHost: envStr("NET_PING_HOST", "8.8.8.8"), PingTimeout: envDuration("NET_PING_TIMEOUT", 2*time.Second), @@ -128,6 +166,10 @@ func Load() Config { } } +// videoConfigs returns the configured video RTSP cameras. Add another entry +// (or set its env var) to record additional streams such as Insta360 (set +// INSTA360_RTSP_URL after confirming the URL via: +// ffprobe rtsp://localhost:8554/insta360 ). func videoConfigs(sessionDir string) []VideoConfig { cameras := []struct { name string @@ -136,6 +178,7 @@ func videoConfigs(sessionDir string) []VideoConfig { {"top_camera", "rtsp://localhost:8554/top_camera_raw"}, {"front_camera", "rtsp://localhost:8554/front_camera"}, {"down_camera", "rtsp://localhost:8554/down_camera"}, + // {"insta360", "rtsp://localhost:8554/insta360"}, // uncomment when URL confirmed } configs := make([]VideoConfig, 0, len(cameras)) @@ -203,6 +246,15 @@ func (c OdomConfig) OdomStreamConfig() odom.Config { } } +func (c LowstateConfig) LowstateStreamConfig() lowstate.Config { + return lowstate.Config{ + ZenohEndpoint: c.ZenohEndpoint, + ZenohTopic: c.ZenohTopic, + TimestampsFile: c.TimestampsFile, + DataFile: c.DataFile, + } +} + func (c NetworkConfig) NetworkStreamConfig() network.Config { return network.Config{ PingHost: c.PingHost, @@ -236,4 +288,4 @@ func envBool(key string, defaultValue bool) bool { return true } return defaultValue -} +} \ No newline at end of file diff --git a/env.g1 b/env.g1 new file mode 100644 index 0000000..8b9fe33 --- /dev/null +++ b/env.g1 @@ -0,0 +1,28 @@ +export ENABLE_LIDAR=false +export ENABLE_POINTCLOUD=true + +export POINTCLOUD_ZENOH_TOPIC="rt/utlidar/cloud_livox_mid360" +export LOWSTATE_ZENOH_TOPIC="rt/lowstate" +export ODOM_ZENOH_TOPIC="odom" +export DEPTH_ZENOH_TOPIC="camera/realsense2_camera_node/depth/image_rect_raw" +export RECORDINGS_DIR="/mnt/usb_ssd/recordings" + +# Network health probe +export NET_PING_HOST="8.8.8.8" +export NET_POLL_INTERVAL="5s" +export NET_PING_TIMEOUT="2s" + +# RTSP URLs (adjust per G1 setup) +export TOP_CAMERA_RTSP_URL="rtsp://localhost:8554/top_camera_raw" +export FRONT_CAMERA_RTSP_URL="rtsp://localhost:8554/front_camera" +export DOWN_CAMERA_RTSP_URL="rtsp://localhost:8554/down_camera" +export AUDIO_RTSP_URL="rtsp://localhost:8554/audio" + +# Zenoh endpoint (localhost) +export LIDAR_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export POINTCLOUD_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export DEPTH_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export ODOM_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export LOWSTATE_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" + +echo "✅ env.g1 sourced — recording profile: G1 (3D pointcloud enabled, 2D lidar disabled)" diff --git a/env.go2 b/env.go2 new file mode 100644 index 0000000..c67888e --- /dev/null +++ b/env.go2 @@ -0,0 +1,48 @@ +# ═══════════════════════════════════════════════════════════════════════ +# Environment configuration for Go2 quadruped robot +# Usage: source env.go2 && ./om1-telemetry +# ═══════════════════════════════════════════════════════════════════════ + +# ── Robot-specific: Go2 records 2D /scan ONLY (no 3D pointcloud) ───── +# Go2's LiDAR is a rotating 2D laser; the 3D variants (cloud_deskewed) +# exist but the 2D /scan is sufficient for this robot's use case. +export ENABLE_LIDAR=true +export ENABLE_POINTCLOUD=false + +export LIDAR_ZENOH_TOPIC="scan" + +# ── Lowstate: same Zenoh topic on Go2 and G1 (message schemas differ +# internally, but recorder is robot-agnostic — stores raw bytes) ────── +export LOWSTATE_ZENOH_TOPIC="rt/lowstate" + +# ── Odom: Go2's basic /odom by default. If you want LIO-SAM quality, +# uncomment the line below. /lowstate already contains raw IMU so +# post-hoc better odometry is also computable offline. ──────────────── +export ODOM_ZENOH_TOPIC="odom" +# export ODOM_ZENOH_TOPIC="rt/lio_sam_ros2/mapping/odometry" # higher quality + +# ── Depth: RealSense default (works on most setups) ────────────────── +export DEPTH_ZENOH_TOPIC="camera/realsense2_camera_node/depth/image_rect_raw" + +# ── Storage location: change to the actual USB SSD mount before the trip +export RECORDINGS_DIR="/mnt/usb_ssd/recordings" + +# ── Network health probe. Change to local gateway if 8.8.8.8 blocked. ─ +export NET_PING_HOST="8.8.8.8" +export NET_POLL_INTERVAL="5s" +export NET_PING_TIMEOUT="2s" + +# ── RTSP URLs (Go2 internal cameras) ───────────────────────────────── +export TOP_CAMERA_RTSP_URL="rtsp://localhost:8554/top_camera_raw" +export FRONT_CAMERA_RTSP_URL="rtsp://localhost:8554/front_camera" +export DOWN_CAMERA_RTSP_URL="rtsp://localhost:8554/down_camera" +export AUDIO_RTSP_URL="rtsp://localhost:8554/audio" + +# ── Zenoh endpoint (localhost; running on the robot) ───────────────── +export LIDAR_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export POINTCLOUD_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export DEPTH_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export ODOM_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" +export LOWSTATE_ZENOH_ENDPOINT="tcp/127.0.0.1:7447" + +echo "✅ env.go2 sourced — recording profile: Go2 (2D lidar enabled, pointcloud disabled)" diff --git a/internal/audio/stream.go b/internal/audio/stream.go index ee8707f..ff74f60 100644 --- a/internal/audio/stream.go +++ b/internal/audio/stream.go @@ -6,25 +6,45 @@ import ( "log/slog" "os" "os/exec" + "path/filepath" "sync/atomic" "time" + + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" ) +const HeartbeatName = "audio" + +const heartbeatInterval = 5 * time.Second + type Config struct { RTSPURL string - OutputFile string - TimestampsFile string + OutputFile string // base path; each segment is a uniquely-named sibling + TimestampsFile string // CSV index of all segments + FramesFile string // per-packet timestamps CSV (highly recommended). + // Audio "frames" are really packets (~1024 samples / packet for AAC/Opus). + + Monitor *heartbeat.Monitor } type AudioRTSPStream struct { - cfg Config - running atomic.Bool - cancel context.CancelFunc - done chan struct{} + cfg Config + running atomic.Bool + cancel context.CancelFunc + done chan struct{} + framesWrite *recordutil.FrameCSVWriter + + pending atomic.Int64 + allDone chan struct{} } func New(cfg Config) *AudioRTSPStream { - return &AudioRTSPStream{cfg: cfg} + return &AudioRTSPStream{ + cfg: cfg, + framesWrite: recordutil.NewFrameCSVWriter(cfg.FramesFile), + allDone: make(chan struct{}), + } } func (a *AudioRTSPStream) Start() { @@ -43,6 +63,7 @@ func (a *AudioRTSPStream) Stop() { } a.cancel() <-a.done + a.waitForPending() slog.Info("audio stream stopped") } @@ -61,32 +82,105 @@ func (a *AudioRTSPStream) loop(ctx context.Context) { func (a *AudioRTSPStream) record(ctx context.Context) error { start := time.Now() + segmentFile := recordutil.UniqueSegmentFile(a.cfg.OutputFile, start) + cmd := exec.CommandContext(ctx, "ffmpeg", "-loglevel", "error", "-rtsp_transport", "tcp", "-i", a.cfg.RTSPURL, "-c", "copy", "-metadata", "creation_time="+start.UTC().Format(time.RFC3339Nano), - "-y", - a.cfg.OutputFile, + segmentFile, ) cmd.Stderr = os.Stderr if err := cmd.Start(); err != nil { return fmt.Errorf("start audio recorder: %w", err) } - if err := writeStartTimestamp(a.cfg.TimestampsFile, start); err != nil { - slog.Error("failed to write audio start timestamp", "file", a.cfg.TimestampsFile, "err", err) + if err := appendSegmentEntry(a.cfg.TimestampsFile, start, segmentFile); err != nil { + slog.Error("failed to append audio segment entry", + "file", a.cfg.TimestampsFile, "err", err) + } + slog.Info("audio segment started", "file", segmentFile) + + hbStop := make(chan struct{}) + hbDone := make(chan struct{}) + go func() { + defer close(hbDone) + // Tick immediately so heartbeat sees life right away. + a.cfg.Monitor.Tick(HeartbeatName) + t := time.NewTicker(heartbeatInterval) + defer t.Stop() + for { + select { + case <-hbStop: + return + case <-t.C: + a.cfg.Monitor.Tick(HeartbeatName) + } + } + }() + + waitErr := cmd.Wait() + close(hbStop) + <-hbDone + + if a.cfg.FramesFile != "" { + a.pending.Add(1) + go func(segFile string, startUnixNs int64) { + defer a.markDone() + if err := a.framesWrite.ExtractAndAppend(segFile, "a:0", startUnixNs); err != nil { + slog.Error("audio frames extraction failed", + "segment", segFile, "err", err) + } else { + slog.Info("audio frames extracted", "segment", filepath.Base(segFile)) + } + }(segmentFile, start.UnixNano()) + } + // ────────────────────────────────────────────────────────────────────── + + return waitErr +} + +func (a *AudioRTSPStream) markDone() { + if a.pending.Add(-1) == 0 { + select { + case a.allDone <- struct{}{}: + default: + } + } +} + +func (a *AudioRTSPStream) waitForPending() { + for a.pending.Load() > 0 { + select { + case <-a.allDone: + case <-time.After(500 * time.Millisecond): + } } - return cmd.Wait() } -func writeStartTimestamp(path string, start time.Time) error { +func appendSegmentEntry(path string, start time.Time, segmentFile string) error { if path == "" { return nil } - return os.WriteFile( - path, - []byte(fmt.Sprintf("recording_start_unix_ns\n%d\n", start.UnixNano())), - 0o644, - ) + f, err := os.OpenFile(path, os.O_APPEND|os.O_CREATE|os.O_WRONLY, 0o644) + if err != nil { + return fmt.Errorf("open: %w", err) + } + defer f.Close() + + stat, err := f.Stat() + if err != nil { + return fmt.Errorf("stat: %w", err) + } + if stat.Size() == 0 { + if _, err := fmt.Fprintln(f, "recording_start_unix_ns,segment_file"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + if _, err := fmt.Fprintf(f, "%d,%s\n", start.UnixNano(), filepath.Base(segmentFile)); err != nil { + return fmt.Errorf("write entry: %w", err) + } + return f.Sync() } diff --git a/internal/audio/stream_test.go b/internal/audio/stream_test.go index b55ff01..047a8f8 100644 --- a/internal/audio/stream_test.go +++ b/internal/audio/stream_test.go @@ -1,7 +1,10 @@ package audio import ( + "fmt" + "os" "path/filepath" + "strings" "testing" "time" @@ -69,3 +72,41 @@ func TestStop_idempotent(t *testing.T) { stream.Stop() stream.Stop() // second call must be a no-op } + +func TestAppendSegmentEntry_emptyPath_isNoOp(t *testing.T) { + err := appendSegmentEntry("", time.Now(), "/data/audio.ogg") + require.NoError(t, err) +} + +func TestAppendSegmentEntry_writesHeaderAndEntry(t *testing.T) { + path := filepath.Join(t.TempDir(), "audio_timestamps.csv") + start := time.Unix(1_000_000_000, 123_456_789) + segFile := "/data/audio_20260612T164629_876543210Z.ogg" + + err := appendSegmentEntry(path, start, segFile) + require.NoError(t, err) + + data, err := os.ReadFile(path) + require.NoError(t, err) + content := string(data) + require.Contains(t, content, "recording_start_unix_ns,segment_file") + require.Contains(t, content, fmt.Sprintf("%d", start.UnixNano())) + require.Contains(t, content, filepath.Base(segFile)) +} + +func TestAppendSegmentEntry_headerWrittenOnce(t *testing.T) { + path := filepath.Join(t.TempDir(), "audio_timestamps.csv") + seg1 := "/data/audio_seg1.ogg" + seg2 := "/data/audio_seg2.ogg" + + require.NoError(t, appendSegmentEntry(path, time.Unix(1_000_000_000, 0), seg1)) + require.NoError(t, appendSegmentEntry(path, time.Unix(2_000_000_000, 0), seg2)) + + data, err := os.ReadFile(path) + require.NoError(t, err) + content := string(data) + + require.Equal(t, 1, strings.Count(content, "recording_start_unix_ns"), "header must appear exactly once") + require.Contains(t, content, filepath.Base(seg1)) + require.Contains(t, content, filepath.Base(seg2)) +} diff --git a/internal/depth/image.go b/internal/depth/image.go index 7519f8d..2969ac7 100644 --- a/internal/depth/image.go +++ b/internal/depth/image.go @@ -170,4 +170,4 @@ func (img *Image) DepthPixels() ([]uint16, error) { } } return pixels, nil -} +} \ No newline at end of file diff --git a/internal/depth/image_test.go b/internal/depth/image_test.go index 21a47bb..a2ad587 100644 --- a/internal/depth/image_test.go +++ b/internal/depth/image_test.go @@ -10,6 +10,10 @@ import ( ) func buildImagePayload(width, height uint32, encoding string, frameID string, data []byte) []byte { + return buildImagePayloadWithStep(width, height, encoding, frameID, data, width*2) +} + +func buildImagePayloadWithStep(width, height uint32, encoding string, frameID string, data []byte, step uint32) []byte { var b []byte b = append(b, 0x00, 0x01, 0x00, 0x00) @@ -36,7 +40,7 @@ func buildImagePayload(width, height uint32, encoding string, frameID string, da putU32(width) // width putStr(encoding) // encoding b = append(b, 0x00) // is_bigendian (uint8) - putU32(width * 2) // step + putU32(step) // step (explicit) putU32(uint32(len(data))) b = append(b, data...) return b @@ -104,3 +108,78 @@ func TestEncodeFrame_rawFallbackForUnparseable(t *testing.T) { require.Equal(t, "raw", f.method) require.Equal(t, []byte{0x01, 0x02}, f.data) } + +func TestParseImage_payloadTooShort(t *testing.T) { + _, err := ParseImage([]byte{0x00, 0x01, 0x00}) // 3 bytes — below the 4-byte CDR header minimum + require.Error(t, err) +} + +func TestDepthPixels_mono16(t *testing.T) { + // mono16 is an alias for 16UC1; DepthPixels must accept it. + const w, h = 4, 2 + pixels := []uint16{100, 200, 300, 400, 500, 600, 700, 800} + data := make([]byte, len(pixels)*2) + for i, p := range pixels { + binary.LittleEndian.PutUint16(data[i*2:], p) + } + payload := buildImagePayload(w, h, "mono16", "cam", data) + img, err := ParseImage(payload) + require.NoError(t, err) + + got, err := img.DepthPixels() + require.NoError(t, err) + require.Equal(t, pixels, got) +} + +func TestDepthPixels_bigEndian(t *testing.T) { + // Build an Image directly with big-endian pixel data. + pixels := []uint16{1000, 2000, 3000, 4000} + data := make([]byte, len(pixels)*2) + for i, p := range pixels { + binary.BigEndian.PutUint16(data[i*2:], p) + } + img := &Image{ + Width: 2, + Height: 2, + Encoding: "16UC1", + IsBigendian: true, + Step: 4, + Data: data, + } + got, err := img.DepthPixels() + require.NoError(t, err) + require.Equal(t, pixels, got) +} + +func TestDepthPixels_dataTooShort(t *testing.T) { + img := &Image{ + Width: 4, + Height: 4, + Encoding: "16UC1", + Step: 8, + Data: make([]byte, 4), // needs 4*4*2 = 32 bytes, only 4 provided + } + _, err := img.DepthPixels() + require.Error(t, err) +} + +func TestEncodeFrame_rawFallbackForPaddedRows(t *testing.T) { + // step != width*2 means the row has padding; encodeFrame must fall back to raw. + const w, h = 4, 2 + data := make([]byte, w*h*2) + payload := buildImagePayloadWithStep(w, h, "16UC1", "cam", data, w*2+4) + + f := encodeFrame(payload) + require.Equal(t, "raw", f.method, "padded rows must fall back to raw") + require.Equal(t, uint32(w), f.width) + require.Equal(t, uint32(h), f.height) +} + +func TestEncodeFrame_rawFallbackForNon16BitImage(t *testing.T) { + // rgb8 is parseable but not a 16-bit depth format; encodeFrame falls back to raw. + payload := buildImagePayload(4, 2, "rgb8", "cam", make([]byte, 4*2*3)) + + f := encodeFrame(payload) + require.Equal(t, "raw", f.method) + require.Equal(t, "rgb8", f.encoding) +} diff --git a/internal/depth/stream.go b/internal/depth/stream.go index 43a4205..10e8e2f 100644 --- a/internal/depth/stream.go +++ b/internal/depth/stream.go @@ -4,22 +4,32 @@ import ( "context" "fmt" "log/slog" - "os" "sync" "sync/atomic" "time" "github.com/eclipse-zenoh/zenoh-go/zenoh" + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" "om1-telemetry/internal/rvl" "om1-telemetry/internal/zenohutil" ) +// HeartbeatName is the stream identifier used with heartbeat.Monitor. +const HeartbeatName = "depth" + +const syncInterval = 2 * time.Second + type Config struct { ZenohEndpoint string ZenohTopic string TimestampsFile string DataFile string + + // Monitor is optional; ticks once per stored frame so the central + // heartbeat monitor can detect a stuck recorder. + Monitor *heartbeat.Monitor } type DepthStream struct { @@ -106,28 +116,48 @@ func (d *DepthStream) record(ctx context.Context) error { } defer session.Drop() - tsFile, err := os.Create(d.cfg.TimestampsFile) + // open files in APPEND mode so reconnects do NOT wipe data + tsResult, err := recordutil.OpenForAppend(d.cfg.TimestampsFile) if err != nil { - return fmt.Errorf("create timestamps file: %w", err) + return fmt.Errorf("open timestamps file: %w", err) } + tsFile := tsResult.File defer func() { if err := tsFile.Close(); err != nil { slog.Error("failed to close timestamps file", "err", err) } }() - dataFile, err := os.Create(d.cfg.DataFile) + dataResult, err := recordutil.OpenForAppend(d.cfg.DataFile) if err != nil { - return fmt.Errorf("create data file: %w", err) + return fmt.Errorf("open data file: %w", err) } + dataFile := dataResult.File defer func() { if err := dataFile.Close(); err != nil { slog.Error("failed to close data file", "err", err) } }() - if _, err := fmt.Fprintln(tsFile, "unix_ns,seq,byte_offset,byte_length,method,width,height,encoding"); err != nil { - return fmt.Errorf("write header: %w", err) + if tsResult.PrevSize == 0 { + if _, err := fmt.Fprintln(tsFile, + "unix_ns,seq,byte_offset,byte_length,method,width,height,encoding"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + lastSeq, err := recordutil.ReadLastSeq(d.cfg.TimestampsFile) + if err != nil { + slog.Warn("could not read last seq; starting from 0", "err", err) + lastSeq = -1 + } + seq := lastSeq + 1 + byteOffset := dataResult.PrevSize + + if dataResult.PrevSize > 0 { + slog.Info("depth resuming previous session", + "starting_seq", seq, + "starting_byte_offset", byteOffset) } keyExpr, err := zenoh.NewKeyExpr(d.cfg.ZenohTopic) @@ -157,16 +187,30 @@ func (d *DepthStream) record(ctx context.Context) error { slog.Info("depth recorder started", "topic", d.cfg.ZenohTopic) - var seq int64 - var byteOffset int64 receiver := subscriber.Handler() + syncTicker := time.NewTicker(syncInterval) + defer syncTicker.Stop() + + flush := func() { + if err := dataFile.Sync(); err != nil { + slog.Warn("depth data sync failed", "err", err) + } + if err := tsFile.Sync(); err != nil { + slog.Warn("depth ts sync failed", "err", err) + } + } + for { select { case <-ctx.Done(): + flush() return nil + case <-syncTicker.C: + flush() case sample, ok := <-receiver: if !ok { + flush() return fmt.Errorf("subscriber channel closed") } @@ -180,20 +224,23 @@ func (d *DepthStream) record(ctx context.Context) error { } payload := sample.Payload().Bytes() - frame := encodeFrame(payload) + f := encodeFrame(payload) - n, err := dataFile.Write(frame.data) + n, err := dataFile.Write(f.data) if err != nil { return fmt.Errorf("write data: %w", err) } if _, err := fmt.Fprintf(tsFile, "%d,%d,%d,%d,%s,%d,%d,%s\n", - unixNs, seq, byteOffset, n, frame.method, frame.width, frame.height, frame.encoding); err != nil { + unixNs, seq, byteOffset, n, f.method, f.width, f.height, f.encoding); err != nil { return fmt.Errorf("write timestamp: %w", err) } byteOffset += int64(n) seq++ + + // Heartbeat tick. Safe if Monitor is nil. + d.cfg.Monitor.Tick(HeartbeatName) } } } @@ -213,6 +260,18 @@ func encodeFrame(payload []byte) frame { return frame{data: payload, method: "raw"} } + // Defensive check: if step has row padding (step != width*2), the + // DepthPixels reader would misalign rows. Fall back to raw — the + // data is preserved as-is, downstream decoders can handle it. + if img.Step != img.Width*2 { + slog.Warn("depth: step has padding, storing raw", + "step", img.Step, "width_times_2", img.Width*2) + return frame{ + data: payload, method: "raw", + width: img.Width, height: img.Height, encoding: img.Encoding, + } + } + pixels, err := img.DepthPixels() if err != nil { slog.Warn("depth: not a 16-bit depth frame, storing raw", "err", err, "encoding", img.Encoding) @@ -226,4 +285,4 @@ func encodeFrame(payload []byte) frame { height: img.Height, encoding: img.Encoding, } -} +} \ No newline at end of file diff --git a/internal/heartbeat/heartbeat.go b/internal/heartbeat/heartbeat.go new file mode 100644 index 0000000..a566a6a --- /dev/null +++ b/internal/heartbeat/heartbeat.go @@ -0,0 +1,189 @@ +// Package heartbeat provides centralized health monitoring for all recorders. +// +// Design principle: SILENT when everything works. The log only contains +// heartbeat messages when something is wrong (recorder stopped receiving, +// or rate dropped below expected). Each transition (healthy → broken, +// broken → recovered) is logged exactly once, so you don't get spam. +// +// Why this matters: during an 8-hour road trip recording session, you do +// NOT want "all recorders healthy" messages every 30 seconds. You want +// the log to be EMPTY unless something needs attention. This module +// achieves that. +// +// USAGE in main.go: +// +// mon := heartbeat.NewMonitor(30 * time.Second) +// mon.Register("lidar", 10) // expected 10 Hz +// mon.Register("lowstate", 1000) // expected ~1 kHz on G1, ~500 Hz on Go2 +// mon.Register("network", 0) // 0 = just check it's alive at all +// go mon.Run(ctx) +// +// // Pass mon to each recorder's Config: +// lidarCfg.Monitor = mon +// +// USAGE in each recorder, after receiving a message: +// +// l.cfg.Monitor.Tick("lidar") // safe even if Monitor is nil +package heartbeat + +import ( + "context" + "log/slog" + "strconv" + "sync" + "sync/atomic" + "time" +) + +// Monitor tracks per-stream tick counters and emits warnings when streams +// stop ticking or fall below their expected rate. +type Monitor struct { + streams sync.Map // name (string) → *state + interval time.Duration +} + +type state struct { + expectedHz float64 + ticks atomic.Int64 // total ticks since registration + + // The following fields are accessed ONLY by the checker goroutine, + // so no mutex is needed. + lastTicks int64 + lastTime time.Time + registered time.Time + warned bool +} + +// NewMonitor creates a monitor that checks all registered streams every +// checkInterval. 30 seconds is a good default — long enough to filter +// transient hiccups, short enough that you notice problems quickly. +func NewMonitor(checkInterval time.Duration) *Monitor { + return &Monitor{interval: checkInterval} +} + +// Register adds a stream to monitor. +// +// expectedHz is the rate this stream should be running at. The monitor +// warns if the actual rate falls below HALF of this. Pass 0 to disable +// rate checking entirely; the monitor will then only warn if the stream +// goes completely silent. +// +// Common values: +// +// LiDAR cloud 10 +// RealSense depth 15 +// RealSense color 30 +// odom 50 +// lowstate Go2 500 +// lowstate G1 1000 +// network ping 0 (or 0.2 if you want to enforce ~5s polling) +// +// Use lower-bound estimates — better to under-warn than spam false positives. +func (m *Monitor) Register(name string, expectedHz float64) { + now := time.Now() + m.streams.Store(name, &state{ + expectedHz: expectedHz, + lastTime: now, + registered: now, + }) +} + +// Tick records that the named stream just received a message. Call this +// once per received message from inside the recorder. +// +// nil-safe: calling Tick on a nil *Monitor is a no-op. This means +// recorders can hold a *Monitor in their config without nil-checking at +// every call site, and unit tests / standalone runs can pass nil to +// disable monitoring entirely. +// +// Hot path: a single atomic.Int64 add. Safe to call from any goroutine. +func (m *Monitor) Tick(name string) { + if m == nil { + return + } + if v, ok := m.streams.Load(name); ok { + v.(*state).ticks.Add(1) + } +} + +// Run blocks until ctx is done, periodically checking all registered +// streams. Launch in a goroutine from main: +// +// go mon.Run(ctx) +func (m *Monitor) Run(ctx context.Context) { + t := time.NewTicker(m.interval) + defer t.Stop() + for { + select { + case <-ctx.Done(): + return + case <-t.C: + m.check() + } + } +} + +func (m *Monitor) check() { + now := time.Now() + + m.streams.Range(func(k, v any) bool { + name := k.(string) + s := v.(*state) + + current := s.ticks.Load() + delta := current - s.lastTicks + elapsed := now.Sub(s.lastTime).Seconds() + var rate float64 + if elapsed > 0 { + rate = float64(delta) / elapsed + } + + // Grace period: don't warn during the first 2× interval after + // registration if we haven't yet received any message. This + // prevents spurious "never received" warnings during startup. + inGrace := current == 0 && now.Sub(s.registered) < 2*m.interval + if inGrace { + s.lastTicks = current + s.lastTime = now + return true + } + + // Health classification + bad := false + var reason string + switch { + case delta == 0 && current == 0: + bad = true + reason = "never received any message" + case delta == 0: + bad = true + reason = "stopped receiving messages" + case s.expectedHz > 0 && rate < s.expectedHz*0.5: + bad = true + reason = "rate dropped below half of expected" + } + + // Log ONLY on state transitions. This is what keeps the log + // silent during normal operation. + switch { + case bad && !s.warned: + slog.Warn("⚠️ recorder NOT WORKING", + "stream", name, + "reason", reason, + "actual_hz", strconv.FormatFloat(rate, 'f', 1, 64), + "expected_hz", strconv.FormatFloat(s.expectedHz, 'f', 0, 64), + "total_msgs_since_start", current) + s.warned = true + + case !bad && s.warned: + slog.Info("✅ recorder recovered", + "stream", name, + "actual_hz", strconv.FormatFloat(rate, 'f', 1, 64)) + s.warned = false + } + + s.lastTicks = current + s.lastTime = now + return true + }) +} \ No newline at end of file diff --git a/internal/heartbeat/heartbeat_test.go b/internal/heartbeat/heartbeat_test.go new file mode 100644 index 0000000..14f3486 --- /dev/null +++ b/internal/heartbeat/heartbeat_test.go @@ -0,0 +1,113 @@ +package heartbeat + +import ( + "context" + "testing" + "time" + + "github.com/stretchr/testify/require" +) + +func TestNewMonitor_returnsNonNil(t *testing.T) { + mon := NewMonitor(30 * time.Second) + require.NotNil(t, mon) +} + +func TestTick_nilMonitor_isNoOp(t *testing.T) { + var mon *Monitor + require.NotPanics(t, func() { mon.Tick("anything") }) +} + +func TestTick_unregisteredStream_isNoOp(t *testing.T) { + mon := NewMonitor(30 * time.Second) + require.NotPanics(t, func() { mon.Tick("not-registered") }) +} + +func TestRegister_thenTick_incrementsCounter(t *testing.T) { + mon := NewMonitor(30 * time.Second) + mon.Register("lidar", 10) + + for range 7 { + mon.Tick("lidar") + } + + v, ok := mon.streams.Load("lidar") + require.True(t, ok, "registered stream must be stored") + require.Equal(t, int64(7), v.(*state).ticks.Load()) +} + +func TestTick_multipleStreams_independent(t *testing.T) { + mon := NewMonitor(30 * time.Second) + mon.Register("lidar", 10) + mon.Register("depth", 15) + + mon.Tick("lidar") + mon.Tick("lidar") + mon.Tick("depth") + + vLidar, _ := mon.streams.Load("lidar") + vDepth, _ := mon.streams.Load("depth") + require.Equal(t, int64(2), vLidar.(*state).ticks.Load()) + require.Equal(t, int64(1), vDepth.(*state).ticks.Load()) +} + +func TestRun_exitsOnContextCancel(t *testing.T) { + mon := NewMonitor(30 * time.Second) + ctx, cancel := context.WithCancel(context.Background()) + + done := make(chan struct{}) + go func() { + mon.Run(ctx) + close(done) + }() + + cancel() + select { + case <-done: + case <-time.After(2 * time.Second): + t.Fatal("Run did not exit after context cancellation") + } +} + +func TestCheck_gracePeriod_noWarn(t *testing.T) { + // Very short interval so check() fires quickly. + mon := NewMonitor(5 * time.Millisecond) + mon.Register("camera", 30) + + // call check() immediately — we're within the 2× interval grace period + // so the stream should NOT be marked as warned. + mon.check() + + v, ok := mon.streams.Load("camera") + require.True(t, ok) + require.False(t, v.(*state).warned, "should not warn within grace period") +} + +func TestCheck_warnsAfterGracePeriodExpires(t *testing.T) { + mon := NewMonitor(5 * time.Millisecond) + mon.Register("camera", 30) + + // Wait longer than 2× interval so grace period expires. + time.Sleep(20 * time.Millisecond) + mon.check() + + v, ok := mon.streams.Load("camera") + require.True(t, ok) + require.True(t, v.(*state).warned, "should warn when stream never received a message past grace period") +} + +func TestCheck_noWarnWhenTicksArrive(t *testing.T) { + mon := NewMonitor(5 * time.Millisecond) + mon.Register("lidar", 10) + + // Tick enough to be above half-rate threshold. + for range 100 { + mon.Tick("lidar") + } + + time.Sleep(20 * time.Millisecond) + mon.check() + + v, _ := mon.streams.Load("lidar") + require.False(t, v.(*state).warned, "should not warn when stream is ticking at expected rate") +} diff --git a/internal/lidar/stream.go b/internal/lidar/stream.go index afe743b..7f1b263 100644 --- a/internal/lidar/stream.go +++ b/internal/lidar/stream.go @@ -4,21 +4,31 @@ import ( "context" "fmt" "log/slog" - "os" "sync" "sync/atomic" "time" "github.com/eclipse-zenoh/zenoh-go/zenoh" + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" "om1-telemetry/internal/zenohutil" ) +// HeartbeatName is the stream identifier used with heartbeat.Monitor. +// Register it in main.go as: mon.Register(lidar.HeartbeatName, 10) +const HeartbeatName = "lidar" + +const syncInterval = 2 * time.Second + type Config struct { ZenohEndpoint string ZenohTopic string TimestampsFile string DataFile string + + // Monitor is optional; if nil, no heartbeat reporting happens. + Monitor *heartbeat.Monitor } type LidarStream struct { @@ -105,28 +115,46 @@ func (l *LidarStream) record(ctx context.Context) error { } defer session.Drop() - tsFile, err := os.Create(l.cfg.TimestampsFile) + tsResult, err := recordutil.OpenForAppend(l.cfg.TimestampsFile) if err != nil { - return fmt.Errorf("create timestamps file: %w", err) + return fmt.Errorf("open timestamps file: %w", err) } + tsFile := tsResult.File defer func() { if err := tsFile.Close(); err != nil { slog.Error("failed to close timestamps file", "err", err) } }() - dataFile, err := os.Create(l.cfg.DataFile) + dataResult, err := recordutil.OpenForAppend(l.cfg.DataFile) if err != nil { - return fmt.Errorf("create data file: %w", err) + return fmt.Errorf("open data file: %w", err) } + dataFile := dataResult.File defer func() { if err := dataFile.Close(); err != nil { slog.Error("failed to close data file", "err", err) } }() - if _, err := fmt.Fprintln(tsFile, "unix_ns,seq,byte_offset"); err != nil { - return fmt.Errorf("write header: %w", err) + if tsResult.PrevSize == 0 { + if _, err := fmt.Fprintln(tsFile, "unix_ns,seq,byte_offset"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + lastSeq, err := recordutil.ReadLastSeq(l.cfg.TimestampsFile) + if err != nil { + slog.Warn("could not read last seq; starting from 0", "err", err) + lastSeq = -1 + } + seq := lastSeq + 1 + byteOffset := dataResult.PrevSize + + if dataResult.PrevSize > 0 { + slog.Info("lidar resuming previous session", + "starting_seq", seq, + "starting_byte_offset", byteOffset) } keyExpr, err := zenoh.NewKeyExpr(l.cfg.ZenohTopic) @@ -156,16 +184,30 @@ func (l *LidarStream) record(ctx context.Context) error { slog.Info("lidar recorder started", "topic", l.cfg.ZenohTopic) - var seq int64 - var byteOffset int64 receiver := subscriber.Handler() + syncTicker := time.NewTicker(syncInterval) + defer syncTicker.Stop() + + flush := func() { + if err := dataFile.Sync(); err != nil { + slog.Warn("lidar data sync failed", "err", err) + } + if err := tsFile.Sync(); err != nil { + slog.Warn("lidar ts sync failed", "err", err) + } + } + for { select { case <-ctx.Done(): + flush() return nil + case <-syncTicker.C: + flush() case sample, ok := <-receiver: if !ok { + flush() return fmt.Errorf("subscriber channel closed") } @@ -190,6 +232,10 @@ func (l *LidarStream) record(ctx context.Context) error { byteOffset += int64(n) seq++ + + // ── HEARTBEAT TICK ── + // Single line. Safe if cfg.Monitor is nil (no-op). + l.cfg.Monitor.Tick(HeartbeatName) } } -} +} \ No newline at end of file diff --git a/internal/lowstate/stream.go b/internal/lowstate/stream.go new file mode 100644 index 0000000..5d28f0d --- /dev/null +++ b/internal/lowstate/stream.go @@ -0,0 +1,258 @@ +package lowstate + +import ( + "context" + "fmt" + "log/slog" + "sync" + "sync/atomic" + "time" + + "github.com/eclipse-zenoh/zenoh-go/zenoh" + + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" + "om1-telemetry/internal/zenohutil" +) + +// HeartbeatName is the stream identifier used with heartbeat.Monitor. +// Same name on Go2 and G1 — the messages have slightly different +// schemas (unitree_go vs unitree_hg) but identical purpose; the data +// pipeline doesn't need to distinguish them. +const HeartbeatName = "lowstate" + +// /lowstate is the catch-all robot state message: +// IMU, joint states, battery, foot forces, wireless remote, temperatures, etc. +// We record raw payload bytes + per-message timestamps; downstream tools +// deserialize when needed. +// +// Measured rates: +// Go2 (unitree_go/msg/LowState): ~500 Hz, ~2.5 KB / msg = ~1.25 MB/s +// G1 (unitree_hg/msg/LowState): ~1053 Hz, ~2.1 KB / msg = ~2.21 MB/s +// +// Daily (8h continuous): Go2 ~36 GB, G1 ~62 GB + +const syncInterval = 2 * time.Second + +type Config struct { + ZenohEndpoint string + ZenohTopic string + TimestampsFile string + DataFile string + + // Monitor is optional; ticks once per message so the central + // heartbeat monitor can detect a stuck recorder. + Monitor *heartbeat.Monitor +} + +type LowstateStream struct { + cfg Config + running atomic.Bool + cancel context.CancelFunc + done chan struct{} + wg sync.WaitGroup +} + +type SessionResult struct { + session zenoh.Session + err error +} + +type SubscriberResult struct { + subscriber zenoh.Subscriber + err error +} + +func New(cfg Config) *LowstateStream { + return &LowstateStream{cfg: cfg} +} + +func (l *LowstateStream) Start() { + if l.running.Swap(true) { + return + } + ctx, cancel := context.WithCancel(context.Background()) + l.cancel = cancel + l.done = make(chan struct{}) + l.wg.Add(1) + go l.loop(ctx) +} + +func (l *LowstateStream) Stop() { + if !l.running.Swap(false) { + return + } + l.cancel() + l.wg.Wait() + close(l.done) + slog.Info("lowstate stream stopped") +} + +func (l *LowstateStream) loop(ctx context.Context) { + defer l.wg.Done() + for ctx.Err() == nil { + if err := l.record(ctx); err != nil && ctx.Err() == nil { + slog.Error("lowstate recorder error; reconnecting in 2 s", "err", err) + select { + case <-ctx.Done(): + case <-time.After(2 * time.Second): + } + } + } +} + +func (l *LowstateStream) record(ctx context.Context) error { + config := zenoh.NewConfigDefault() + if err := config.InsertJson5(zenoh.ConfigModeKey, `"client"`); err != nil { + return err + } + endpoint := l.cfg.ZenohEndpoint + if err := config.InsertJson5(zenoh.ConfigConnectKey, `["`+endpoint+`"]`); err != nil { + return fmt.Errorf("set connect endpoint: %w", err) + } + + sessionChan := make(chan SessionResult, 1) + go func() { + session, err := zenoh.Open(config, nil) + sessionChan <- SessionResult{session, err} + }() + + var session zenoh.Session + select { + case <-ctx.Done(): + return ctx.Err() + case result := <-sessionChan: + if result.err != nil { + return fmt.Errorf("open zenoh session: %w", result.err) + } + session = result.session + } + defer session.Drop() + + // Append-mode open: do NOT clobber prior data on reconnect. + tsResult, err := recordutil.OpenForAppend(l.cfg.TimestampsFile) + if err != nil { + return fmt.Errorf("open timestamps file: %w", err) + } + tsFile := tsResult.File + defer func() { + if err := tsFile.Close(); err != nil { + slog.Error("failed to close timestamps file", "err", err) + } + }() + + dataResult, err := recordutil.OpenForAppend(l.cfg.DataFile) + if err != nil { + return fmt.Errorf("open data file: %w", err) + } + dataFile := dataResult.File + defer func() { + if err := dataFile.Close(); err != nil { + slog.Error("failed to close data file", "err", err) + } + }() + + if tsResult.PrevSize == 0 { + if _, err := fmt.Fprintln(tsFile, "unix_ns,seq,byte_offset"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + lastSeq, err := recordutil.ReadLastSeq(l.cfg.TimestampsFile) + if err != nil { + slog.Warn("could not read last seq; starting from 0", "err", err) + lastSeq = -1 + } + seq := lastSeq + 1 + byteOffset := dataResult.PrevSize + + if dataResult.PrevSize > 0 { + slog.Info("lowstate resuming previous session", + "starting_seq", seq, + "starting_byte_offset", byteOffset) + } + + keyExpr, err := zenoh.NewKeyExpr(l.cfg.ZenohTopic) + if err != nil { + return fmt.Errorf("create key expression: %w", err) + } + + // FIFO buffer sized for high-frequency data. + // G1 @ 1053 Hz: 2048 entries ≈ 2 s of slack + // Go2 @ 500 Hz: 2048 entries ≈ 4 s of slack + handler := zenoh.NewFifoChannel[zenoh.Sample](2048) + + subscriberChan := make(chan SubscriberResult, 1) + go func() { + subscriber, err := session.DeclareSubscriber(keyExpr, handler, nil) + subscriberChan <- SubscriberResult{subscriber, err} + }() + + var subscriber zenoh.Subscriber + select { + case <-ctx.Done(): + return ctx.Err() + case result := <-subscriberChan: + if result.err != nil { + return fmt.Errorf("declare subscriber: %w", result.err) + } + subscriber = result.subscriber + } + defer subscriber.Drop() + + slog.Info("lowstate recorder started", "topic", l.cfg.ZenohTopic) + + receiver := subscriber.Handler() + + syncTicker := time.NewTicker(syncInterval) + defer syncTicker.Stop() + + flush := func() { + if err := dataFile.Sync(); err != nil { + slog.Warn("lowstate data sync failed", "err", err) + } + if err := tsFile.Sync(); err != nil { + slog.Warn("lowstate ts sync failed", "err", err) + } + } + + for { + select { + case <-ctx.Done(): + flush() + return nil + case <-syncTicker.C: + flush() + case sample, ok := <-receiver: + if !ok { + flush() + return fmt.Errorf("subscriber channel closed") + } + + var unixNs int64 + tsOpt := sample.TimeStamp() + if tsOpt.IsSome() { + ts := tsOpt.Unwrap() + unixNs = zenohutil.TimestampToUnixNs(ts) + } else { + unixNs = time.Now().UnixNano() + } + + payload := sample.Payload() + n, err := dataFile.Write(payload.Bytes()) + if err != nil { + return fmt.Errorf("write data: %w", err) + } + + if _, err := fmt.Fprintf(tsFile, "%d,%d,%d\n", unixNs, seq, byteOffset); err != nil { + return fmt.Errorf("write timestamp: %w", err) + } + + byteOffset += int64(n) + seq++ + + // Heartbeat tick. Safe if Monitor is nil. + l.cfg.Monitor.Tick(HeartbeatName) + } + } +} \ No newline at end of file diff --git a/internal/lowstate/stream_test.go b/internal/lowstate/stream_test.go new file mode 100644 index 0000000..b73841c --- /dev/null +++ b/internal/lowstate/stream_test.go @@ -0,0 +1,81 @@ +package lowstate + +import ( + "path/filepath" + "testing" + "time" + + "github.com/stretchr/testify/require" +) + +func TestNew_returnsNonNilStream(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "rt/lowstate", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + require.NotNil(t, stream, "New() returned nil") +} + +func TestStartStop_cleanLifecycle(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "rt/lowstate/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + + stream.Start() + + time.Sleep(50 * time.Millisecond) + + done := make(chan struct{}) + go func() { + stream.Stop() + close(done) + }() + + select { + case <-done: + case <-time.After(5 * time.Second): + require.Fail(t, "Stop() did not return within 5 s") + } +} + +func TestStart_idempotent(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "rt/lowstate/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + + stream.Start() + stream.Start() + + stream.Stop() +} + +func TestStop_beforeStart_isNoOp(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "rt/lowstate/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + stream.Stop() +} + +func TestStop_idempotent(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "rt/lowstate/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + + stream.Start() + stream.Stop() + stream.Stop() +} diff --git a/internal/network/stream.go b/internal/network/stream.go index 21550b2..0c1eed2 100644 --- a/internal/network/stream.go +++ b/internal/network/stream.go @@ -4,15 +4,20 @@ import ( "context" "fmt" "log/slog" - "os" "os/exec" "regexp" "strconv" "sync" "sync/atomic" "time" + + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" ) +// HeartbeatName is the stream identifier for the heartbeat monitor. +const HeartbeatName = "network" + type Config struct { PingHost string PingTimeout time.Duration @@ -20,6 +25,10 @@ type Config struct { PollInterval time.Duration DataFile string + + // Monitor is optional; if non-nil, ticks once per successful sample + // write so heartbeat.Monitor can detect when this recorder dies. + Monitor *heartbeat.Monitor } type NetworkStream struct { @@ -58,8 +67,11 @@ func (n *NetworkStream) Stop() { func (n *NetworkStream) loop(ctx context.Context) { defer n.wg.Done() for ctx.Err() == nil { + // Note: record() returns errors only for disk-write failures, + // not for ping failures (those are recorded as reachable=false). + // So this loop retries disk errors, not network errors. if err := n.record(ctx); err != nil && ctx.Err() == nil { - slog.Error("network recorder error; reconnecting in 2 s", "err", err) + slog.Error("network recorder error; retrying in 2 s", "err", err) select { case <-ctx.Done(): case <-time.After(2 * time.Second): @@ -69,18 +81,31 @@ func (n *NetworkStream) loop(ctx context.Context) { } func (n *NetworkStream) record(ctx context.Context) error { - dataFile, err := os.Create(n.cfg.DataFile) + result, err := recordutil.OpenForAppend(n.cfg.DataFile) if err != nil { - return fmt.Errorf("create data file: %w", err) + return fmt.Errorf("open data file: %w", err) } + dataFile := result.File defer func() { if err := dataFile.Close(); err != nil { slog.Error("failed to close data file", "err", err) } }() - if _, err := fmt.Fprintln(dataFile, "unix_ns,seq,reachable,rtt_ms,packet_loss_pct"); err != nil { - return fmt.Errorf("write header: %w", err) + if result.PrevSize == 0 { + if _, err := fmt.Fprintln(dataFile, "unix_ns,seq,reachable,rtt_ms,packet_loss_pct"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + lastSeq, err := recordutil.ReadLastSeq(n.cfg.DataFile) + if err != nil { + slog.Warn("could not read last seq; starting from 0", "err", err) + lastSeq = -1 + } + seq := lastSeq + 1 + if result.PrevSize > 0 { + slog.Info("network resuming previous session", "starting_seq", seq) } slog.Info("network recorder started", "host", n.cfg.PingHost, "interval", n.cfg.PollInterval) @@ -88,9 +113,12 @@ func (n *NetworkStream) record(ctx context.Context) error { ticker := time.NewTicker(n.cfg.PollInterval) defer ticker.Stop() - var seq int64 + // Track whether we've already warned about a broken ping binary so we + // don't spam the log every poll interval. + pingBinaryWarned := false + for { - sample := n.ping(ctx) + sample := n.ping(ctx, &pingBinaryWarned) if ctx.Err() != nil { return nil } @@ -106,6 +134,9 @@ func (n *NetworkStream) record(ctx context.Context) error { } seq++ + // Heartbeat: one tick per recorded sample. Safe if Monitor is nil. + n.cfg.Monitor.Tick(HeartbeatName) + select { case <-ctx.Done(): return nil @@ -120,13 +151,34 @@ type pingResult struct { lossPct float64 } -func (n *NetworkStream) ping(ctx context.Context) pingResult { +// ping runs a single ping and parses the result. The binaryWarned pointer +// is used to log "ping binary missing" only once across the lifetime of the +// recorder, rather than every poll interval. +func (n *NetworkStream) ping(ctx context.Context, binaryWarned *bool) pingResult { timeoutSec := int(n.cfg.PingTimeout.Seconds()) if timeoutSec < 1 { timeoutSec = 1 } cmd := exec.CommandContext(ctx, "ping", "-c", "1", "-W", strconv.Itoa(timeoutSec), n.cfg.PingHost) - out, _ := cmd.Output() + out, err := cmd.Output() + if err != nil { + // `ping` exiting non-zero (e.g., host unreachable) returns + // *exec.ExitError — that's normal, we still want to parse output. + // Anything else (binary missing, permission denied, etc.) is an + // environment problem and we should surface it once. + if _, isExit := err.(*exec.ExitError); !isExit { + if !*binaryWarned { + slog.Warn("network: ping command failed for non-exit reason "+ + "(check that `ping` is installed and PATH is correct)", + "err", err, "host", n.cfg.PingHost) + *binaryWarned = true + } + } + } else if *binaryWarned { + // We previously warned about a broken ping; it's working now. + slog.Info("network: ping command working again") + *binaryWarned = false + } return parsePing(string(out)) } @@ -168,4 +220,4 @@ func formatFloat(v float64, valid bool) string { return "" } return strconv.FormatFloat(v, 'f', 3, 64) -} +} \ No newline at end of file diff --git a/internal/network/stream_test.go b/internal/network/stream_test.go index bb9b369..0ae77ac 100644 --- a/internal/network/stream_test.go +++ b/internal/network/stream_test.go @@ -80,3 +80,34 @@ func TestParsePing_emptyOutput(t *testing.T) { require.False(t, res.reachable) require.InDelta(t, 100, res.lossPct, 0.001) } + +func TestParsePing_macOSFormat(t *testing.T) { + // macOS ping uses "round-trip" instead of "rtt" and a slightly different layout. + out := `PING 8.8.8.8 (8.8.8.8): 56 data bytes +64 bytes from 8.8.8.8: icmp_seq=0 ttl=116 time=11.234 ms + +--- 8.8.8.8 ping statistics --- +1 packets transmitted, 1 packets received, 0.0% packet loss +round-trip min/avg/max/stddev = 11.234/11.234/11.234/0.000 ms` + + res := parsePing(out) + require.True(t, res.reachable) + require.InDelta(t, 11.234, res.rttMs, 0.001) + require.InDelta(t, 0, res.lossPct, 0.01) +} + +func TestStop_idempotent(t *testing.T) { + stream := New(testConfig(t)) + stream.Start() + stream.Stop() + stream.Stop() // second call must be a no-op +} + +func TestFormatFloat_validValue(t *testing.T) { + require.Equal(t, "12.345", formatFloat(12.345, true)) + require.Equal(t, "0.000", formatFloat(0, true)) +} + +func TestFormatFloat_invalidValue(t *testing.T) { + require.Equal(t, "", formatFloat(99.9, false), "invalid flag must return empty string") +} diff --git a/internal/odom/stream.go b/internal/odom/stream.go index b2f9ebb..439cd1d 100644 --- a/internal/odom/stream.go +++ b/internal/odom/stream.go @@ -4,21 +4,31 @@ import ( "context" "fmt" "log/slog" - "os" "sync" "sync/atomic" "time" "github.com/eclipse-zenoh/zenoh-go/zenoh" + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" "om1-telemetry/internal/zenohutil" ) +// HeartbeatName is the stream identifier used with heartbeat.Monitor. +const HeartbeatName = "odom" + +const syncInterval = 2 * time.Second + type Config struct { ZenohEndpoint string ZenohTopic string TimestampsFile string DataFile string + + // Monitor is optional; ticks once per message so the central + // heartbeat monitor can detect a stuck recorder. + Monitor *heartbeat.Monitor } type OdomStream struct { @@ -105,28 +115,47 @@ func (o *OdomStream) record(ctx context.Context) error { } defer session.Drop() - tsFile, err := os.Create(o.cfg.TimestampsFile) + // open files in APPEND mode so reconnects do NOT wipe data ───── + tsResult, err := recordutil.OpenForAppend(o.cfg.TimestampsFile) if err != nil { - return fmt.Errorf("create timestamps file: %w", err) + return fmt.Errorf("open timestamps file: %w", err) } + tsFile := tsResult.File defer func() { if err := tsFile.Close(); err != nil { slog.Error("failed to close timestamps file", "err", err) } }() - dataFile, err := os.Create(o.cfg.DataFile) + dataResult, err := recordutil.OpenForAppend(o.cfg.DataFile) if err != nil { - return fmt.Errorf("create data file: %w", err) + return fmt.Errorf("open data file: %w", err) } + dataFile := dataResult.File defer func() { if err := dataFile.Close(); err != nil { slog.Error("failed to close data file", "err", err) } }() - if _, err := fmt.Fprintln(tsFile, "unix_ns,seq,byte_offset"); err != nil { - return fmt.Errorf("write header: %w", err) + if tsResult.PrevSize == 0 { + if _, err := fmt.Fprintln(tsFile, "unix_ns,seq,byte_offset"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + lastSeq, err := recordutil.ReadLastSeq(o.cfg.TimestampsFile) + if err != nil { + slog.Warn("could not read last seq; starting from 0", "err", err) + lastSeq = -1 + } + seq := lastSeq + 1 + byteOffset := dataResult.PrevSize + + if dataResult.PrevSize > 0 { + slog.Info("odom resuming previous session", + "starting_seq", seq, + "starting_byte_offset", byteOffset) } keyExpr, err := zenoh.NewKeyExpr(o.cfg.ZenohTopic) @@ -156,16 +185,30 @@ func (o *OdomStream) record(ctx context.Context) error { slog.Info("odom recorder started", "topic", o.cfg.ZenohTopic) - var seq int64 - var byteOffset int64 receiver := subscriber.Handler() + syncTicker := time.NewTicker(syncInterval) + defer syncTicker.Stop() + + flush := func() { + if err := dataFile.Sync(); err != nil { + slog.Warn("odom data sync failed", "err", err) + } + if err := tsFile.Sync(); err != nil { + slog.Warn("odom ts sync failed", "err", err) + } + } + for { select { case <-ctx.Done(): + flush() return nil + case <-syncTicker.C: + flush() case sample, ok := <-receiver: if !ok { + flush() return fmt.Errorf("subscriber channel closed") } @@ -190,6 +233,9 @@ func (o *OdomStream) record(ctx context.Context) error { byteOffset += int64(n) seq++ + + // Heartbeat tick. Safe if Monitor is nil. + o.cfg.Monitor.Tick(HeartbeatName) } } -} +} \ No newline at end of file diff --git a/internal/odom/stream_test.go b/internal/odom/stream_test.go new file mode 100644 index 0000000..1924498 --- /dev/null +++ b/internal/odom/stream_test.go @@ -0,0 +1,81 @@ +package odom + +import ( + "path/filepath" + "testing" + "time" + + "github.com/stretchr/testify/require" +) + +func TestNew_returnsNonNilStream(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "odom/test", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + require.NotNil(t, stream, "New() returned nil") +} + +func TestStartStop_cleanLifecycle(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "odom/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + + stream.Start() + + time.Sleep(50 * time.Millisecond) + + done := make(chan struct{}) + go func() { + stream.Stop() + close(done) + }() + + select { + case <-done: + case <-time.After(5 * time.Second): + require.Fail(t, "Stop() did not return within 5 s") + } +} + +func TestStart_idempotent(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "odom/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + + stream.Start() + stream.Start() + + stream.Stop() +} + +func TestStop_beforeStart_isNoOp(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "odom/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + stream.Stop() +} + +func TestStop_idempotent(t *testing.T) { + stream := New(Config{ + ZenohEndpoint: "tcp/192.0.2.1:7447", + ZenohTopic: "odom/unreachable", + TimestampsFile: filepath.Join(t.TempDir(), "timestamps.csv"), + DataFile: filepath.Join(t.TempDir(), "data.bin"), + }) + + stream.Start() + stream.Stop() + stream.Stop() +} diff --git a/internal/pointcloud/stream.go b/internal/pointcloud/stream.go index ce9d10b..a20548b 100644 --- a/internal/pointcloud/stream.go +++ b/internal/pointcloud/stream.go @@ -4,7 +4,6 @@ import ( "context" "fmt" "log/slog" - "os" "sync" "sync/atomic" "time" @@ -12,14 +11,25 @@ import ( "github.com/eclipse-zenoh/zenoh-go/zenoh" "github.com/klauspost/compress/zstd" + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" "om1-telemetry/internal/zenohutil" ) +// HeartbeatName is the stream identifier used with heartbeat.Monitor. +const HeartbeatName = "pointcloud" + +const syncInterval = 2 * time.Second + type Config struct { ZenohEndpoint string ZenohTopic string TimestampsFile string DataFile string + + // Monitor is optional; ticks once per stored frame so the + // central heartbeat monitor can detect a stuck recorder. + Monitor *heartbeat.Monitor } type PointCloudStream struct { @@ -106,28 +116,50 @@ func (c *PointCloudStream) record(ctx context.Context) error { } defer session.Drop() - tsFile, err := os.Create(c.cfg.TimestampsFile) + // ── P0-1: open files in APPEND mode so reconnects do NOT clobber ── + tsResult, err := recordutil.OpenForAppend(c.cfg.TimestampsFile) if err != nil { - return fmt.Errorf("create timestamps file: %w", err) + return fmt.Errorf("open timestamps file: %w", err) } + tsFile := tsResult.File defer func() { if err := tsFile.Close(); err != nil { slog.Error("failed to close timestamps file", "err", err) } }() - dataFile, err := os.Create(c.cfg.DataFile) + dataResult, err := recordutil.OpenForAppend(c.cfg.DataFile) if err != nil { - return fmt.Errorf("create data file: %w", err) + return fmt.Errorf("open data file: %w", err) } + dataFile := dataResult.File defer func() { if err := dataFile.Close(); err != nil { slog.Error("failed to close data file", "err", err) } }() - if _, err := fmt.Fprintln(tsFile, "unix_ns,seq,byte_offset,byte_length,method"); err != nil { - return fmt.Errorf("write header: %w", err) + // Only write CSV header on first open (empty file). + if tsResult.PrevSize == 0 { + if _, err := fmt.Fprintln(tsFile, + "unix_ns,seq,byte_offset,byte_length,method"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + // Continue counters from where the previous session left off. + lastSeq, err := recordutil.ReadLastSeq(c.cfg.TimestampsFile) + if err != nil { + slog.Warn("could not read last seq; starting from 0", "err", err) + lastSeq = -1 + } + seq := lastSeq + 1 + byteOffset := dataResult.PrevSize + + if dataResult.PrevSize > 0 { + slog.Info("pointcloud resuming previous session", + "starting_seq", seq, + "starting_byte_offset", byteOffset) } encoder, err := zstd.NewWriter(nil) @@ -167,16 +199,31 @@ func (c *PointCloudStream) record(ctx context.Context) error { slog.Info("pointcloud recorder started", "topic", c.cfg.ZenohTopic) - var seq int64 - var byteOffset int64 receiver := subscriber.Handler() + // ── P0-2: periodic fsync so a crash never loses more than syncInterval ── + syncTicker := time.NewTicker(syncInterval) + defer syncTicker.Stop() + + flush := func() { + if err := dataFile.Sync(); err != nil { + slog.Warn("pointcloud data sync failed", "err", err) + } + if err := tsFile.Sync(); err != nil { + slog.Warn("pointcloud ts sync failed", "err", err) + } + } + for { select { case <-ctx.Done(): + flush() return nil + case <-syncTicker.C: + flush() case sample, ok := <-receiver: if !ok { + flush() return fmt.Errorf("subscriber channel closed") } @@ -196,20 +243,29 @@ func (c *PointCloudStream) record(ctx context.Context) error { return fmt.Errorf("write data: %w", err) } - if _, err := fmt.Fprintf(tsFile, "%d,%d,%d,%d,%s\n", unixNs, seq, byteOffset, n, method); err != nil { + if _, err := fmt.Fprintf(tsFile, "%d,%d,%d,%d,%s\n", + unixNs, seq, byteOffset, n, method); err != nil { return fmt.Errorf("write timestamp: %w", err) } byteOffset += int64(n) seq++ + + // Heartbeat tick. Safe if Monitor is nil. + c.cfg.Monitor.Tick(HeartbeatName) } } } +// encodeFrame zstd-compresses the raw PointCloud2 CDR payload. If the +// "compressed" output would actually be LARGER than the input (rare but +// possible on extremely small/random payloads), the function falls back +// to storing the raw bytes. The CSV row's "method" column tells you +// which path was taken so decoding code knows whether to decompress. func encodeFrame(encoder *zstd.Encoder, raw []byte) (data []byte, method string) { compressed := encoder.EncodeAll(raw, make([]byte, 0, len(raw))) if len(compressed) >= len(raw) { return raw, "raw" } return compressed, "zstd" -} +} \ No newline at end of file diff --git a/internal/recordutil/append.go b/internal/recordutil/append.go new file mode 100644 index 0000000..62fe743 --- /dev/null +++ b/internal/recordutil/append.go @@ -0,0 +1,130 @@ +// Package recordutil provides helpers for safely appending to recorder +// data files across reconnects, so that a transient Zenoh / RTSP / network +// failure does NOT clobber previously recorded data. +package recordutil + +import ( + "bufio" + "fmt" + "os" + "path/filepath" + "strconv" + "strings" + "time" +) + +// OpenAppendResult holds the file handle and the file's size BEFORE this +// open call. Use PrevSize to decide whether to write a CSV header +// (only when PrevSize == 0), and to continue any byte-offset counter +// from where the previous session left off. +type OpenAppendResult struct { + File *os.File + PrevSize int64 +} + +// OpenForAppend opens (or creates) a file in append+create mode for writing. +// Unlike os.Create, this does NOT truncate an existing file. +// +// If the file already exists with data, subsequent writes append to its end. +// Use Result.PrevSize to: +// - Decide whether to (re)write the CSV header (only if PrevSize == 0) +// - Initialize a byte-offset counter to continue from the previous session +func OpenForAppend(path string) (*OpenAppendResult, error) { + var prevSize int64 + if stat, err := os.Stat(path); err == nil { + prevSize = stat.Size() + } else if !os.IsNotExist(err) { + return nil, fmt.Errorf("stat %s: %w", path, err) + } + + // Make sure the directory exists. os.OpenFile won't create parent dirs. + if dir := filepath.Dir(path); dir != "" && dir != "." { + if err := os.MkdirAll(dir, 0o755); err != nil { + return nil, fmt.Errorf("mkdir %s: %w", dir, err) + } + } + + f, err := os.OpenFile(path, os.O_APPEND|os.O_CREATE|os.O_WRONLY, 0o644) + if err != nil { + return nil, fmt.Errorf("open %s: %w", path, err) + } + + return &OpenAppendResult{File: f, PrevSize: prevSize}, nil +} + +// ReadLastSeq scans a CSV file and returns the value of the SECOND column +// (column index 1, conventionally "seq") on its last non-empty data row. +// +// Returns -1 if: +// - The file does not exist +// - The file is empty or has only a header row +// - No row could be parsed (logs are silent here; callers can warn) +// +// This is used so that after a reconnect, the recorder can continue the +// sequence counter from where the previous session left off, instead of +// resetting it to 0 and creating ambiguous duplicate seq values. +func ReadLastSeq(path string) (int64, error) { + f, err := os.Open(path) + if err != nil { + if os.IsNotExist(err) { + return -1, nil + } + return -1, fmt.Errorf("open %s: %w", path, err) + } + defer f.Close() + + scanner := bufio.NewScanner(f) + // Allow longer lines than the default 64 KiB (some payloads have long + // per-row metadata, e.g. depth's encoding field). + scanner.Buffer(make([]byte, 1<<20), 16<<20) + + var lastSeq int64 = -1 + isHeader := true + for scanner.Scan() { + line := scanner.Text() + if isHeader { + isHeader = false + continue + } + if line == "" { + continue + } + // We only need the second column; SplitN(_, _, 3) is enough to + // extract parts[1] without allocating for the rest of the row. + parts := strings.SplitN(line, ",", 3) + if len(parts) < 2 { + continue + } + n, perr := strconv.ParseInt(strings.TrimSpace(parts[1]), 10, 64) + if perr == nil { + lastSeq = n + } + } + if err := scanner.Err(); err != nil { + return -1, fmt.Errorf("scan %s: %w", path, err) + } + return lastSeq, nil +} + +// UniqueSegmentFile produces a unique filename for a recording segment by +// inserting a UTC timestamp suffix before the extension. +// +// Example: +// +// base = "/data/top_camera.mp4" +// t = 2026-06-12 16:46:29.876543210 UTC +// → "/data/top_camera_20260612T164629_876543210Z.mp4" +// +// This is used for ffmpeg recorders (video/audio) where each reconnect +// must write to a brand new file, because MP4 / MOV containers are not +// append-friendly. +func UniqueSegmentFile(base string, t time.Time) string { + ext := filepath.Ext(base) + stem := strings.TrimSuffix(base, ext) + utc := t.UTC() + suffix := fmt.Sprintf("%s_%09dZ", + utc.Format("20060102T150405"), + utc.Nanosecond(), + ) + return fmt.Sprintf("%s_%s%s", stem, suffix, ext) +} \ No newline at end of file diff --git a/internal/recordutil/frames.go b/internal/recordutil/frames.go new file mode 100644 index 0000000..1d1b870 --- /dev/null +++ b/internal/recordutil/frames.go @@ -0,0 +1,139 @@ +package recordutil + +import ( + "bufio" + "bytes" + "fmt" + "os" + "os/exec" + "path/filepath" + "strconv" + "strings" + "sync" +) + +// FrameCSVWriter appends per-frame timestamps (extracted post-hoc via +// ffprobe) into a master CSV. One instance per stream (video/audio). +// +// CSV schema: +// +// segment_file,frame_idx,pts,pts_time_sec,wallclock_unix_ns +// +// Where: +// - segment_file basename of the .mp4 the frame came from +// - frame_idx 0-based index within the segment +// - pts raw PTS value (in container time_base units) +// - pts_time_sec PTS in seconds since the segment's first frame +// - wallclock_unix_ns absolute time the frame was captured: +// segment_start_unix_ns + pts_time_sec * 1e9 +type FrameCSVWriter struct { + path string + mu sync.Mutex +} + +func NewFrameCSVWriter(path string) *FrameCSVWriter { + return &FrameCSVWriter{path: path} +} + +// ExtractAndAppend runs `ffprobe` on segmentFile to dump per-packet PTS, +// computes each frame's wallclock unix-ns timestamp using +// segmentStartUnixNs as the anchor, and appends a row per frame to the +// master CSV. +// +// streamSelector is what ffprobe's -select_streams flag wants: +// - "v:0" for the first video stream +// - "a:0" for the first audio stream +// +// If FrameCSVWriter.path is empty, this is a no-op (frame extraction +// is opt-in via Config). +// +// On any error (ffprobe fails, segment file corrupt, etc.), this +// returns the error but does NOT crash the caller. The segment is +// still in the segments index — only its per-frame CSV is missing, +// which can be regenerated later by running ffprobe manually. +func (w *FrameCSVWriter) ExtractAndAppend(segmentFile, streamSelector string, segmentStartUnixNs int64) error { + if w == nil || w.path == "" { + return nil // disabled + } + + cmd := exec.Command("ffprobe", + "-v", "error", + "-select_streams", streamSelector, + "-show_entries", "packet=pts,pts_time", + "-of", "csv=p=0", + segmentFile, + ) + out, err := cmd.Output() + if err != nil { + return fmt.Errorf("ffprobe %s: %w", segmentFile, err) + } + + w.mu.Lock() + defer w.mu.Unlock() + + f, err := os.OpenFile(w.path, os.O_APPEND|os.O_CREATE|os.O_WRONLY, 0o644) + if err != nil { + return fmt.Errorf("open frames csv: %w", err) + } + defer f.Close() + + stat, err := f.Stat() + if err != nil { + return fmt.Errorf("stat frames csv: %w", err) + } + if stat.Size() == 0 { + if _, err := fmt.Fprintln(f, + "segment_file,frame_idx,pts,pts_time_sec,wallclock_unix_ns"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + buf := bufio.NewWriter(f) + base := filepath.Base(segmentFile) + frameIdx := 0 + scanner := bufio.NewScanner(bytes.NewReader(out)) + scanner.Buffer(make([]byte, 1<<20), 16<<20) + + for scanner.Scan() { + line := strings.TrimSpace(scanner.Text()) + if line == "" { + continue + } + // Each line: "," + parts := strings.SplitN(line, ",", 2) + if len(parts) != 2 { + continue + } + ptsStr := strings.TrimSpace(parts[0]) + ptsTimeStr := strings.TrimSpace(parts[1]) + + // pts_time may be "N/A" for some malformed packets; skip those. + if ptsTimeStr == "" || ptsTimeStr == "N/A" { + frameIdx++ + continue + } + ptsTimeF, err := strconv.ParseFloat(ptsTimeStr, 64) + if err != nil { + frameIdx++ + continue + } + + wallclockNs := segmentStartUnixNs + int64(ptsTimeF*1e9) + + if _, err := fmt.Fprintf(buf, "%s,%d,%s,%s,%d\n", + base, frameIdx, ptsStr, ptsTimeStr, wallclockNs); err != nil { + return fmt.Errorf("write row: %w", err) + } + frameIdx++ + } + if err := scanner.Err(); err != nil { + return fmt.Errorf("scan ffprobe output: %w", err) + } + if err := buf.Flush(); err != nil { + return fmt.Errorf("flush frames csv: %w", err) + } + if err := f.Sync(); err != nil { + return fmt.Errorf("sync frames csv: %w", err) + } + return nil +} \ No newline at end of file diff --git a/internal/recordutil/recordutil_test.go b/internal/recordutil/recordutil_test.go new file mode 100644 index 0000000..703abf2 --- /dev/null +++ b/internal/recordutil/recordutil_test.go @@ -0,0 +1,167 @@ +package recordutil + +import ( + "fmt" + "os" + "path/filepath" + "strings" + "testing" + "time" + + "github.com/stretchr/testify/require" +) + +// ── OpenForAppend ────────────────────────────────────────────────────────── + +func TestOpenForAppend_createsNewFile(t *testing.T) { + path := filepath.Join(t.TempDir(), "data.csv") + + result, err := OpenForAppend(path) + require.NoError(t, err) + require.NotNil(t, result) + result.File.Close() + + require.Zero(t, result.PrevSize, "new file must report PrevSize 0") + _, err = os.Stat(path) + require.NoError(t, err, "file should exist after open") +} + +func TestOpenForAppend_prevSizeReflectsExistingContent(t *testing.T) { + path := filepath.Join(t.TempDir(), "data.csv") + content := []byte("unix_ns,seq\n1000,0\n") + require.NoError(t, os.WriteFile(path, content, 0o644)) + + result, err := OpenForAppend(path) + require.NoError(t, err) + result.File.Close() + + require.Equal(t, int64(len(content)), result.PrevSize) +} + +func TestOpenForAppend_appendsWithoutTruncating(t *testing.T) { + path := filepath.Join(t.TempDir(), "data.csv") + require.NoError(t, os.WriteFile(path, []byte("line1\n"), 0o644)) + + result, err := OpenForAppend(path) + require.NoError(t, err) + + _, err = fmt.Fprintln(result.File, "line2") + require.NoError(t, err) + result.File.Close() + + data, err := os.ReadFile(path) + require.NoError(t, err) + require.Equal(t, "line1\nline2\n", string(data), "existing content must not be truncated") +} + +func TestOpenForAppend_createsMissingDirectories(t *testing.T) { + path := filepath.Join(t.TempDir(), "nested", "deep", "data.csv") + + result, err := OpenForAppend(path) + require.NoError(t, err) + result.File.Close() + + _, err = os.Stat(filepath.Dir(path)) + require.NoError(t, err, "parent directories must be created") +} + +// ── ReadLastSeq ──────────────────────────────────────────────────────────── + +func TestReadLastSeq_nonExistentFile(t *testing.T) { + seq, err := ReadLastSeq("/nonexistent/path/timestamps.csv") + require.NoError(t, err) + require.Equal(t, int64(-1), seq) +} + +func TestReadLastSeq_emptyFile(t *testing.T) { + path := filepath.Join(t.TempDir(), "ts.csv") + require.NoError(t, os.WriteFile(path, nil, 0o644)) + + seq, err := ReadLastSeq(path) + require.NoError(t, err) + require.Equal(t, int64(-1), seq) +} + +func TestReadLastSeq_headerOnly(t *testing.T) { + path := filepath.Join(t.TempDir(), "ts.csv") + require.NoError(t, os.WriteFile(path, []byte("unix_ns,seq,byte_offset\n"), 0o644)) + + seq, err := ReadLastSeq(path) + require.NoError(t, err) + require.Equal(t, int64(-1), seq) +} + +func TestReadLastSeq_singleDataRow(t *testing.T) { + path := filepath.Join(t.TempDir(), "ts.csv") + require.NoError(t, os.WriteFile(path, []byte("unix_ns,seq,byte_offset\n1000000,42,0\n"), 0o644)) + + seq, err := ReadLastSeq(path) + require.NoError(t, err) + require.Equal(t, int64(42), seq) +} + +func TestReadLastSeq_returnsLastRow(t *testing.T) { + path := filepath.Join(t.TempDir(), "ts.csv") + content := "unix_ns,seq,byte_offset\n1000000,0,0\n2000000,1,128\n3000000,2,256\n" + require.NoError(t, os.WriteFile(path, []byte(content), 0o644)) + + seq, err := ReadLastSeq(path) + require.NoError(t, err) + require.Equal(t, int64(2), seq) +} + +func TestReadLastSeq_skipsBlankLines(t *testing.T) { + path := filepath.Join(t.TempDir(), "ts.csv") + content := "unix_ns,seq,byte_offset\n1000000,0,0\n\n2000000,5,128\n\n" + require.NoError(t, os.WriteFile(path, []byte(content), 0o644)) + + seq, err := ReadLastSeq(path) + require.NoError(t, err) + require.Equal(t, int64(5), seq) +} + +// ── UniqueSegmentFile ────────────────────────────────────────────────────── + +func TestUniqueSegmentFile_insertsTimestampBeforeExtension(t *testing.T) { + base := "/data/top_camera.mp4" + ts := time.Date(2026, 6, 12, 16, 46, 29, 876543210, time.UTC) + + result := UniqueSegmentFile(base, ts) + + require.Contains(t, result, "20260612T164629") + require.Contains(t, result, "876543210Z") + require.Equal(t, ".mp4", filepath.Ext(result), "must keep original extension") + require.True(t, strings.HasPrefix(filepath.Base(result), "top_camera_")) +} + +func TestUniqueSegmentFile_keepsAudioExtension(t *testing.T) { + result := UniqueSegmentFile("/data/audio.ogg", time.Now().UTC()) + require.Equal(t, ".ogg", filepath.Ext(result)) +} + +func TestUniqueSegmentFile_differentTimesProduceDifferentNames(t *testing.T) { + base := "/data/video.mp4" + t1 := time.Date(2026, 1, 1, 0, 0, 0, 0, time.UTC) + t2 := time.Date(2026, 1, 1, 0, 0, 1, 0, time.UTC) + + require.NotEqual(t, UniqueSegmentFile(base, t1), UniqueSegmentFile(base, t2)) +} + +// ── FrameCSVWriter ───────────────────────────────────────────────────────── + +func TestNewFrameCSVWriter_emptyPath(t *testing.T) { + w := NewFrameCSVWriter("") + require.NotNil(t, w) +} + +func TestExtractAndAppend_nilWriter_isNoOp(t *testing.T) { + var w *FrameCSVWriter + err := w.ExtractAndAppend("/some/file.mp4", "v:0", 0) + require.NoError(t, err) +} + +func TestExtractAndAppend_emptyPath_isNoOp(t *testing.T) { + w := NewFrameCSVWriter("") + err := w.ExtractAndAppend("/some/file.mp4", "v:0", 0) + require.NoError(t, err) +} diff --git a/internal/rvl/rvl.go b/internal/rvl/rvl.go index d23d735..1c120dd 100644 --- a/internal/rvl/rvl.go +++ b/internal/rvl/rvl.go @@ -139,4 +139,4 @@ func Decode(data []byte, numPixels int) []uint16 { remaining -= nonzeros } return out -} +} \ No newline at end of file diff --git a/internal/video/stream.go b/internal/video/stream.go index e1dcfcb..a63cad5 100644 --- a/internal/video/stream.go +++ b/internal/video/stream.go @@ -6,25 +6,48 @@ import ( "log/slog" "os" "os/exec" + "path/filepath" "sync/atomic" "time" + + "om1-telemetry/internal/heartbeat" + "om1-telemetry/internal/recordutil" ) +const HeartbeatName = "video" + +const heartbeatInterval = 5 * time.Second + type Config struct { RTSPURL string - OutputFile string - TimestampsFile string + OutputFile string // base path; each segment is a uniquely-named sibling + TimestampsFile string // CSV index of all segments + FramesFile string // per-frame timestamps CSV (highly recommended). + + Monitor *heartbeat.Monitor + + HeartbeatName string } type VideoRTSPStream struct { - cfg Config - running atomic.Bool - cancel context.CancelFunc - done chan struct{} + cfg Config + running atomic.Bool + cancel context.CancelFunc + done chan struct{} + framesWrite *recordutil.FrameCSVWriter + pending atomic.Int64 + allDone chan struct{} } func New(cfg Config) *VideoRTSPStream { - return &VideoRTSPStream{cfg: cfg} + if cfg.HeartbeatName == "" { + cfg.HeartbeatName = HeartbeatName + } + return &VideoRTSPStream{ + cfg: cfg, + framesWrite: recordutil.NewFrameCSVWriter(cfg.FramesFile), + allDone: make(chan struct{}), + } } func (v *VideoRTSPStream) Start() { @@ -43,6 +66,11 @@ func (v *VideoRTSPStream) Stop() { } v.cancel() <-v.done + + // Wait for any in-flight ffprobe goroutines to finish so the + // frames CSV is complete on disk before we return. + v.waitForPending() + slog.Info("video stream stopped") } @@ -50,7 +78,8 @@ func (v *VideoRTSPStream) loop(ctx context.Context) { defer close(v.done) for ctx.Err() == nil { if err := v.record(ctx); err != nil && ctx.Err() == nil { - slog.Error("video recorder error; reconnecting in 2 s", "err", err) + slog.Error("video recorder error; reconnecting in 2 s", + "camera", v.cfg.HeartbeatName, "err", err) select { case <-ctx.Done(): case <-time.After(2 * time.Second): @@ -61,6 +90,8 @@ func (v *VideoRTSPStream) loop(ctx context.Context) { func (v *VideoRTSPStream) record(ctx context.Context) error { start := time.Now() + segmentFile := recordutil.UniqueSegmentFile(v.cfg.OutputFile, start) + cmd := exec.CommandContext(ctx, "ffmpeg", "-loglevel", "error", "-rtsp_transport", "tcp", @@ -68,8 +99,7 @@ func (v *VideoRTSPStream) record(ctx context.Context) error { "-c", "copy", "-movflags", "+frag_keyframe+empty_moov+default_base_moof", "-metadata", "creation_time="+start.UTC().Format(time.RFC3339Nano), - "-y", - v.cfg.OutputFile, + segmentFile, ) cmd.Cancel = func() error { return cmd.Process.Signal(os.Interrupt) } cmd.WaitDelay = 5 * time.Second @@ -77,19 +107,91 @@ func (v *VideoRTSPStream) record(ctx context.Context) error { if err := cmd.Start(); err != nil { return fmt.Errorf("start video recorder: %w", err) } - if err := writeStartTimestamp(v.cfg.TimestampsFile, start); err != nil { - slog.Error("failed to write video start timestamp", "file", v.cfg.TimestampsFile, "err", err) + if err := appendSegmentEntry(v.cfg.TimestampsFile, start, segmentFile); err != nil { + slog.Error("failed to append video segment entry", + "file", v.cfg.TimestampsFile, "err", err) + } + slog.Info("video segment started", "camera", v.cfg.HeartbeatName, "file", segmentFile) + + hbStop := make(chan struct{}) + hbDone := make(chan struct{}) + go func() { + defer close(hbDone) + // Tick once immediately so heartbeat sees life right away. + v.cfg.Monitor.Tick(v.cfg.HeartbeatName) + t := time.NewTicker(heartbeatInterval) + defer t.Stop() + for { + select { + case <-hbStop: + return + case <-t.C: + v.cfg.Monitor.Tick(v.cfg.HeartbeatName) + } + } + }() + + waitErr := cmd.Wait() + close(hbStop) + <-hbDone + + if v.cfg.FramesFile != "" { + v.pending.Add(1) + go func(segFile string, startUnixNs int64) { + defer v.markDone() + if err := v.framesWrite.ExtractAndAppend(segFile, "v:0", startUnixNs); err != nil { + slog.Error("video frames extraction failed", + "camera", v.cfg.HeartbeatName, "segment", segFile, "err", err) + } else { + slog.Info("video frames extracted", + "camera", v.cfg.HeartbeatName, "segment", filepath.Base(segFile)) + } + }(segmentFile, start.UnixNano()) + } + + return waitErr +} + +func (v *VideoRTSPStream) markDone() { + if v.pending.Add(-1) == 0 { + select { + case v.allDone <- struct{}{}: + default: + } + } +} + +func (v *VideoRTSPStream) waitForPending() { + for v.pending.Load() > 0 { + select { + case <-v.allDone: + case <-time.After(500 * time.Millisecond): + } } - return cmd.Wait() } -func writeStartTimestamp(path string, start time.Time) error { +func appendSegmentEntry(path string, start time.Time, segmentFile string) error { if path == "" { return nil } - return os.WriteFile( - path, - []byte(fmt.Sprintf("recording_start_unix_ns\n%d\n", start.UnixNano())), - 0o644, - ) + f, err := os.OpenFile(path, os.O_APPEND|os.O_CREATE|os.O_WRONLY, 0o644) + if err != nil { + return fmt.Errorf("open: %w", err) + } + defer f.Close() + + stat, err := f.Stat() + if err != nil { + return fmt.Errorf("stat: %w", err) + } + if stat.Size() == 0 { + if _, err := fmt.Fprintln(f, "recording_start_unix_ns,segment_file"); err != nil { + return fmt.Errorf("write header: %w", err) + } + } + + if _, err := fmt.Fprintf(f, "%d,%s\n", start.UnixNano(), filepath.Base(segmentFile)); err != nil { + return fmt.Errorf("write entry: %w", err) + } + return f.Sync() } diff --git a/internal/video/stream_test.go b/internal/video/stream_test.go index a4b025e..97c527a 100644 --- a/internal/video/stream_test.go +++ b/internal/video/stream_test.go @@ -1,8 +1,10 @@ package video import ( + "fmt" "os" "path/filepath" + "strings" "testing" "time" @@ -17,6 +19,24 @@ func TestNew_returnsNonNilStream(t *testing.T) { require.NotNil(t, stream, "New() returned nil") } +func TestNew_defaultHeartbeatName(t *testing.T) { + stream := New(Config{ + RTSPURL: "rtsp://localhost:8554/live", + OutputFile: filepath.Join(t.TempDir(), "video.mp4"), + // HeartbeatName intentionally empty → should default to HeartbeatName constant + }) + require.Equal(t, HeartbeatName, stream.cfg.HeartbeatName) +} + +func TestNew_customHeartbeatName(t *testing.T) { + stream := New(Config{ + RTSPURL: "rtsp://localhost:8554/live", + OutputFile: filepath.Join(t.TempDir(), "video.mp4"), + HeartbeatName: "video_top", + }) + require.Equal(t, "video_top", stream.cfg.HeartbeatName) +} + func TestStartStop_cleanLifecycle(t *testing.T) { stream := New(Config{ @@ -91,3 +111,41 @@ func TestRecord_createsOutputFileDirectory(t *testing.T) { _, err = os.Stat(outputDir) require.False(t, os.IsNotExist(err), "output directory was unexpectedly removed") } + +func TestAppendSegmentEntry_emptyPath_isNoOp(t *testing.T) { + err := appendSegmentEntry("", time.Now(), "/data/video.mp4") + require.NoError(t, err) +} + +func TestAppendSegmentEntry_writesHeaderAndEntry(t *testing.T) { + path := filepath.Join(t.TempDir(), "video_timestamps.csv") + start := time.Unix(1_000_000_000, 123_456_789) + segFile := "/data/top_camera_20260612T164629_876543210Z.mp4" + + err := appendSegmentEntry(path, start, segFile) + require.NoError(t, err) + + data, err := os.ReadFile(path) + require.NoError(t, err) + content := string(data) + require.Contains(t, content, "recording_start_unix_ns,segment_file") + require.Contains(t, content, fmt.Sprintf("%d", start.UnixNano())) + require.Contains(t, content, filepath.Base(segFile)) +} + +func TestAppendSegmentEntry_headerWrittenOnce(t *testing.T) { + path := filepath.Join(t.TempDir(), "video_timestamps.csv") + seg1 := "/data/video_seg1.mp4" + seg2 := "/data/video_seg2.mp4" + + require.NoError(t, appendSegmentEntry(path, time.Unix(1_000_000_000, 0), seg1)) + require.NoError(t, appendSegmentEntry(path, time.Unix(2_000_000_000, 0), seg2)) + + data, err := os.ReadFile(path) + require.NoError(t, err) + content := string(data) + + require.Equal(t, 1, strings.Count(content, "recording_start_unix_ns"), "header must appear exactly once") + require.Contains(t, content, filepath.Base(seg1)) + require.Contains(t, content, filepath.Base(seg2)) +} diff --git a/internal/zenohutil/timestamp.go b/internal/zenohutil/timestamp.go index 032f40a..12b4afd 100644 --- a/internal/zenohutil/timestamp.go +++ b/internal/zenohutil/timestamp.go @@ -4,19 +4,16 @@ import ( "github.com/eclipse-zenoh/zenoh-go/zenoh" ) +// TimestampToUnixNs converts a Zenoh HLC timestamp to Unix nanoseconds. +// Despite the name "NTP64", Zenoh stores the upper 32 bits as Unix-epoch +// seconds, NOT NTP-epoch seconds. Do NOT subtract the 70-year offset. func TimestampToUnixNs(ts zenoh.TimeStamp) int64 { return ntpTimeToUnixNs(ts.Time()) } func ntpTimeToUnixNs(ntpTime uint64) int64 { - const ntpToUnixOffset = 2208988800 - seconds := int64(ntpTime >> 32) fraction := uint32(ntpTime & 0xFFFFFFFF) - - unixSeconds := seconds - ntpToUnixOffset - nanos := (int64(fraction) * 1e9) >> 32 - - return unixSeconds*1e9 + nanos + return seconds*1e9 + nanos } diff --git a/internal/zenohutil/timestamp_test.go b/internal/zenohutil/timestamp_test.go index af5092c..62e8d98 100644 --- a/internal/zenohutil/timestamp_test.go +++ b/internal/zenohutil/timestamp_test.go @@ -6,38 +6,40 @@ import ( "github.com/stretchr/testify/require" ) -func TestNtpTimeToUnixNs(t *testing.T) { - tests := []struct { - name string - ntpTime uint64 - expected int64 - }{ - { - name: "epoch_time", - ntpTime: 2208988800 << 32, - expected: 0, - }, - { - name: "with_fraction", - ntpTime: (2208988801 << 32) | 0x80000000, - expected: 1_500_000_000, - }, - { - name: "recent_time", - ntpTime: (2208988800 + 1000000) << 32, - expected: 1000000 * 1e9, - }, - { - name: "time_with_precise_fraction", - ntpTime: (2208988801 << 32) | 0x40000000, - expected: 1_250_000_000, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := ntpTimeToUnixNs(tt.ntpTime) - require.Equal(t, tt.expected, result, "timestamp conversion mismatch") - }) - } +func TestNtpTimeToUnixNs_zero(t *testing.T) { + require.Equal(t, int64(0), ntpTimeToUnixNs(0)) +} + +func TestNtpTimeToUnixNs_oneSecond(t *testing.T) { + // upper 32 bits = 1 Unix-epoch second, no fractional part + result := ntpTimeToUnixNs(uint64(1) << 32) + require.Equal(t, int64(1_000_000_000), result) +} + +func TestNtpTimeToUnixNs_halfSecondFraction(t *testing.T) { + // upper = 0 seconds, lower = 0x80000000 ≈ 0.5 s + result := ntpTimeToUnixNs(uint64(0x80000000)) + require.InDelta(t, float64(500_000_000), float64(result), 1.0) +} + +func TestNtpTimeToUnixNs_quarterSecondFraction(t *testing.T) { + // lower = 0x40000000 ≈ 0.25 s + result := ntpTimeToUnixNs(uint64(0x40000000)) + require.InDelta(t, float64(250_000_000), float64(result), 1.0) +} + +func TestNtpTimeToUnixNs_recentTimestamp(t *testing.T) { + // 1_000_000_000 Unix seconds (Sep 2001) with no fraction + ntpTime := uint64(1_000_000_000) << 32 + result := ntpTimeToUnixNs(ntpTime) + require.Equal(t, int64(1_000_000_000)*int64(1_000_000_000), result) +} + +func TestNtpTimeToUnixNs_doesNotSubtractNTPOffset(t *testing.T) { + // The old incorrect code subtracted 2,208,988,800 s (NTP→Unix offset). + // With the corrected code, 2,208,988,800 << 32 should return + // 2,208,988,800 * 1e9, not 0. + ntpTime := uint64(2208988800) << 32 + result := ntpTimeToUnixNs(ntpTime) + require.Equal(t, int64(2208988800)*int64(1_000_000_000), result) } diff --git a/precheck_go2.sh b/precheck_go2.sh new file mode 100644 index 0000000..2dfe4bc --- /dev/null +++ b/precheck_go2.sh @@ -0,0 +1,263 @@ +set -u +PASS=0 +FAIL=0 +WARN=0 + + +ok() { echo " ✅ $1"; PASS=$((PASS+1)); } +fail() { echo " ❌ $1"; FAIL=$((FAIL+1)); } +warn() { echo " ⚠️ $1"; WARN=$((WARN+1)); } +hdr() { echo ""; echo "═══ $1 ═══"; } + +check_topic_exists() { + local topic="$1" + if ros2 topic list 2>/dev/null | grep -q "^${topic}\$"; then + return 0 + fi + return 1 +} + +check_topic_hz() { + local topic="$1" + local result + result=$(timeout 4 ros2 topic hz "$topic" 2>&1 | grep -oP 'average rate: \K[\d.]+' | head -1) + if [ -z "$result" ]; then + echo "0" + return 1 + fi + echo "$result" + return 0 +} + +check_topic_decodable() { + local topic="$1" + if timeout 3 ros2 topic echo --once "$topic" 2>&1 | grep -q "invalid"; then + return 1 + fi + return 0 +} + +echo "╔═══════════════════════════════════════════════════════════════╗" +echo "║ Go2 Recording Pre-Trip Check ║" +echo "║ Profile: 2D lidar /scan + depth + lowstate + RTSP ║" +echo "╚═══════════════════════════════════════════════════════════════╝" + + +hdr "Required ROS topics (for the recorders that ARE enabled on Go2)" + +# lidar (2D scan) — ENABLED +if check_topic_exists "/scan"; then + rate=$(check_topic_hz "/scan" "5") + if [ "$rate" != "0" ]; then + ok "/scan @ ${rate} Hz (2D laser scan, recorded by lidar recorder)" + else + fail "/scan exists but no data flowing" + fi +else + fail "/scan not found — lidar recorder will get nothing (this is Go2's primary recorded LiDAR source)" +fi + +# depth — ENABLED +DEPTH="/camera/realsense2_camera_node/depth/image_rect_raw" +if check_topic_exists "$DEPTH"; then + rate=$(check_topic_hz "$DEPTH" "10") + if [ "$rate" != "0" ]; then + ok "$DEPTH @ ${rate} Hz" + rate_int=${rate%.*} + if [ "$rate_int" -lt 10 ]; then + warn " depth rate is low (${rate} Hz, expected ~30); check RealSense fps config" + fi + else + fail "$DEPTH exists but no data" + fi +else + fail "$DEPTH not found" +fi + +# odom — ENABLED +if check_topic_exists "/odom"; then + rate=$(check_topic_hz "/odom" "5") + if [ "$rate" != "0" ]; then + ok "/odom @ ${rate} Hz" + else + warn "/odom exists but no data flowing" + fi +else + fail "/odom not found" +fi + +# lowstate — ENABLED +if check_topic_exists "/lowstate"; then + rate=$(check_topic_hz "/lowstate" "100") + if [ "$rate" != "0" ]; then + ok "/lowstate @ ${rate} Hz" + if check_topic_decodable "/lowstate"; then + ok " /lowstate message decodable (unitree_go installed)" + else + fail " /lowstate message type INVALID — install unitree_go ROS package" + fi + else + fail "/lowstate exists but no data" + fi +else + fail "/lowstate not found" +fi + + +hdr "RTSP streams (for video + audio recorders)" + +check_rtsp() { + local url="$1" + local label="$2" + local info + # 6-second timeout: some streams (non-standard H.264 profiles like + # yuv444p / High 4:4:4 Predictive) need extra time for ffprobe to + # negotiate and read first frames. Empirically 3 s was sometimes + # not enough on Go2's down_camera. + info=$(timeout 6 ffprobe -v error -show_streams "$url" 2>&1 | grep -oP 'codec_name=\K\w+' | head -1) + if [ -n "$info" ]; then + ok "$label ($url) → codec: $info" + else + fail "$label ($url) → not reachable (try manually: timeout 10 ffprobe -v error -show_streams $url)" + fi +} + +check_rtsp "rtsp://localhost:8554/top_camera_raw" "video top_camera" +check_rtsp "rtsp://localhost:8554/front_camera" "video front_camera" +check_rtsp "rtsp://localhost:8554/down_camera" "video down_camera" +check_rtsp "rtsp://localhost:8554/audio" "audio" + + +hdr "Calibration / latched topics (dump once per session for sensor fusion)" + +for topic in /tf_static \ + /camera/realsense2_camera_node/color/camera_info \ + /camera/realsense2_camera_node/depth/camera_info \ + /camera/realsense2_camera_node/extrinsics/depth_to_color; do + if check_topic_exists "$topic"; then + if timeout 3 ros2 topic echo --once "$topic" >/dev/null 2>&1; then + ok "$topic decodable" + else + warn "$topic exists but cannot echo" + fi + else + warn "$topic not found" + fi +done + + +# Go2 profile expectations +GO2_EXPECTED_ENABLE_LIDAR="true" +GO2_EXPECTED_ENABLE_POINTCLOUD="false" + +# ENABLE_LIDAR should be true (or unset, which defaults true) +if [ "${ENABLE_LIDAR:-true}" = "true" ]; then + ok "ENABLE_LIDAR=true (lidar recorder will run, recording /scan)" +else + fail "ENABLE_LIDAR=${ENABLE_LIDAR} but Go2 profile expects true" +fi + +# ENABLE_POINTCLOUD should be false for Go2 (the 3D Livox topic doesn't exist on Go2) +if [ "${ENABLE_POINTCLOUD:-true}" = "false" ]; then + ok "ENABLE_POINTCLOUD=false (pointcloud recorder disabled — correct for Go2)" +elif [ -z "${ENABLE_POINTCLOUD:-}" ]; then + warn "ENABLE_POINTCLOUD not set; default is true → pointcloud recorder will try to subscribe to a G1-only topic" + warn " Either: source env.go2 (sets ENABLE_POINTCLOUD=false)" + warn " Or: export ENABLE_POINTCLOUD=false" +else + fail "ENABLE_POINTCLOUD=${ENABLE_POINTCLOUD} but Go2 should disable it" +fi + +# LIDAR_ZENOH_TOPIC should be "scan" on Go2 +if [ "${LIDAR_ZENOH_TOPIC:-scan}" = "scan" ]; then + ok "LIDAR_ZENOH_TOPIC=scan" +else + warn "LIDAR_ZENOH_TOPIC=${LIDAR_ZENOH_TOPIC} (expected 'scan' for Go2; non-default is OK if intentional)" +fi + +# LOWSTATE_ZENOH_TOPIC should be rt/lowstate +if [ "${LOWSTATE_ZENOH_TOPIC:-rt/lowstate}" = "rt/lowstate" ]; then + ok "LOWSTATE_ZENOH_TOPIC=rt/lowstate" +else + warn "LOWSTATE_ZENOH_TOPIC=${LOWSTATE_ZENOH_TOPIC} (expected 'rt/lowstate')" +fi + +# RECORDINGS_DIR +if [ -z "${RECORDINGS_DIR:-}" ]; then + warn "RECORDINGS_DIR not set (will use ./recordings; change to USB SSD for trip)" +else + if [ -d "${RECORDINGS_DIR}" ] && [ -w "${RECORDINGS_DIR}" ]; then + ok "RECORDINGS_DIR=${RECORDINGS_DIR} (writable)" + else + fail "RECORDINGS_DIR=${RECORDINGS_DIR} but path not writable / missing" + fi +fi + +hdr "External tools" + +for tool in ffmpeg ffprobe ping ros2; do + if command -v $tool >/dev/null 2>&1; then + ok "$tool installed ($(command -v $tool))" + else + fail "$tool NOT FOUND — required dependency" + fi +done + +hdr "Disk capacity" + +target_dir="${RECORDINGS_DIR:-./recordings}" +target_base=$(dirname "$target_dir") +[ -d "$target_base" ] || target_base="/" +avail=$(df -h "$target_base" 2>/dev/null | awk 'NR==2 {print $4}') +avail_gb=$(df -BG "$target_base" 2>/dev/null | awk 'NR==2 {print $4}' | tr -d 'G') +echo " Target: $target_base" +echo " Available: $avail" + +# Go2 records ~110 GB/day so 500 GB free = ~4 days +if [ -z "$avail_gb" ]; then + warn "Could not determine disk space" +elif [ "$avail_gb" -ge 500 ]; then + ok "Disk has ${avail} free — comfortable for road trip (Go2 ~110 GB/day)" +elif [ "$avail_gb" -ge 100 ]; then + warn "Disk has ${avail} free — OK for a day or two of Go2 recording" +else + fail "Disk has only ${avail} free — not enough for serious recording" +fi + +hdr "Calibration dump (one-time per session)" + +if [ -n "${RECORDINGS_DIR:-}" ]; then + cal_dir="${RECORDINGS_DIR}/$(date +%Y-%m-%d)/calibration" + if [ -d "$cal_dir" ] && [ -f "$cal_dir/tf_static.yaml" ]; then + ok "Calibration already dumped at $cal_dir" + else + echo " Recommended: dump latched calibration topics ONCE before recording:" + echo "" + echo " mkdir -p ${cal_dir}" + echo " ros2 topic echo --once /tf_static > ${cal_dir}/tf_static.yaml" + echo " ros2 topic echo --once /camera/realsense2_camera_node/color/camera_info > ${cal_dir}/color_cam_info.yaml" + echo " ros2 topic echo --once /camera/realsense2_camera_node/depth/camera_info > ${cal_dir}/depth_cam_info.yaml" + echo " ros2 topic echo --once /camera/realsense2_camera_node/extrinsics/depth_to_color > ${cal_dir}/depth_to_color.yaml" + echo "" + echo " Without these, you cannot project LiDAR onto camera images later." + WARN=$((WARN+1)) + fi +fi + + +echo "" +echo "═══════════════════════════════════════════════════════════════" +echo " Pass: $PASS Fail: $FAIL Warn: $WARN" +echo "═══════════════════════════════════════════════════════════════" +if [ $FAIL -gt 0 ]; then + echo "❌ Critical checks failed — fix before recording." + exit 1 +elif [ $WARN -gt 0 ]; then + echo "⚠️ All critical checks passed but with warnings — review them above." + echo " Most common: ENABLE_POINTCLOUD not explicitly set," + echo " or calibration not yet dumped." + exit 0 +else + echo "✅ Everything checks out. Ready to record on Go2." + exit 0 +fi \ No newline at end of file diff --git a/verify_recording.py b/verify_recording.py new file mode 100644 index 0000000..fc3b14f --- /dev/null +++ b/verify_recording.py @@ -0,0 +1,534 @@ +#!/usr/bin/env python3 +""" +verify_recording.py — comprehensive validation of an om1-telemetry recording session. + +Checks: + 1. Per-stream rate, monotonicity, gaps, seq number continuity + 2. Format validity (mp4/ogg decode, binary file size consistency) + 3. Cross-modal time alignment (nearest-neighbor analysis) + 4. Calibration presence + 5. Final ML-readiness verdict + +Usage: + python3 verify_recording.py /path/to/session + python3 verify_recording.py /path/to/session --plot +""" + +import argparse +import os +import subprocess +import sys +from pathlib import Path + +import numpy as np +import pandas as pd + +G, R, Y, B, BOLD, END = ( + "\033[92m", "\033[91m", "\033[93m", "\033[94m", "\033[1m", "\033[0m" +) +OK_, BAD, WARN = f"{G}✅{END}", f"{R}❌{END}", f"{Y}⚠️{END}" + +# ── Streams to check (Go2 default; pointcloud disabled per env.go2) ── +STREAMS = [ + # (name, timestamps_csv, expected_hz, time_col) + ("lowstate", "lowstate_timestamps.csv", 400, "unix_ns"), + ("lidar", "lidar_timestamps.csv", 10, "unix_ns"), + ("pointcloud", "pointcloud_timestamps.csv", 10, "unix_ns"), + ("depth", "depth_timestamps.csv", 15, "unix_ns"), + ("odom", "odom_timestamps.csv", 150, "unix_ns"), # Go2 publishes /odom at ~150 Hz + ("audio", "audio_packets.csv", 100, "unix_ns"), # Opus 10ms frames → 100 pkt/s + ("top_cam", "top_camera_frames.csv", 30, "wallclock_unix_ns"), + ("front_cam", "front_camera_frames.csv", 30, "wallclock_unix_ns"), + ("down_cam", "down_camera_frames.csv", 15, "wallclock_unix_ns"), + ("network", "network_status.csv", 0.2, "unix_ns"), +] + +# ── 1. Per-stream statistics ───────────────────────────────────────── +def stream_stats(csv_path: Path, expected_hz: float, time_col: str): + """Return dict of stats, or None if file missing/unparseable.""" + if not csv_path.exists(): + return None + try: + df = pd.read_csv(csv_path) + except Exception as e: + return {"error": f"unparseable: {e}"} + if len(df) == 0: + return {"error": "empty"} + if time_col not in df.columns: + # Try alternate + for alt in ("wallclock_unix_ns", "unix_ns"): + if alt in df.columns: + time_col = alt + break + else: + return {"error": f"no time column in {df.columns.tolist()}"} + + ts = df[time_col].astype(np.int64).values + if len(ts) < 2: + return {"error": "< 2 samples"} + + duration = (ts[-1] - ts[0]) / 1e9 + rate = (len(ts) - 1) / duration if duration > 0 else 0 + intervals = np.diff(ts) / 1e6 # ms + monotonic = bool((intervals > 0).all()) + + # Gap threshold: 3x expected interval, or 1s if no expected rate + gap_threshold = 3 * (1000.0 / expected_hz) if expected_hz > 0 else 1000.0 + gaps = int((intervals > gap_threshold).sum()) + + # Sequence gaps + seq_gaps = None + if "seq" in df.columns: + seq = df["seq"].values + seq_gaps = int((seq[-1] - seq[0] + 1) - len(seq)) + + return { + "n": len(ts), + "duration_s": duration, + "rate_hz": rate, + "expected_hz": expected_hz, + "p50_interval_ms": float(np.median(intervals)), + "p95_interval_ms": float(np.percentile(intervals, 95)), + "max_interval_ms": float(intervals.max()), + "gaps": gaps, + "monotonic": monotonic, + "seq_gaps": seq_gaps, + "ts": ts, + "ts_start": int(ts[0]), + "ts_end": int(ts[-1]), + "time_col": time_col, + "csv_path": str(csv_path), + } + + +def print_stream_table(stats: dict): + print(f"\n{BOLD}Per-stream statistics{END}") + print(f"{'Stream':<12} {'Frames':>8} {'Duration':>9} {'Rate':>10} " + f"{'P95 jitter':>12} {'Gaps':>5} {'Seq':>5} Status") + print("─" * 80) + + for name, s in stats.items(): + if s is None: + print(f"{name:<12} {'---':>8} (not recorded / file missing)") + continue + if "error" in s: + print(f"{name:<12} {BAD} {s['error']}") + continue + + # Status: ok unless something wrong + status = OK_ + notes = [] + if not s["monotonic"]: + status = BAD; notes.append("non-monotonic") + elif s["gaps"] > 5: + status = WARN; notes.append(f"{s['gaps']} gaps") + if s["seq_gaps"] and s["seq_gaps"] > 5: + status = WARN; notes.append(f"{s['seq_gaps']} seq gaps") + + # Rate sanity: within 50% of expected + if s["expected_hz"] > 0: + ratio = s["rate_hz"] / s["expected_hz"] + if ratio < 0.5 or ratio > 2.0: + status = WARN + notes.append(f"rate {ratio*100:.0f}% of expected") + + rate_str = f"{s['rate_hz']:.1f} Hz" + dur_str = f"{s['duration_s']:.1f} s" + jitter_str = f"{s['p95_interval_ms']:.1f} ms" + seq_str = "—" if s["seq_gaps"] is None else str(s["seq_gaps"]) + + line = (f"{name:<12} {s['n']:>8} {dur_str:>9} {rate_str:>10} " + f"{jitter_str:>12} {s['gaps']:>5} {seq_str:>5} {status}") + if notes: + line += f" ({', '.join(notes)})" + print(line) + + +def ffprobe_streams(path: Path): + try: + r = subprocess.run( + ["ffprobe", "-v", "error", "-show_streams", str(path)], + capture_output=True, text=True, timeout=10, + ) + if r.returncode != 0: + return None + info = {} + for line in r.stdout.split("\n"): + if "=" in line: + k, v = line.split("=", 1) + info[k.strip()] = v.strip() + return info + except Exception: + return None + + +def check_binary(csv_path: Path, bin_path: Path): + """Check that the bin file size is consistent with the CSV byte_offsets. + Handles two formats: + (a) byte_offset + byte_length columns + (b) byte_offset only — derive length from successive offsets + """ + if not bin_path.exists(): + return None + try: + df = pd.read_csv(csv_path) + if "byte_offset" not in df.columns: + return {"error": "no byte_offset column"} + + offsets = df["byte_offset"].astype(np.int64).values + actual_size = bin_path.stat().st_size + + if "byte_length" in df.columns: + lengths = df["byte_length"].astype(np.int64).values + expected_end = int(offsets[-1] + lengths[-1]) + else: + # Derive lengths from successive offsets + # Last frame length = file_size - last_offset + next_offsets = np.concatenate([offsets[1:], [actual_size]]) + lengths = next_offsets - offsets + expected_end = actual_size # by construction + + # Sanity: offsets monotonic, lengths positive + mono = bool((np.diff(offsets) > 0).all()) + all_positive = bool((lengths > 0).all()) + + return { + "frames": len(df), + "expected_size": expected_end, + "actual_size": actual_size, + "match": abs(expected_end - actual_size) < 1024, + "offsets_monotonic": mono, + "lengths_positive": all_positive, + "avg_frame_size": float(lengths.mean()), + "min_frame_size": int(lengths.min()), + "max_frame_size": int(lengths.max()), + } + except Exception as e: + return {"error": str(e)} + + +def validate_formats(session: Path): + print(f"\n{BOLD}Format validation{END}") + + # Binary streams: bin size should match sum of byte_lengths + bin_streams = [ + ("lowstate", "lowstate_timestamps.csv", "lowstate_frames.bin"), + ("lidar", "lidar_timestamps.csv", "lidar_scans.bin"), + ("pointcloud", "pointcloud_timestamps.csv", "pointcloud_frames.bin"), + ("depth", "depth_timestamps.csv", "depth_frames.bin"), + ("odom", "odom_timestamps.csv", "odom_frames.bin"), + ] + for name, csv_name, bin_name in bin_streams: + info = check_binary(session / csv_name, session / bin_name) + if info is None: + continue + if "error" in info: + print(f" {BAD} {name}: {info['error']}") + continue + mark = OK_ if info["match"] else WARN + size_str = f"{info['actual_size']:,} B" + avg_str = f"{info['avg_frame_size']:.0f} B/frame avg" + range_str = (f"({info['min_frame_size']}–{info['max_frame_size']} B)" + if info['min_frame_size'] != info['max_frame_size'] + else "(constant size)") + print(f" {mark} {name}: {info['frames']} frames, {size_str}, {avg_str} {range_str}") + if not info["match"]: + print(f" expected bin size {info['expected_size']:,} B, " + f"actual {info['actual_size']:,} B (off by {abs(info['expected_size']-info['actual_size'])} B)") + + # Video files + for prefix in ["top_camera", "front_camera", "down_camera"]: + mp4 = session / f"{prefix}.mp4" + if not mp4.exists(): + continue + info = ffprobe_streams(mp4) + if info is None: + print(f" {BAD} {prefix}.mp4: ffprobe failed") + continue + codec = info.get("codec_name", "?") + w, h = info.get("width", "?"), info.get("height", "?") + fps_raw = info.get("r_frame_rate", "0/1") + try: + num, den = fps_raw.split("/") + fps = float(num) / float(den) if float(den) else 0 + except Exception: + fps = 0 + print(f" {OK_} {prefix}.mp4: {codec} {w}x{h} @ {fps:.1f} fps " + f"({mp4.stat().st_size:,} B)") + + # Audio + ogg = session / "audio.ogg" + if ogg.exists(): + info = ffprobe_streams(ogg) + if info is None: + print(f" {BAD} audio.ogg: ffprobe failed") + else: + codec = info.get("codec_name", "?") + sr = info.get("sample_rate", "?") + ch = info.get("channels", "?") + print(f" {OK_} audio.ogg: {codec} {sr} Hz, {ch} ch ({ogg.stat().st_size:,} B)") + + +def nearest_neighbor_align(master_ts, slave_ts): + """For each master timestamp, find nearest slave timestamp.""" + if len(slave_ts) == 0: + return None + idx = np.searchsorted(slave_ts, master_ts) + idx_left = np.clip(idx - 1, 0, len(slave_ts) - 1) + idx_right = np.clip(idx, 0, len(slave_ts) - 1) + d_left = np.abs(master_ts - slave_ts[idx_left]) + d_right = np.abs(master_ts - slave_ts[idx_right]) + d = np.minimum(d_left, d_right) / 1e6 # ms + return { + "median_ms": float(np.median(d)), + "p95_ms": float(np.percentile(d, 95)), + "max_ms": float(d.max()), + "n_master": len(master_ts), + } + + +def cross_alignment(stats: dict): + print(f"\n{BOLD}Cross-modal alignment (nearest-neighbor){END}") + + # Filter candidates: must have >= 50 samples for stable alignment stats + # (this excludes low-rate streams like network ping which has only ~10 samples) + candidates = [(n, s) for n, s in stats.items() + if s and "error" not in s and s["n"] >= 50] + if len(candidates) < 2: + # Fall back to >= 10 samples + candidates = [(n, s) for n, s in stats.items() + if s and "error" not in s and s["n"] >= 10] + if len(candidates) < 2: + print(f" {WARN} not enough streams to align") + return {}, [] + + # Master = slowest among qualified candidates (gives best coverage of slaves) + master_name, master = min(candidates, key=lambda kv: kv[1]["rate_hz"]) + print(f" Master: {BOLD}{master_name}{END} " + f"({master['rate_hz']:.1f} Hz, {master['n']} samples)\n") + + # Also restrict master samples to overlap with each slave's time range + # so tail mismatches (streams stopping at different times) don't dominate. + print(f" {'Pair':<30} {'Median':>9} {'P95':>9} {'Max':>9} OK?") + print(" " + "─" * 65) + + results = {} + skipped = [] # streams that couldn't be aligned (time range doesn't overlap) + # Build master full range + for name, s in candidates: + if name == master_name: + continue + + # Restrict master_ts to slave's time coverage + slave_t0, slave_t1 = s["ts_start"], s["ts_end"] + mask = (master["ts"] >= slave_t0) & (master["ts"] <= slave_t1) + master_in_range = master["ts"][mask] + if len(master_in_range) == 0: + # No overlap — almost always means timestamps are in different + # clock domains (e.g. wall clock vs monotonic, or wrong epoch). + skipped.append((name, s)) + continue + + align = nearest_neighbor_align(master_in_range, s["ts"]) + if align is None: + continue + align["n_used"] = len(master_in_range) + align["n_skipped"] = len(master["ts"]) - len(master_in_range) + results[name] = align + + # Verdict + if align["p95_ms"] < 50: mark = OK_ + elif align["p95_ms"] < 100: mark = OK_ + elif align["p95_ms"] < 500: mark = WARN + else: mark = BAD + + pair_str = f"{master_name} ↔ {name}" + note = "" + if align["n_skipped"] > 0: + note = f" (excl {align['n_skipped']} master samples outside slave range)" + print(f" {pair_str:<30} " + f"{align['median_ms']:>7.1f}ms " + f"{align['p95_ms']:>7.1f}ms " + f"{align['max_ms']:>7.1f}ms {mark}{note}") + + # Report streams that couldn't be aligned at all + if skipped: + print() + print(f" {BAD} {len(skipped)} stream(s) could NOT be aligned with master:") + for name, s in skipped: + # Compute the time offset between master and slave first timestamps + offset_sec = (s["ts_start"] - master["ts_start"]) / 1e9 + offset_str = f"{offset_sec:+,.1f} sec" + # Detect well-known offsets + hint = "" + if abs(abs(offset_sec) - 2208988800) < 10: + hint = " ← LOOKS LIKE NTP→Unix epoch offset (70 years)" + elif abs(offset_sec) > 86400 * 365: # > 1 year + hint = " ← different epoch / clock domain" + print(f" {name}: first timestamp offset from master = {offset_str}{hint}") + + # Return both alignments and the list of unaligned streams so the verdict + # can flag them. + return results, skipped + + +def check_calibration(session: Path): + print(f"\n{BOLD}Calibration{END}") + cal = session.parent / "calibration" + if not cal.exists(): + print(f" {WARN} No calibration directory at {cal}") + print(f" → without this you can't do sensor fusion") + return False + found = list(cal.glob("*.yaml")) + if not found: + print(f" {WARN} Calibration directory empty: {cal}") + return False + expected = { + "tf_static.yaml": "TF tree (LiDAR/camera/base mounting)", + "color_cam_info.yaml": "RGB camera intrinsics", + "depth_cam_info.yaml": "Depth camera intrinsics", + "depth_to_color.yaml": "Depth → RGB extrinsics", + } + have_critical = True + for fname, desc in expected.items(): + p = cal / fname + if p.exists() and p.stat().st_size > 50: + print(f" {OK_} {fname} ({p.stat().st_size} B) — {desc}") + else: + print(f" {WARN} {fname} MISSING — {desc}") + if "tf_static" in fname or "cam_info" in fname: + have_critical = False + return have_critical + + +def final_verdict(stats: dict, align: dict, skipped: list, has_calib: bool): + print(f"\n{BOLD}ML training readiness verdict{END}") + print("─" * 50) + + issues, warns = [], [] + + # Per-stream issues + for name, s in stats.items(): + if s is None: + continue + if "error" in s: + warns.append(f"{name}: {s['error']}") + continue + if not s["monotonic"]: + issues.append(f"{name} timestamps not monotonic — BLOCKER") + if s["gaps"] > 10: + warns.append(f"{name} has {s['gaps']} time gaps") + if s["seq_gaps"] and s["seq_gaps"] > 10: + warns.append(f"{name} dropped {s['seq_gaps']} messages") + if s["expected_hz"] > 0: + ratio = s["rate_hz"] / s["expected_hz"] + if ratio < 0.5: + warns.append(f"{name} rate is {ratio*100:.0f}% of expected ({s['rate_hz']:.1f} vs {s['expected_hz']} Hz)") + + # Streams that couldn't be aligned at all → different clock domains → BLOCKER + if skipped: + for name, s in skipped: + issues.append(f"{name} could NOT be aligned with master — different clock domain (BLOCKER for sensor fusion)") + + # Alignment quality of streams that DID align + if align: + worst = max(align.values(), key=lambda a: a["p95_ms"]) + worst_p95 = worst["p95_ms"] + if worst_p95 > 500: + issues.append(f"Worst alignment p95 = {worst_p95:.0f}ms — BLOCKER") + elif worst_p95 > 100: + warns.append(f"Worst alignment p95 = {worst_p95:.0f}ms (>100ms borderline)") + + # Calibration + if not has_calib: + warns.append("Calibration files missing — cross-sensor fusion not possible") + + # Print + print() + if issues: + print(f"{BOLD}{R}❌ NOT READY — must fix:{END}") + for i in issues: + print(f" • {i}") + if warns: + print(f"\n{Y}Also warnings:{END}") + for w in warns: + print(f" • {w}") + return False + if warns: + print(f"{BOLD}{Y}🟡 USABLE WITH CAVEATS{END}") + for w in warns: + print(f" • {w}") + print(f"\n{BOLD}{G}✅ Data is trainable; address caveats above.{END}") + return True + print(f"{BOLD}{G}✅ READY FOR TRAINING{END}") + print(" All streams monotonic · no gaps · alignment <100ms p95 · calibration present") + return True + + +def plot_timeline(stats: dict, output_path: Path): + try: + import matplotlib.pyplot as plt + except ImportError: + print(f" {WARN} matplotlib not installed, skipping plot") + return + + fig, ax = plt.subplots(figsize=(14, 5)) + valid = {n: s for n, s in stats.items() if s and "error" not in s} + if not valid: + print(f" {WARN} no streams to plot") + return + + # Time origin = earliest timestamp across all streams + t0 = min(s["ts_start"] for s in valid.values()) + for i, (name, s) in enumerate(valid.items()): + rel_t = (s["ts"] - t0) / 1e9 + # Sample down if too many + if len(rel_t) > 5000: + rel_t = rel_t[::len(rel_t) // 5000] + ax.scatter(rel_t, [i] * len(rel_t), s=2, alpha=0.5, label=f"{name} ({s['rate_hz']:.1f} Hz)") + + ax.set_yticks(range(len(valid))) + ax.set_yticklabels(list(valid.keys())) + ax.set_xlabel("Time (s, relative to recording start)") + ax.set_title("Per-stream message timeline") + ax.grid(True, alpha=0.3) + plt.tight_layout() + plt.savefig(output_path, dpi=100) + print(f" {OK_} Saved timeline plot to {output_path}") + + +def main(): + p = argparse.ArgumentParser() + p.add_argument("session", help="Path to a recording session directory") + p.add_argument("--plot", action="store_true", help="Save a timeline plot") + args = p.parse_args() + + session = Path(args.session) + if not session.exists(): + print(f"{BAD} Session not found: {session}") + sys.exit(1) + + print(f"{BOLD}╔══════════════════════════════════════════════════════════════╗{END}") + print(f"{BOLD}║ Recording verification: {session.name:<35} ║{END}") + print(f"{BOLD}╚══════════════════════════════════════════════════════════════╝{END}") + + # Collect per-stream stats + stats = {} + for name, csv_name, expected_hz, time_col in STREAMS: + stats[name] = stream_stats(session / csv_name, expected_hz, time_col) + + print_stream_table(stats) + validate_formats(session) + align, skipped = cross_alignment(stats) + has_calib = check_calibration(session) + ok = final_verdict(stats, align, skipped, has_calib) + + if args.plot: + print() + plot_timeline(stats, session / "timeline.png") + + sys.exit(0 if ok else 1) + + +if __name__ == "__main__": + main() \ No newline at end of file