feat(recording): Go2 + Mid-360 Point-LIO map recording#2557
Open
jeff-hykin wants to merge 214 commits into
Open
feat(recording): Go2 + Mid-360 Point-LIO map recording#2557jeff-hykin wants to merge 214 commits into
jeff-hykin wants to merge 214 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.
…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 SummaryAdds end-to-end map recording for Go2 + Livox Mid-360 using Point-LIO odometry, with a companion mid360_realsense rig, offline post-processing, AprilTag-corrected ground-truth, and a multi-map anchor tool for cross-recording alignment.
Confidence Score: 3/5Two concrete defects need fixing before this is reliable on hardware: a crash in RealSense camera start and a NIC mismatch that silently prevents Point-LIO from receiving lidar data on the Go2 rig. The RealSense IMU initialisation crashes with a bare StopIteration if the device's accel profile isn't found in the extrinsics graph — the IMU pipeline is already running when the exception propagates, leaving the camera in a partially-started state. Separately, go2_mid360/record.py passes host_ip=_LIDAR_HOST_IP (default 192.168.1.100) to Mid360 but no host_ip to PointLio, which falls back to the C++ binary default 192.168.1.5. On a recording machine whose NIC is at .100, Point-LIO silently binds the wrong interface and produces no odometry. dimos/hardware/sensors/camera/realsense/camera.py (_start_imu extrinsics query) and dimos/mapping/recording/go2_mid360/record.py (PointLio host_ip) Important Files Changed
Flowchart%%{init: {'theme': 'neutral'}}%%
flowchart TD
subgraph Record["record.py (runtime)"]
Mid360["Mid360\n(raw livox + IMU)"]
PointLio["PointLio\n(SLAM odometry + cloud)"]
RealSense["RealSenseCamera\n(color + depth + IMU)"]
Go2["GO2Connection\n(go2_lidar + go2_odom)"]
Recorder["Go2Recorder / RealsenseRecorder\n(mem2.db)"]
Mid360 -->|livox_lidar, livox_imu| Recorder
PointLio -->|pointlio_odometry, pointlio_lidar| Recorder
RealSense -->|color_image, realsense_imu, ...| Recorder
Go2 -->|go2_lidar, go2_odom| Recorder
end
subgraph PostProcess["post_process.py (offline)"]
DB["mem2.db"]
Tags["detect_apriltags\n(apriltags.py)"]
GTSAM["build_gtsam_gt\n(gtsam_gt.py)"]
RRD["build_rrd\n(build_rrd.py)"]
DB --> Tags --> GTSAM --> RRD
end
subgraph Anchor["multi_map_anchor.py (offline)"]
AnchorDB["mid360_realsense\nmem2.db"]
TargetDB["go2_mid360\nmem2.db"]
Kabsch["Kabsch alignment\n(shared AprilTags)"]
CombinedRRD["combined .rrd"]
AnchorDB --> Kabsch
TargetDB --> Kabsch
Kabsch --> CombinedRRD
end
Recorder --> DB
PostProcess --> Anchor
%%{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"}}}%%
flowchart TD
subgraph Record["record.py (runtime)"]
Mid360["Mid360\n(raw livox + IMU)"]
PointLio["PointLio\n(SLAM odometry + cloud)"]
RealSense["RealSenseCamera\n(color + depth + IMU)"]
Go2["GO2Connection\n(go2_lidar + go2_odom)"]
Recorder["Go2Recorder / RealsenseRecorder\n(mem2.db)"]
Mid360 -->|livox_lidar, livox_imu| Recorder
PointLio -->|pointlio_odometry, pointlio_lidar| Recorder
RealSense -->|color_image, realsense_imu, ...| Recorder
Go2 -->|go2_lidar, go2_odom| Recorder
end
subgraph PostProcess["post_process.py (offline)"]
DB["mem2.db"]
Tags["detect_apriltags\n(apriltags.py)"]
GTSAM["build_gtsam_gt\n(gtsam_gt.py)"]
RRD["build_rrd\n(build_rrd.py)"]
DB --> Tags --> GTSAM --> RRD
end
subgraph Anchor["multi_map_anchor.py (offline)"]
AnchorDB["mid360_realsense\nmem2.db"]
TargetDB["go2_mid360\nmem2.db"]
Kabsch["Kabsch alignment\n(shared AprilTags)"]
CombinedRRD["combined .rrd"]
AnchorDB --> Kabsch
TargetDB --> Kabsch
Kabsch --> CombinedRRD
end
Recorder --> DB
PostProcess --> Anchor
Reviews (5): Last reviewed commit: "merge" | 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.
Comment on lines
+218
to
+224
| accel_stream = next( | ||
| profile | ||
| for sensor in self._profile.get_device().query_sensors() | ||
| for profile in sensor.get_stream_profiles() | ||
| if profile.stream_type() == rs.stream.accel | ||
| ) | ||
| self._depth_to_imu_extrinsics = depth_stream.get_extrinsics_to(accel_stream) |
Contributor
There was a problem hiding this comment.
StopIteration crash when accel profile not found
next() with no default raises StopIteration if no sensor on the device exposes an rs.stream.accel profile. This exception is outside the surrounding try/except RuntimeError block, so it propagates up through _start_imu() into start(), crashing camera initialisation with a confusing traceback. The IMU pipeline has already been started and stored in self._imu_pipeline at this point, so the state is partially initialised when the crash occurs. Using next(..., None) and guarding the extrinsics call prevents this.
Suggested change
| accel_stream = next( | |
| profile | |
| for sensor in self._profile.get_device().query_sensors() | |
| for profile in sensor.get_stream_profiles() | |
| if profile.stream_type() == rs.stream.accel | |
| ) | |
| self._depth_to_imu_extrinsics = depth_stream.get_extrinsics_to(accel_stream) | |
| accel_stream = next( | |
| ( | |
| profile | |
| for sensor in self._profile.get_device().query_sensors() | |
| for profile in sensor.get_stream_profiles() | |
| if profile.stream_type() == rs.stream.accel | |
| ), | |
| None, | |
| ) | |
| if accel_stream is not None: | |
| self._depth_to_imu_extrinsics = depth_stream.get_extrinsics_to(accel_stream) | |
| else: | |
| print("RealSense: no accel profile found in device graph; IMU->depth TF will be skipped") |
leshy
reviewed
Jun 22, 2026
Member
|
blueprint plz :) remove post processing |
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.