From 103bf1cedd3bc3f706fd40b7146190f5b8be6e2b Mon Sep 17 00:00:00 2001 From: Poulpe Date: Sat, 25 Apr 2026 22:15:43 +0000 Subject: [PATCH] feat(viewer): v4 time-series graphs (depth, PWM USV/AUV, status) --- tools/check_sync.py | 71 ++++++++++++++++++++++ tools/extract_mcap_signals.py | 107 +++++++++++++++++++++++++++++++++ tools/extract_usv_pwm.py | 89 +++++++++++++++++++++++++++ viewer/index.html | 109 ++++++++++++++++++++++++++++++++-- 4 files changed, 372 insertions(+), 4 deletions(-) create mode 100644 tools/check_sync.py create mode 100644 tools/extract_mcap_signals.py create mode 100644 tools/extract_usv_pwm.py diff --git a/tools/check_sync.py b/tools/check_sync.py new file mode 100644 index 0000000..72cb8dd --- /dev/null +++ b/tools/check_sync.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""Check temporal alignment between MCAP AUV, USV PWM, and USBL data.""" +import json, os, sys +from datetime import datetime, timezone + +def fmt(ms): + if ms == 0: return 'N/A' + return datetime.fromtimestamp(ms/1000, tz=timezone.utc).strftime('%Y-%m-%d %H:%M:%S') + +def load(path): + with open(path) as f: + return json.load(f) + +base = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'output') + +sources = {} + +# MCAP signals +mcap_path = os.path.join(base, 'mcap_signals.json') +if os.path.exists(mcap_path): + d = load(mcap_path) + n = len(d.get('depth',[])) + len(d.get('pwm_auv',{}).get('samples',[])) + len(d.get('state',[])) + sources['MCAP AUV'] = {'t_min': d['t_min_utc_ms'], 't_max': d['t_max_utc_ms'], 'n': n} +else: + print(f"MISSING: {mcap_path}") + +# USV PWM +usv_path = os.path.join(base, 'usv_pwm.json') +if os.path.exists(usv_path): + d = load(usv_path) + n = sum(len(v) for v in d.get('M',{}).values()) + sum(len(v) for v in d.get('RC',{}).values()) + sources['USV PWM'] = {'t_min': d['t_min_utc_ms'], 't_max': d['t_max_utc_ms'], 'n': n} +else: + print(f"MISSING: {usv_path}") + +# USBL +usbl_path = os.path.join(base, 'usbl.json') +if os.path.exists(usbl_path): + d = load(usbl_path) + pts = d.get('points', []) + if pts: + t_vals = [p['t_ms'] for p in pts] + sources['USBL'] = {'t_min': min(t_vals), 't_max': max(t_vals), 'n': len(pts)} + else: + sources['USBL'] = {'t_min': 0, 't_max': 0, 'n': 0} +else: + print(f"MISSING: {usbl_path}") + +print(f"\n{'Source':<12} | {'t_min UTC':<20} | {'t_max UTC':<20} | {'n_pts':>6}") +print('-' * 68) +for name, s in sources.items(): + print(f"{name:<12} | {fmt(s['t_min']):<20} | {fmt(s['t_max']):<20} | {s['n']:>6}") + +# Overlap MCAP vs USV +if 'MCAP AUV' in sources and 'USV PWM' in sources: + mcap = sources['MCAP AUV'] + usv = sources['USV PWM'] + overlap_ms = min(mcap['t_max'], usv['t_max']) - max(mcap['t_min'], usv['t_min']) + print(f"\nMCAP t_min: {fmt(mcap['t_min'])} UTC") + print(f"USV t_min: {fmt(usv['t_min'])} UTC") + diff_min = (mcap['t_min'] - usv['t_min']) / 60000 + print(f"t_min diff: {diff_min:+.1f} min (MCAP vs USV)") + if overlap_ms > 60000: + print(f"OK - overlap: {overlap_ms//1000} s") + elif overlap_ms < 0: + print(f"WARNING: no overlap! gap = {-overlap_ms//1000} s") + else: + print(f"SUSPECT: overlap <60s: {overlap_ms//1000} s") + +if __name__ == '__main__': + pass diff --git a/tools/extract_mcap_signals.py b/tools/extract_mcap_signals.py new file mode 100644 index 0000000..35ea7df --- /dev/null +++ b/tools/extract_mcap_signals.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +"""Extract AUV signals from MCAP files: depth, PWM, state.""" +import argparse, glob, json, os, sys + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--session-dir', required=True) + parser.add_argument('--max-pts', type=int, default=5000) + args = parser.parse_args() + + session_name = os.path.basename(args.session_dir.rstrip('/')) + pattern = os.path.join(args.session_dir, '*.mcap') + mcap_files = sorted(glob.glob(pattern)) + if not mcap_files: + print(f"No MCAP files in {args.session_dir}", file=sys.stderr) + sys.exit(1) + print(f"Found {len(mcap_files)} MCAP files") + + try: + from mcap.reader import make_reader + from mcap_ros2.decoder import DecoderFactory + except ImportError as e: + print(f"Import error: {e}", file=sys.stderr) + sys.exit(1) + + depth_raw = [] + pwm_raw = [] + state_raw = [] + TOPICS = ['/mavros/imu/static_pressure', '/mavros/rc/out', '/mavros/state'] + + for mcap_file in mcap_files: + try: + with open(mcap_file, 'rb') as f: + reader = make_reader(f, decoder_factories=[DecoderFactory()]) + for schema, channel, message, ros_msg in reader.iter_decoded_messages(topics=TOPICS): + t_ms = message.publish_time // 1_000_000 + topic = channel.topic + if topic == '/mavros/imu/static_pressure': + try: + p = float(ros_msg.fluid_pressure) + depth_m = (p - 101325.0) / (1025.0 * 9.80665) + depth_raw.append({'t': t_ms, 'v': round(depth_m, 4)}) + except Exception: + pass + elif topic == '/mavros/rc/out': + try: + ch = list(ros_msg.channels) + pwm_raw.append({'t': t_ms, 'v': ch}) + except Exception: + pass + elif topic == '/mavros/state': + try: + state_raw.append({'t': t_ms, 'mode': str(ros_msg.mode), 'armed': bool(ros_msg.armed)}) + except Exception: + pass + except Exception as e: + print(f" Skip {os.path.basename(mcap_file)}: {e}") + + def sample(lst, max_pts): + if len(lst) <= max_pts: + return lst + stride = len(lst) // max_pts + sampled = lst[::stride] + if sampled[-1] is not lst[-1]: + sampled.append(lst[-1]) + return sampled + + depth = sample(depth_raw, args.max_pts) + pwm_samples = sample(pwm_raw, args.max_pts) + state = state_raw # events, keep all + + all_t = [p['t'] for p in depth_raw + pwm_raw + state_raw] + t_min = min(all_t) if all_t else 0 + t_max = max(all_t) if all_t else 0 + + n_ch = max((len(s['v']) for s in pwm_raw), default=0) + channels = list(range(n_ch)) + + from datetime import datetime, timezone + fmt = lambda ms: datetime.fromtimestamp(ms/1000, tz=timezone.utc).isoformat() + print(f"depth: {len(depth)} pts (raw {len(depth_raw)})") + if depth: + dvals = [p['v'] for p in depth] + print(f" depth range: {min(dvals):.3f} .. {max(dvals):.3f} m") + print(f"pwm_auv: {len(pwm_samples)} samples (raw {len(pwm_raw)}), {n_ch} channels") + print(f"state: {len(state)} events") + print(f"t_min: {fmt(t_min)}") + print(f"t_max: {fmt(t_max)}") + + out = { + 'session': session_name, + 't_min_utc_ms': t_min, + 't_max_utc_ms': t_max, + 'depth': depth, + 'pwm_auv': {'channels': channels, 'samples': pwm_samples}, + 'state': state, + } + + outdir = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'output') + os.makedirs(outdir, exist_ok=True) + outpath = os.path.join(outdir, 'mcap_signals.json') + with open(outpath, 'w') as f: + json.dump(out, f) + print(f"Written: {outpath}") + +if __name__ == '__main__': + main() diff --git a/tools/extract_usv_pwm.py b/tools/extract_usv_pwm.py new file mode 100644 index 0000000..c0b98ea --- /dev/null +++ b/tools/extract_usv_pwm.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +"""Extract USV PWM signals from navigation log CSVs.""" +import argparse, csv, glob, json, os, re, sys +from datetime import datetime, timezone + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--nav-dir', required=True) + args = parser.parse_args() + + pattern = os.path.join(args.nav_dir, '*_navigation_log.csv') + csv_files = sorted(glob.glob(pattern)) + if not csv_files: + print(f"No navigation_log.csv in {args.nav_dir}", file=sys.stderr) + sys.exit(1) + print(f"Found {len(csv_files)} nav CSV files") + + M_data = {} + RC_data = {} + + for csv_file in csv_files: + print(f" Parsing {os.path.basename(csv_file)}") + try: + with open(csv_file, 'r') as f: + reader = csv.DictReader(f) + for row in reader: + ts_str = row.get('timestamp', '').strip() + data = row.get('data', '').strip() + val_str = row.get('value', '').strip() + if not ts_str or not data or not val_str: + continue + is_M = re.match(r'^M\d+$', data) + is_RC = re.match(r'^RC\d+$', data) + if not is_M and not is_RC: + continue + try: + val = float(val_str) + except ValueError: + continue + try: + dt = datetime.strptime(ts_str, '%Y-%m-%d %H:%M:%S.%f') + except ValueError: + try: + dt = datetime.strptime(ts_str, '%Y-%m-%d %H:%M:%S') + except ValueError: + continue + # CET -> UTC: subtract 3600s + t_ms = int(dt.timestamp() * 1000) - 3600 * 1000 + pt = {'t': t_ms, 'v': val} + if is_M: + M_data.setdefault(data, []).append(pt) + else: + RC_data.setdefault(data, []).append(pt) + except Exception as e: + print(f" Error {csv_file}: {e}") + + all_t = [] + for pts in list(M_data.values()) + list(RC_data.values()): + all_t.extend(p['t'] for p in pts) + t_min = min(all_t) if all_t else 0 + t_max = max(all_t) if all_t else 0 + + for k in sorted(M_data): + print(f" {k}: {len(M_data[k])} pts") + for k in sorted(RC_data): + print(f" {k}: {len(RC_data[k])} pts") + + fmt = lambda ms: datetime.fromtimestamp(ms/1000, tz=timezone.utc).isoformat() + print(f"t_min UTC: {fmt(t_min)}") + print(f"t_max UTC: {fmt(t_max)}") + + out = { + 'tz_assumed': 'CET (UTC+1)', + 'tz_converted_to': 'UTC', + 't_min_utc_ms': t_min, + 't_max_utc_ms': t_max, + 'M': M_data, + 'RC': RC_data, + } + + outdir = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'output') + os.makedirs(outdir, exist_ok=True) + outpath = os.path.join(outdir, 'usv_pwm.json') + with open(outpath, 'w') as f: + json.dump(out, f) + print(f"Written: {outpath}") + +if __name__ == '__main__': + main() diff --git a/viewer/index.html b/viewer/index.html index d5c7201..b701479 100644 --- a/viewer/index.html +++ b/viewer/index.html @@ -2,13 +2,13 @@ -COSMA — NAV Viewer v3 +COSMA — NAV Viewer v4 + +
@@ -198,6 +204,77 @@ let trackLayers = []; let auvTrackLayer = null; let windowPoints = []; let usblWindow = []; + +// == Graph state == +let charts = {}; +let mcapSignals = null; +let usvPwm = null; + +const PWM_COLORS_AUV = ['#ff8800','#ffd166','#06d6a0','#00b4d8','#a855f7','#f97316','#e94560','#88c0d0']; +const PWM_COLORS_USV = ['#00b4d8','#0096b4','#0077a0','#005f8c','#ffd166','#e6b800','#c49a00','#a07d00']; + +function makeChartOptions() { + return { + animation: false, parsing: false, responsive: true, maintainAspectRatio: false, + plugins: { + legend: { display: false, labels: { color:'#a0c4ff', font:{size:9,family:'monospace'}, boxWidth:12 } }, + annotation: { annotations: {} } + }, + scales: { + x: { type:'linear', ticks:{ color:'#666', font:{size:9,family:'monospace'}, maxTicksLimit:8, + callback:(v)=>new Date(v).toISOString().substr(11,8) }, grid:{color:'#1a1a3e'} }, + y: { ticks:{ color:'#666', font:{size:9,family:'monospace'}, maxTicksLimit:5 }, grid:{color:'#1a1a3e'} }, + }, + }; +} + +function initCharts() { + const mkDs = (lbl, col) => ({ label:lbl, data:[], borderColor:col, borderWidth:1.5, pointRadius:0, tension:0 }); + charts.depth = new Chart(document.getElementById('chart-depth'), { type:'line', data:{datasets:[mkDs('depth','#06d6a0')]}, options:makeChartOptions() }); + charts.pwmAuv = new Chart(document.getElementById('chart-pwm-auv'), { type:'line', data:{datasets:[]}, options:makeChartOptions() }); + charts.pwmUsv = new Chart(document.getElementById('chart-pwm-usv'), { type:'line', data:{datasets:[]}, options:makeChartOptions() }); + charts.usbl = new Chart(document.getElementById('chart-usbl'), { type:'line', data:{datasets:[mkDs('dist','#a855f7')]}, options:makeChartOptions() }); +} + +function updateGraphCursor(t_ms) { + const ann = { type:'line', xMin:t_ms, xMax:t_ms, borderColor:'#e94560', borderWidth:1.5, borderDash:[4,2] }; + for (const c of Object.values(charts)) { c.options.plugins.annotation.annotations = {cursor:ann}; c.update('none'); } +} + +function updateGraphWindow(t0, t1) { + for (const c of Object.values(charts)) { c.options.scales.x.min=t0; c.options.scales.x.max=t1; c.update('none'); } +} + +function populateCharts() { + if (mcapSignals) { + if (mcapSignals.depth) { + charts.depth.data.datasets[0].data = mcapSignals.depth.map(p=>({x:p.t,y:p.v})); + charts.depth.update('none'); + } + if (mcapSignals.pwm_auv) { + const {channels,samples} = mcapSignals.pwm_auv; + charts.pwmAuv.data.datasets = channels.map((ch,i)=>({ + label:'Ch'+ch, data:samples.map(s=>({x:s.t,y:s.v[i]})), + borderColor:PWM_COLORS_AUV[i%8], borderWidth:1, pointRadius:0, tension:0 + })); + charts.pwmAuv.options.plugins.legend.display = channels.length > 1; + charts.pwmAuv.update('none'); + } + } + if (usvPwm && usvPwm.M) { + const keys = Object.keys(usvPwm.M).sort(); + charts.pwmUsv.data.datasets = keys.map((k,i)=>({ + label:k, data:usvPwm.M[k].map(p=>({x:p.t,y:p.v})), + borderColor:PWM_COLORS_USV[i%8], borderWidth:1, pointRadius:0, tension:0 + })); + charts.pwmUsv.options.plugins.legend.display = keys.length > 1; + charts.pwmUsv.update('none'); + } + if (usblPoints && usblPoints.length) { + charts.usbl.data.datasets[0].data = usblPoints.map(p=>({x:p.t_ms, y:p.dist!==undefined?p.dist:p.distance})); + charts.usbl.update('none'); + } +} let cursorMarker = null; let auvMarker = null; let usblVector = null; @@ -469,7 +546,7 @@ async function loadData() { if (tMin === tMax) tMax = tMin + 1000; document.getElementById('title').textContent = - `COSMA v3 — USV ${manifest.n_sessions} sess. ${manifest.n_points_sampled} pts | AUV ${usblMeta.n_points} pts`; + `COSMA v4 — USV ${manifest.n_sessions} sess. ${manifest.n_points_sampled} pts | AUV ${usblMeta.n_points} pts`; buildLegend(); initSliders(); @@ -483,7 +560,31 @@ async function loadData() { } } -loadData(); +async function loadGraphData() { + initCharts(); + try { + const [mcapResp, usvPwmResp] = await Promise.allSettled([ + fetch('data/mcap_signals.json'), + fetch('data/usv_pwm.json'), + ]); + if (mcapResp.status === 'fulfilled' && mcapResp.value.ok) { + mcapSignals = await mcapResp.value.json(); + } + if (usvPwmResp.status === 'fulfilled' && usvPwmResp.value.ok) { + usvPwm = await usvPwmResp.value.json(); + } + populateCharts(); + } catch (e) { console.warn('Graph data load error:', e); } +} + +loadData().then(() => { populateCharts(); }); +loadGraphData(); +
+
Profondeur AUV (m)
+
PWM AUV (canaux)
+
PWM USV (M1-M8)
+
USBL Distance (m)
+