feat(viewer): v4 time-series graphs (depth, PWM USV/AUV, status)
This commit is contained in:
Poulpe
71
tools/check_sync.py
Normal file
71
tools/check_sync.py
Normal file
@@ -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
|
||||
107
tools/extract_mcap_signals.py
Normal file
107
tools/extract_mcap_signals.py
Normal file
@@ -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()
|
||||
89
tools/extract_usv_pwm.py
Normal file
89
tools/extract_usv_pwm.py
Normal file
@@ -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()
|
||||
@@ -2,13 +2,13 @@
|
||||
<html lang="fr">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<title>COSMA — NAV Viewer v3</title>
|
||||
<title>COSMA — NAV Viewer v4</title>
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||||
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css"/>
|
||||
<link rel="stylesheet" href="https://cdnjs.cloudflare.com/ajax/libs/noUiSlider/15.7.1/nouislider.min.css"/>
|
||||
<style>
|
||||
* { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
body { font-family: monospace; background: #1a1a2e; color: #e0e0e0; display: flex; flex-direction: column; height: 100vh; }
|
||||
body { font-family: monospace; background: #1a1a2e; color: #e0e0e0; display: flex; flex-direction: column; min-height: 100vh; }
|
||||
#map { flex: 1; min-height: 0; }
|
||||
#controls {
|
||||
background: #16213e;
|
||||
@@ -89,7 +89,13 @@
|
||||
#btn-auv { color: #ff8800; border-color: #ff8800; }
|
||||
#btn-vec { color: #888; border-color: #888; }
|
||||
#btn-usbl-panel { color: #aaa; border-color: #444; }
|
||||
#graphs-section { background:#12122a; border-top:1px solid #0f3460; overflow-y:auto; max-height:40vh; flex-shrink:0; }
|
||||
.chart-container { padding:6px 12px 4px; border-bottom:1px solid #0f3460; }
|
||||
.chart-container canvas { display:block; }
|
||||
.chart-title { font-size:10px; color:#a0c4ff; margin-bottom:2px; font-family:monospace; }
|
||||
</style>
|
||||
<script src="https://cdn.jsdelivr.net/npm/chart.js@4.4.0/dist/chart.umd.min.js"></script>
|
||||
<script src="https://cdn.jsdelivr.net/npm/chartjs-plugin-annotation@3/dist/chartjs-plugin-annotation.min.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="map"></div>
|
||||
@@ -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();
|
||||
</script>
|
||||
<div id="graphs-section">
|
||||
<div class="chart-container"><div class="chart-title">Profondeur AUV (m)</div><canvas id="chart-depth" height="80"></canvas></div>
|
||||
<div class="chart-container"><div class="chart-title">PWM AUV (canaux)</div><canvas id="chart-pwm-auv" height="80"></canvas></div>
|
||||
<div class="chart-container"><div class="chart-title">PWM USV (M1-M8)</div><canvas id="chart-pwm-usv" height="80"></canvas></div>
|
||||
<div class="chart-container"><div class="chart-title">USBL Distance (m)</div><canvas id="chart-usbl" height="60"></canvas></div>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
Reference in New Issue
Block a user