Navigating · Chapter 18 · Time: 1-2 days · Level: Advanced · Status: Partly test-built
Nav2, NVIDIA nvblox and the robot's own navigation supervisor running from boot as rosorin-nav.service, placing the robot on its kept map by itself and reporting one "ready" fact on /nav/status. A real floor drive with mapping switched on is not yet verified.
Navigation turns "go to that spot on the map" into wheel commands that avoid everything the LiDAR and the depth
camera can see. This robot uses the stock ROS 2 navigation stack (Nav2 1.1.20 from apt) and NVIDIA's nvblox for the
depth camera, with as few own parts as possible: a parameter file, a behaviour tree with two recoveries removed, a
small depth gate, and a supervisor node that starts, checks and heals the stack. Navigation runs all the time from
boot (rosorin-nav.service); every skill (explore, go home, come here) is a client that waits for "ready" and then
sends goals.
Not done
A real floor drive through this always-on stack with mapping switched on has not been verified. The stand
test on 2026-10-06 placed the robot, switched mapping on and off, and a goal succeeded with the wheels in the air
(docs/decisions.md2026-10-06: "NOT yet verified: a real drive (needs the floor and the owner)"). Earlier floor
runs used older versions of this stack (AMCL on a fixed map, 2026-10-02 and 2026-10-05). Read the last section of
this chapter before you trust the robot to drive the room.
Chapter 17 gave the robot a map and a way to know where it is on it. Nav2 adds the rest. Five ideas carry the whole
chapter.
New idea: costmap
A costmap is a grid laid over the floor (here 5 cm cells) where each cell holds a number from 0 (free) to 254
(lethal: an obstacle is there). It is built in layers: a static layer copies the walls from the map, an
obstacle layer marks what the LiDAR sees right now, the nvblox layer marks what the depth camera sees, a keepout
filter marks "never go here", and an inflation layer spreads a falling cost around every obstacle so paths keep
their distance. Nav2 keeps two: the global costmap (the whole room, in themapframe, for planning) and the
local costmap (a 3 x 3 m window that moves with the robot, in theodomframe, for steering).
New idea: planner and controller
The planner finds a route through the global costmap from where the robot is to the goal, as a list of poses
(here NavFn, a grid search). It runs about once a second. The controller follows that route: 20 times a
second it tries hundreds of short possible motions (vx, vy, turn rate), simulates each for 1.7 s on the local
costmap, scores them (close to the path, heading for the goal, far from obstacles, forward preferred) and sends
the best one as a velocity command. This robot uses DWB, Nav2's sampling controller.
New idea: behaviour tree
Nav2's navigator does not hard-code "plan, then follow". It runs a behaviour tree: an XML file of nodes that
each return success, failure or running. Sequence nodes run children in order, recovery nodes retry a child after
running a fix, rate controllers limit how often a child runs. Changing the XML changes what the robot does when
something fails, without touching code. Recovery behaviours (Spin, BackUp, Wait, ClearCostmap) are the
actions the tree can call when stuck.
New idea: lifecycle nodes
Nav2 servers are managed (lifecycle) nodes. Each starts unconfigured, is told to configure (read
parameters, allocate), then to activate (start publishing and accepting goals). A lifecycle manager walks a
list of nodes through these steps (STARTUP), can take them down (RESET), and keeps a heartbeat "bond" with each.
On Humble the manager never retries: if one node fails to come up, the stack stays half-alive until someone
sends RESET then STARTUP. That is the job of this robot's supervisor.
New idea: the velocity chain
Several nodes produce motion (the controller, the recovery behaviours). Their commands do not go straight to the
wheels. They pass a velocity smoother (limits speed and acceleration) and a collision monitor (slows or
stops a command that would hit something the LiDAR sees right now). Only the last stage publishes/cmd_vel,
the one topic the base driver obeys.
This is the whole data flow as it runs on the robot:
You need, from the earlier chapters, all running and checked:
rosorin-base with the driver, LiDAR on /scan, the EKF on /odometry/filtered (chapters 11, 14, 16).rosorin-camera publishing /aurora/depth/image_raw and /aurora/rgb/camera_info (chapter 15).~/maps/room.posegraph + room.data (installed by install_live_map.sh),~/maps/room_live.yaml + room_live.png, the keepout mask ~/maps/room_live_keepout.yaml + .pgm, and~/maps/home_pose.json.rosorin_base package in ~/ros2_ws/src built with colcon build (chapter 11). It already contains everyAll install scripts read their files from ~/setup on the robot. Copying them there is not scripted; do it from
the repo on your laptop each time a script or unit changes:
On your laptop:
cd ~/CCode/rosorin-pro
ssh rosorin-wifi 'mkdir -p ~/setup/tests'
scp -q scripts/install_isaac_ros.sh scripts/rollback_isaac_ros.sh \
scripts/install_nav2_behaviors_fix.sh scripts/rollback_nav2_behaviors_fix.sh \
scripts/install_nav_service.sh scripts/nav_stop.sh systemd/rosorin-nav.service rosorin-wifi:~/setup/
scp -q scripts/tests/nav2_isolated_checks.sh scripts/tests/nav2_isolated_checks.py \
scripts/tests/nav2_bringup_check.sh rosorin-wifi:~/setup/tests/
Nav2 comes from the ROS apt repository you added in chapter 9. There is no install script for it in the repo; this
is the command that was run on 2026-09-28:
On the robot:
sudo apt-get install -y --no-install-recommends ros-humble-navigation2 ros-humble-nav2-bringup
--no-install-recommends keeps apt from pulling in desktop and simulation extras.
Check
Count the held L4T packages (still 45, nothing upgraded underneath you) and read the versions:On the robot:
dpkg -l | grep -c "^hi.*nvidia-l4t" dpkg -l | grep -E "ros-humble-(nav2-(amcl|controller|collision)|dwb-core)" | awk '{print $2, $3}'On this robot the install said
0 upgraded, 74 newly installed, 0 to remove and 50 not upgraded.and then:On the robot:
45 ros-humble-dwb-core 1.1.20-1jammy.20260910.010604 ros-humble-nav2-amcl 1.1.20-1jammy.20260909.214117 ros-humble-nav2-collision-monitor 1.1.20-1jammy.20260910.012304 ros-humble-nav2-controller 1.1.20-1jammy.20260910.010656
Version on a fresh install
The ROS apt repository serves only the newest build of each package. Everything in this chapter was measured on
Nav2 1.1.20. If apt gives you a newer Humble release, thenav2_behaviorsfix below refuses to run (by
design), and the Humble behaviours measured in this chapter and in chapter 19 may differ. Nobody has tried another
version on this robot.
NVIDIA publishes ready-built Isaac ROS packages for JetPack 6 and Humble in its own apt repository. The project rule
since 2026-10-01 is to prefer the NVIDIA option when one exists, so the depth camera reaches Nav2 through nvblox
(a GPU 3D map that publishes a 2D obstacle slice), and the start pose search uses NVIDIA's Occupancy Grid
Localizer (a GPU search of the whole map with one LiDAR scan).
New idea: nvblox
nvblox builds a 3D grid of 5 cm voxels from depth images, on the GPU. For each voxel it stores how far it is
from the nearest surface (a "distance field"). From that it cuts a horizontal slice between two heights and
publishes it as a 2D obstacle map. Nav2's costmaps read the slice through theNvbloxCostmapLayerplugin. So a
chair seat, a low box or a table edge that the 2D LiDAR plane (about 16 cm above the floor) misses still ends up
in the costmap.
scripts/install_isaac_ros.sh is 19 lines and runs as root. The excerpts below explain it; run the whole script, not
the excerpts. The first part adds the NVIDIA key and repository. Note
release-3.0 as the component: that is Isaac ROS 3.2 for JetPack 6.2 + Humble. Isaac ROS 5.0 needs JetPack 7.2 and
a different ROS release; do not use it here (docs/decisions.md 2026-10-01).
On the robot:
curl -fsSL https://isaac.download.nvidia.com/isaac-ros/repos.key | gpg --dearmor -o /usr/share/keyrings/isaac-ros.gpg
echo "deb [signed-by=/usr/share/keyrings/isaac-ros.gpg] https://isaac.download.nvidia.com/isaac-ros/release-3 jammy release-3.0" \
> /etc/apt/sources.list.d/isaac-ros.list
apt-get update -o Dir::Etc::sourcelist=sources.list.d/isaac-ros.list -o Dir::Etc::sourceparts=- -o APT::Get::List-Cleanup=0
The apt-get update options refresh only the new list, so nothing else on the robot is touched. Then the packages:
On the robot:
apt-get install -y libnvvpi3 cuda-nvtx-12-6 ros-humble-isaac-ros-nvblox ros-humble-nvblox-nav2 ros-humble-isaac-ros-visual-slam ros-humble-isaac-ros-occupancy-grid-localizer ros-humble-isaac-ros-pointcloud-utils # libnvvpi3 (VPI) + cuda-nvtx-12-6 (libnvToolsExt): JetPack libs nvblox_node needs, missing on the rebuilt image
libnvvpi3 and cuda-nvtx-12-6 are parts of JetPack that the rebuilt image does not have (chapter 9: the full
nvidia-jetpack meta-package is never installed), and nvblox does not start without them. The last part of the
script installs NVIDIA's cuVSLAM v17 wheel for chapter 16. This is the complete file:
On the robot:
#!/bin/bash
# Isaac ROS 3.2 (JetPack 6.2 / Humble) from NVIDIA's apt repo: nvblox (GPU 3D map) + nvblox_nav2 + cuVSLAM.
# The 2026-10-01 plan: wheels stage = cuVSLAM + nvblox. Rollback: sudo bash ~/setup/rollback_isaac_ros.sh
set -euo pipefail
[ "$(id -u)" = 0 ] || { echo "run with sudo"; exit 1; }
curl -fsSL https://isaac.download.nvidia.com/isaac-ros/repos.key | gpg --dearmor -o /usr/share/keyrings/isaac-ros.gpg
echo "deb [signed-by=/usr/share/keyrings/isaac-ros.gpg] https://isaac.download.nvidia.com/isaac-ros/release-3 jammy release-3.0" \
> /etc/apt/sources.list.d/isaac-ros.list
apt-get update -o Dir::Etc::sourcelist=sources.list.d/isaac-ros.list -o Dir::Etc::sourceparts=- -o APT::Get::List-Cleanup=0
apt-get install -y libnvvpi3 cuda-nvtx-12-6 ros-humble-isaac-ros-nvblox ros-humble-nvblox-nav2 ros-humble-isaac-ros-visual-slam ros-humble-isaac-ros-occupancy-grid-localizer ros-humble-isaac-ros-pointcloud-utils # libnvvpi3 (VPI) + cuda-nvtx-12-6 (libnvToolsExt): JetPack libs nvblox_node needs, missing on the rebuilt image
dpkg -l | grep -E "isaac-ros-(nvblox|visual-slam)|nvblox-nav2" | awk '{print $2, $3}'
# cuVSLAM v17 (RGB-D mode for the Aurora; the apt visual_slam package bundles v12 = stereo only). User install.
W=cuvslam-17.0.0+cu12-cp310-cp310-manylinux_2_35_aarch64.whl
sudo -u burgerbarn mkdir -p /home/burgerbarn/setup/wheels
[ -f /home/burgerbarn/setup/wheels/$W ] || sudo -u burgerbarn curl -sfL -o /home/burgerbarn/setup/wheels/$W \
"https://github.com/nvidia-isaac/cuVSLAM/releases/download/v17.0.0/cuvslam-17.0.0%2Bcu12-cp310-cp310-manylinux_2_35_aarch64.whl"
echo "658250d97c96c5db0d61ed68d8c0e99514c130574339367f0d917ad60f05d315 /home/burgerbarn/setup/wheels/$W" | sha256sum -c
sudo -u burgerbarn pip3 install --user /home/burgerbarn/setup/wheels/$W
Order
On this robot the CUDA 12.6 libraries came from chapter 20'sinstall_vision.shon 2026-09-29, three days before
Isaac ROS (2026-10-02). The Isaac packages do not declare every CUDA library they load (the missing
libnvToolsExtbelow shows it), and nvblox has never been run here without chapter 20's libraries. If you follow
the chapters in order, run chapter 20'sinstall_vision.shfirst. Chapter 16 installed only the cuVSLAM wheel
lines of this script by hand; running the whole script now skips the download because the wheel file is already
in~/setup/wheels.
On the robot:
sudo bash ~/setup/install_isaac_ros.sh
Check
On the robot:dpkg-query -W -f='${Package} ${Version}\n' ros-humble-isaac-ros-nvblox ros-humble-nvblox-nav2 ros-humble-nvblox-examples-bringup source /opt/ros/humble/setup.bash ldd /opt/ros/humble/lib/nvblox_ros/nvblox_node | grep -c "not found"The versions, read live on the robot on 2026-10-07 (dpkg-query sorts by name), and the
lddcount from the
fixed install on 2026-10-02:On the robot:
ros-humble-isaac-ros-nvblox 3.2.5-0jammy ros-humble-nvblox-examples-bringup 3.2.13-0jammy ros-humble-nvblox-nav2 3.2.5-0jammy 0
nvblox-examples-bringupis not in the install line; it arrives as a dependency (apt-mark showautolists
it).nav.launch.pyneeds it, because it loads NVIDIA'snvblox_base.yamlfrom that package. The0means no
missing libraries.
If it fails
nvblox_node: error while loading shared libraries: libnvToolsExt.so.1: cannot open shared object fileand
[ros2run]: Process exited with failure 127:cuda-nvtx-12-6is missing. This is exactly what happened on
2026-10-02 before the package was added to the script. Install it and runsudo ldconfig.- Same error naming VPI:
libnvvpi3is missing; on R36.4 its candidate is 3.2.4 from
repo.download.nvidia.com/jetson/common r36.4.- The robot's own
~/setup/install_isaac_ros.shis an older copy than the repo's (it lacks the localizer and
pointcloud-utils packages). Always copy the repo version first.
Safety: the rollback script
scripts/rollback_isaac_ros.shcontainsapt-get autoremove -y. The project's own rule (chapter 1,
docs/lessons.md2026-09-26) is never to runapt autoremoveon the robot: after a removal apt offers to remove
initramfs-tools, which the Jetson needs to boot. Do not run that rollback script as it is. To roll back, run
its other lines by hand:apt-get removeof the Isaac packages, delete the list and key file,apt-get update,
pip3 uninstall cuvslam.
New idea: AssistedTeleop
AssistedTeleop is one of Nav2's behaviours. A client sends a velocity oncmd_vel_teleop; the behaviour
projects the robot forward for 1 s in 0.1 s steps, checks the footprint against the local costmap at each step,
and slows or zeroes the command if it would hit something. This robot's body-practice skill
(behavior/practice.py, chapter 24) drives all its practice moves through it, including sideways ones.
On Humble the projection has the sign of the sideways term wrong. A strafe to the left is checked on the right side:
a wall on the strafe side does not slow the robot, a wall on the other side slows a strafe that is clear. Upstream
confirmed it (issue #6534) and fixed it on the main branch on 2026-09-17 (#6535, "Fix AssistedTeleop lateral
projection sign"), tagged only for backport to the newest ROS release. No Humble release has it.
It was measured on this robot's installed build on 2026-10-02 with the isolated test described later in this
chapter (velocities in m/s):
| Case | Strafe left in | Out, apt 1.1.20 | Out, with the fix |
|---|---|---|---|
| no wall | 0.20 | 0.20 | 0.20 |
| wall on the LEFT (strafe side) | 0.20 | 0.20, not slowed | 0.14, slowed |
| wall on the RIGHT | 0.20 | 0.14, slowed for the wrong side | 0.20 |
| wall ahead, drive forward (vx) | 0.20 | 0.14 | 0.14 |
The fix is upstream's own two-line change, built as an overlay: a copy of the one package nav2_behaviors,
built in ~/ros2_ws, which shadows the apt build for every process that sources ~/ros2_ws/install/setup.bash
(rosorin-nav does). scripts/install_nav2_behaviors_fix.sh does it in four steps.
It refuses to run against any version other than the one the patch was made for:
On the robot:
set -e
WANT=1.1.20
V=$(dpkg-query -W -f='${Version}' ros-humble-nav2-behaviors | cut -d- -f1)
[ "$V" = "$WANT" ] || { echo "installed nav2_behaviors is $V; this fix was made for $WANT - check upstream first"; exit 1; }
It fetches only the nav2_behaviors folder of the upstream tag (a sparse clone) into ~/ros2_ws/src:
On the robot:
SRC=$HOME/ros2_ws/src/nav2_behaviors
TMP=$(mktemp -d)
git clone -q --depth 1 --branch $WANT --filter=blob:none --sparse https://github.com/ros-navigation/navigation2.git $TMP/nav2
git -C $TMP/nav2 sparse-checkout set nav2_behaviors
rm -rf $SRC && cp -r $TMP/nav2/nav2_behaviors $SRC && rm -rf $TMP
It replaces the projection lines, and stops if they are not exactly what was expected (so it never patches a file it
does not understand). The two vy terms change sign; that is the standard body-to-world rotation:
On the robot:
old = """ projected_pose.x += projection_time * (
twist.linear.x * cos(pose.theta) +
twist.linear.y * sin(pose.theta));
projected_pose.y += projection_time * (
twist.linear.x * sin(pose.theta) -
twist.linear.y * cos(pose.theta));
"""
new = """ // rosorin: upstream fix #6535 (lateral projection sign), not backported to Humble
projected_pose.x += projection_time * (
twist.linear.x * cos(pose.theta) -
twist.linear.y * sin(pose.theta));
projected_pose.y += projection_time * (
twist.linear.x * sin(pose.theta) +
twist.linear.y * cos(pose.theta));
"""
assert s.count(old) == 1, 'assisted_teleop.cpp does not contain the expected projectPose lines'
And it builds that one package in Release mode:
On the robot:
source /opt/ros/humble/setup.bash
cd $HOME/ros2_ws
colcon build --packages-select nav2_behaviors --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF 2>&1 | tail -5
The complete script (scripts/install_nav2_behaviors_fix.sh):
On the robot:
#!/bin/bash
# Run ON THE ROBOT. nav2_behaviors 1.1.20 (the installed apt version) rebuilt from the upstream tag with ONE upstream
# fix: "Fix AssistedTeleop lateral projection sign" (ros-navigation/navigation2 #6535, on main 2026-09-17, tagged for
# backport to Lyrical only - no Humble release has it). Without it AssistedTeleop checks the side the robot strafes
# AWAY from. Measured on this robot 2026-10-02 (scripts/tests/nav2_isolated_checks.sh teleop):
# wall on the left, strafe left 0.2 m/s -> 0.20 out (not slowed) | wall on the right, strafe left -> 0.14 (slowed)
# Result: ~/ros2_ws/install/nav2_behaviors overlays /opt/ros/humble for everything that sources ~/ros2_ws
# (rosorin-nav, explore_run.sh). behavior/practice.py then practises sideways again by itself.
# Check afterwards: bash ~/setup/tests/nav2_isolated_checks.sh teleop (left wall must slow a left strafe).
# Rollback: scripts/rollback_nav2_behaviors_fix.sh (back to the apt build).
set -e
WANT=1.1.20
V=$(dpkg-query -W -f='${Version}' ros-humble-nav2-behaviors | cut -d- -f1)
[ "$V" = "$WANT" ] || { echo "installed nav2_behaviors is $V; this fix was made for $WANT - check upstream first"; exit 1; }
SRC=$HOME/ros2_ws/src/nav2_behaviors
TMP=$(mktemp -d)
git clone -q --depth 1 --branch $WANT --filter=blob:none --sparse https://github.com/ros-navigation/navigation2.git $TMP/nav2
git -C $TMP/nav2 sparse-checkout set nav2_behaviors
rm -rf $SRC && cp -r $TMP/nav2/nav2_behaviors $SRC && rm -rf $TMP
python3 - $SRC/plugins/assisted_teleop.cpp <<'PY'
import sys
p = sys.argv[1]; s = open(p).read()
old = """ projected_pose.x += projection_time * (
twist.linear.x * cos(pose.theta) +
twist.linear.y * sin(pose.theta));
projected_pose.y += projection_time * (
twist.linear.x * sin(pose.theta) -
twist.linear.y * cos(pose.theta));
"""
new = """ // rosorin: upstream fix #6535 (lateral projection sign), not backported to Humble
projected_pose.x += projection_time * (
twist.linear.x * cos(pose.theta) -
twist.linear.y * sin(pose.theta));
projected_pose.y += projection_time * (
twist.linear.x * sin(pose.theta) +
twist.linear.y * cos(pose.theta));
"""
assert s.count(old) == 1, 'assisted_teleop.cpp does not contain the expected projectPose lines'
open(p, 'w').write(s.replace(old, new))
print('patched', p)
PY
source /opt/ros/humble/setup.bash
cd $HOME/ros2_ws
colcon build --packages-select nav2_behaviors --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF 2>&1 | tail -5
source install/setup.bash
echo "nav2_behaviors now from: $(ros2 pkg prefix nav2_behaviors)"
Run it with nothing driving (it was run on 2026-10-02 with the wheels disabled and the owner's OK):
On the robot:
bash ~/setup/install_nav2_behaviors_fix.sh
Check
The run on this robot ended with:On the robot:
patched /home/burgerbarn/ros2_ws/src/nav2_behaviors/plugins/assisted_teleop.cpp Finished <<< nav2_behaviors [1min 12s] Summary: 1 package finished [1min 13s] nav2_behaviors now from: /home/burgerbarn/ros2_ws/install/nav2_behaviorsThe last line must name
~/ros2_ws/install, not/opt/ros/humble. The behaviour check comes later in this
chapter ("Test it without the floor").
If it fails
installed nav2_behaviors is X; this fix was made for 1.1.20: apt has moved on. Check whether the new
release contains #6535; if it does, you do not need the overlay. If not, the patch may still apply, but that
has not been tried.- A plain
colcon buildin~/ros2_wsrebuilds the overlay too (about a minute). That is expected.- On 2026-10-05 a stop of navigation left the overlay's
behavior_serverrunning, and a second behaviour server
then answered the next run.scripts/nav_stop.shnow kills processes fromros2_ws/install/nav2_behaviors
by path.- To go back to the apt build:
bash ~/setup/rollback_nav2_behaviors_fix.sh(deletes the overlay's src, build
and install folders).
ros2/rosorin_base/config/nav2.yaml (281 lines) is Nav2's stock nav2_bringup/params/nav2_params.yaml with this
robot's changes. Open it next to this section. Its header lists what differs from stock:
On the robot:
# Nav2 1.1.20 nav2_bringup/params/nav2_params.yaml (apt) with ROSOrin changes (docs/navigation.md):
# use_sim_time false; AMCL base_link + OmniMotionModel + laser 0.12..8 m; odom /odometry/filtered;
# DWB holonomic, NO reverse (min_vel_x 0, smoother min vx 0: LiDAR rear ~65 deg blind), 0.35 m/s forward,
# 0.08 m/s strafe, 1.0 rad/s; footprint polygon from chassis model; BT without BackUp and without Spin;
# goal tolerance 0.2 m / 0.3 rad. (Header corrected 2026-10-02: it still listed the 09-28 values.)
docs/navigation.md still lists the 2026-09-28 values (0.15 m/s, 0.6 rad/s, goal tolerance 0.08, a stop polygon).
The file is what runs; this section teaches the file.
One rule decides most of the numbers: no reverse. The LiDAR is blind over the rear ~65 degrees (138 to 180 to
155 degrees, the arm tower), so the robot never drives backwards in navigation. The limits are repeated in four
places, and the lowest one wins:
| Where | Forward vx | Sideways vy | Turn wz | Acceleration |
|---|---|---|---|---|
DWB controller (FollowPath) |
0.0 to 0.35 m/s, min_speed_xy 0.1 |
-0.08 to 0.08 m/s | 1.0 rad/s, min_speed_theta 0.4 |
0.5 m/s², 2.0 rad/s² |
velocity_smoother |
max_velocity 0.35, min_velocity 0.0 |
±0.08 | ±1.0 | max_accel [0.5, 0.5, 2.0] |
behavior_server |
(BackUp/DriveOnHeading not in the tree) | – | max_rotational_vel 1.0, min 0.4 |
rotational_acc_lim 3.2 |
base driver (arm_limits.yaml, chapter 11) |
max_linear 0.35 |
0.35 | max_angular 1.0 |
0.5 m/s², 2.0 rad/s² |
Why each odd value:
max_vel_x: 0.35 (owner 2026-10-01: faster, was 0.2).min_speed_xy: 0.1 and min_speed_theta: 0.4: on the rug, slower commands stall ("45 % at 0.1, none below").max_vel_y: 0.08: forward first since 2026-10-02. The robot stalled strafing onto the rug edge (it gets over itOn the robot:
FollowPath:
plugin: dwb_core::DWBLocalPlanner
debug_trajectory_details: false # was true (a debugging leftover); no measurable effect on load (617 vs 656 late loops)
min_vel_x: 0.0
max_vel_x: 0.35 # owner 2026-10-01: faster (was 0.2). (Line lost 2026-10-02 in commit 2b2dc07: the
# controller then had NO forward speed and drove sideways only - found from /evaluation)
On the robot:
# Sampled trajectories = vx x vy x vtheta. Was 20 x 5 x 20 = 2000: the controller used 95 % of one core and missed
# its 20 Hz loop on nearly every cycle (656 late loops in 36 s, measured on the robot 2026-10-05 with
# scripts/tests/nav_load_test.sh, wheels disabled); a whole run then failed on stale TF. 10 x 5 x 10 = 500: 31 %
# of a core, 2 late loops. Resolution: 0.035 m/s forward, 0.2 rad/s turn.
vx_samples: 10
vy_samples: 5
vtheta_samples: 10
sim_time: 1.7
The critics that score each sampled motion:
On the robot:
# ObstacleFootprint (not BaseObstacle): the whole footprint is checked, not the centre point - it had parked its
# nose 6 cm from a chair leg (2026-10-02)
critics: [RotateToGoal, Oscillation, ObstacleFootprint, GoalAlign, PathAlign, PathDist,
GoalDist, PreferForward]
PreferForward.scale: 5.0
PreferForward penalises pure strafing; NVIDIA's reference controller also prefers forward.
The progress checker aborts a goal when the robot has not moved 0.25 m within 10 s. The goal checker
declares success inside 0.2 m and 0.3 rad. stateful: true means once the position is reached, only the heading is
still corrected.
On the robot:
progress_checker: {plugin: 'nav2_controller::SimpleProgressChecker', required_movement_radius: 0.25,
movement_time_allowance: 10.0}
general_goal_checker: {stateful: true, plugin: 'nav2_controller::SimpleGoalChecker',
xy_goal_tolerance: 0.2, yaw_goal_tolerance: 0.3} # 0.08 made it dither a minute 10 cm from a goal (2026-10-02)
docs/decisions.md 2026-10-02 mentions a progress checker of "4 s / 0.15 m"; the file says 0.25 m / 10 s, and the
file runs.
On the robot:
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: false
planner_plugins: [GridBased]
GridBased: {plugin: nav2_navfn_planner/NavfnPlanner, tolerance: 0.5, use_astar: false,
allow_unknown: true}
smoother_server:
ros__parameters:
use_sim_time: false
smoother_plugins: [simple_smoother]
simple_smoother: {plugin: 'nav2_smoother::SimpleSmoother', tolerance: 1.0e-10,
max_its: 1000, do_refinement: true}
NavFn searches the global costmap (tolerance: 0.5: a goal inside an obstacle is accepted if a free cell lies within
0.5 m). allow_unknown: true lets paths cross cells nobody has seen yet. The smoother_server smooths the planned
path; it has nothing to do with the velocity_smoother, which limits commands. Swapping NavFn for Smac Lattice
was deliberately not done (it needs the floor to test).
On the robot:
# ms the tree waits for a service answer (stock 20). Measured 2026-10-05 on this robot, navigation idle: a global
# costmap clear takes 25-82 ms, a local one up to 412 ms. In the first floor run through the always-on stack every
# clear timed out (43 times in 2 min) and the stale obstacle cells around the robot never went away.
default_server_timeout: 1000
wait_for_service_timeout: 1000
The unit is milliseconds. With the stock 20 ms the recovery branch silently never cleared anything on this Jetson.
The same block sets the tree file by absolute path:
default_nav_to_pose_bt_xml: /home/burgerbarn/ros2_ws/install/rosorin_base/share/rosorin_base/bt/navigate_no_backup.xml.
The robot's outline (footprint) comes from the chassis model: 30.5 cm long, 23.5 cm wide, with 1 cm padding:
On the robot:
footprint: '[[0.155, 0.12], [0.155, -0.115], [-0.15, -0.115], [-0.15, 0.12]]'
footprint_padding: 0.01
Local costmap (odom frame, rolling 3 x 3 m, 5 cm cells, updated at 5 Hz). Layers: voxel_layer (LiDAR),
nvblox_layer (depth camera), inflation_layer (0.55 m radius, cost scaling 3.0):
On the robot:
plugins: [voxel_layer, nvblox_layer, inflation_layer]
# NVIDIA nvblox: obstacles 4.5-60 cm above the floor seen by the depth camera (what the LiDAR plane misses)
nvblox_layer: {plugin: 'nvblox::nav2::NvbloxCostmapLayer', enabled: true, nav2_costmap_global_frame: odom,
nvblox_map_slice_topic: /nvblox_node/static_map_slice, convert_to_binary_costmap: true}
# no keepout here: before AMCL converges it lands at a wrong place, under the robot, and Spin refuses
# ("Collision Ahead", run 2 2026-10-01). Keepout lives in the global costmap = every planned path.
The voxel layer has a depth: block for the camera point cloud, but it is not in observation_sources:
On the robot:
observation_sources: scan # depth REMOVED 2026-09-30: flooded the global costmap (38 % lethal) -> baseline first; depth gets its own test
The depth camera reaches the costmaps only through nvblox. track_unknown_space is off in the local costmap; this
was measured on 2026-10-02: with it on, 38 of the 156 cells around the standing robot were unknown, and in-place turns
were allowed only for 5-10 degrees, sideways moves not at all. It stays off until the camera can look beside and
behind the body before a turn.
Global costmap (map frame, the whole room, 1 Hz). Layers: static_layer (walls from slam_toolbox's /map),
obstacle_layer (LiDAR), nvblox_layer, inflation_layer, plus the room keepout as a filter:
On the robot:
track_unknown_space: true
plugins: [static_layer, obstacle_layer, nvblox_layer, inflation_layer]
nvblox_layer: {plugin: 'nvblox::nav2::NvbloxCostmapLayer', enabled: true, nav2_costmap_global_frame: map,
nvblox_map_slice_topic: /nvblox_node/static_map_slice, convert_to_binary_costmap: true}
filters: [keepout_filter] # room keepout (owner 2026-10-01): outside the room walls = lethal
keepout_filter: {plugin: 'nav2_costmap_2d::KeepoutFilter', enabled: true, filter_info_topic: /costmap_filter_info}
The mask itself is served by two small servers at the end of the file. Both topic names are absolute on purpose:
On the robot:
costmap_filter_info_server:
ros__parameters:
use_sim_time: False
type: 0
filter_info_topic: /costmap_filter_info
mask_topic: /keepout_filter_mask
base: 0.0
multiplier: 1.0
On the robot:
behavior_plugins: [spin, backup, drive_on_heading, assisted_teleop, wait]
spin: {plugin: nav2_behaviors/Spin} # loaded, NOT in the tree (bt/navigate_no_backup.xml says why)
# Not in the tree either. Ramps set so that any use starts and stops like every other motion: with the stock
# 0.0 limits Nav2 1.1.20 sends the commanded speed as a step (drive_on_heading.hpp). minimum_speed = the rug
# stall speed (DWB min_speed_xy).
backup: {plugin: nav2_behaviors/BackUp, acceleration_limit: 0.5, deceleration_limit: -0.5, minimum_speed: 0.1}
drive_on_heading: {plugin: nav2_behaviors/DriveOnHeading, acceleration_limit: 0.5, deceleration_limit: -0.5,
minimum_speed: 0.1}
Spin and BackUp stay loaded because old tools call them directly (scripts/relocalize_spin.py,
behavior/room_explorer.py); nothing in the current flow does. AssistedTeleop is the patched one from the overlay.
The amcl: block (NVIDIA's Nova Carter tuning, omni motion model for mecanum wheels) is used only with
slam:=false, the fallback to a fixed map. With the default slam:=true, slam_toolbox localizes (chapter 17).
scripts/tests/check_nav2_config.py exists because of a real loss. On 2026-10-02 an edit that "reverted" a change
also cut the line max_vel_x. Nav2 raised no error: the controller sampled only vx = 0, and for five floor runs the
robot could only strafe and turn, straight into the rug edge. It was found from DWB's own scores (/evaluation), not
from any warning. The check is 43 lines: it loads the YAML, requires each limit to exist and sit in a sane range,
requires every costmap plugin to have parameters, refuses a tree that contains Spin, BackUp or DriveOnHeading, and
refuses BackUp/DriveOnHeading without ramps.
On your laptop:
#!/usr/bin/env python3
"""Sanity check of config/nav2.yaml before it is deployed (2026-10-02: an edit deleted max_vel_x; DWB then sampled
vx = 0 only and the robot could only strafe and turn for five runs). Exit 1 with the reason if a limit is missing or
out of range. Usage: check_nav2_config.py [path]"""
import sys
import yaml
p = sys.argv[1] if len(sys.argv) > 1 else 'ros2/rosorin_base/config/nav2.yaml'
d = yaml.safe_load(open(p))
f = d['controller_server']['ros__parameters']['FollowPath']
vs = d['velocity_smoother']['ros__parameters']
bad = []
for k, lo, hi in (('max_vel_x', 0.1, 0.5), ('max_vel_theta', 0.3, 2.0), ('max_speed_xy', 0.1, 0.5), ('acc_lim_x', 0.1, 2.5),
('acc_lim_theta', 0.5, 5.0), ('vx_samples', 5, 50), ('vtheta_samples', 5, 50), ('sim_time', 0.5, 3.0)):
if k not in f or not lo <= f[k] <= hi:
bad.append(f'FollowPath.{k} = {f.get(k, "MISSING")} (want {lo}..{hi})')
for k in ('min_vel_x', 'min_vel_y', 'max_vel_y', 'decel_lim_x', 'critics'):
if k not in f:
bad.append(f'FollowPath.{k} MISSING')
if f.get('max_vel_x', 0) > vs['max_velocity'][0] + 1e-6:
bad.append('controller max_vel_x above the velocity smoother limit')
if vs['max_velocity'][0] < 0.1 or vs['max_velocity'][2] < 0.3:
bad.append(f'velocity_smoother max_velocity {vs["max_velocity"]}')
for cm in ('local_costmap', 'global_costmap'):
pl = d[cm][cm]['ros__parameters']
for layer in pl['plugins']:
if layer not in pl:
bad.append(f'{cm}: plugin {layer} has no parameters')
# 2026-10-02 (research fixes): the recovery tree has neither Spin nor BackUp, and any direct use of the drive
# behaviours ramps (stock 0.0 limits = velocity step)
import os, re
bt = os.path.join(os.path.dirname(os.path.abspath(p)), '..', 'bt', 'navigate_no_backup.xml')
if os.path.exists(bt):
for tag in re.findall(r'<(Spin|BackUp|DriveOnHeading)\b', open(bt).read()):
bad.append(f'behaviour tree contains <{tag}> (blind recovery motion; see the header of the tree file)')
b = d['behavior_server']['ros__parameters']
for name in ('backup', 'drive_on_heading'):
if not (b.get(name, {}).get('acceleration_limit', 0) > 0 and b.get(name, {}).get('deceleration_limit', 0) < 0):
bad.append(f'behavior_server.{name}: acceleration_limit / deceleration_limit not set (velocity step)')
if bad:
print('nav2.yaml NOT OK:'); [print(' -', b) for b in bad]; sys.exit(1)
print('nav2.yaml ok: vx 0..%.2f, vy %.2f..%.2f, wz %.1f' % (f['max_vel_x'], f['min_vel_y'], f['max_vel_y'], f['max_vel_theta']))
Run it on your laptop, in the repo, after any edit of nav2.yaml and before copying it to the robot:
On your laptop:
cd ~/CCode/rosorin-pro
python3 scripts/tests/check_nav2_config.py
git diff ros2/rosorin_base/config/
Check
On your laptop:nav2.yaml ok: vx 0..0.35, vy -0.08..0.08, wz 1.0Run against the broken version from that night (
git show 2b2dc07:ros2/rosorin_base/config/nav2.yaml > /tmp/nav2_bad.yaml), it prints what it was written to catch, and exits 1:On your laptop:
nav2.yaml NOT OK: - FollowPath.max_vel_x = MISSING (want 0.1..0.5)
If it fails
- A parameter key that no node reads raises no error anywhere. The check covers the keys that caused trouble so
far; it cannot prove a value is used. For anything else, read it back from the running node
(ros2 param get /controller_server FollowPath.max_vel_x).- Keepout
mask_topicandfilter_info_topicmust start with/. Relative names made the local costmap
subscribe under/local_costmap/...(2026-10-01). The filter also logs errors aboutodom -> mapuntil the
robot is localized; that is expected.
ros2/rosorin_base/bt/navigate_no_backup.xml is Nav2 1.1.20's navigate_to_pose_w_replanning_and_recovery.xml with
two recoveries taken out. What is left is NVIDIA's Nova Carter tree: clear the costmaps, wait, try again. The
complete file:
On the robot:
<!--
nav2_bt_navigator 1.1.20 navigate_to_pose_w_replanning_and_recovery.xml with the BackUp and Spin recoveries
removed (rosorin) = NVIDIA's Nova Carter tree: clear the costmaps, wait, try again.
BackUp: LiDAR is blind over the rear ~65 deg (138..180..155 deg), reversing is unsafe (screen door, 2026-10-02).
Spin (removed 2026-10-02): Nav2 1.1.20 publishes its first command without any collision check and then looks
ahead only as far as it has already turned; measured on the robot's build with a post 3 cm from the nose: 1.0 rad/s
for the whole run (scripts/tests/nav2_isolated_checks.sh spin; docs/research/nav2_reference.md section 20).
-->
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<RoundRobin name="RecoveryActions">
<Sequence name="ClearingActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Wait wait_duration="5"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>
Read it from the inside out:
PipelineSequence NavigateWithReplanning runs two children side by side: a new path once a secondRateController hz="1.0" around ComputePathToPose), and FollowPath driving along the latest path.RecoveryNode (6 retries) runs the RecoveryFallback. It takes turnsRoundRobin): first clear both costmaps, next time wait 5 s. GoalUpdated cuts the recovery short when a new goalWhy the two missing recoveries are missing, from the record:
The cost of this tree: when the robot ends up inside the inflation ring of a soft obstacle (a bean bag 16 cm from the
LiDAR), NavFn cannot plan from there, and clearing the costmaps does nothing because the obstacle is re-marked at once.
The way out is a short sensed crawl, which lives in the explore skill (behavior/unstick.py, chapter 24), not in the
tree.
nav.launch.py loads NVIDIA's stock nvblox_base.yaml first and then this file on top, so everything not listed
is NVIDIA's default. The complete ros2/rosorin_base/config/nvblox.yaml:
On the robot:
# nvblox (Isaac ROS 3.2) on the Aurora 930 depth: GPU 3D map -> 2D obstacle slice -> Nav2 costmaps (nvblox_layer).
# Loaded after NVIDIA's stock nvblox_base.yaml (nav.launch.py); everything not listed here is NVIDIA's default.
# TSDF decay stays STOCK (0.95 at 5 Hz): what the camera no longer sees is gone from the slice within ~14 s (p90
# 13.8 s, max 21 s, replay of bag 20261002_121621). Slower or no decay was tested by replay on 2026-10-02 and
# rejected: obstacle cells then stay on floor the robot itself drove over in 83-95 % of the snapshots (stock: 31-41 %).
# Depth reaches nvblox through rosorin_base/depth_gate (only while the arm is still).
/**:
ros__parameters:
global_frame: odom # as in NVIDIA's reference setup; the costmap layer transforms to map itself
use_color: false
use_lidar: false # the COIN-D6 is 2D; the LiDAR stays in Nav2's own layers
use_depth: true
esdf_slice_min_height: 0.03 # odom z = base_link height: from ~4.5 cm above the floor (rug stays floor)
esdf_slice_max_height: 0.6 # up to the top of the robot with the arm in the drive pose
esdf_slice_height: 0.3
integrate_depth_rate_hz: 10.0
map_clearing_radius_m: 6.0
docs/lessons.md).The camera sits on the arm. Its position in space (TF) comes from the arm's joint states. While the arm moves, that
TF does not match the image: a slow single arm move blocks the driver so no joint states arrive at all, and streamed
moves report the commanded position while the servos lag behind. On every wheel enable the driver moves the arm to the
drive pose. Replaying a recorded run on 2026-10-02 showed what nvblox did with those frames: the obstacle slice jumped
from 330 to 1244 cells, put 73 obstacle cells on plain rug 0.5-1.7 m straight ahead, and stock decay needed about 12 s
to remove them. nvblox has no input gate, so the robot got one: depth_gate republishes a depth frame only when the
arm has been still for 0.6 s.
You will find the file at ~/ros2_ws/src/rosorin_base/rosorin_base/depth_gate.py. Read it in four parts.
Parameters and subscriptions. Note raw=True on the depth subscription: the node never decodes the image, it passes
the serialized bytes through untouched, which costs almost nothing:
On the robot:
class DepthGate(Node):
def __init__(self):
super().__init__('depth_gate')
dp = self.declare_parameter
self.settle = dp('settle_s', 0.6).value
self.stale = dp('stale_s', 0.25).value # joint states come at 10 Hz
self.eps = dp('joint_eps_rad', 0.002).value
self.last_pos, self.t_joint, self.t_change = None, None, None
self.busy, self.t_busy_end = False, None
self.passed = self.dropped = 0
self.why = {}
self.pub = self.create_publisher(Image, '~/depth', 5) # reliable: matches any subscriber QoS (nvblox)
self.create_subscription(JointState, 'joint_states', self.on_joints, 50)
latched = QoSProfile(depth=1, reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
self.create_subscription(Bool, 'rosorin_board_driver/arm_busy', self.on_busy, latched)
self.create_subscription(Image, 'depth_in', self.on_depth, qos_profile_sensor_data, raw=True)
self.create_timer(30.0, self.report)
Tracking the arm. A joint that moved more than 0.002 rad marks a change; a silence longer than stale_s (a blocking
move) also restarts the settle clock; the driver's latched arm_busy flag covers its own blocking moves:
On the robot:
def on_joints(self, m):
t = m.header.stamp.sec + m.header.stamp.nanosec * 1e-9
pos = dict(zip(m.name, m.position))
if self.last_pos is not None and any(abs(pos.get(k, v) - v) > self.eps for k, v in self.last_pos.items()):
self.t_change = t
if self.last_pos is None or (self.t_joint is not None and t - self.t_joint > self.stale):
self.t_change = t # first message, or back after a silence: settle first
self.last_pos, self.t_joint = pos, t
def on_busy(self, m):
if self.busy and not m.data:
self.t_busy_end = self.now()
self.busy = m.data
The decision, per frame, against the frame's own timestamp. It reads the stamp straight out of the serialized
message: 4 bytes of CDR encapsulation header, then header.stamp (int32 seconds, uint32 nanoseconds):
On the robot:
def reason(self, t):
if self.t_joint is None:
return 'no joint states yet'
if t - self.t_joint > self.stale:
return 'joint states silent (blocking arm move)'
if self.t_change is not None and t - self.t_change < self.settle:
return 'arm moved'
if self.busy:
return 'driver: arm busy'
if self.t_busy_end is not None and self.now() - self.t_busy_end < self.settle:
return 'arm busy just ended'
return None
def on_depth(self, raw):
sec, nsec = struct.unpack_from('<iI', raw, 4) # CDR: 4-byte encapsulation header, then header.stamp
why = self.reason(sec + nsec * 1e-9)
if why is None:
self.passed += 1
self.pub.publish(raw)
else:
self.dropped += 1
self.why[why] = self.why.get(why, 0) + 1
A report every 30 s (only when something was held back), and main. The complete file:
On the robot:
"""Depth for nvblox only while the head (arm) is still.
Why (measured 2026-10-02, bag 20261002_121621, scripts/tests/nvblox_*): the camera sits on the arm and its TF comes
from /joint_states. While the arm moves, the TF does not match the image (a single slow move blocks the driver: no
joint states at all until it ends; streamed moves report the command, the servos lag behind). nvblox integrated
those frames: in the second the arm went to its drive pose the obstacle slice jumped from 330 to 1244 cells and put
73 obstacle cells on plain rug 0.5-1.7 m straight ahead; stock decay needed ~12 s to remove them, the robot stood
still for 8 s. nvblox has no input gate of its own, so this node is the gate: it republishes the depth image
(serialized, untouched) on ~/depth only when the arm has been still for settle_s.
A depth frame with stamp T passes when
- joint states are fresh at T (the newest one is at most stale_s older than T) - a blocking move silences them;
- no joint moved more than eps within settle_s before T;
- the driver does not say arm_busy (set around its blocking moves) and has not for settle_s.
Custom code because neither nvblox nor Nav2 has this; vslam_odom does the same for the camera odometry.
Note: with wheels AND arm disabled the driver re-reads the servos every 2 s (joint states pause ~0.5 s), so an idle
robot loses some frames here; nvblox only runs inside navigation, where the wheels are enabled and nothing is read."""
import struct
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy, qos_profile_sensor_data
from sensor_msgs.msg import Image, JointState
from std_msgs.msg import Bool
class DepthGate(Node):
def __init__(self):
super().__init__('depth_gate')
dp = self.declare_parameter
self.settle = dp('settle_s', 0.6).value
self.stale = dp('stale_s', 0.25).value # joint states come at 10 Hz
self.eps = dp('joint_eps_rad', 0.002).value
self.last_pos, self.t_joint, self.t_change = None, None, None
self.busy, self.t_busy_end = False, None
self.passed = self.dropped = 0
self.why = {}
self.pub = self.create_publisher(Image, '~/depth', 5) # reliable: matches any subscriber QoS (nvblox)
self.create_subscription(JointState, 'joint_states', self.on_joints, 50)
latched = QoSProfile(depth=1, reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
self.create_subscription(Bool, 'rosorin_board_driver/arm_busy', self.on_busy, latched)
self.create_subscription(Image, 'depth_in', self.on_depth, qos_profile_sensor_data, raw=True)
self.create_timer(30.0, self.report)
def now(self):
return self.get_clock().now().nanoseconds * 1e-9
def on_joints(self, m):
t = m.header.stamp.sec + m.header.stamp.nanosec * 1e-9
pos = dict(zip(m.name, m.position))
if self.last_pos is not None and any(abs(pos.get(k, v) - v) > self.eps for k, v in self.last_pos.items()):
self.t_change = t
if self.last_pos is None or (self.t_joint is not None and t - self.t_joint > self.stale):
self.t_change = t # first message, or back after a silence: settle first
self.last_pos, self.t_joint = pos, t
def on_busy(self, m):
if self.busy and not m.data:
self.t_busy_end = self.now()
self.busy = m.data
def reason(self, t):
if self.t_joint is None:
return 'no joint states yet'
if t - self.t_joint > self.stale:
return 'joint states silent (blocking arm move)'
if self.t_change is not None and t - self.t_change < self.settle:
return 'arm moved'
if self.busy:
return 'driver: arm busy'
if self.t_busy_end is not None and self.now() - self.t_busy_end < self.settle:
return 'arm busy just ended'
return None
def on_depth(self, raw):
sec, nsec = struct.unpack_from('<iI', raw, 4) # CDR: 4-byte encapsulation header, then header.stamp
why = self.reason(sec + nsec * 1e-9)
if why is None:
self.passed += 1
self.pub.publish(raw)
else:
self.dropped += 1
self.why[why] = self.why.get(why, 0) + 1
def report(self):
if self.dropped:
self.get_logger().info(f'depth frames: {self.passed} passed, {self.dropped} held back {self.why}')
self.passed = self.dropped = 0
self.why = {}
def main():
rclpy.init()
n = DepthGate()
try:
rclpy.spin(n)
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
n.report()
n.destroy_node()
if rclpy.ok():
rclpy.shutdown()
setup.py registers it as depth_gate = rosorin_base.depth_gate:main (already there from chapter 11). In the launch
file its input depth_in is remapped to /aurora/depth/image_raw, and nvblox reads its output /depth_gate/depth.
Check
These two lines appear once navigation is running (next sections). The first is from the live test on
2026-10-02 (45 s, wheels disabled); the second from the first nvblox run the same morning:On the robot:
journalctl -u rosorin-nav --since "-5 min" --no-pager -o cat | grep "depth frames" source /opt/ros/humble/setup.bash timeout 8 ros2 topic hz /nvblox_node/static_map_slice | grep average | tail -1On the robot:
[INFO] [1790959975.070133442] [depth_gate]: depth frames: 399 passed, 8 held back {'arm moved': 8} average rate: 9.365
If it fails
- nvblox's slice fills the floor ahead with obstacles right after the wheels are enabled: frames from the arm
move reached nvblox. Check that nvblox's depth input is remapped to/depth_gate/depth, not straight to the
camera.- nvblox was first run with
/aurora/ir/camera_info. The driver registers depth to the RGB camera (its own
point cloud uses the RGB intrinsics fx 419.9, cx 317.5, fy 421.0, cy 192.9), so the launch file uses
/aurora/rgb/camera_info.- With wheels and arm both disabled the driver re-reads the servos every 2 s and joint states pause about 0.5 s,
so an idle robot loses some frames here. That is expected.
The base driver latches its e-stop the moment /cmd_vel has more than one publisher (chapter 11). So inside
navigation, exactly one node may publish /cmd_vel: the collision monitor, last in line.
In the stock Nav2 launch the behaviour server publishes cmd_vel directly. That would be a second /cmd_vel
publisher and trip the driver, so nav.launch.py remaps both the controller and the behaviour server into
cmd_vel_nav. The velocity smoother reads cmd_vel_nav and publishes cmd_vel_smoothed; the collision monitor reads
that and publishes /cmd_vel.
The collision monitor's parameter file ros2/rosorin_base/config/collision_monitor.yaml is complete here; what it
really does on Humble (which is not all of what it says) is measured in chapter 19.
On the robot:
# nav2_collision_monitor 1.1.20 params/collision_monitor_params.yaml (apt) with ROSOrin changes:
# base_link, no sim time, input = velocity_smoother output, polygons sized to the chassis
# (x -0.15..0.155, y -0.115..0.12), no pointcloud source. Final stage before board_driver: the ONLY
# cmd_vel publisher. Added after the 2026-09-28 Nav2 collision (docs/navigation.md).
collision_monitor:
ros__parameters:
use_sim_time: False
base_frame_id: "base_link"
odom_frame_id: "odom"
cmd_vel_in_topic: "cmd_vel_smoothed"
cmd_vel_out_topic: "cmd_vel"
transform_tolerance: 0.5
# Humble (1.1.20): a scan older than this is IGNORED - the monitor then has no points and passes the command
# through unchanged (measured 2026-10-02, scripts/tests/nav2_isolated_checks.sh: wall at the nose, scan silent
# -> 0.2 m/s out). "Stop when the source is silent" exists only from Jazzy. The stop for a silent LiDAR is the
# base driver's scan gate (board_driver scan_timeout 0.5 s).
source_timeout: 1.0
base_shift_correction: True
stop_pub_timeout: 2.0
polygons: ["FootprintApproach", "PolygonSlow"]
# owner 2026-10-01: the stop polygon froze the robot facing a closed door (it blocks ALL motion, turning away
# too; no reverse in the BT). Approach (Nav2 native) only limits motion that would reach contact within
# time_before_collision, so turning/strafing away stays possible.
# AS IT RUNS on Humble (measured 2026-10-02 + polygon.cpp): an approach polygon does not use `points`. It takes
# the footprint the local costmap publishes (local_costmap/published_footprint = nav2.yaml footprint +
# footprint_padding 0.01), i.e. chassis + 1 cm, NOT the + 3 cm written here on 2026-10-01; and with no local
# costmap running it has no polygon at all. `points` is kept only as the record of that intent. To get 3 cm the
# knob is the local costmap's footprint_padding (it also widens what the controller keeps clear).
# One return inside the footprint stops ALL axes (static check, >= max_points); the look-ahead and the slowdown
# zone need TWO returns (> max_points). max_points 0 would never move.
FootprintApproach:
type: "polygon"
action_type: "approach"
points: [0.185, 0.15, 0.185, -0.145, -0.18, -0.145, -0.18, 0.15]
time_before_collision: 1.2
simulation_time_step: 0.1
max_points: 1 # Humble: points ALLOWED before acting
visualize: True
polygon_pub_topic: "polygon_approach"
enabled: True
PolygonSlow: # front +0.25 m, sides +0.08 m, 60 % (owner 2026-10-01: "way too conservative" -
# was +0.45/+0.20 at 40 %: every in-place spin near a wall ran at 0.4 rad/s)
type: "polygon"
points: [0.405, 0.20, 0.405, -0.195, -0.10, -0.195, -0.10, 0.20]
action_type: "slowdown"
max_points: 1 # Humble: points ALLOWED before acting; 3 let a 3-point object pass (tested)
slowdown_ratio: 0.6
visualize: True
polygon_pub_topic: "polygon_slowdown"
enabled: True
observation_sources: ["scan"]
scan:
type: "scan"
topic: "/scan"
enabled: True
Check
With navigation running (after the service section below):On the robot:
source /opt/ros/humble/setup.bash ros2 topic info -v /cmd_vel | grep -E "count|Node name|Endpoint type"Live on 2026-10-07 (
Node namealso matches theNode namespacelines):On the robot:
Publisher count: 1 Node name: collision_monitor Node namespace: / Endpoint type: PUBLISHER Subscription count: 1 Node name: rosorin_board_driver Node namespace: / Endpoint type: SUBSCRIPTION
If it fails
Publisher count: 2and the driver log saysE-STOP latched: multiple cmd_vel publishers: something else
publishes/cmd_vel. Seen on this robot: a killed test run's lingering publisher (2026-09-30), a diagnostic
script that created a/cmd_velpublisher while navigation ran (2026-10-02), and the stop-proof tools of
chapter 19 if navigation is not stopped first. Rule: read-only tools create no publishers on command topics.- Navigation stopped with
Ctrl-Conros2 launchonce left Nav2 nodes running (2026-09-28); their
collision monitor then answered the next run. Always stop withscripts/nav_stop.shor through systemd.
The numbers in nav2.yaml are where the robot starts. Since 2026-10-02 the driving limits may come from the robot's
own practice: behavior/learn_body.py (chapter 24) writes ~/practice/body_model.json from the robot's practice
moves, and at every launch learned_limits.apply() patches the loaded parameters before Nav2 sees them.
The rules, at the top of the file: nothing changes until 60 practice moves exist; a row counts only after 3 moves of
the same kind and speed; a speed "works" when it stalls in at most 10 % of tries and gets at least 60 % of the
distance, on every floor it was tried on; and nothing ever goes above the driver's 0.35 m/s:
On the robot:
MODEL = os.path.expanduser('~/practice/body_model.json')
MIN_TRIALS = 60 # practice moves before the model is used at all
MIN_N = 3 # moves of one kind/speed/floor before that row counts
OK_STALL, OK_RATIO = 0.10, 0.60
CEIL = 0.35 # never above the driver's own limit
def works(rows, move):
"""Speeds of this kind of move that work on EVERY floor it has tried them on (enough tries, rarely stalls,
gets most of the distance). Sorted."""
by_speed = {}
for r in rows:
if r['move'] == move and r['n'] >= MIN_N:
by_speed.setdefault(r['speed'], []).append(r['stall_rate'] <= OK_STALL and r['got_ratio'] >= OK_RATIO)
return sorted(s for s, ok in by_speed.items() if all(ok))
apply() then sets the DWB forward limits and the smoother from the fastest and slowest working forward speeds, and
the sideways limit from sideways practice (0 if no sideways speed works, with a single sideways sample). The complete
file, ~/ros2_ws/src/rosorin_base/rosorin_base/learned_limits.py:
On the robot:
"""Driving limits come from what the robot learned about its own body (~/practice/body_model.json, written by
behavior/learn_body.py from its own practice moves) - not from numbers typed into nav2.yaml.
apply(cfg) patches a loaded nav2.yaml dict in place and returns what it changed (also written to
~/practice/applied_limits.json). With too little practice it changes nothing: nav2.yaml's values are only the starting
point the robot is born with."""
import json, os, time
MODEL = os.path.expanduser('~/practice/body_model.json')
MIN_TRIALS = 60 # practice moves before the model is used at all
MIN_N = 3 # moves of one kind/speed/floor before that row counts
OK_STALL, OK_RATIO = 0.10, 0.60
CEIL = 0.35 # never above the driver's own limit
def works(rows, move):
"""Speeds of this kind of move that work on EVERY floor it has tried them on (enough tries, rarely stalls,
gets most of the distance). Sorted."""
by_speed = {}
for r in rows:
if r['move'] == move and r['n'] >= MIN_N:
by_speed.setdefault(r['speed'], []).append(r['stall_rate'] <= OK_STALL and r['got_ratio'] >= OK_RATIO)
return sorted(s for s, ok in by_speed.items() if all(ok))
def apply(cfg, model_path=MODEL):
try:
m = json.load(open(model_path))
except (OSError, ValueError):
return {'used': False, 'why': 'no body model yet'}
if m.get('trials', 0) < MIN_TRIALS:
return {'used': False, 'why': f"only {m.get('trials', 0)} practice moves (needs {MIN_TRIALS})"}
rows = m.get('moves', [])
f = cfg['controller_server']['ros__parameters']['FollowPath']
vs = cfg['velocity_smoother']['ros__parameters']
out = {'used': True, 'trials': m['trials'], 'changes': {}}
ahead, side = works(rows, 'ahead'), works(rows, 'sideways')
tried_side = any(r['move'] == 'sideways' and r['n'] >= MIN_N for r in rows)
if ahead:
vmax, vmin = min(CEIL, ahead[-1]), ahead[0]
out['changes'].update(max_vel_x=vmax, max_speed_xy=vmax, min_speed_xy=vmin)
f['max_vel_x'], f['max_speed_xy'], f['min_speed_xy'] = vmax, vmax, vmin
vs['max_velocity'][0] = vmax
if tried_side: # it knows how sideways goes on its floors
vy = min(CEIL, side[-1]) if side else 0.0
out['changes'].update(max_vel_y=vy)
f['max_vel_y'], f['min_vel_y'] = vy, -vy
f['vy_samples'] = 5 if vy > 0 else 1
vs['max_velocity'][1], vs['min_velocity'][1] = vy, -vy
out['t'] = time.strftime('%Y-%m-%d %H:%M:%S')
try:
json.dump(out, open(os.path.join(os.path.dirname(model_path), 'applied_limits.json'), 'w'), indent=1)
except OSError:
pass
return out
Check
The launch prints what it applied, and writes the same to~/practice/applied_limits.json:On the robot:
journalctl -u rosorin-nav --no-pager -o cat | grep "learned limits:" | tail -1On a fresh robot (no practice yet) you will see
{'used': False, 'why': 'no body model yet'}. Live on
2026-10-07 the robot had 67 practice moves and no row qualified, so nothing changed:On the robot:
learned limits: {'used': True, 'trials': 67, 'changes': {}, 't': '2026-10-07 17:23:08'}
Safety
Learned limits change speeds without anyone editingnav2.yaml, socheck_nav2_config.pydoes not see them.
The ceiling isCEIL = 0.35, the driver's own limit. If you change the driver limit, changeCEILtoo.
Stock Nav2 on Humble has three gaps for a robot that keeps navigation running all day, and the supervisor fills them
with stock calls only (docs/research/always_on_navigation_reference.md):
map -> base_link. Until the robot has a pose on the map, the plannerautostart off, and the supervisor sends STARTUP only once the robot is placed.map -> odom transform while AMCL kept publishing it; standing still, nothing logged it, and the status saidIt never moves the robot. It publishes one fact, /nav/status, which every skill waits for.
The file is ros2/rosorin_base/rosorin_base/nav_supervisor.py (572 lines). Its parts, in order:
| Part | Lines | What it does |
|---|---|---|
ScanMatcher |
55-133 | scores a pose on the map against one LiDAR scan: fits, through_walls, refine (local), search (whole map) |
NavSupervisor.__init__ |
136-183 | parameters, subscriptions (map, scan, amcl_pose, localizer result, driver state), service clients |
| inputs | 186-227 | rebuild the matcher only for a changed map; wheels on / plugged from the driver; scan to points |
| outputs | 230-307 | publish the status; call a service with a timeout; set_mapping; keep_map copies |
| the pose chain | 310-377 | seeds, place, seed_amcl |
probe_plan |
379-434 | plan to its own pose, then follow that path with the wheels disabled |
watch_pose |
436-452 | while ready: does the scan still fit? remember the pose |
run |
455-526 | the main loop |
heal, restart |
528-539 | RESET + STARTUP; exit code 1 |
main, wait_main |
542-572 | executor thread + loop; the nav_wait client |
The matcher turns the map into a distance image once: for every cell, how far it is from the nearest wall. A pose's
fit is then the share of scan points that land within NEAR = 10 cm of a wall when the scan is placed at that pose:
On the robot:
def fits(self, poses, pts):
"""poses (N,3) x,y,yaw; pts (M,2) -> share of points within NEAR of a wall, per pose."""
out = np.empty(len(poses), np.float32)
step = max(1, 400000 // max(len(pts), 1))
for a in range(0, len(poses), step):
p = poses[a:a + step]
c, s = np.cos(p[:, 2])[:, None], np.sin(p[:, 2])[:, None]
i = ((p[:, 0:1] + pts[None, :, 0] * c - pts[None, :, 1] * s - self.ox) / self.res).astype(np.int32)
j = ((p[:, 1:2] + pts[None, :, 0] * s + pts[None, :, 1] * c - self.oy) / self.res).astype(np.int32)
ok = (i >= 0) & (i < self.w) & (j >= 0) & (j < self.h)
d = np.where(ok, self.dist[np.clip(j, 0, self.h - 1), np.clip(i, 0, self.w - 1)], 9.9)
out[a:a + step] = (d < NEAR).mean(1)
return out
search tries every free cell on a 10 cm grid at every 6 degrees, refines the best places, and returns the best pose,
its fit, and the fit of the best other place. That second number is the important one. Replaying a recorded run on
2026-10-05 showed that near a wall, wrong places fit 0.83-0.98, so a high fit alone protects nothing. What protects is
the margin: fit >= 0.65 and at least 0.10 better than anywhere else accepted 17 of 86 scans, none at a wrong
place; fit >= 0.75 with margin 0.05 accepted 5 wrong ones.
On the robot:
self.accept = dp('accept_fit', 0.65).value # a pose is believed at or above this fit ...
self.margin = dp('search_margin', 0.10).value # ... and only when it beats the best other place by this
self.lost_fit = dp('lost_fit', 0.40).value # below this for lost_checks checks while standing -> lost
self.lost_checks = dp('lost_checks', 6).value
docs/status.md (2026-10-05) still says "Accept at fit >= 0.75"; the code is 0.65 plus the 0.10 margin, and the code
runs.
place() runs only while the robot stands still (wheels disabled for 3 s). It searches the whole map, then refines
each seed: the pose slam_toolbox (or AMCL) currently has, the remembered pose from ~/maps/last_pose.json, the home
pose, and NVIDIA's GPU localizer's answer. A seed is never trusted on its own. The amcl or remembered pose is kept if it
fits at least accept_fit and nothing else fits clearly better; otherwise the search result is taken if it passes fit
and margin:
On the robot:
agree = []
for name, seed in seeds:
p, sf = self.matcher.refine(seed, pts)
tried[name] = round(sf, 2)
if near(p):
agree.append(name)
if name in ('amcl', 'remembered') and sf >= self.accept and sf >= f - 0.03:
return self.seed_amcl(p, sf, name + ' (nothing else fits clearly better)')
if best is not None and f >= self.accept and f - second >= self.margin:
return self.seed_amcl(best, f, 'search' + (' + ' + ', '.join(agree) if agree else ''))
self.publish('lost', why='no single place explains the scan', tried=tried, points=int(len(pts)))
seed_amcl publishes the checked pose on /initialpose every 0.5 s for up to 15 s, until the localizer reports a pose
within 0.25 m and 0.3 rad of it. Before seeding, place() calls set_mapping(False): slam_toolbox 2.6.10 takes seeds
only in localization mode (chapter 17).
The GPU localizer's scan coverage is set in the launch file to 200 degrees. NVIDIA uses 270 for a full 360-degree
LiDAR; this one returns valid points over only 227 degrees (rear gap 133 degrees: blind sector plus the arm tower,
measured 2026-10-05), and with 270 the localizer silently returned nothing.
The loop runs once a second. The part that brings the Nav2 servers up and heals them:
On the robot:
nav = self.active(self.nav_active)
if nav is None:
# A manager that does not answer is stuck inside a lifecycle call: on Humble it waits without limit for
# a server that froze (measured 2026-10-05: controller_server stopped 25 s -> bond broken -> the manager
# hung in "Deactivating controller_server" for minutes). No service can heal that; restart.
silent += 1
self.publish('healing', why='navigation manager does not answer')
if silent >= 5:
return self.restart('navigation manager silent for 25 s')
continue
silent = 0
if not nav:
self.publish('starting_nav' if not nav_started else 'healing', why='navigation servers not active')
ok = self.manage(self.nav_manage, ManageLifecycleNodes.Request.STARTUP, 120.0) if not nav_started \
else self.heal(self.nav_manage, 'navigation')
nav_started = True
if ok is None:
return self.restart('navigation manager did not answer a lifecycle request')
if not ok:
self.nav_fails += 1
self.get_logger().error(f'navigation did not come up ({self.nav_fails}/2)')
if self.nav_fails >= 2:
return self.restart('navigation did not come up twice')
continue
heal() is the stock recovery: RESET (40 s timeout), then STARTUP (120 s). restart() returns exit code 1. The
launch file shuts the whole launch down when the supervisor exits, and systemd starts everything again after 20 s.
The module docstring and the unit file speak of "three failed heals"; the code restarts after two failed bringups,
and the code runs.
Also in run(): localization not alive for 90 s restarts navigation; while placed and not plugged in, slam_toolbox is
switched to mapping, and back to localization when plugged in or lost; while mapping, keep_map writes copies of the
pose graph and image after every drive (standing for more than 5 s after the wheels were on) and every 10 minutes,
keeping the previous 10 in ~/maps/history/. Chapter 17 explains the map side.
On the robot:
def probe_plan(self):
"""Ready means navigation can plan from where it stands: ask the planner for a path to the robot's own pose.
2026-10-05/06: three times every navigation server stopped receiving AMCL's map->odom transform (a fresh
listener still got it; the servers' own copy stayed at one stamp for hours). Standing still nothing logs it;
with a goal the controller refuses ("Transform data too old") and the planner fails. Only a restart of the
service brought it back. This probe is the cheapest honest question (one plan on a 90x140 map, every
PROBE_EVERY s); two failures in a row -> restart. Returns True/False, None = no answer in time."""
Every 30 s while ready it sends ComputePathToPose to the robot's own pose. On 2026-10-06 the fault came back with the
planner still planning and only the controller deaf, so the probe also sends that zero-length path to the controller
as a FollowPath goal, but only while the wheels are disabled (the driver then ignores /cmd_vel; nothing moves). A
deaf controller answers "Transform data too old" and the goal is ABORTED. Two failures in a row restart navigation.
publish() writes a latched JSON string on /nav/status. A skill either reads it, or runs
ros2 run rosorin_base nav_wait [seconds], which exits 0 as soon as ready is true and the status is younger than
15 s (an older one means the supervisor hung), and exits 1 after the time with the last status printed:
On the robot:
def wait_main():
"""nav_wait [seconds]: exit 0 as soon as navigation says ready, 1 after the time with the last status printed."""
limit = float(sys.argv[1]) if len(sys.argv) > 1 else 180.0
rclpy.init(); n = rclpy.create_node('nav_wait'); st = {}
n.create_subscription(String, 'nav/status', lambda m: (st.clear(), st.update(json.loads(m.data))), LATCHED)
end = time.time() + limit
ready = lambda: st.get('ready') and time.time() - st.get('t', 0) < 15 # a status that old = supervisor hung
while time.time() < end and not ready():
rclpy.spin_once(n, timeout_sec=0.5)
print(json.dumps(st) if st else 'no navigation status (rosorin-nav not running?)', flush=True)
ok = bool(ready())
rclpy.shutdown()
sys.exit(0 if ok else 1)
Traps this node already hit
- QoS mismatch. slam_toolbox publishes its pose VOLATILE; AMCL publishes it TRANSIENT_LOCAL ("latched"). A
latched reader gets nothing from slam_toolbox, and the error shows only in a fresh node's warning
(offering incompatible QoS ... Last incompatible policy: DURABILITY). The supervisor subscribes volatile,
which takes both.- A Python TF listener costs a core. Listening to the whole
/tfstream cost about 25 % of a core; the
first version of the supervisor used 32 %, the current one 7.7 %. It reads the fixed laser mount once and then
unregisters the listener.- Lost is a correct answer. On 2026-10-05, plugged in where the room no longer matched the map, best fit 0.58,
second 0.57: the supervisor said "lost", and that was right. Moving the robot to a mapped spot fixes it;
loweringaccept_fit(ros2 param set /nav_supervisor accept_fit 0.5) was used only for tests, and a restart
resets it.
ros2/rosorin_base/launch/nav.launch.py (135 lines) wires all of the above together. Read it in parts.
The header says what differs from nav2_bringup, and the imports:
On the robot:
"""Localization + the map that updates itself as it drives (slam_toolbox continuing the kept pose graph; slam:=false = the
old map_server + AMCL) + Nav2 navigation on top of base.launch.py. ALWAYS ON: rosorin-nav.service runs this
from boot; skills are clients (ros2 run rosorin_base nav_wait, then goals). rosorin_base/nav_supervisor.py places the
robot on the map, starts the navigation servers once it has a pose, heals a half-alive stack and publishes /nav/status.
NVIDIA's Nova Carter reference setup (nova_carter_navigation/lidar_localization.launch.py, release-3.2): Nav2 AMCL on a
saved occupancy map; Isaac ROS Occupancy Grid Localizer (GPU search of the whole map with one LiDAR scan) when
/trigger_grid_search_localization is called. Its answer goes to the supervisor (/localizer/result), which checks it
against the scan before AMCL gets it (docs/research/relocalization_reference.md: the result carries no confidence).
Differs from nav2_bringup: behavior_server also publishes into cmd_vel_nav; chain is
controller/behaviors -> cmd_vel_nav -> velocity_smoother -> cmd_vel_smoothed -> collision_monitor -> cmd_vel,
so collision_monitor is the ONLY cmd_vel publisher (board_driver latches e-stop on >1).
Stop gamepad_teleop and slam first. Stop with scripts/nav_stop.sh (launch SIGINT left nodes running)."""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
import tempfile
import yaml
Arguments and the start pose. map:= and slam:= are read straight from the command line (plain Python strings, not
launch substitutions, because the code below needs them as values). The kept pose graph is continued from the last
pose the supervisor remembered, else from home:
On the robot:
def generate_launch_description():
share = get_package_share_directory('rosorin_base')
params = os.path.join(share, 'config', 'nav2.yaml')
cm_params = os.path.join(share, 'config', 'collision_monitor.yaml')
default_map = '/home/burgerbarn/maps/room_live.yaml'
map_file = next((a.split(':=', 1)[1] for a in __import__('sys').argv if a.startswith('map:=')), default_map)
slam = next((a.split(':=', 1)[1] for a in __import__('sys').argv if a.startswith('slam:=')), 'true').lower() == 'true'
maps = os.path.dirname(default_map)
# the kept pose graph (~/maps/room.posegraph/.data, scripts/install_live_map.sh) is continued from where the robot
# last knew it stood (nav_supervisor writes last_pose.json); the supervisor re-places it anyway before mapping starts
start = [0.0, 0.0, 0.0]
for f in ('last_pose.json', 'home_pose.json'):
try:
d = yaml.safe_load(open(os.path.join(maps, f))); start = [float(d['x']), float(d['y']), float(d['yaw'])]; break
except Exception:
continue
The parameter file is patched in memory and written to a temporary file: learned limits are applied, and the keepout
filter is removed when there is no mask next to the map (room_live.yaml -> room_live_keepout.yaml):
On the robot:
mask_file = map_file[:-5] + '_keepout.yaml'
keepout = os.path.exists(mask_file)
cfg = yaml.safe_load(open(params))
from rosorin_base import learned_limits # driving limits the robot learned from its own practice
print('learned limits:', learned_limits.apply(cfg))
if not keepout: # no room boundary drawn for this map: same params, no filter
gc = cfg['global_costmap']['global_costmap']['ros__parameters']
gc.pop('filters', None); gc.pop('keepout_filter', None)
fd, params = tempfile.mkstemp(prefix='nav2_', suffix='.yaml'); os.close(fd)
yaml.safe_dump(cfg, open(params, 'w'))
map_yaml = LaunchConfiguration('map')
NVIDIA's GPU localizer and its scan converter run as components in one container process (a composable node is a
node loaded as a plugin into a shared process instead of its own executable):
On the robot:
localizer = ComposableNodeContainer(
package='rclcpp_components', name='occupancy_grid_localizer_container', namespace='',
executable='component_container_mt', output='screen', composable_node_descriptions=[
ComposableNode(package='isaac_ros_occupancy_grid_localizer', name='occupancy_grid_localizer',
plugin='nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode',
# min_scan_fov_degrees: NVIDIA's value is 270 (a 360 deg LiDAR). This LiDAR gives valid returns over
# 227 deg only (rear gap 133 deg: blind sector + arm tower, measured 2026-10-05), so with 270 the
# localizer silently returned nothing. Lowered to fit what the sensor really delivers.
parameters=[map_file, {'loc_result_frame': 'map', 'map_yaml_path': map_file,
'robot_radius': 0.18, 'min_scan_fov_degrees': 200.0}],
remappings=[('localization_result', '/localizer/result')]),
ComposableNode(package='isaac_ros_pointcloud_utils', name='laserscan_to_flatscan',
plugin='nvidia::isaac_ros::pointcloud_utils::LaserScantoFlatScanNode')])
The node lists for the two lifecycle managers, the cmd_vel remap, the keepout servers, and the supervisor with its
parameters:
On the robot:
loc_nodes = ([] if slam else ['map_server', 'amcl']) + (['filter_mask_server', 'costmap_filter_info_server'] if keepout else [])
nav = ['controller_server', 'smoother_server', 'planner_server', 'behavior_server', 'bt_navigator',
'velocity_smoother', 'collision_monitor']
to_nav = [('cmd_vel', 'cmd_vel_nav')]
filt = [
Node(package='nav2_map_server', executable='map_server', name='filter_mask_server', output='screen',
parameters=[params, {'yaml_filename': mask_file}]),
Node(package='nav2_map_server', executable='costmap_filter_info_server', name='costmap_filter_info_server',
output='screen', parameters=[params]),
] if keepout else []
supervisor = Node(package='rosorin_base', executable='nav_supervisor', name='nav_supervisor', output='screen',
parameters=[{'slam': slam, 'loc_manager': bool(loc_nodes), 'map_dir': maps}])
Localization: slam_toolbox's localize-and-map node on the kept graph, its pose renamed to amcl_pose so every
client works with either; or the old map server + AMCL with slam:=false. Its own lifecycle manager starts by itself
(autostart: True):
On the robot:
localization = [
# one map, continued on every drive: slam_toolbox's localize-and-map node starts in localization mode on the
# kept graph (pose tracked, map untouched); nav_supervisor switches it to mapping once the scan confirms the
# pose and back when the pose is lost; it saves copies (pose graph + image) after drives. Its `pose` is
# published as amcl_pose so every client stays as it was. docs/research/whole_project_review.md section 4.
Node(package='slam_toolbox', executable='map_and_localization_slam_toolbox_node', name='slam_toolbox',
output='screen', parameters=[os.path.join(share, 'config', 'slam.yaml'),
{'mode': 'localization', 'map_file_name': os.path.join(maps, 'room'),
'map_start_pose': start}],
remappings=[('pose', 'amcl_pose')]),
] if slam else [
Node(package='nav2_map_server', executable='map_server', name='map_server', output='screen',
parameters=[params, {'yaml_filename': map_yaml}]),
Node(package='nav2_amcl', executable='amcl', name='amcl', output='screen', parameters=[params]),
]
loc_manager = [Node(package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization',
output='screen', parameters=[{'autostart': True, 'node_names': loc_nodes, 'bond_timeout': 10.0}])] if loc_nodes else []
Everything goes into the launch description. Three details matter: the depth gate sits in front of nvblox; the
navigation manager has autostart: False and bond_timeout: 10.0 (a server must answer within half of it; the stock
4 s is short on a loaded Jetson); and when the supervisor exits, the whole launch shuts down so systemd can restart it:
On the robot:
Node(package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation',
# autostart off: the global costmap waits without limit for map -> base_link, so the supervisor sends
# STARTUP once the robot has a pose. bond_timeout 10: a server must answer within half of it, and the
# stock 4 s (2 s) is short for a loaded Jetson (docs/research/always_on_navigation_reference.md)
output='screen', parameters=[{'autostart': False, 'node_names': nav, 'bond_timeout': 10.0}]),
supervisor,
RegisterEventHandler(OnProcessExit(target_action=supervisor, on_exit=[EmitEvent(event=Shutdown(
reason='nav_supervisor exited'))])),
The complete file:
On the robot:
"""Localization + the map that updates itself as it drives (slam_toolbox continuing the kept pose graph; slam:=false = the
old map_server + AMCL) + Nav2 navigation on top of base.launch.py. ALWAYS ON: rosorin-nav.service runs this
from boot; skills are clients (ros2 run rosorin_base nav_wait, then goals). rosorin_base/nav_supervisor.py places the
robot on the map, starts the navigation servers once it has a pose, heals a half-alive stack and publishes /nav/status.
NVIDIA's Nova Carter reference setup (nova_carter_navigation/lidar_localization.launch.py, release-3.2): Nav2 AMCL on a
saved occupancy map; Isaac ROS Occupancy Grid Localizer (GPU search of the whole map with one LiDAR scan) when
/trigger_grid_search_localization is called. Its answer goes to the supervisor (/localizer/result), which checks it
against the scan before AMCL gets it (docs/research/relocalization_reference.md: the result carries no confidence).
Differs from nav2_bringup: behavior_server also publishes into cmd_vel_nav; chain is
controller/behaviors -> cmd_vel_nav -> velocity_smoother -> cmd_vel_smoothed -> collision_monitor -> cmd_vel,
so collision_monitor is the ONLY cmd_vel publisher (board_driver latches e-stop on >1).
Stop gamepad_teleop and slam first. Stop with scripts/nav_stop.sh (launch SIGINT left nodes running)."""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
import tempfile
import yaml
def generate_launch_description():
share = get_package_share_directory('rosorin_base')
params = os.path.join(share, 'config', 'nav2.yaml')
cm_params = os.path.join(share, 'config', 'collision_monitor.yaml')
default_map = '/home/burgerbarn/maps/room_live.yaml'
map_file = next((a.split(':=', 1)[1] for a in __import__('sys').argv if a.startswith('map:=')), default_map)
slam = next((a.split(':=', 1)[1] for a in __import__('sys').argv if a.startswith('slam:=')), 'true').lower() == 'true'
maps = os.path.dirname(default_map)
# the kept pose graph (~/maps/room.posegraph/.data, scripts/install_live_map.sh) is continued from where the robot
# last knew it stood (nav_supervisor writes last_pose.json); the supervisor re-places it anyway before mapping starts
start = [0.0, 0.0, 0.0]
for f in ('last_pose.json', 'home_pose.json'):
try:
d = yaml.safe_load(open(os.path.join(maps, f))); start = [float(d['x']), float(d['y']), float(d['yaw'])]; break
except Exception:
continue
mask_file = map_file[:-5] + '_keepout.yaml'
keepout = os.path.exists(mask_file)
cfg = yaml.safe_load(open(params))
from rosorin_base import learned_limits # driving limits the robot learned from its own practice
print('learned limits:', learned_limits.apply(cfg))
if not keepout: # no room boundary drawn for this map: same params, no filter
gc = cfg['global_costmap']['global_costmap']['ros__parameters']
gc.pop('filters', None); gc.pop('keepout_filter', None)
fd, params = tempfile.mkstemp(prefix='nav2_', suffix='.yaml'); os.close(fd)
yaml.safe_dump(cfg, open(params, 'w'))
map_yaml = LaunchConfiguration('map')
localizer = ComposableNodeContainer(
package='rclcpp_components', name='occupancy_grid_localizer_container', namespace='',
executable='component_container_mt', output='screen', composable_node_descriptions=[
ComposableNode(package='isaac_ros_occupancy_grid_localizer', name='occupancy_grid_localizer',
plugin='nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode',
# min_scan_fov_degrees: NVIDIA's value is 270 (a 360 deg LiDAR). This LiDAR gives valid returns over
# 227 deg only (rear gap 133 deg: blind sector + arm tower, measured 2026-10-05), so with 270 the
# localizer silently returned nothing. Lowered to fit what the sensor really delivers.
parameters=[map_file, {'loc_result_frame': 'map', 'map_yaml_path': map_file,
'robot_radius': 0.18, 'min_scan_fov_degrees': 200.0}],
remappings=[('localization_result', '/localizer/result')]),
ComposableNode(package='isaac_ros_pointcloud_utils', name='laserscan_to_flatscan',
plugin='nvidia::isaac_ros::pointcloud_utils::LaserScantoFlatScanNode')])
loc_nodes = ([] if slam else ['map_server', 'amcl']) + (['filter_mask_server', 'costmap_filter_info_server'] if keepout else [])
nav = ['controller_server', 'smoother_server', 'planner_server', 'behavior_server', 'bt_navigator',
'velocity_smoother', 'collision_monitor']
to_nav = [('cmd_vel', 'cmd_vel_nav')]
filt = [
Node(package='nav2_map_server', executable='map_server', name='filter_mask_server', output='screen',
parameters=[params, {'yaml_filename': mask_file}]),
Node(package='nav2_map_server', executable='costmap_filter_info_server', name='costmap_filter_info_server',
output='screen', parameters=[params]),
] if keepout else []
supervisor = Node(package='rosorin_base', executable='nav_supervisor', name='nav_supervisor', output='screen',
parameters=[{'slam': slam, 'loc_manager': bool(loc_nodes), 'map_dir': maps}])
localization = [
# one map, continued on every drive: slam_toolbox's localize-and-map node starts in localization mode on the
# kept graph (pose tracked, map untouched); nav_supervisor switches it to mapping once the scan confirms the
# pose and back when the pose is lost; it saves copies (pose graph + image) after drives. Its `pose` is
# published as amcl_pose so every client stays as it was. docs/research/whole_project_review.md section 4.
Node(package='slam_toolbox', executable='map_and_localization_slam_toolbox_node', name='slam_toolbox',
output='screen', parameters=[os.path.join(share, 'config', 'slam.yaml'),
{'mode': 'localization', 'map_file_name': os.path.join(maps, 'room'),
'map_start_pose': start}],
remappings=[('pose', 'amcl_pose')]),
] if slam else [
Node(package='nav2_map_server', executable='map_server', name='map_server', output='screen',
parameters=[params, {'yaml_filename': map_yaml}]),
Node(package='nav2_amcl', executable='amcl', name='amcl', output='screen', parameters=[params]),
]
loc_manager = [Node(package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization',
output='screen', parameters=[{'autostart': True, 'node_names': loc_nodes, 'bond_timeout': 10.0}])] if loc_nodes else []
return LaunchDescription(filt + [
DeclareLaunchArgument('map', default_value=default_map),
localizer,
# 3D obstacle map from the depth camera (NVIDIA nvblox, GPU). The depth image is registered to the RGB camera
# by the driver (its own point cloud uses the RGB intrinsics, checked 2026-10-02) -> RGB camera_info.
# nvblox gets depth only while the head is still (rosorin_base/depth_gate.py: measured 2026-10-02, frames taken
# during an arm move put ~900 false obstacle cells on the floor ahead)
Node(package='rosorin_base', executable='depth_gate', name='depth_gate', output='screen',
remappings=[('depth_in', '/aurora/depth/image_raw')]),
Node(package='nvblox_ros', executable='nvblox_node', name='nvblox_node', output='screen',
parameters=['/opt/ros/humble/share/nvblox_examples_bringup/config/nvblox/nvblox_base.yaml',
os.path.join(share, 'config', 'nvblox.yaml')],
remappings=[('camera_0/depth/image', '/depth_gate/depth'),
('camera_0/depth/camera_info', '/aurora/rgb/camera_info')]),
*localization, *loc_manager,
Node(package='nav2_controller', executable='controller_server', output='screen',
parameters=[params], remappings=to_nav),
Node(package='nav2_smoother', executable='smoother_server', name='smoother_server', output='screen',
parameters=[params]),
Node(package='nav2_planner', executable='planner_server', name='planner_server', output='screen',
parameters=[params]),
Node(package='nav2_behaviors', executable='behavior_server', name='behavior_server', output='screen',
parameters=[params], remappings=to_nav),
Node(package='nav2_bt_navigator', executable='bt_navigator', name='bt_navigator', output='screen',
parameters=[params]),
Node(package='nav2_velocity_smoother', executable='velocity_smoother', name='velocity_smoother',
output='screen', parameters=[params],
remappings=[('cmd_vel', 'cmd_vel_nav')]), # publishes cmd_vel_smoothed
Node(package='nav2_collision_monitor', executable='collision_monitor', name='collision_monitor',
output='screen', parameters=[cm_params]),
Node(package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation',
# autostart off: the global costmap waits without limit for map -> base_link, so the supervisor sends
# STARTUP once the robot has a pose. bond_timeout 10: a server must answer within half of it, and the
# stock 4 s (2 s) is short for a loaded Jetson (docs/research/always_on_navigation_reference.md)
output='screen', parameters=[{'autostart': False, 'node_names': nav, 'bond_timeout': 10.0}]),
supervisor,
RegisterEventHandler(OnProcessExit(target_action=supervisor, on_exit=[EmitEvent(event=Shutdown(
reason='nav_supervisor exited'))])),
])
nav_slam.launch.py is the same stack for a fresh mapping run (async_slam_toolbox_node in mapping mode, no keepout,
manager autostart on). It is the owner's manual option only; chapter 17 covers it.
After any change to the package on your laptop, copy it and rebuild on the robot. The package is built without
--symlink-install, so launch, config and tree files are copied into install/ at build time: edits in src/ do
nothing until you rebuild.
On your laptop:
cd ~/CCode/rosorin-pro
python3 scripts/tests/check_nav2_config.py
rsync -a --exclude __pycache__ ros2/rosorin_base/ rosorin-wifi:ros2_ws/src/rosorin_base/
On the robot:
source /opt/ros/humble/setup.bash
cd ~/ros2_ws && colcon build --packages-select rosorin_base 2>&1 | tail -1
scripts/ops/deploy.sh nav does the same for *.py, launch and config files and restarts navigation, but it does not
copy bt/; after changing the tree, use the rsync above.
New idea: an isolated ROS domain
Every ROS 2 process with the sameROS_DOMAIN_IDcan see every other one (chapter 9). The robot's real nodes run
on domain 0. A test that setsROS_DOMAIN_ID=77andROS_LOCALHOST_ONLY=1starts its own copies of the Nav2
servers that cannot see, or be seen by, the base driver: whatever they publish on/cmd_velgoes nowhere. The
tests feed them a fake TF, a fake scan and a fake costmap, and read what comes out.
Two scripts use this, both with the robot's deployed parameter files and the installed binaries (so they test what
really runs, including the overlay):
scripts/tests/nav2_bringup_check.sh: starts controller, smoother, planner, behaviour server and navigator withnav2.yaml and the tree; passes when all five reach active. bt_navigator loads the tree when it activates, soscripts/tests/nav2_isolated_checks.sh [teleop] [spin] [cm]: the behaviour server and collision monitor againstBoth clean up after themselves by finding every process with ROS_DOMAIN_ID=77 in /proc/*/environ. On 2026-10-02
they ran with the robot on the charger, the wheels disabled and rosorin-nav inactive. Running them next to a live
rosorin-nav was not tried (they add several Nav2 servers' worth of CPU on a Jetson that is already loaded), so stop
navigation first and start it again afterwards.
On the robot:
sudo systemctl stop rosorin-nav # only if it is already installed (later in this chapter)
bash ~/setup/tests/nav2_bringup_check.sh
bash ~/setup/tests/nav2_isolated_checks.sh teleop spin cm 2>&1 | grep -v "Ignoring the source\|^JSON\|^\[WARN\]"
sudo systemctl start rosorin-nav # only if it was running
Check
The bring-up check on 2026-10-02:On the robot:
controller_server active smoother_server active planner_server active behavior_server active bt_navigator active --- tree: default_nav_to_pose_bt_xml: /home/burgerbarn/ros2_ws/install/rosorin_base/share/rosorin_base/bt/navigate_no_backup.xmlThe behaviour checks after the overlay was installed (vx, vy, wz in m/s and rad/s). The left-wall line is the
one that proves the fix:On the robot:
teleop_free_strafe_left [0.0, 0.2, 0.0] teleop_wall_left_strafe_left [0.0, 0.14, 0.0] teleop_wall_right_strafe_left [0.0, 0.2, 0.0] teleop_wall_ahead_forward [0.14, 0.0, 0.0] spin_free {'commands': 20, 'first_wz': 1.0, 'max_wz': 1.0} spin_post_at_nose {'commands': 21, 'first_wz': 1.0, 'max_wz': 1.0} cm_free_forward [0.2, 0.0, 0.0] cm_1pt_in_slow_zone [0.2, 0.0, 0.0] cm_2pt_in_slow_zone [0.12, 0.0, 0.0] cm_1pt_ahead_0.45 [0.35, 0.0, 0.0]The record's copy of this output stops there. The remaining collision-monitor rows, from the same day
(docs/research/nav2_reference.mdsection 26): two returns 0.45 m ahead at 0.35 m/s -> 0.233; two returns to the
left, strafe left / right -> 0.133 / -0.20; wall at the nose with the scan silent -> 0.20, passed through.
spin_post_at_noseat 1.0 rad/s is the reason Spin is not in the tree; it is expected, not a failure.
If it fails
There may be more than one action server for the action 'spin': a previous test run's behaviour server is
still alive.ros2 runwrappers do not pass a kill on to the node they started; the scripts now start the
binaries directly and kill by domain. Run the script again; it clears domain 77 first.behavior_server library: /opt/ros/humble: the overlay is not built or~/ros2_ws/install/setup.bashis not
sourced, and the teleop lines will show the mirrored result (left wall 0.20, right wall 0.14).
Stopping ros2 launch with SIGINT once left Nav2 nodes running (2026-09-28), and on 2026-10-05 the overlay's behaviour
server and the depth gate survived a stop and answered the next run. scripts/nav_stop.sh disables the wheels first,
then stops everything navigation started, by path, with SIGINT and then SIGKILL:
On the robot:
#!/bin/bash
# Stop Nav2 completely (launch SIGINT left orphaned nav2 nodes running, 2026-09-28) and disable motion.
source /opt/ros/humble/setup.bash
timeout 10 ros2 service call /rosorin_board_driver/enable std_srvs/srv/SetBool "{data: false}" >/dev/null 2>&1
pkill -INT -f "ros2 launch rosorin_base nav" # nav.launch.py / nav_slam.launch.py
# our own nodes started by the same launch files (2026-10-05: the workspace nav2_behaviors overlay and the depth gate were
# left running after a stop - a second behaviour server / depth gate then answered the next run)
OWN="ros2_ws/install/nav2_behaviors/lib/nav2_behaviors/|lib/rosorin_base/depth_gate|lib/rosorin_base/nav_supervisor|occupancy_grid_localizer_container"
pkill -INT -f "$OWN"
pkill -INT -f "/opt/ros/humble/lib/nav2_"; pkill -INT -f "slam_toolbox_node"; pkill -INT -f "nvblox_ros/nvblox_node"
for i in $(seq 1 10); do pgrep -f "/opt/ros/humble/lib/nav2_" >/dev/null || break; sleep 0.5; done
pkill -KILL -f "$OWN"; pkill -KILL -f "/opt/ros/humble/lib/nav2_"; pkill -KILL -f "slam_toolbox_node"; pkill -KILL -f "nvblox_ros/nvblox_node"
sleep 0.5
pgrep -fa "/opt/ros/humble/lib/nav2_" || echo "nav2 stopped"
timeout 8 ros2 topic info /cmd_vel | grep "Publisher count"
Safety: pkill inside ssh
pkill -f "<pattern>"typed insidessh rosorin-wifi '...'matches the ssh shell's own command line and kills
your session (hit several times in this project). Runnav_stop.shas a file, as the unit does, or bracket the
first letter of a pattern ("[n]av2").
systemd/rosorin-nav.service, complete:
On the robot:
[Unit]
Description=ROSOrin navigation, always on: kept room map + AMCL + Nav2 + nvblox, kept usable by rosorin_base/nav_supervisor (skills are clients: nav_wait, then goals)
After=rosorin-base.service rosorin-camera.service rosorin-vslam.service
Requires=rosorin-base.service
# restarts are the supervisor's last resort (three failed heals); never give up
StartLimitIntervalSec=0
[Service]
User=burgerbarn
Environment=ROS_DOMAIN_ID=0
# leftovers of an earlier navigation (2026-09-28: launch SIGINT left nav2 nodes running) would answer for this one
ExecStartPre=/bin/bash /home/burgerbarn/setup/nav_stop.sh
ExecStart=/bin/bash -c 'source /opt/ros/humble/setup.bash && source /home/burgerbarn/ros2_ws/install/setup.bash && exec ros2 launch rosorin_base nav.launch.py'
ExecStopPost=/bin/bash /home/burgerbarn/setup/nav_stop.sh
KillSignal=SIGINT
TimeoutStopSec=30
Restart=always
# 20 s: a stack started within seconds of the old one being killed fails to come up (reproduced 2026-10-05,
# docs/research/always_on_navigation_reference.md section 6)
RestartSec=20
[Install]
WantedBy=multi-user.target
Line by line:
Requires=rosorin-base.service: navigation without the driver is pointless; when the base stops or restarts,StartLimitIntervalSec=0: systemd never gives up restarting it.nav_stop.sh both before the start and after the stop: no leftover node from an earlier run can answer for thisExecStart sources ~/ros2_ws, which brings in the nav2_behaviors overlay.RestartSec=20. On 2026-10-05 a test started map server + AMCL + a lifecycle manager on an isolated domain,Failed to change state for node: amcl / Failed to bring up all requested nodes. Aborting bringup., the samescripts/install_nav_service.sh removes the older keeper unit (rosorin-navkeeper) if present, installs, enables and
starts the unit:
On the robot:
#!/bin/bash
# Install rosorin-nav.service: navigation always on from boot (docs/research/always_on_navigation_reference.md).
# Replaces the older nav keeper (rosorin-navkeeper, removed). Run on the robot: sudo bash ~/setup/install_nav_service.sh
# Rollback: sudo systemctl disable --now rosorin-nav; sudo rm /etc/systemd/system/rosorin-nav.service; sudo systemctl daemon-reload
set -euo pipefail
[ "$(id -u)" = 0 ] || { echo "run with sudo"; exit 1; }
systemctl disable --now rosorin-navkeeper.service 2>/dev/null || true
rm -f /etc/systemd/system/rosorin-navkeeper.service
install -m 0644 /home/burgerbarn/setup/rosorin-nav.service /etc/systemd/system/
systemctl daemon-reload
systemctl enable rosorin-nav.service
systemctl restart rosorin-nav.service
sleep 3; systemctl --no-pager --lines=3 status rosorin-nav.service | head -6
Make sure the kept map from chapter 17 is in place, then install:
On the robot:
ls ~/maps/room.posegraph ~/maps/room_live.yaml ~/maps/room_live_keepout.yaml
sudo bash ~/setup/install_nav_service.sh
Check
Give it a minute, then read the supervisor's own story of the start and the status every skill sees:On the robot:
systemctl is-enabled rosorin-nav; systemctl is-active rosorin-nav journalctl -u rosorin-nav --since "-3 min" --no-pager -o cat | \ grep -E "\[nav_supervisor\]:|lifecycle_manager_navigation\]: (Starting managed|Managed nodes are active)" source /opt/ros/humble/setup.bash; source ~/ros2_ws/install/setup.bash ros2 run rosorin_base nav_wait 120; echo "exit $?"A start on the robot on 2026-10-07 (service started 17:23:06 UTC). Note the order: map, localization only,
placed, STARTUP, servers active, mapping on, ready; 25 s from the map arriving to ready:On the robot:
[nav_supervisor-16] [INFO] [1791393800.183676905] [nav_supervisor]: map 169x297 at 0.05000000074505806 m, 10552 free cells [nav_supervisor-16] [INFO] [1791393800.766511334] [nav_supervisor]: slam_toolbox: localization only [nav_supervisor-16] [INFO] [1791393807.989298076] [nav_supervisor]: placed by remembered (nothing else fits clearly better): x -0.19 y 0.11 yaw 31 deg, fit 0.94 [nav_supervisor-16] [INFO] [1791393807.997983908] [nav_supervisor]: starting -> starting_nav {'why': 'navigation servers not active'} [lifecycle_manager-15] [INFO] [1791393808.001031624] [lifecycle_manager_navigation]: Starting managed nodes bringup... [nav_supervisor-16] [INFO] [1791393809.222654703] [nav_supervisor]: map 169x297 at 0.05000000074505806 m, 10552 free cells [lifecycle_manager-15] [INFO] [1791393812.865426924] [lifecycle_manager_navigation]: Managed nodes are active [nav_supervisor-16] [INFO] [1791393814.012191872] [nav_supervisor]: slam_toolbox: mapping - the kept map grows with this drive [nav_supervisor-16] [INFO] [1791393814.532858630] [nav_supervisor]: map 174x298 at 0.05000000074505806 m, 10350 free cells [nav_supervisor-16] [WARN] [1791393814.692372438] [nav_supervisor]: planner could not plan from the robot's own pose (1/2, failed) [nav_supervisor-16] [INFO] [1791393814.693156319] [nav_supervisor]: starting_nav -> ready {'fit': 0.92, 'probe_fails': 1, 'mapping': True}And
/nav/status(whatnav_waitprints before exiting 0), live the same evening:On the robot:
{"state": "ready", "ready": true, "heals": 0, "t": 1791405005.3, "pose": [-0.187, 0.242, 8.0], "fit": 0.84, "probe_fails": 0, "mapping": false}Earlier measurements: pose known -> servers active in 4.0-4.6 s (three starts, 2026-10-05); from power-on,
navigation ready within about 2 minutes (2026-10-06, reboot-tested twice).
If it fails
- Stuck in
localizing/lostwithwhy: no single place explains the scan: the room does not look like
the map where the robot stands, or it is plugged in somewhere unmapped. Thetriedfield shows the fits.
Move it to a mapped spot; the supervisor looks again by itself, less and less often (4 s up to 60 s).- Status ready for hours, every goal aborted, controller log
Transform data too old when converting from map to odomwith one fixed time: the "deaf servers" fault (three times 2026-10-05/06; root cause unproven, a Fast
DDS shared-memory issue is the candidate). The probe should catch it within about a minute and restart
navigation. If it does not,sudo systemctl restart rosorin-navand read back/nav/status. Diagnose by asking
what each process actually receives (a fresh listener next to the deaf one), not by looking at the publisher.- One probe failure right after bringup (
planner could not plan from the robot's own pose (1/2, failed), as in
the 2026-10-07 start above) is counted and cleared by the next good probe; only two in a row restart.map kept (every 10 min): graph ok, image FAILEDwas seen in the journal on 2026-10-07 17:49 UTC. The pose
graph copy worked; the image save did not. The cause has not been looked into.- The navigation manager hangs in "Deactivating controller_server" for minutes after a server froze: Humble
waits without limit. The supervisor counts 5 unanswered checks and restarts the service (measured 2026-10-05:
not ready after 16 s, restart decided at 36 s, back and localizing 78 s after the freeze).- Two wheel tasks at once. A task's exit disables the wheels under whatever else is driving (2026-10-06).
Before starting a wheel task by hand, checksystemctl is-active rosorin-explore 'rosorin-task@*'and the mind's
lastbody_skill_startedevent.
Safety
rosorin-navpublishes/cmd_velall the time it is up (the collision monitor). Any tool that publishes
/cmd_vel(the stop proofs in chapter 19, a teleop node) will trip the driver's e-stop unless you stop navigation
first:sudo systemctl stop rosorin-nav. Start it again afterwards and read back/nav/statusbefore you leave.
Not test-built
- A real floor drive with mapping on. The always-on stack with slam_toolbox continuing the kept graph was
tested on the stand only (2026-10-06: placed at fit 0.73 in 21 s, mapping on and off with the pose, goal
succeeded with the wheels in the air, copies written). Mapping on spinning stand wheels corrupted the graph
(phantom nodes, 4.0 -> 8.5 MB) and was restored from the copy; mapping is now only allowed unplugged. How the
kept map behaves over a real drive, and over many drives, is unknown.- Stopping at navigation speed on the floor. Every stop path was measured on the floor at 0.1 and 0.2 m/s
(chapter 19). Nav2 is configured for 0.35 m/s forward; stops at 0.35 m/s were measured only with the wheels in
the air.- The deaf-servers fault: the probe is the heal, not a cure; the root cause is unknown, and the probe has not
yet caught a live occurrence.- Approach margin: the collision monitor's approach polygon is chassis + 1 cm, not the + 3 cm decided on
2026-10-01 (chapter 19). Every floor run so far ran with 1 cm; changing it is open for the owner.- Not tried: Smac Lattice instead of NavFn, MPPI instead of DWB (NVIDIA's reference uses MPPI), the EKF's
use_control, composition of the Nav2 servers into one process, the shared-memory DDS profile
config/fastdds_shm.xml.- Fresh install order. On this robot Nav2, Isaac ROS, the overlay and the service were installed on different
days in a different order than this chapter. The order here (dependencies first) has not been run start to
finish on a blank robot.
python3 scripts/tests/check_nav2_config.py prints nav2.yaml ok: vx 0..0.35, vy -0.08..0.08, wz 1.0.nav2_bringup_check.sh shows five servers active, and nav2_isolated_checks.sh teleop showsteleop_wall_left_strafe_left [0.0, 0.14, 0.0].systemctl is-active rosorin-nav says active, and ros2 run rosorin_base nav_wait 120 exits 0 with"state": "ready".ros2 topic info -v /cmd_vel shows one publisher, collision_monitor, and one subscriber, rosorin_board_driver.Where this comes from
Repo (branchrebuild, HEAD bfb61d8):ros2/rosorin_base/launch/nav.launch.py,config/nav2.yaml,
config/collision_monitor.yaml,config/nvblox.yaml,bt/navigate_no_backup.xml,rosorin_base/depth_gate.py,
rosorin_base/nav_supervisor.py,rosorin_base/learned_limits.py,setup.py;scripts/install_isaac_ros.sh,
scripts/rollback_isaac_ros.sh,scripts/install_nav2_behaviors_fix.sh,scripts/install_nav_service.sh,
scripts/nav_stop.sh,scripts/ops/deploy.sh,scripts/tests/check_nav2_config.py,
scripts/tests/nav2_bringup_check.sh,scripts/tests/nav2_isolated_checks.sh+scripts/tests/nav2_isolated_checks.py;systemd/rosorin-nav.service.
Docs:docs/decisions.md2026-10-01 (prefer NVIDIA, keepout, approach polygon), 2026-10-02 (Isaac ROS, research
fixes, overlay installed with the owner's OK), 2026-10-05 (always on), 2026-10-06 (probe, self-updating map, "NOT
yet verified: a real drive");docs/lessons.md2026-09-28, 2026-10-01, 2026-10-02 (max_vel_x, BackUp screen
door, three Humble guards, depth gate), 2026-10-05, 2026-10-06 (deaf servers, default_server_timeout,
slam_toolbox traps);docs/status.md2026-10-05/06;docs/navigation.md(stale values noted);
docs/research/always_on_navigation_reference.mdsections 1, 2, 6, 7;docs/research/nav2_reference.mdsections
20, 24-26. Command log (cmdlog_robot.md): Nav2 apt install 2026-09-28 07:00, nvblox library errors and fixes
2026-10-02 04:58-04:59, isolated checks 2026-10-02 16:01-17:02, overlay build 17:01, bring-up check 16:16, depth
gate 16:52, config check output. Live read-only lookups on the robot 2026-10-07: package versions, apt-mark
showauto, keepout files, unit states,learned limits:and supervisor journal lines,/cmd_velendpoints,
/nav/status.
← Maps: SLAM and the kept map · Contents · Prove that it stops →