Sensing · Chapter 16 · Time: 3 hours, plus a floor session after chapter 19 · Level: Advanced · Status: Partly test-built
The EKF that turns commanded wheel velocity and the gyro into the odom → base_link transform at 50 Hz, LiDAR odometry (rf2o) built from source with its sign fix, and camera odometry (cuVSLAM v17) running as its own service, with what each was measured to get right and what is still unproven.
Between two looks at the map, the robot has to know how far it moved. Most robots count wheel turns with encoders.
This one cannot: its controller board has no encoders and reports nothing about the motors. So the robot uses what
it told the wheels to do, corrects the turning part with the gyro, and runs two independent measurements beside that
guess: LiDAR scan matching (rf2o) and camera tracking (cuVSLAM). Today only the guess plus the gyro drive the robot's
odometry. The two measurements are used to catch the guess lying, mainly when a wheel stalls.
New idea: odometry and drift
Odometry is the robot's own running estimate of where it is relative to where it started, built only from
its own motion sensors, with no map. It is published as the transformodom→base_link: "since I started,
I have moved this far and turned this much". It is smooth and fast but it drifts: every small error is added
to the next. On this robot, before the gyro bias fix of 2026-09-27, the heading of a robot standing completely
still had drifted to 11.4° eight minutes after a restart. Drift is why the map (chapter 17) has to correct
odometry all the time.
New idea: covariance
Every odometry message carries, next to each number, how much to trust it: a covariance, which is the
square of the expected error. The wheel odometry here says its speed is good to about 0.05 m/s, so its
covariance is 0.05² = 0.0025. The gyro's turning rate is good to 0.0012 rad/s (measured at rest), so its
covariance is 0.0012². A covariance of 1000 is so large that the number is effectively ignored. Programs that
combine sources weigh each number by these values.
New idea: sensor fusion with an EKF
An extended Kalman filter (EKF) keeps one best estimate of the robot's motion and updates it every time a
sensor reports, trusting each input according to its covariance. You tell it which fields of which input to
use. The one here comes from therobot_localizationpackage. It runs 50 times a second and publishes the
result as/odometry/filteredand as theodom→base_linktransform.
wheel/odom is the velocity the driver commanded, after its limits, ramps and deadman, and zero when thewheel/odom drives away while the robot does not move. The gyro is honest: in the stand test below the wheelsYou built this in chapter 11. Here is the part that matters for odometry, from
ros2/rosorin_base/rosorin_base/board_driver.py. The parameters:
On the robot:
# no wheel encoders (board protocol has no feedback): wheel/odom = gated COMMANDED velocity
self.odom_frame = dp('odom_frame_id', 'odom').value
self.base_frame = dp('base_frame_id', 'base_link').value
self.odom_lin_var = dp('odom_linear_std', 0.05).value ** 2
self.odom_ang_var = dp('odom_angular_std', 0.2).value ** 2
# commanded -> actual body velocity (floor, LiDAR scan match, docs/odometry.md):
# strafe 0.79/0.78 left, 0.73/0.76 right -> mean 0.765; forward ~1.02 (1 run, not applied)
self.odom_scale_x = dp('odom_scale_x', 1.0).value
self.odom_scale_y = dp('odom_scale_y', 0.765).value
and the publisher, called from the control loop with the velocity the gate let through:
On the robot:
def publish_wheel_odom(self, now, vx, vy, wz):
vx, vy = vx * self.odom_scale_x, vy * self.odom_scale_y # odometry only; motors get the command
dt = 0.0 if self.odom_t is None else min(now - self.odom_t, 0.1)
self.odom_t = now
x, y, th = self.odom_pose = integrate_pose(*self.odom_pose, vx, vy, wz, dt)
m = Odometry()
m.header.stamp = self.get_clock().now().to_msg()
m.header.frame_id, m.child_frame_id = self.odom_frame, self.base_frame
m.pose.pose.position.x, m.pose.pose.position.y = x, y
m.pose.pose.orientation.z, m.pose.pose.orientation.w = math.sin(th / 2), math.cos(th / 2)
m.twist.twist.linear.x, m.twist.twist.linear.y, m.twist.twist.angular.z = vx, vy, wz
for i, v in ((0, self.odom_lin_var), (7, self.odom_lin_var), (14, 1e-6), (21, 1e-6), (28, 1e-6),
(35, self.odom_ang_var)):
m.twist.covariance[i] = v
m.pose.covariance[i] = 1e3 if i in (0, 7, 35) else 1e-6 # pose is dead reckoning: not trusted
self.odom_pub.publish(m)
Three things to notice:
odom → base_link.Chapter 9 installs it with the other ROS packages. Check, and install only if it is missing:
On the robot:
dpkg -l ros-humble-robot-localization | tail -1
sudo apt-get install -y --no-install-recommends ros-humble-robot-localization
Check
The install on 2026-09-28 added 6 packages andros-humble-robot-localization 3.5.4-1jammy.20260908.051527.
dpkg -lshows it withiiin front.
Safety
If apt suggestsapt autoremoveafterwards, do not run it (chapter 1: it offers to removeinitramfs-tools,
which the Jetson needs to boot).
Create ~/ros2_ws/src/rosorin_base/config/ekf.yaml in three parts.
First, the frames and the rate. two_d_mode tells the filter the robot lives on a flat floor: it ignores height,
roll and pitch. world_frame: odom makes it publish odom → base_link (the other choice, map, is for a filter
that also takes map positions; here slam_toolbox publishes map → odom itself):
On the robot:
# robot_localization EKF: odom -> base_link. 2D.
# Inputs: wheel/odom twist (vx, vy) = gated COMMANDED velocity (no wheel encoders on this board),
# imu/data_raw gyro z (yaw rate). Commanded yaw rate is not fused: the gyro measures it.
# Absolute drift correction (map -> odom) comes later from scan matching / SLAM.
ekf_filter_node:
ros__parameters:
frequency: 50.0
two_d_mode: true
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
Second, the wheel input. Each input gets a table of 15 true/false values, in this fixed order: position x, y, z,
roll, pitch, yaw; velocity vx, vy, vz, vroll, vpitch, vyaw; acceleration ax, ay, az. From the wheels the filter
takes only vx and vy, the second row:
On the robot:
# Verified nav baseline (docs/navigation.md, 2026-09-28): wheel/odom = gated commanded velocity, AMCL corrects drift.
# 2026-09-30 a switch to LiDAR odometry (odom_lidar from rf2o_fix) was REVERTED (owner): not yet proven in isolation.
# rf2o + rf2o_fix still run in base.launch, but nothing consumes it until it is verified.
odom0: wheel/odom
# x y z roll pitch yaw
# vx vy vz vroll vpitch vyaw
# ax ay az
odom0_config: [false, false, false, false, false, false,
true, true, false, false, false, false,
false, false, false]
odom0_differential: false
Third, the gyro. From the IMU the filter takes only vyaw, the turning rate: not the accelerations, and not an
orientation (the driver publishes none; its orientation covariance is -1, chapter 11):
On the robot:
imu0: imu/data_raw
imu0_config: [false, false, false, false, false, false,
false, false, false, false, false, true,
false, false, false]
imu0_differential: false
imu0_remove_gravitational_acceleration: false
The commanded yaw rate from the wheels is left out on purpose: the gyro measures turning, the command only says
what was asked for. The whole file, as it is in the repo (ros2/rosorin_base/config/ekf.yaml) and on the robot:
On the robot:
# robot_localization EKF: odom -> base_link. 2D.
# Inputs: wheel/odom twist (vx, vy) = gated COMMANDED velocity (no wheel encoders on this board),
# imu/data_raw gyro z (yaw rate). Commanded yaw rate is not fused: the gyro measures it.
# Absolute drift correction (map -> odom) comes later from scan matching / SLAM.
ekf_filter_node:
ros__parameters:
frequency: 50.0
two_d_mode: true
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# Verified nav baseline (docs/navigation.md, 2026-09-28): wheel/odom = gated commanded velocity, AMCL corrects drift.
# 2026-09-30 a switch to LiDAR odometry (odom_lidar from rf2o_fix) was REVERTED (owner): not yet proven in isolation.
# rf2o + rf2o_fix still run in base.launch, but nothing consumes it until it is verified.
odom0: wheel/odom
# x y z roll pitch yaw
# vx vy vz vroll vpitch vyaw
# ax ay az
odom0_config: [false, false, false, false, false, false,
true, true, false, false, false, false,
false, false, false]
odom0_differential: false
imu0: imu/data_raw
imu0_config: [false, false, false, false, false, false,
false, false, false, false, false, true,
false, false, false]
imu0_differential: false
imu0_remove_gravitational_acceleration: false
Why commanded velocity and not a measurement
On 2026-09-30 the EKF's input was switched to LiDAR odometry and switched back the same day by the owner: it had
not been proven on its own. The design review that day kept "the TESTED commanded-velocity EKF input (AMCL
corrects)" (docs/decisions.md). The camera odometry was added on 2026-10-02 with the plan to replace the wheel
input "if it holds" on a long measured path; that test has not been done. The docs differ on this point:
a comment inlaunch/base.launch.pystill callsodom_lidar"the EKF's motion input".config/ekf.yaml, which
is byte-identical on the robot, is what runs.
launch/base.launch.py (chapter 11) starts it with the rest of the base, so it runs whenever rosorin-base runs:
On the robot:
Node(package='robot_localization', executable='ekf_node', name='ekf_filter_node',
parameters=[os.path.join(share, 'config', 'ekf.yaml')]),
setup.py installs config/ekf.yaml; after creating or changing it, rebuild and restart:
On the robot:
cd ~/ros2_ws && source /opt/ros/humble/setup.bash && colcon build --packages-select rosorin_base
sudo systemctl restart rosorin-base
sleep 12
source ~/ros2_ws/install/setup.bash
timeout 8 ros2 topic hz /odometry/filtered 2>&1 | tail -2
timeout 5 ros2 topic echo --once /odometry/filtered 2>&1 | head -12
timeout 6 ros2 run tf2_ros tf2_echo odom base_link 2>&1 | grep -A2 Translation | head -3
Check
From 2026-09-28, right after the first start:On the robot:
average rate: 50.002 min: 0.019s max: 0.021s std dev: 0.00023s window: 304 header: stamp: sec: 1790554253 nanosec: 743618990 frame_id: odom child_frame_id: base_linkand the journal line
[INFO] [ekf_node-2]: process started. Standing still, the position stays at 0.0 and
tf2_echoprints a translation near zero.
If it fails
- Every topic reads
NONEor nothing arrives, whilerosorin-baseisactive. On 2026-09-28 that was
logind deleting Fast DDS's shared memory when the last ssh session closed (RemoveIPC, chapter 9), not the
EKF.- The heading creeps while the robot stands still. Before the gyro bias tracker the EKF yaw followed the
gyro's bias exactly (0.312° vs 0.314° per 60 s) and reached 11.4° after 8 minutes. With the tracker
(chapter 11) it stayed within ±0.11° over 5 minutes. If yours drifts, check the driver's state for
gyro_bias_dpsandgyro_bias_tracking.
scripts/odom_test.py enables the wheels, drives 3 s forward, 3 s sideways and 3 s turning, disables the wheels
after each move, and prints how far wheel/odom and the EKF think the robot went.
Safety
Wheels in the air on the stand. The test enables the wheels, which first moves the arm to the drive pose
(chapter 11). Nothing else may publishcmd_vel: the driver latches an e-stop when it sees two publishers. If
navigation is installed (chapter 18), stop it first withsudo systemctl stop rosorin-navand start it again
after.
On your laptop:
cd ~/CCode/rosorin-pro
scp scripts/odom_test.py rosorin-wifi:setup/
On the robot:
source /opt/ros/humble/setup.bash; source ~/ros2_ws/install/setup.bash
python3 ~/setup/odom_test.py
Check
The run on 2026-09-28 with the gyro bias tracker:On the robot:
enable(True) -> True enabled >> forward 0.1 m/s 3 s enable(False) -> True disabled wheel delta x +0.295 y +0.000 yaw +0.00 deg ekf delta x +0.295 y -0.001 yaw -0.02 deg enable(True) -> True enabled >> strafe left 0.1 m/s 3 s enable(False) -> True disabled wheel delta x +0.000 y +0.295 yaw +0.00 deg ekf delta x +0.001 y +0.295 yaw -0.04 deg enable(True) -> True enabled >> rotate +0.5 rad/s 3 s enable(False) -> True disabled wheel delta x +0.000 y +0.000 yaw +83.79 deg ekf delta x -0.000 y -0.000 yaw -0.03 degThat run was before the 0.765 sideways scale. With the current driver expect the strafe to read about
y +0.226in both lines (0.295 × 0.765, computed, not a recorded stand run). The rotate line is the point of
the test: the wheels claim 84°, the EKF says 0° because the gyro on a stand sees no turn.
If it fails
- The first run aborted before sending anything. The driver's topics were not yet discovered. The script
now waits up to 10 s for them; if it still finds nothing, see the DDS failure above.enable(True) -> False. The driver refused, usually because the arm could not reach the drive pose or an
e-stop is latched. Read the driver's state topic forestopandblocked.
These runs need the robot on the floor, the owner present, and nobody walking through the LiDAR's view (the first
attempts failed with a person moving). scripts/floor_odom_test.py compares odometry with LiDAR scan matching
between standing scans; scripts/tests/odom_truth.py uses the change in LiDAR range along the motion and the gyro
as truth. Recorded results:
Move (2026-09-28, floor_odom_test.py) |
wheel / EKF | LiDAR truth |
|---|---|---|
| forward 3 s at 0.1 m/s | +0.291 m | +0.297 m |
| strafe left 3 s at 0.1 m/s | y +0.297 m | y +0.234 m (79 %) |
| rotate right 3.2 s at 0.5 rad/s | wheel −88.9°, EKF −84.5° | −83° |
| strafe left after the 0.765 scale | y +0.226 m | y +0.241 m (−6 %) |
The strafe ratio varied from 0.73 to 0.81 between runs, so no single scale is right every time.
odom_truth.py on 2026-10-02 gave the EKF 99-100 % of true distance, /odom_vslam 95-96 % and about 1° on 90°
turns, rf2o 88-100 %.
New idea: scan matching
If you take two LiDAR scans a moment apart and slide one over the other until the walls line up, the slide is
how far the sensor moved. That is scan-matching odometry. It measures real motion, so a stalled wheel shows
up as zero. Its weakness is geometry: if the walls look the same after a move (a long corridor, or a sideways
move along a featureless wall), it cannot tell that anything happened.
rf2o (MAPIRlab/rf2o_laser_odometry) does this for 2D LiDARs. It is not in apt for Humble: on 2026-09-28
apt-cache policy ros-humble-rf2o-laser-odometry showed no candidate, and a search on 2026-09-29 found only other
scan matchers. So it is built from source in its own workspace, ~/ext_ws, kept apart from your own code.
Order
base.launch.pystarts rf2o androsorin-base.servicesources~/ext_ws. If you built the base in chapter 11
with the full launch file, the base needs this workspace to start. Build it now if you have not.
On the robot:
mkdir -p ~/ext_ws/src
git clone -b ros2 https://github.com/MAPIRlab/rf2o_laser_odometry.git ~/ext_ws/src/rf2o_laser_odometry
git -C ~/ext_ws/src/rf2o_laser_odometry checkout b38c68e
git -C ~/ext_ws/src/rf2o_laser_odometry log -1 --format="commit %h %cd"
source /opt/ros/humble/setup.bash
cd ~/ext_ws && colcon build --packages-select rf2o_laser_odometry --cmake-args -DCMAKE_BUILD_TYPE=Release
Check
From 2026-09-29:On the robot:
commit b38c68e Fri Apr 28 17:29:49 2023 +0200 [Processing: rf2o_laser_odometry] Finished <<< rf2o_laser_odometry [57.6s] Summary: 1 package finished [58.2s]Its build needs Eigen, which was already there (
libeigen3-dev 3.4.0-2ubuntu2).
Not test-built
On the robot the clone took the head of branchros2, which on 2026-09-29 was commitb38c68e. The
git checkout b38c68eline pins that commit so a future change upstream cannot change your build; it was not
part of the recorded run. No script in the repo builds~/ext_ws.
rf2o is configured from a parameter file, ~/ros2_ws/src/rosorin_base/config/rf2o.yaml:
On the robot:
# rf2o laser odometry (built from source in ~/ext_ws). publish_tf false: the EKF owns odom->base_link. Its POSE
# translation is reversed -> rosorin_base rf2o_fix republishes it corrected as odom_lidar (the EKF's motion input).
/**:
ros__parameters:
laser_scan_topic: /scan
odom_topic: /odom_rf2o
publish_tf: false
base_frame_id: laser # 2026-09-30: odometry OF THE LASER; no TF lookup at start (it raced the static TF -> sign flipped per boot). rf2o_fix converts to base_link.
odom_frame_id: odom_rf2o
init_pose_from_topic: ""
freq: 10.0
publish_tf: false: only one node may own odom → base_link, and that is the EKF.odom_frame_id: odom_rf2o: its own frame name, so its pose is never confused with the EKF's.freq: 10.0: the LiDAR turns at 10 Hz; rf2o's own launch file defaults to 20.base_frame_id: laser: rf2o reports the motion of the LiDAR itself and never asks TF where the LiDAR is mounted.It runs from base.launch.py, followed by the converter:
On the robot:
# LiDAR odometry (2026-09-30): rf2o -> rf2o_fix (sign-corrected) -> odom_lidar = the EKF's motion input
Node(package='rf2o_laser_odometry', executable='rf2o_laser_odometry_node', name='rf2o_laser_odometry',
parameters=[os.path.join(share, 'config', 'rf2o.yaml')], arguments=['--ros-args', '--log-level', 'warn']),
Node(package='rosorin_base', executable='rf2o_fix', name='rf2o_fix'),
If it fails
Couldn't parse parameter override rule: '-p init_pose_from_topic:='and the node aborts. An empty value
cannot be passed on the command line (2026-09-29). Use the parameter file.Waiting for laser_scans....over and over. On 2026-09-29 the node printed this with/scanrunning,
untilinit_pose_from_topicwas set to""(the parameter file does that). It also prints it when there is
no/scan; check the LiDAR reader (chapter 14)."laser" passed to lookupTransform argument source_frame does not existat boot, and odometry whose
sign is right after one restart and reversed after the next. Withbase_frame_id: base_linkrf2o looked up
base_link→laseronce, on its first scan; at boot that raced the static transform publisher. Fixed on
2026-09-30 bybase_frame_id: laserplus the fixed mount inrf2o_fix.
rf2o now reports how the LiDAR moved. The robot needs how base_link moved. The LiDAR is mounted 4.8 cm ahead of
base_link and turned 180° (chapter 12), so the converter rotates the velocity by 180° and corrects for the lever
arm: when the body turns, a point 4.8 cm ahead of its centre also moves sideways. It also works from velocities over
a short window instead of rf2o's integrated pose, because rf2o's pose translation came out reversed. Build
~/ros2_ws/src/rosorin_base/rosorin_base/rf2o_fix.py in parts.
The header and the constants. SIGN is the measured sign fix:
On the robot:
"""rf2o laser odometry -> robot body motion on /odom_lidar (the EKF's motion input; the board has no wheel encoders).
rf2o runs with base_frame_id = laser (config/rf2o.yaml): it reports the LASER's own motion and never looks up the
mounting. (2026-09-30: with base_frame_id base_link, rf2o looked up base_link->laser once on its first scan; at boot
that raced the static TF - "laser does not exist" - and the sign of its output flipped between restarts.)
Here: laser displacement over WIN s -> laser-frame velocity -> base_link velocity with the fixed, verified mount
(yaw pi, x 0.048 m: LiDAR ranges matched the depth camera, 0.15 m median, 2026-09-30):
v_base = R(pi) v_laser - w x r_laser (w = yaw rate, r_laser = (0.048, 0) in base_link)
Scale: rf2o read ~66-76 % of the true distance in self-checks (open). EKF uses vx, vy only (yaw rate from the gyro)."""
import math
import rclpy
from nav_msgs.msg import Odometry
from rclpy.node import Node
WIN = 0.3
MOUNT_YAW, MOUNT_X = math.pi, 0.048
SIGN = -1.0 # set by the robot's floor self-check 2026-09-30: +1 gave -0.374 m for a 0.371 m forward move (magnitude exact)
def yaw_of(q):
return math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))
yaw_of turns a quaternion (how ROS stores rotations) into a heading angle in radians.
The node: it reads odom_rf2o and publishes odom_lidar, and keeps a short history and an integrated pose for
logs and self-checks:
On the robot:
class Rf2oFix(Node):
def __init__(self):
super().__init__('rf2o_fix')
self.pub = self.create_publisher(Odometry, 'odom_lidar', 20)
self.create_subscription(Odometry, 'odom_rf2o', self.on_odom, 20)
self.hist = []
self.x = self.y = self.th = 0.0 # integrated base pose (for logs / self-checks)
self.last_t = None
The work, on every rf2o message. Keep the last 0.45 s of laser poses; take the newest one at least WIN (0.3 s)
old; the difference divided by the time is the laser's velocity, first in the odometry frame (dx, dy), then
turned into the laser's own frame (vlx, vly) using the old heading:
On the robot:
def on_odom(self, m):
t = m.header.stamp.sec + m.header.stamp.nanosec * 1e-9
p = m.pose.pose.position
yl = yaw_of(m.pose.pose.orientation)
self.hist.append((t, p.x, p.y, yl))
self.hist = [h for h in self.hist if t - h[0] <= WIN + 0.15]
old = [h for h in self.hist if h[0] <= t - WIN + 0.02]
vx = vy = wz = 0.0
if old:
t0, x0, y0, yl0 = old[-1]
dt = max(t - t0, 1e-3)
dx, dy = (p.x - x0) / dt, (p.y - y0) / dt
vlx = math.cos(yl0) * dx + math.sin(yl0) * dy # laser-frame velocity
vly = -math.sin(yl0) * dx + math.cos(yl0) * dy
wz = math.atan2(math.sin(yl - yl0), math.cos(yl - yl0)) / dt
Then rotate by the mount (180°), apply the sign, and subtract the lever arm from the sideways speed:
On the robot:
c, s = math.cos(MOUNT_YAW), math.sin(MOUNT_YAW)
vx = SIGN * (c * vlx - s * vly)
vy = SIGN * (s * vlx + c * vly) - wz * MOUNT_X # lever arm: the laser sits 4.8 cm ahead of base_link
Integrate a body pose from those speeds (only for logs) and publish speeds plus pose. The twist covariance says
0.05 m/s for vx, vy:
On the robot:
if self.last_t is not None:
dtp = max(t - self.last_t, 0.0)
self.x += (vx * math.cos(self.th) - vy * math.sin(self.th)) * dtp
self.y += (vx * math.sin(self.th) + vy * math.cos(self.th)) * dtp
self.th += wz * dtp
self.last_t = t
o = Odometry()
o.header.stamp = m.header.stamp
o.header.frame_id, o.child_frame_id = 'odom_lidar', 'base_link'
o.pose.pose.position.x, o.pose.pose.position.y = self.x, self.y
o.pose.pose.orientation.z, o.pose.pose.orientation.w = math.sin(self.th / 2), math.cos(self.th / 2)
o.twist.twist.linear.x, o.twist.twist.linear.y, o.twist.twist.angular.z = vx, vy, wz
cov = [0.0] * 36
cov[0] = cov[7] = 0.0025; cov[35] = 0.01 # twist: vx, vy (0.05 m/s)^2, wz
o.twist.covariance = cov
self.pub.publish(o)
And the usual entry point:
On the robot:
def main():
rclpy.init()
n = Rf2oFix()
try:
rclpy.spin(n)
except KeyboardInterrupt:
pass
finally:
n.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()
The complete file (ros2/rosorin_base/rosorin_base/rf2o_fix.py, 83 lines):
On the robot:
"""rf2o laser odometry -> robot body motion on /odom_lidar (the EKF's motion input; the board has no wheel encoders).
rf2o runs with base_frame_id = laser (config/rf2o.yaml): it reports the LASER's own motion and never looks up the
mounting. (2026-09-30: with base_frame_id base_link, rf2o looked up base_link->laser once on its first scan; at boot
that raced the static TF - "laser does not exist" - and the sign of its output flipped between restarts.)
Here: laser displacement over WIN s -> laser-frame velocity -> base_link velocity with the fixed, verified mount
(yaw pi, x 0.048 m: LiDAR ranges matched the depth camera, 0.15 m median, 2026-09-30):
v_base = R(pi) v_laser - w x r_laser (w = yaw rate, r_laser = (0.048, 0) in base_link)
Scale: rf2o read ~66-76 % of the true distance in self-checks (open). EKF uses vx, vy only (yaw rate from the gyro)."""
import math
import rclpy
from nav_msgs.msg import Odometry
from rclpy.node import Node
WIN = 0.3
MOUNT_YAW, MOUNT_X = math.pi, 0.048
SIGN = -1.0 # set by the robot's floor self-check 2026-09-30: +1 gave -0.374 m for a 0.371 m forward move (magnitude exact)
def yaw_of(q):
return math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))
class Rf2oFix(Node):
def __init__(self):
super().__init__('rf2o_fix')
self.pub = self.create_publisher(Odometry, 'odom_lidar', 20)
self.create_subscription(Odometry, 'odom_rf2o', self.on_odom, 20)
self.hist = []
self.x = self.y = self.th = 0.0 # integrated base pose (for logs / self-checks)
self.last_t = None
def on_odom(self, m):
t = m.header.stamp.sec + m.header.stamp.nanosec * 1e-9
p = m.pose.pose.position
yl = yaw_of(m.pose.pose.orientation)
self.hist.append((t, p.x, p.y, yl))
self.hist = [h for h in self.hist if t - h[0] <= WIN + 0.15]
old = [h for h in self.hist if h[0] <= t - WIN + 0.02]
vx = vy = wz = 0.0
if old:
t0, x0, y0, yl0 = old[-1]
dt = max(t - t0, 1e-3)
dx, dy = (p.x - x0) / dt, (p.y - y0) / dt
vlx = math.cos(yl0) * dx + math.sin(yl0) * dy # laser-frame velocity
vly = -math.sin(yl0) * dx + math.cos(yl0) * dy
wz = math.atan2(math.sin(yl - yl0), math.cos(yl - yl0)) / dt
c, s = math.cos(MOUNT_YAW), math.sin(MOUNT_YAW)
vx = SIGN * (c * vlx - s * vly)
vy = SIGN * (s * vlx + c * vly) - wz * MOUNT_X # lever arm: the laser sits 4.8 cm ahead of base_link
if self.last_t is not None:
dtp = max(t - self.last_t, 0.0)
self.x += (vx * math.cos(self.th) - vy * math.sin(self.th)) * dtp
self.y += (vx * math.sin(self.th) + vy * math.cos(self.th)) * dtp
self.th += wz * dtp
self.last_t = t
o = Odometry()
o.header.stamp = m.header.stamp
o.header.frame_id, o.child_frame_id = 'odom_lidar', 'base_link'
o.pose.pose.position.x, o.pose.pose.position.y = self.x, self.y
o.pose.pose.orientation.z, o.pose.pose.orientation.w = math.sin(self.th / 2), math.cos(self.th / 2)
o.twist.twist.linear.x, o.twist.twist.linear.y, o.twist.twist.angular.z = vx, vy, wz
cov = [0.0] * 36
cov[0] = cov[7] = 0.0025; cov[35] = 0.01 # twist: vx, vy (0.05 m/s)^2, wz
o.twist.covariance = cov
self.pub.publish(o)
def main():
rclpy.init()
n = Rf2oFix()
try:
rclpy.spin(n)
except KeyboardInterrupt:
pass
finally:
n.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()
Its entry point rf2o_fix = rosorin_base.rf2o_fix:main is already in setup.py (chapter 11). Rebuild and restart
the base, then look:
On the robot:
cd ~/ros2_ws && source /opt/ros/humble/setup.bash && colcon build --packages-select rosorin_base
sudo systemctl restart rosorin-base
sleep 12
source ~/ext_ws/install/setup.bash; source ~/ros2_ws/install/setup.bash
ros2 topic info /odom_rf2o
timeout 6 ros2 topic hz /odom_lidar 2>&1 | grep -m1 "average rate"
Check
/odom_rf2ohas one publisher. Its subscribers arerf2o_fixplus, later, the contact monitor (chapter 19).
The record has no clean rate for the final setup; the first standing test on 2026-09-29 got 38 messages in 5 s
withfreq: 10.0and a standing noise of at most 0.0128 m/s forward and 0.0543 rad/s in turn. Standing still,
/odom_lidarspeeds stay within a few cm/s of zero.
What rf2o is used for today, and what it is not:
/odom_lidar has no reader./odom_rf2o (with /odom_vslam) to notice commanded motion that does notSIGN = -1 came from one floor self-check (2026-09-30: +1 read −0.374 m for adocs/status.md records that "the final sign was never confirmed".rf2o_fix.py read 66-76 % of the true distance;odom_truth.py on 2026-10-02 read 88-100 %.New idea: visual odometry
Visual odometry tracks points in the camera image from frame to frame. With a depth value for each point it
knows how far away they are, and from how they shift it works out how the camera moved, in all six directions.
It measures real motion like scan matching does, and it sees sideways motion that a 2D LiDAR may miss. It needs
a scene with texture that stays still while the camera moves.
NVIDIA's cuVSLAM runs this on the Jetson's GPU. The Isaac ROS apt package (ros-humble-isaac-ros-visual-slam
3.2.6, chapter 18) bundles cuVSLAM v12, which only takes a stereo pair. Version 17, NVIDIA's PyCuVSLAM wheel, has an
RGB-D mode: one image plus an aligned depth image, which is what the Aurora gives (chapter 15). The record has this
as a lesson: on 2026-10-02 the first answer was "cuVSLAM cannot use this camera", from testing only the apt version;
the rule since is to check the current release and its modes before saying an NVIDIA component cannot do something.
Order
The wheel is built for CUDA 12 and needs the CUDA 12.6 libraries andpip3that chapter 20's
install_vision.shinstalls. On this robot they went in on 2026-09-29 and cuVSLAM on 2026-10-02. If you follow
the chapters in order, do this section after chapter 20.
These are the wheel lines of scripts/install_isaac_ros.sh, which chapter 18 runs as a whole; run as
burgerbarn they are the commands used on 2026-10-02:
On the robot:
W=cuvslam-17.0.0+cu12-cp310-cp310-manylinux_2_35_aarch64.whl
mkdir -p ~/setup/wheels && cd ~/setup/wheels
[ -f "$W" ] || curl -sfL -o "$W" "https://github.com/nvidia-isaac/cuVSLAM/releases/download/v17.0.0/cuvslam-17.0.0%2Bcu12-cp310-cp310-manylinux_2_35_aarch64.whl"
ls -l; sha256sum "$W"
pip3 install --user "$W"
python3 -c "import cuvslam as v; print(v.__version__)"
Check
From 2026-10-02:On the robot:
-rw-rw-r-- 1 burgerbarn burgerbarn 113375996 Oct 2 05:50 cuvslam-17.0.0+cu12-cp310-cp310-manylinux_2_35_aarch64.whl 658250d97c96c5db0d61ed68d8c0e99514c130574339367f0d917ad60f05d315 cuvslam-17.0.0+cu12-cp310-cp310-manylinux_2_35_aarch64.whl Processing ./cuvslam-17.0.0+cu12-cp310-cp310-manylinux_2_35_aarch64.whl Requirement already satisfied: pyyaml>=5.3.1 in /usr/lib/python3/dist-packages (from cuvslam==17.0.0+cu12) (5.4.1) Installing collected packages: cuvslam Successfully installed cuvslam-17.0.0+cu12 17.0.0The hash must be exactly
658250d9...5d315; the install script refuses anything else. The wheel stays in
~/setup/wheelsso a reinstall does not depend on GitHub.
If it fails
AttributeError: module 'cuvslam' has no attribute 'Odometry'. That name is from older examples. In v17
the classes areCamera,Distortion,Rig,Trackerand friends; the RGB-D setup is
Tracker.OdometryConfig(... odometry_mode=vslam.Tracker.OdometryMode.RGBD ...), as in the node below.- Rollback:
pip3 uninstall cuvslam.
ros2/rosorin_base/rosorin_base/vslam_odom.py (189 lines) feeds the Aurora's RGB and depth images to cuVSLAM and
publishes how base_link moved on /odom_vslam. It is part of the rosorin_base package you put in
~/ros2_ws/src in chapter 11, and its entry point vslam_odom is already in setup.py; it is too long to build up
line by line here. It imports the wheel as import cuvslam as vslam. Its structure:
| Part | What it does |
|---|---|
| top of file | OPENBLAS_NUM_THREADS=1 before numpy loads; constants |
__init__ |
TF listener; subscribes /aurora/rgb/camera_info; pairs /aurora/rgb/image_raw with /aurora/depth/image_raw |
new_tracker |
builds a cuVSLAM RGB-D tracker from the RGB intrinsics |
base_to_cam |
asks TF where the camera is on the body at the image's time |
on_frame |
tracks one frame, refuses steps taken while the arm moves, turns camera motion into body motion, publishes |
report |
one log line per minute |
The key parts, verbatim.
One maths thread. numpy's OpenBLAS starts worker threads that spin after every matrix call. Measured with gdb on
2026-10-05, three spinning threads cost 1.9 of 8 cores for 4x4 matrices; cuVSLAM itself costs 0.16 core:
On the robot:
os.environ.setdefault('OPENBLAS_NUM_THREADS', '1') # setdefault: the unit's environment can override it (self-care test)
The limits. Depth arrives in millimetres; a head move is a change of more than 3 mm or 0.5° between camera and
body from one frame to the next; a step faster than the robot can move is a tracking jump:
On the robot:
DEPTH_PER_M = 1000.0 # depth image: mono16 millimetres
HEAD_MOVE = (0.003, math.radians(0.5)) # camera vs base change between frames (m, rad) that counts as a head move
HEAD_SETTLE_S = 0.4 # wait this long after the head stops (arm TF lags the image)
MAX_V, MAX_W = 1.5, 4.0 # m/s, rad/s of the base: beyond this the estimate jumped (base tops out far lower)
RESTART_AFTER = 15 # consecutive failed frames (~1 s) -> new tracker
Pairing colour with depth. The RGB frame is stamped about 70 ms after its depth frame (chapter 15), so the
pairing window is 0.09 s. The variable is still called ir from the first version, which used the IR image; it
reads RGB:
On the robot:
self.create_subscription(CameraInfo, '/aurora/rgb/camera_info', self.on_info, 1)
ir = Subscriber(self, Image, '/aurora/rgb/image_raw', qos_profile=qos_profile_sensor_data)
dp = Subscriber(self, Image, '/aurora/depth/image_raw', qos_profile=qos_profile_sensor_data)
self.sync = ApproximateTimeSynchronizer([ir, dp], 10, 0.09) # RGB is stamped ~70 ms after depth
The tracker, from the RGB camera's intrinsics and distortion, in RGB-D mode:
On the robot:
def new_tracker(self):
i = self.info
cam = vslam.Camera()
cam.distortion = (vslam.Distortion(vslam.Distortion.Model.Brown, list(i.d[:5]))
if i.distortion_model == 'plumb_bob' and len(i.d) >= 5
else vslam.Distortion(vslam.Distortion.Model.Pinhole))
cam.focal = i.k[0], i.k[4]
cam.principal = i.k[2], i.k[5]
cam.size = i.width, i.height
rgbd = vslam.Tracker.OdometryRGBDSettings()
rgbd.depth_scale_factor = DEPTH_PER_M
rgbd.depth_camera_id = 0
rgbd.enable_depth_stereo_tracking = False
cfg = vslam.Tracker.OdometryConfig(async_sba=self.async_sba, odometry_mode=vslam.Tracker.OdometryMode.RGBD,
rgbd_settings=rgbd)
self.tracker = vslam.Tracker(vslam.Rig([cam]), cfg)
self.prev = None
self.fails = 0
Ignore the arm's own motion. cuVSLAM tracks the camera, and the camera rides on the arm. H is how the camera
moved relative to the body since the last frame. If it moved more than HEAD_MOVE, the head is moving: this frame
and the next 0.4 s are not counted:
On the robot:
H = np.linalg.inv(prev[2]) @ T_bc # camera vs base since the last frame = head move
ang = math.acos(max(-1.0, min(1.0, (np.trace(H[:3, :3]) - 1) / 2)))
if np.linalg.norm(H[:3, 3]) > HEAD_MOVE[0] or ang > HEAD_MOVE[1]:
if t_ns > self.head_until:
self.head_moves += 1
self.head_until = t_ns + int(HEAD_SETTLE_S * 1e9)
if t_ns <= self.head_until: # head moving: this step is not counted
return
Camera motion to body motion. D chains three transforms: where the camera sat on the body last frame, how
cuVSLAM says the camera moved, and back from the camera to the body now. What is left is how base_link moved.
Only x, y and yaw are kept, because the body moves on a floor. A step faster than 1.5 m/s or 4 rad/s is dropped:
On the robot:
D = prev[2] @ np.linalg.inv(prev[1]) @ T_wc @ np.linalg.inv(T_bc)
dx, dy, dyaw = float(D[0, 3]), float(D[1, 3]), math.atan2(D[1, 0], D[0, 0])
if math.hypot(dx, dy) / dt > MAX_V or abs(dyaw) / dt > MAX_W:
self.jumps += 1 # not a motion of this robot: dropped
return
The node then integrates x, y, yaw, publishes nav_msgs/Odometry on odom_vslam (frame odom_vslam, child
base_link, pose and twist with covariance) and no transform. When 15 frames in a row fail (about 1 s), it starts
a new tracker and logs it.
The node runs as its own service, systemd/rosorin-vslam.service:
On the robot:
[Unit]
Description=ROSOrin measured odometry from the depth camera (NVIDIA cuVSLAM RGB-D on the Aurora RGB + depth -> /odom_vslam)
After=rosorin-base.service rosorin-camera.service
Wants=rosorin-base.service rosorin-camera.service
[Service]
User=burgerbarn
Environment=ROS_DOMAIN_ID=0
ExecStart=/bin/bash -c 'source /opt/ros/humble/setup.bash && source /home/burgerbarn/ros2_ws/install/setup.bash && exec ros2 run rosorin_base vslam_odom'
KillSignal=SIGINT
Restart=always
RestartSec=5
[Install]
WantedBy=multi-user.target
It runs as burgerbarn because the wheel is installed in that user's ~/.local. There is no install script for this
unit; it was installed by hand on 2026-10-02 with these commands:
On your laptop:
cd ~/CCode/rosorin-pro
scp systemd/rosorin-vslam.service rosorin-wifi:/tmp/
On the robot:
sudo cp /tmp/rosorin-vslam.service /etc/systemd/system/
sudo systemctl daemon-reload
sudo systemctl enable --now rosorin-vslam
sleep 12
systemctl is-active rosorin-vslam
journalctl -u rosorin-vslam -n 3 --no-pager -o cat
source /opt/ros/humble/setup.bash; source ~/ros2_ws/install/setup.bash
timeout 6 ros2 topic hz /odom_vslam 2>&1 | tail -2
Check
From 2026-10-02:On the robot:
Created symlink /etc/systemd/system/multi-user.target.wants/rosorin-vslam.service → /etc/systemd/system/rosorin-vslam.service. active Started ROSOrin measured odometry from the depth camera (NVIDIA cuVSLAM RGB-D on the Aurora RGB + depth -> /odom_vslam). average rate: 9.225 min: 0.012s max: 0.267s std dev: 0.06210s window: 31About 9 Hz is what the record shows, below the camera's 13-15 Hz. The node reads the images best-effort, and
docs/status.mdnotes that such readers of the 768 kB images lose frames through the default 512 kB Fast DDS
shared-memory segment. Once a minute the journal shows a line of the form
tracked N frames, head moves H, jumps dropped J, tracker restarts R. Intop,vslam_odomshould use about a
third of one core.
If it fails
vslam_odomuses 1.5-2 cores while the robot stands still. OpenBLAS threads spinning (2026-10-05). The
OPENBLAS_NUM_THREADSline must come beforeimport numpy. After the fix: 0.33 core, 549 → 666 frames
tracked per minute, load average about 9 → 3.6. Self-care (chapter 23) can apply the same fix as a systemd
drop-in if it ever comes back.- No
/odom_vslam. Check the camera first (ros2 topic hz /aurora/rgb/image_raw, chapter 15), then TF:
the node needsbase_link→ camera fromrobot_state_publisher(chapter 12) and publishes nothing until it
has it.tracking lost for 15 frames -> new tracker (restart N)in the journal: cuVSLAM lost track for about
1 s and the node started a fresh tracker. The published pose carries on from where it was; the motion during
the lost second is missing.- Rollback:
sudo systemctl disable --now rosorin-vslam; pip3 uninstall cuvslam.
The obvious input for cuVSLAM looks like the IR image: it comes from the same sensor as depth, pixel for pixel. It
tracked 945 of 945 frames on 2026-10-02 and was first reported as working. Then the record compared it with an
independent measurement: the arm's own joint angles, which say exactly how far the head turned. The test script
(scripts/tests/cuvslam_rgbd_test.py) printed cuVSLAM's rotation next to the arm's. With the IR image, during a
14° head turn:
On the robot:
tracked 1888 failed 0 track 23.4 ms cuvslam xyz(m) +0.007 -0.005 +0.008 rot 0.5 deg | arm TF xyz(m) -0.001 +0.017 -0.039 rot 14.2 deg
With the RGB image and the same depth, during a similar turn:
On the robot:
tracked 37 failed 0 track 12.7 ms cuvslam xyz(m) -0.041 +0.005 +0.037 rot 14.2 deg | arm TF xyz(m) +0.000 -0.008 +0.039 rot 13.8 deg
The IR image shows the camera's own projected dot pattern (chapter 15). The dots move with the camera, so cuVSLAM
locks onto them and concludes the camera is not moving: 0.5° for a 14.2° turn. The RGB image does not contain the
pattern, and it measured 14.2° for a 13.8° turn. The rule written down that day: "a perception component is
verified only by comparing its output with an independent measurement of motion". "Tracked 945/945" was not a
measurement.
| Source | Measured | Not proven |
|---|---|---|
| EKF (commanded vx, vy + gyro) | 99-100 % of true distance on floor drives (2026-10-02); strafe 73-81 % before the 0.765 scale, −6 % after (one run); standing yaw ±0.11° over 5 min | Anything that slips: the rug, a stalled wheel, a push. It cannot see those. |
| rf2o + rf2o_fix | 88-100 % (2026-10-02); 66-76 % in earlier self-checks; standing noise ≤ 0.013 m/s | The final sign; sideways motion (it is blind to it); whether it should ever feed the EKF |
cuVSLAM (/odom_vslam) |
head turn 13.8° read as 14.2°; head still: about 1 cm over minutes; 95-96 % of distance and about 1° on 90° turns (2026-10-02); speeds agree with rf2o during stops (0.08-0.22 m/s for 0.1-0.2 commanded) | A long measured drive. Over 3-10 cm stops it read 1-2.5 cm more than rf2o, and which one is right is unknown. A fast 90° head sweep left 0.5 m of false translation before the head-move hold. |
Not test-built
The plan of 2026-10-02 was to drive a known path, compare/odom_vslamwith the LiDAR, and make it the EKF's
velocity input "if it holds". That test has not been done, so the EKF still runs on commanded velocity. Nothing
in this chapter has been tested over a long autonomous drive. The robot has never run 24 hours unattended
(chapter 1).
ros2 topic hz /odometry/filtered reads about 50 Hz, and ros2 run tf2_ros tf2_echo odom base_link prints aodom_test.py on the stand: the wheel and EKF lines agree on forward and sideways, and the EKF reports no turnls ~/ext_ws/install/rf2o_laser_odometry exists and /odom_rf2o has a publisher.python3 -c "import cuvslam as v; print(v.__version__)" prints 17.0.0, systemctl is-active rosorin-vslamactive, and /odom_vslam runs at about 9 Hz.Where this comes from
ros2/rosorin_base/config/ekf.yaml,config/rf2o.yaml,rosorin_base/rf2o_fix.py,rosorin_base/vslam_odom.py,
rosorin_base/board_driver.py(publish_wheel_odom, odometry parameters),launch/base.launch.py,
ros2/rosorin_base/systemd/rosorin-base.service,systemd/rosorin-vslam.service,scripts/install_isaac_ros.sh(wheel lines),
scripts/odom_test.py,scripts/floor_odom_test.py,scripts/tests/odom_truth.py,
scripts/tests/cuvslam_rgbd_test.py,behavior/contact_monitor.py(reads/odom_rf2o,/odom_vslam);
docs/odometry.md(encoders, EKF, stand and floor tests, scale),docs/decisions.md2026-09-30 (EKF input kept)
and 2026-10-02 (cuVSLAM v17, RGB not IR, measurements),docs/lessons.md2026-09-30 (rf2o TF race) and
2026-10-02 ("cuVSLAM can't use this camera", rf2o blind sideways),docs/status.md2026-10-02 (odom_truth,
rf2o sign not confirmed) and 2026-10-05 (OpenBLAS). Command log: robot_localization install and first EKF start
2026-09-28, odom_test 2026-09-28, rf2o clone and build 2026-09-29 19:45 UTC, rf2o parameter crash 2026-09-29,
rf2o TF race 2026-09-30, cuVSLAM wheel 2026-10-02 05:50 UTC, IR vs RGB test and service install 2026-10-02
05:57-06:13 UTC. The rf2o clone commands and commit are fromsurvey_live_robot.md(the robot's own checkout).
← The depth camera · Contents · Maps: SLAM and the kept map →