diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 9c7bcb3714..4dee760e32 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -53,8 +53,26 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static PointLio* g_point_lio = nullptr; +// Sensor→system time anchor, locked once at the first published frame: +// g_ts_offset = system_now − first_frame_sensor_time. Every output is then +// stamped (frame_sensor_time + g_ts_offset), so timestamps follow the data's +// real cadence no matter how fast packets are fed. On live hardware sensor time +// already advances in real time, so the offset is effectively constant and this +// is indistinguishable from wall-clock — Point-LIO and downstream consumers +// behave identically online and offline. +static std::atomic g_latest_pkt_ns{0}; // newest point-packet sensor ts (also the main loop's data clock) +static uint64_t g_publish_pkt_ns = 0; // sensor ts of the latest drained frame (main thread) +static double g_ts_offset = 0.0; +static bool g_ts_offset_set = false; + static double get_publish_ts() { - return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); + if (!g_ts_offset_set) { + const double system_now = std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()).count(); + g_ts_offset = system_now - static_cast(g_publish_pkt_ns) / 1e9; + g_ts_offset_set = true; + } + return static_cast(g_publish_pkt_ns) / 1e9 + g_ts_offset; } // Parse a comma-separated list of doubles (CLI vector args); empty on bad input. @@ -203,6 +221,9 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ g_frame_start_ns = ts_ns; g_frame_has_timestamp = true; } + if (ts_ns > g_latest_pkt_ns.load(std::memory_order_relaxed)) { + g_latest_pkt_ns.store(ts_ns, std::memory_order_relaxed); + } if (data->data_type == DATA_TYPE_CARTESIAN_HIGH) { auto* pts = reinterpret_cast(data->data); @@ -429,10 +450,15 @@ int main(int argc, char** argv) { g_point_lio = &point_lio; if (debug) { printf("[pointlio] Point-LIO initialized.\n"); } - // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced - // main thread. + // Main-loop state. Body lives in `run_main_iter`. The emission bookmarks + // (last_emit/last_pc_publish/last_odom_publish) live on a DATA-time grid — they + // advance by how far the packet stream's sensor time has progressed, not by the + // wall clock — so a frame always spans one frame_interval of data regardless of + // replay speed. last_wall_emit is the one wall-time bookmark, used only by the + // overload check (which is inherently about real elapsed time). auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); std::optional last_emit; + std::optional last_wall_emit; const double process_period_ms = 1000.0 / main_freq; auto pc_interval = std::chrono::microseconds( static_cast(1e6 / pointcloud_freq)); @@ -441,17 +467,20 @@ int main(int argc, char** argv) { std::optional last_odom_publish; - auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { - // Lazy-seed rate-limit bookmarks on the first iteration so they align - // with the wall clock. - if (!last_emit.has_value()) { - last_emit = now; - } - if (!last_pc_publish.has_value()) { - last_pc_publish = now; - } - if (!last_odom_publish.has_value()) { - last_odom_publish = now; + // data_now is a clock built from the newest packet's sensor time (see the call + // site). All emission gating advances on it; wall_now is read here and used only + // for the overload measurement below. + auto run_main_iter = [&](std::chrono::steady_clock::time_point data_now) { + const auto wall_now = std::chrono::steady_clock::now(); + const bool have_data_clock = data_now.time_since_epoch().count() != 0; + + // Seed the data-time bookmarks from the first packet — never from 0, or the + // grid would try to "replay" the entire sensor epoch to catch up. + if (have_data_clock && !last_emit.has_value()) { + last_emit = data_now; + last_wall_emit = wall_now; + last_pc_publish = data_now; + last_odom_publish = data_now; } // At frame rate: drain accumulated points into a CustomMsg and feed @@ -459,17 +488,68 @@ int main(int argc, char** argv) { // clock + accumulator are observed atomically (no packet slips between). std::vector points; uint64_t frame_start = 0; - { + uint64_t backlog_ns = 0; + double wall_elapsed_s = 0.0; + bool measured_backlog = false; + if (have_data_clock) { std::lock_guard lock(g_pc_mutex); - if (now - *last_emit >= frame_interval) { + if (data_now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); frame_start = g_frame_start_ns; + // Sensor-time advanced since the previous drain (= span of data + // this single frame swallows). Skip the first drain, where + // g_publish_pkt_ns is still 0. + const uint64_t latest = g_latest_pkt_ns.load(std::memory_order_relaxed); + if (g_publish_pkt_ns != 0) { + backlog_ns = latest - g_publish_pkt_ns; + wall_elapsed_s = std::chrono::duration(wall_now - *last_wall_emit).count(); + measured_backlog = true; + } + g_publish_pkt_ns = latest; g_frame_has_timestamp = false; } - last_emit = now; + // Advance on a fixed data-time grid; snap forward only if the data + // clock leapt several frames (a stall) so we don't spin catching up. + *last_emit += frame_interval; + if (data_now - *last_emit >= frame_interval) { last_emit = data_now; } + last_wall_emit = wall_now; } } + + // Overload guard. backlog_ns is how much sensor-time this frame had to + // ingest; wall_elapsed_s is the real time it covered. Their ratio is how + // much faster than real-time packets are arriving. Live hardware sits at + // ~1; a fast replay (or a flooded link) pushes it up, and once the feed + // outpaces the fixed-rate drain the kernel UDP buffer overflows and drops + // packets — gappy LiDAR/IMU input is exactly what makes Point-LIO diverge. + // Surface it (throttled) so the overload is observable instead of silent. + if (measured_backlog && wall_elapsed_s > 0.0) { + const double overload_warn_ratio = 1.5; + const int warn_throttle_sec = 10; + const double realtime_ratio = (static_cast(backlog_ns) / 1e9) / wall_elapsed_s; + + // Overload happens every 10 Hz frame while a fast feed lasts, so warning + // per-frame is pure noise. Instead track the worst frame in each throttle + // window and emit one line for it — collapsing hundreds of lines to a few + // while still surfacing the spikes (stall-induced pile-ups) that matter + // most. Epoch-init last_warn (not ::min(), which overflows wall_now - last_warn) + // so the first overload reports immediately. + static std::chrono::steady_clock::time_point last_warn{}; + static double window_peak_ratio = 0.0; + if (realtime_ratio > window_peak_ratio) { window_peak_ratio = realtime_ratio; } + if (window_peak_ratio > overload_warn_ratio && + wall_now - last_warn >= std::chrono::seconds(warn_throttle_sec)) { + fprintf(stderr, + "[pointlio] WARNING: high risk of odom drift - pointlio can't " + "process fast enough. Try reducing CPU load. (input up to %.1fx " + "real-time over last %ds)\n", + window_peak_ratio, warn_throttle_sec); + last_warn = wall_now; + window_peak_ratio = 0.0; + } + } + // Serialize EKF access against the SDK IMU callback (on_imu_data) for the // rest of the iteration — feed_lidar/process/get_* all touch the estimator. std::lock_guard lio_lock(g_lio_mutex); @@ -494,10 +574,10 @@ int main(int argc, char** argv) { point_lio.process(); auto pose = point_lio.get_pose(); - if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { + if (have_data_clock && !pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); - const bool lidar_due = !g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval; + const bool lidar_due = !g_lidar_topic.empty() && data_now - *last_pc_publish >= pc_interval; // get_body_cloud is the loop's costliest step, so build it only when // a publish is due. @@ -505,7 +585,8 @@ int main(int argc, char** argv) { auto body_cloud = point_lio.get_body_cloud(); if (body_cloud && !body_cloud->empty()) { publish_lidar(body_cloud, ts); - last_pc_publish = now; + *last_pc_publish += pc_interval; + if (data_now - *last_pc_publish >= pc_interval) { last_pc_publish = data_now; } if (pointlio_debug) { fprintf(stderr, "[pointlio] publish lidar: %zu points pose=(%.3f, %.3f, %.3f)\n", body_cloud->size(), pose[0], pose[1], pose[2]); } @@ -513,9 +594,10 @@ int main(int argc, char** argv) { } // Pose + covariance at odom_freq. - if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { + if (!g_odometry_topic.empty() && data_now - *last_odom_publish >= odom_interval) { publish_odometry(point_lio.get_odometry(), ts); - last_odom_publish = now; + *last_odom_publish += odom_interval; + if (data_now - *last_odom_publish >= odom_interval) { last_odom_publish = data_now; } if (pointlio_debug) { fprintf(stderr, "[pointlio] publish odom: pose=(%.3f, %.3f, %.3f)\n", pose[0], pose[1], pose[2]); } @@ -540,7 +622,12 @@ int main(int argc, char** argv) { while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); - run_main_iter(std::chrono::steady_clock::now()); + // Data clock: the newest packet's sensor time. Advances in real time on live + // hardware, at --rate on replay, and freezes when the stream drains. + const uint64_t data_ns = g_latest_pkt_ns.load(std::memory_order_relaxed); + run_main_iter(std::chrono::steady_clock::time_point( + std::chrono::duration_cast( + std::chrono::nanoseconds(data_ns)))); lcm.handleTimeout(0); diff --git a/dimos/hardware/sensors/lidar/pointlio/replay_static_tf.py b/dimos/hardware/sensors/lidar/pointlio/replay_static_tf.py new file mode 100644 index 0000000000..b3e986fb59 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/replay_static_tf.py @@ -0,0 +1,95 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Static tf edges a Go2 + Mid-360 pcap replay needs on top of PointLio's odometry. + +PointLio already rebroadcasts its odometry as the moving ``odom -> body`` tf edge, +so a replay only lacks the fixed edges: the ``map -> odom`` root (identity until a +downstream PGO overwrites it with the loop-closure correction), the ``body -> +base_link`` bridge (Point-LIO's body frame and the rig's base_link are the same +physical frame), and the Go2/Mid-360 sensor mounts. Publishing all of these lets a +recorder capture a fully connected ``map -> odom -> body(=base_link) -> front_camera +-> {mid360_link, camera_optical}`` tree. + +The stock ``StaticTfPublisher`` stamps each cycle with wall-clock ``time.time()``. +Under a rate-scaled replay that clock diverges from PointLio's data-time odometry +stamps, so the static edges would only cover the wall duration of the run and leave +the tree disconnected across the rest of the data-time trajectory. Instead this +module re-stamps the fixed edges off the odometry stream, so they track the exact +data-time span PointLio produces (throttled so consecutive edges stay well inside a +tf buffer's match tolerance). +""" + +from __future__ import annotations + +from pydantic import Field + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.navigation.cmu_nav.frames import FRAME_BODY, FRAME_MAP, FRAME_ODOM +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms +from dimos.robot.unitree.go2.go2_mid360_static_transforms import FRAMES + +# The rig's base_link and Point-LIO's body frame are the same physical frame. +FRAME_BASE_LINK = "base_link" + + +def _identity_edge(parent: str, child: str) -> Transform: + return Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id=parent, + child_frame_id=child, + ) + + +def _replay_static_edges() -> list[Transform]: + return [ + *frames_to_edge_transforms(FRAMES), + _identity_edge(FRAME_MAP, FRAME_ODOM), + _identity_edge(FRAME_BODY, FRAME_BASE_LINK), + ] + + +class Go2Mid360ReplayStaticTfConfig(ModuleConfig): + # Min data-time gap between republishing the static edges; keep well under a tf + # buffer's match tolerance so any query lands next to a stamped edge. + republish_min_interval_s: float = Field(default=0.2, gt=0.0) + + +class Go2Mid360ReplayStaticTf(Module): + """Republishes the Go2/Mid-360 mount tree plus the two identity edges PointLio's + odom tf omits, stamped on the odometry data clock.""" + + config: Go2Mid360ReplayStaticTfConfig + + odometry: In[Odometry] + + _edges: list[Transform] = [] + _last_emit_ts: float | None = None + + async def handle_odometry(self, value: Odometry) -> None: + if not self._edges: + self._edges = _replay_static_edges() + if self._last_emit_ts is not None: + if value.ts - self._last_emit_ts < self.config.republish_min_interval_s: + return + self._last_emit_ts = value.ts + for edge in self._edges: + edge.ts = value.ts + self.tf.publish(*self._edges) diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index e362a34461..bf173c684f 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -52,8 +52,8 @@ # Poll the db on this cadence while the replay drains the pcap. _POLL_SEC = 1.0 -# Stop after the odom stream has been stagnant this long (pcap fully drained). -_STAGNANT_SEC = 5.0 +# Stop this long after the lidar stream stops advancing (pcap fully drained). +_STAGNANT_SEC = 1.0 # No odometry within this long after start = Point-LIO failed to come up (missing # artifact, bad pcap, SLAM-init crash); bounds the poll loop. Generous to cover # Point-LIO's IMU-init latency. @@ -219,6 +219,18 @@ def _odom_stats(db_path: Path, table: str) -> tuple[int, float, float]: con.close() +def _stream_exists(db_path: Path, name: str) -> bool: + """True if a memory2 stream of this name is registered in the db.""" + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + row = con.execute("SELECT 1 FROM _streams WHERE name=?", (name,)).fetchone() + return row is not None + except sqlite3.OperationalError: + return False + finally: + con.close() + + def _quat_to_rot(qx: float, qy: float, qz: float, qw: float) -> Any: import numpy as np @@ -323,6 +335,7 @@ def _build_blueprint( from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder + from dimos.hardware.sensors.lidar.pointlio.replay_static_tf import Go2Mid360ReplayStaticTf from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 pointlio_kwargs: dict[str, Any] = dict( @@ -344,7 +357,20 @@ def _build_blueprint( setup_network=not args.no_network_setup, ), PointLio.blueprint(**pointlio_kwargs), - PointlioRecorder.blueprint(db_path=str(db_path)), + # Supplies the fixed tf edges PointLio's odom->body rebroadcast omits, so + # the recorded tree is map->odom->body(=base_link)->front_camera->{...}. + Go2Mid360ReplayStaticTf.blueprint(), + PointlioRecorder.blueprint( + db_path=str(db_path), + stream_remapping={ + _ODOM_STREAM: args.odom_stream, + _LIDAR_STREAM: args.lidar_stream, + }, + # PointLio rebroadcasts odometry as tf and Go2Mid360ReplayStaticTf + # supplies the static edges, so record the full tf tree. + record_tf=True, + skip_ports=frozenset({_LIDAR_STREAM}) if args.no_lidar else frozenset(), + ), ) .remappings( [ @@ -356,18 +382,17 @@ def _build_blueprint( ) -def _poll_until_drained( - db_path: Path, odom_stream: str, lidar_stream: str, max_sensor_sec: float -) -> bool: +def _poll_until_drained(db_path: Path, odom_stream: str, max_sensor_sec: float) -> bool: """Block until the pcap drains or a cap is hit; False if Point-LIO never produced odometry within the startup timeout. - Drain is detected on the *lidar* stream's latest timestamp going flat: lidar - is input-driven, so it stops advancing the moment the pcap is exhausted. The - odometry stream can't be used for this — Point-LIO keeps publishing odometry - (dead-reckoning) at odom_freq after input stops, with ever-advancing - timestamps, so its stream never looks stagnant and the run would hang.""" - last_lidar_max: float | None = None + Drain is detected on the *odometry* stream's latest timestamp going flat. + Output timestamps are derived from the packet sensor time of the most + recently drained frame, so once the pcap is exhausted no new frames drain + and odom_max stops advancing — even though odometry messages keep being + published. (This replaces the old lidar-based detection, which is gone now + that the lidar stream is optional.)""" + last_odom_max: float | None = None first_max: float | None = None stagnant_since: float | None = None start_time = time.time() @@ -391,16 +416,13 @@ def _poll_until_drained( if max_sensor_sec > 0 and (odom_max - first_max) >= max_sensor_sec: print(f"[pcap_to_db] reached --max-sensor-sec={max_sensor_sec:.1f}s", flush=True) return True - _, _, lidar_max = _odom_stats(db_path, lidar_stream) - if lidar_max <= 0.0: - continue - if lidar_max == last_lidar_max: + if odom_max == last_odom_max: if stagnant_since is None: stagnant_since = time.time() elif time.time() - stagnant_since > _STAGNANT_SEC: return True else: - last_lidar_max = lidar_max + last_odom_max = odom_max stagnant_since = None @@ -460,8 +482,26 @@ def _run(args: argparse.Namespace) -> int: return 2 pcap_path = pcap_path.resolve() args.pcap_path = pcap_path + suffix = f"__{args.run_name}" if args.run_name else "" + args.odom_stream = f"{_ODOM_STREAM}{suffix}" + args.lidar_stream = f"{_LIDAR_STREAM}{suffix}" db_path = _resolve_db_path(args, pcap_path) db_path.parent.mkdir(parents=True, exist_ok=True) + + # Never clobber existing data unless explicitly asked: refuse if a target + # stream is already present (and tf is left alone — see record_tf=False below). + if db_path.exists() and not args.replace: + targets = [args.odom_stream] + ([] if args.no_lidar else [args.lidar_stream]) + clashing = [name for name in targets if _stream_exists(db_path, name)] + if clashing: + print( + f"[pcap_to_db] refusing to overwrite existing stream(s): " + f"{', '.join(clashing)}. Pass --run-name for a fresh name, or " + f"--replace to overwrite.", + file=sys.stderr, + flush=True, + ) + return 2 overrides = _load_overrides(args.config) overrides.update(_cli_overrides(args)) # --tuning flags win over --config @@ -478,6 +518,7 @@ def _run(args: argparse.Namespace) -> int: print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " f"({'append' if db_path.exists() else 'new'}) rate={args.rate} " + f"streams={args.odom_stream}/{args.lidar_stream} " f"ips={args.host_ip}/{args.lidar_ip} stop_at={max_sensor_sec or 'drain'}", flush=True, ) @@ -485,12 +526,12 @@ def _run(args: argparse.Namespace) -> int: coord = None try: coord = ModuleCoordinator.build(_build_blueprint(args, db_path, overrides)) - drained = _poll_until_drained(db_path, _ODOM_STREAM, _LIDAR_STREAM, max_sensor_sec) + drained = _poll_until_drained(db_path, args.odom_stream, max_sensor_sec) finally: if coord is not None: coord.stop() - o_cnt, o_min, o_max = _odom_stats(db_path, _ODOM_STREAM) + o_cnt, o_min, o_max = _odom_stats(db_path, args.odom_stream) if o_cnt == 0 or not drained: print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) return 1 @@ -500,7 +541,7 @@ def _run(args: argparse.Namespace) -> int: ) if not args.no_rrd: try: - rrd = _write_rrd(db_path, _ODOM_STREAM, _LIDAR_STREAM, args.voxel) + rrd = _write_rrd(db_path, args.odom_stream, args.lidar_stream, args.voxel) if rrd is not None: print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) except Exception as exc: # viz is a non-fatal bonus @@ -520,6 +561,26 @@ def main(argv: list[str]) -> int: parser.add_argument( "--rate", type=float, default=1.0, help="replay-speed multiplier (default 1.0)" ) + parser.add_argument( + "--run-name", + default="", + help="suffix the recorded streams as pointlio_odometry__ / " + "pointlio_lidar__, so several runs can be added to one db side by " + "side (e.g. --run-name rate16). Omit for the bare pointlio_odometry / " + "pointlio_lidar names.", + ) + parser.add_argument( + "--no-lidar", + action="store_true", + help="don't record the lidar stream (odometry only) — keeps the db small " + "when you only need the trajectory.", + ) + parser.add_argument( + "--replace", + action="store_true", + help="overwrite target streams if they already exist in the db. Default is " + "to refuse, so an existing run is never clobbered.", + ) parser.add_argument( "--odom-freq", type=float, default=30.0, help="Point-LIO odometry rate Hz (default 30)" ) diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index e16c1527e7..edbc55b1a8 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -272,6 +272,8 @@ class RecorderConfig(MemoryModuleConfig): # read the active remappings from inside the module (AFAIK), so this config # arg does the per-stream rename directly. stream_remapping: dict[str, str] = Field(default_factory=dict) + # In-port names to leave unrecorded (e.g. skip a bulky stream you don't need). + skip_ports: frozenset[str] = Field(default_factory=frozenset) PoseSetter = Callable[[Any], "Awaitable[Pose | None]"] @@ -358,6 +360,8 @@ def start(self) -> None: return for name, port in self.inputs.items(): + if name in self.config.skip_ports: + continue stream_name = self.config.stream_remapping.get(name, name) stream: Stream[Any] = self.store.stream(stream_name, port.type) self._port_to_stream(name, port, stream) @@ -399,7 +403,11 @@ def _prepare_streams(self) -> None: of duplicating, while leaving any other streams in the db untouched.""" if self.config.on_existing is not OnExisting.APPEND: return - targets = {self.config.stream_remapping.get(name, name) for name in self.inputs} + targets = { + self.config.stream_remapping.get(name, name) + for name in self.inputs + if name not in self.config.skip_ports + } if self.config.record_tf: targets.add("tf") for stream in targets.intersection(self.store.list_streams()):