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()
|
||||
Reference in New Issue
Block a user