feat(recording): Go2 + Mid-360 Point-LIO map recording#2557
Open
jeff-hykin wants to merge 213 commits into
Open
feat(recording): Go2 + Mid-360 Point-LIO map recording#2557jeff-hykin wants to merge 213 commits into
jeff-hykin wants to merge 213 commits into
Conversation
Move the recorder + tcpdump pcap logic out of the go2_record blueprint
into dimos/hardware/sensors/lidar/fastlio2/recorder.py. Pcap recording
is now opt-in (record_pcap defaults to False), and the default paths
land under ./go2_recordings/<date_time>/{mem2.db,raw_mid360.pcap}.
Add the offline post-processing pipeline for Go2 + Livox recordings:
- recording/{apriltags,gtsam_gt,lidar_reanchor,build_rrd,camera,rec_check}
- scripts/go2_mid360_post_process.py orchestrator
AprilTag detection now drops distant/oblique glimpses (keep <=1m, head-on
within 45deg), clusters same-id detections within 5s, and emits one medoid
representative per cluster (most spatially/rotationally central).
…process TARGET positional accepts a mem2.db, a dir containing one (process just that recording), or a dir to scan. With no TARGET, process the most recently created recording under --recordings-dir. Folds in the old --db flag.
…/dimos into jeff/feat/go2_record_clean
build_rrd now looks for a main.jsonl next to the mem2.db (else one dir up), replays each JSON line as a rerun TextLog on the `ts` timeline (level + logger + extra fields preserved), and docks a TextLogView below the 3D/camera views when present.
--check runs only the rec_check report on each recording and writes a structured summary.json (files, pcap stats, per-stream rows/hz/pose%, fastlio odometry travel) into the recording dir, skipping GTSAM/re-anchor/.rrd.
Drop the one-dir-up fallback in build_rrd's jsonl discovery.
…/dimos into jeff/feat/go2_record_clean
…lio_native flake/cmake consuming dimos-module-fastlio2 pointlio branch + Estimator/parameters sources)
…y path) The fast-lio input was pinned to file:///Users/jeffhykin/... which only exists on the Mac. Repoint to the dimensionalOS/dimos-module-fastlio2 pointlio branch on github so the flake builds on Linux. Same locked rev.
Rename the mirrored fastlio_blueprints.py to pointlio_blueprints.py and wire it to PointLio (was incorrectly using FastLio2). Adds mid360-pointlio and mid360-pointlio-voxels to the blueprint registry.
ZedSimple: UVC color (YUYV side-by-side, left eye) plus ZED-M IMU over USB-HID via the zed-open-capture protocol; used when pyzed is absent. Color and IMU run as independent fail-soft loops. (hid is an optional runtime dep; the module logs and skips IMU if it or libhidapi is missing.) record.py: - Select ZEDCamera (SDK) or ZedSimple, remap color_image->zed_color_image and imu->zed_imu; record both. Add an imu Out stream to ZEDCamera too. - Replace the in-process TUI teleop with the KeyboardTeleop module (cmd_vel->tele_cmd_vel); sit/stand via the auto-wired GO2ConnectionSpec. Mid360: auto-correct host_ip to a local interface on the lidar subnet (mirrors FastLio2), fixing the native bind failure when the configured host_ip is not assigned to this machine.
…go2_record_clean # Conflicts: # dimos/hardware/sensors/lidar/livox/module.py
…dimos into jeff/feat/go2_record_clean # Conflicts: # dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml # dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp # dimos/hardware/sensors/lidar/fastlio2/module.py # dimos/hardware/sensors/lidar/fastlio2/recorder.py # dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py # dimos/robot/all_blueprints.py
…jeff/feat/go2_record_clean
Replace FastLio2 with PointLio in both recording drivers. PointlioRecorder stamps each lidar frame with the live odometry pose at record time, so the drivers drop the FastLio2 TfHack static-transform pose logic entirely — they just declare the companion In ports and let the recorder handle poses. Rename the recorded stream names fastlio_* -> pointlio_* throughout the post-process toolchain (gtsam_gt, build_rrd, rec_check, multi_map_anchor) so recordings post-process and visualize. multi_map_anchor: drop the dead lidar_reanchor import (removed in the post-process refactor) and use the recorder-stamped pointlio_lidar for the map viz. Add docs/capabilities/navigation/recording_a_map.md guide. Regenerate all_blueprints.
jeff-hykin
requested review from
arkluc,
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
jeff-hykin
commented
Jun 22, 2026
| return f"{self.config.camera_name}_depth_optical_frame" | ||
|
|
||
| @property | ||
| def _imu_frame(self) -> str: |
Member
Author
There was a problem hiding this comment.
Recorded IMU with realsense cause I wanted an all-in-one recording
❌ 1 Tests Failed:
View the top 1 failed test(s) by shortest run time
To view more test analytics, go to the Test Analytics Dashboard |
Contributor
Greptile SummaryThis PR adds a complete Go2 + Livox Mid-360 map-recording pipeline: a recording script (
Confidence Score: 4/5Safe to merge with one fix: a The recording path itself is unaffected — data is written correctly to dimos/mapping/recording/utils/build_rrd.py (lines 499–509) and dimos/mapping/recording/utils/rec_check.py (odometry_travel / pointlio_odometry hardcoding). Important Files Changed
Sequence Diagram%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
participant R as record.py
participant G as Go2Recorder
participant PL as PointLio (C++)
participant M as Mid360
participant DB as mem2.db
R->>G: start blueprint
M-->>G: livox_lidar / livox_imu
PL-->>G: pointlio_odometry (Odometry)
PL-->>G: pointlio_lidar (PointCloud2)
G->>G: "@pose_setter_for caches latest Pose"
G->>DB: write pointlio_odometry (with pose cols)
G->>DB: write pointlio_lidar (with pose cols baked in)
G->>DB: write go2_odom / go2_lidar / color_image
Note over R,DB: Ctrl+C stops recording
participant PP as post_process.py
participant RC as rec_check
participant AT as apriltags
participant GT as gtsam_gt
participant RRD as build_rrd
PP->>RC: report + write_summary
PP->>AT: "detect_apriltags -> april_tags stream"
PP->>GT: build_gtsam_gt (odom + tag landmarks + loop closures)
GT-->>DB: write gtsam_odom stream
PP->>RRD: "build_rrd -> .rrd file"
%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
sequenceDiagram
participant R as record.py
participant G as Go2Recorder
participant PL as PointLio (C++)
participant M as Mid360
participant DB as mem2.db
R->>G: start blueprint
M-->>G: livox_lidar / livox_imu
PL-->>G: pointlio_odometry (Odometry)
PL-->>G: pointlio_lidar (PointCloud2)
G->>G: "@pose_setter_for caches latest Pose"
G->>DB: write pointlio_odometry (with pose cols)
G->>DB: write pointlio_lidar (with pose cols baked in)
G->>DB: write go2_odom / go2_lidar / color_image
Note over R,DB: Ctrl+C stops recording
participant PP as post_process.py
participant RC as rec_check
participant AT as apriltags
participant GT as gtsam_gt
participant RRD as build_rrd
PP->>RC: report + write_summary
PP->>AT: "detect_apriltags -> april_tags stream"
PP->>GT: build_gtsam_gt (odom + tag landmarks + loop closures)
GT-->>DB: write gtsam_odom stream
PP->>RRD: "build_rrd -> .rrd file"
Reviews (4): Last reviewed commit: "RESTORE: revert KeyboardTeleop lie-down/..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
Comment on lines
+44
to
+47
| def _default_recording_dir() -> Path: | ||
| now = datetime.now() | ||
| stamp = now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-PST" | ||
| return Path("recordings") / stamp |
Contributor
There was a problem hiding this comment.
-PST unconditionally, regardless of the system's actual timezone. A user running this on a UTC or EST machine gets a directory labeled with the wrong timezone, which is confusing when correlating recordings with wall-clock logs. Using datetime.now().astimezone() and %z produces the real local-offset string instead.
Suggested change
| def _default_recording_dir() -> Path: | |
| now = datetime.now() | |
| stamp = now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-PST" | |
| return Path("recordings") / stamp | |
| def _default_recording_dir() -> Path: | |
| now = datetime.now().astimezone() | |
| stamp = now.strftime("%Y-%m-%d") + "_" + now.strftime("%H-%M") + now.strftime("%z") | |
| return Path("recordings") / stamp |
Comment on lines
+74
to
+75
| Prefer the SDK-backed ``ZEDCamera`` (depth/imu/pointcloud); fall back to the | ||
| UVC-only ``ZedSimple`` (color only) when ``pyzed`` is not installed. |
Contributor
There was a problem hiding this comment.
ZedSimple publishes both color_image and imu (USB-HID path from zed-open-capture). The remapping already wires ZedSimple.imu -> zed_imu, so it works correctly — the comment just misleads future readers about what the fallback provides.
Suggested change
| Prefer the SDK-backed ``ZEDCamera`` (depth/imu/pointcloud); fall back to the | |
| UVC-only ``ZedSimple`` (color only) when ``pyzed`` is not installed. | |
| Prefer the SDK-backed ``ZEDCamera`` (depth/imu/pointcloud); fall back to the | |
| ``ZedSimple`` (UVC color + USB-HID IMU, no depth/pointcloud) when ``pyzed`` is not installed. |
Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!
Comment on lines
+133
to
+134
| pose_non_null = cur.execute(f'SELECT COUNT(pose_x) FROM "{name}"').fetchone()[0] | ||
| return n, t0, t1, pose_non_null |
Contributor
There was a problem hiding this comment.
COUNT(pose_x) will throw sqlite3.OperationalError: table "X" has no column named pose_x if the table was ever created without the standard dimos stream schema. There is no try/except around these two queries inside stream_rows, so any such table in the DB would cause the entire report() call to raise an exception. The outer try/except in process_db catches it, but the rec_check output is then silently skipped with a terse error message. Wrapping the second query in its own try/except sqlite3.OperationalError and defaulting to pose_non_null = 0 would make the function robust to schema variations (e.g. legacy or manually created tables).
Comment on lines
+328
to
+334
| nearest = [(1e18, None) for _ in camera_targets] # (time delta, image obs) per target | ||
| for image_obs in store.stream("color_image", Image): | ||
| for target_index, (_entity, _pose, target_ts) in enumerate(camera_targets): | ||
| delta = abs(image_obs.ts - target_ts) | ||
| if delta < nearest[target_index][0]: | ||
| nearest[target_index] = (delta, image_obs) | ||
| logged = 0 |
Contributor
There was a problem hiding this comment.
n_camera_targets grows as max_views_per_tag × n_unique_tags, so for a long recording with many tags (e.g. 10 tags × 40 views = 400 targets) and a full-rate color stream (e.g. 30 Hz × 300 s = 9 000 frames), this inner double loop performs ~3.6 M comparisons and also materialises every image from the store in memory simultaneously. Sorting or binary-searching the image timestamps per target would reduce this to O((n_images + n_targets) × log n_images).
Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!
jeff-hykin
commented
Jun 22, 2026
| base_transform: Transform | None = Field(default_factory=default_base_transform) | ||
| align_depth_to_color: bool = True | ||
| enable_depth: bool = True | ||
| enable_imu: bool = True |
Member
Author
There was a problem hiding this comment.
this weekend I did a zed recording and wanted IMU
jeff-hykin
commented
Jun 22, 2026
| _GYRO_SCALE = (1000.0 / 32768.0) * (math.pi / 180.0) # raw -> rad/s (+-1000 deg/s) | ||
|
|
||
|
|
||
| def autodetect_zed_device() -> str | None: |
Member
Author
There was a problem hiding this comment.
Treat this file as disposable. Go2 was struggling to see the april tags with limited light, did a Zed recording this weekend. ~2Gb zed SDK kept failing, this module uses the Zed without their SDK which is less overhead (e.g. good for recording) anyways.
jeff-hykin
commented
Jun 22, 2026
|
|
||
| cmd_vel: Out[Twist] | ||
|
|
||
| _go2: GO2ConnectionSpec | None = None |
Member
Author
There was a problem hiding this comment.
This should be reverted/cleaned later, just needed to be able to make the dog lie-down to save battery on the huge loop (which was right around 100% battery usage)
jeff-hykin
commented
Jun 22, 2026
| } | ||
|
|
||
|
|
||
| class KeyboardTeleopTUI(Module): |
Member
Author
There was a problem hiding this comment.
The UX on this is horrible, so hard to control the dog. Needed for when running a jetson headless and tele-oping and avoiding rerun for performance reasons.
This module should be considered disposable/scaffolding, completely vibed.
jeff-hykin
commented
Jun 22, 2026
| "mcap", | ||
| "mcap.*", | ||
| "mujoco", | ||
| "hid", |
jeff-hykin
commented
Jun 22, 2026
| def trajectory_task_name(hardware_id: str) -> TaskName: | ||
| return f"traj_{hardware_id}" | ||
| # Mid-360 mount pose on the FlowBase (position + orientation) in the base frame. | ||
| FLOWBASE_MID360_MOUNT = Pose(0.20, -0.20, 0.10, *Quaternion.from_euler(Vector3(0, 0, 0))) |
Member
Author
There was a problem hiding this comment.
will be needed to fix the flowbase blueprint(s) later
Drop ZedSimple (the import-hid module), revert ZEDCamera changes to main, and remove the ZED color/imu streams from the go2 recorder. Drop the now-orphaned hid mypy override and regenerate all_blueprints.
Comment on lines
+137
to
+154
| def odometry_travel(cur: sqlite3.Cursor) -> dict | None: | ||
| rows = cur.execute( | ||
| "SELECT pose_x, pose_y, pose_z FROM pointlio_odometry WHERE pose_x IS NOT NULL ORDER BY ts" | ||
| ).fetchall() | ||
| if not rows: | ||
| return None | ||
| xs, ys, zs = zip(*rows, strict=False) | ||
| path_length = sum(math.dist(rows[i - 1], rows[i]) for i in range(1, len(rows))) | ||
| return { | ||
| "rows": len(rows), | ||
| "start": rows[0], | ||
| "end": rows[-1], | ||
| "path_length": path_length, | ||
| "straight_line": math.dist(rows[0], rows[-1]), | ||
| "bbox_x": (min(xs), max(xs)), | ||
| "bbox_y": (min(ys), max(ys)), | ||
| "bbox_z": (min(zs), max(zs)), | ||
| } |
Contributor
There was a problem hiding this comment.
odometry_travel crashes on FastLIO recordings
odometry_travel queries FROM pointlio_odometry with no existence check. The mid360_realsense rig in this same PR (using FastLIO) produces databases without pointlio_odometry. Both summarize() (line 208) and report() (line 279) call odometry_travel with no surrounding try/except for this specific call — the outer catch in process_db swallows the entire output. Wrapping the SELECT in a try/except that returns None on OperationalError, or checking tables membership first (as stream_rows already does), would fix this.
Removes this branch's additions to the pygame KeyboardTeleop (the _go2 GO2ConnectionSpec, _call_go2_pose, Z/X liedown/standup key handlers, and help text) — reverted to main. The Z=lie-down hack was handy for saving Go2 battery on long loops; tagged RESTORE so it's easy to git-revert this commit to bring it back later.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
For Ivan
How to record map
Writes
recordings/<timestamp>/mem2.dbgo2 topics andpointlio_odometry/lidarPose values are correct.
How to view
Note this is the imperfect but okay-enough post-processing (the good one is in the next PR)
.rrd:(no arg = newest recording). Writes
recordings/<timestamp>/<timestamp>.rrd.The one that generates a pc2.lcm is in the next pr
_
For others
/24, and get the dog + your computer on the same phone hotspot if you're recording outside or away from wifi:2. Record — drive with WASD;
Ctrl+Cto stop:Writes
recordings/<timestamp>/mem2.db. Point-LIO odometry + Mid-360 cloud + camera, with the pose baked into each lidar frame.3. Post-process — AprilTag-corrected ground-truth + a Rerun
.rrd:(no arg = newest recording). Writes
recordings/<timestamp>/<timestamp>.rrd.4. Look at it:
Full walkthrough:
docs/capabilities/navigation/recording_a_map.md.