floppyrj45/cosma-nav
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: extract_mcap — /navigation/altitude (Kogger) -> HDF5 altitude_m + seafloor_depth_m

Browse Source 
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Floppyrj45
2026-04-24 11:48:46 +02:00
parent ac7b89bed5
commit 7330b230b8
2 changed files with 60 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
extract/extract_mcap.py
View File

@@ -7,13 +7,15 @@ from mcap_ros2.reader import read_ros2_messages
TOPIC_GPS = "/mavros/global_position/global" # sensor_msgs/NavSatFix
TOPIC_DEPTH = "/mavros/global_position/rel_alt" # std_msgs/Float64
TOPIC_ALT = "/navigation/altitude" # std_msgs/Float64 - altitude above seafloor
def write_auv_mcap_group(h5_path: str,
t_ns: np.ndarray,
lat: np.ndarray,
lon: np.ndarray,
depth_m: np.ndarray) -> None:
depth_m: np.ndarray,
altitude_m: np.ndarray | None = None) -> None:
with h5py.File(h5_path, "a") as f:
if "auv_mcap" in f:
del f["auv_mcap"]
@@ -22,6 +24,9 @@ def write_auv_mcap_group(h5_path: str,
grp.create_dataset("lat", data=lat.astype(np.float64), compression="gzip")
grp.create_dataset("lon", data=lon.astype(np.float64), compression="gzip")
grp.create_dataset("depth_m", data=depth_m.astype(np.float64), compression="gzip")
if altitude_m is not None:
grp.create_dataset("altitude_m", data=altitude_m.astype(np.float64), compression="gzip")
grp.create_dataset("seafloor_depth_m", data=(depth_m + altitude_m).astype(np.float64), compression="gzip")
def _iter_topic(bag_dir: str, topic: str):
@@ -60,8 +65,22 @@ def extract(bag_dir: str, out_h5: str) -> None:
ts_arr = np.array(ts, dtype=np.int64)
depths = np.interp(ts_arr, depth_times_arr, depth_vals)
write_auv_mcap_group(out_h5, ts_arr, np.array(lats), np.array(lons), depths)
print(f"AUV MCAP: {len(ts)} fixes -> {out_h5} [/auv_mcap]")
alt_by_t: dict[int, float] = {}
for msg in _iter_topic(bag_dir, TOPIC_ALT):
alt_by_t[msg.log_time_ns] = float(msg.ros_msg.data)
if alt_by_t:
alt_times = sorted(alt_by_t.keys())
alt_vals = np.array([alt_by_t[k] for k in alt_times], dtype=np.float64)
alt_times_arr = np.array(alt_times, dtype=np.int64)
altitudes = np.interp(ts_arr, alt_times_arr, alt_vals)
else:
altitudes = None
print("WARNING: /navigation/altitude not found in bags", file=sys.stderr)
write_auv_mcap_group(out_h5, ts_arr, np.array(lats), np.array(lons), depths, altitudes)
alt_status = "ok" if altitudes is not None else "missing"
print(f"AUV MCAP: {len(ts)} fixes, depth ok, altitude {alt_status} -> {out_h5} [/auv_mcap]")
if __name__ == "__main__":