Root-caused the SLAM divergence that has been blocking real-robot runs. It is
not the environment, as previously assumed — it is a missed compute deadline.
Fixed and verified. Separately, both sensor drivers now run onboard the
robot's Jetson in a single container, which also resolved a driver failure
that had blocked that path since late July.
SLAM divergence: cause and fix
Turning the robot would occasionally send the position estimate into an
unrecoverable runaway — reporting 84 m/s and a 190 m path inside a small pit.
This had been attributed to featureless walls confusing the LiDAR.
Recorded runs show otherwise. The estimator was taking longer to process each
scan than the interval between scans, so its input queue aged to 2.5 seconds.
Processing a 2.5 s old scan is harmless standing still, but while turning at
75°/s it means matching a scan against a pose that has since rotated up to
186°. Matching then fails, bad poses enter the map, and the map bloat makes
processing slower still.
The decisive evidence: two stationary runs with worse queue backlogs
did not diverge, while the run with the smallest backlog did — the difference
was rotation.
The deadline was being missed because the laptop sat in a power-saving CPU
mode and the estimator's critical path is single-threaded. Correcting the
power mode and keeping that thread on the fast cores:
Before
After
Frame processing time
103 → 428 ms
47 ms, flat
Input queue age
264 → 2655 ms
240 ms, flat
Matching failures
2901
0
Reported path (in a small pit)
190 m
18.8 m
Confirmed by repeating the exact manoeuvre that used to break it — 60 s of
turning and driving — with zero matching failures.
After the fix: a coherent map of the room, with the robot's
18.8 m of travel contained in a 3.8 × 2.2 m area. Before, the same run
produced a 190 m path through a space that does not exist.
Current limitations
Verified while driving manually; not yet tested with the autonomous
planner also competing for CPU.
The power mode is set by hand and resets on reboot. The launch now
prints the preconditions, but does not enforce them — what "performance
mode" means differs between the laptop and the onboard computers.
Sensors moved onboard
The depth camera and LiDAR drivers now both run on the robot's Jetson in one
ROS 2 container, so every sensor is timestamped by the same clock — a
prerequisite for fusing camera and LiDAR later. The workstation receives all
three streams at full rate with no loss over the network.
This also cleared a failure from late July, where the LiDAR driver crashed on
the Jetson and delivered nothing. It was not a hardware or memory problem, as
suspected at the time — a clean rebuild in the container runs without fault,
and that deployment path is now validated for the first time.
One further defect found and fixed along the way: the two computers' clocks
were 302 ms apart, which showed up as odometry consistently trailing the
robot's true position by roughly 15 cm.
Current limitations
The camera is streaming but not yet used by anything — it is
groundwork for camera-LiDAR fusion, not a working feature.
Camera-to-LiDAR calibration data has been captured but not yet solved.
Next
Re-test the compute headroom with the autonomous planner running.
Solve the camera-LiDAR calibration from the captured data.
Camera-LiDAR fusion (FAST-LIVO2) now sequenced after the compute
work rather than before it: it is more expensive than what already
struggled to meet its deadline, so a camera cannot compensate for a
compute shortfall.
