feat(pointlio): rate-independent pcap_to_db replay

dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp

@@ -53,8 +53,26 @@ static std::atomic<bool> 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<uint64_t> 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<double>( std::chrono::system_clock::now().time_since_epoch()).count();
+    if (!g_ts_offset_set) {
+        const double system_now = std::chrono::duration<double>(
+            std::chrono::system_clock::now().time_since_epoch()).count();
+        g_ts_offset = system_now - static_cast<double>(g_publish_pkt_ns) / 1e9;
+        g_ts_offset_set = true;
+    }
+    return static_cast<double>(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<const LivoxLidarCartesianHighRawPoint*>(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<int64_t>(1e6 / g_frequency));
     std::optional<std::chrono::steady_clock::time_point> last_emit;
+    std::optional<std::chrono::steady_clock::time_point> last_wall_emit;
     const double process_period_ms = 1000.0 / main_freq;
 
     auto pc_interval = std::chrono::microseconds( static_cast<int64_t>(1e6 / pointcloud_freq));
@@ -441,35 +467,89 @@ int main(int argc, char** argv) {
     std::optional<std::chrono::steady_clock::time_point> 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
         // Point-LIO. Hold g_pc_mutex across the rate-limit check AND swap so the
         // clock + accumulator are observed atomically (no packet slips between).
         std::vector<custom_messages::CustomPoint> 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<std::mutex> 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<double>(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<double>(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<std::mutex> lio_lock(g_lio_mutex);
@@ -494,28 +574,30 @@ 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.
             if (lidar_due) {
                 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]);
                     }
                 }
             }
 
             // 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::steady_clock::duration>(
+                std::chrono::nanoseconds(data_ns))));
 
         lcm.handleTimeout(0);
 

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)