Navigating · Chapter 17 · Time: 2 hours, plus a floor session after chapter 19 · Level: Intermediate · Status: Partly test-built
slam_toolbox installed and configured, a first room map driven with the gamepad and saved as a pose graph, the kept graph that the always-on navigation continues (install_live_map.sh), its automatic copies, a keepout mask for the room, and the traps that spoiled maps on this robot.
To plan a path and to stop odometry drifting, the robot needs a map of its room and a way to find itself on it.
slam_toolbox builds that map from the LiDAR and the odometry of chapter 16, and keeps it as a pose graph that can
be continued later. This robot keeps one graph for its room: the always-on navigation (chapter 18) loads it at boot,
tracks the robot on it, lets it grow only while the robot really drives, and writes copies after each drive. This
chapter makes that first graph and puts it where navigation expects it.
New idea: occupancy grid map
A 2D map here is a picture of the floor plan in 5 cm squares (cells). Each cell is free, occupied (a wall, a
chair leg at LiDAR height) or unknown. It is stored as an image (.pgmor.png: white free, black occupied,
grey unknown) plus a small YAML file that says how big a cell is and where the image's corner sits in the room.
The first room map, of 2026-09-28, is 90 x 140 cells = 4.5 x 7.0 m.
New idea: the map frame and map → odom
Odometry drifts (chapter 16). Themapframe does not: it is fixed to the room. Whatever finds the robot on the
map publishes the transformmap→odom, which is the correction that moves the driftingodomframe back
onto the room. The full chain ismap→odom→base_link:
New idea: SLAM, mapping and localization
Localization finds the robot on a map that is not changing. SLAM (simultaneous localization and mapping)
does that while also adding to the map. slam_toolbox has both modes. In mapping mode every new place the
robot drives to is added. In localization mode it only tracks the pose, and the map stays as it was. The
robot's navigation switches between the two (below).
New idea: pose graph and loop closure
slam_toolbox does not store the map as a picture. It stores a pose graph: a list of places where the robot
stood (nodes), the LiDAR scan it saw at each, and how the places are connected (edges, from odometry and from
matching the scans). The picture is drawn from that graph. When the robot comes back to a place it has seen,
the matcher finds the overlap and adds an edge (loop closure); the whole graph then shifts slightly so that
the old and new views agree. The graph is saved as two files,<name>.posegraphand<name>.data(about 4 MB
and 0.6 MB for the first room map). Only these let slam_toolbox continue a map; the image alone does not.
Chapter 9 installs it with the other ROS packages. Check, and install only if it is missing:
On the robot:
dpkg -l ros-humble-slam-toolbox | tail -1
sudo apt-get install -y --no-install-recommends ros-humble-slam-toolbox
dpkg -l | grep -c "^hi.*nvidia-l4t"
ls /opt/ros/humble/lib/slam_toolbox/
Check
The install on 2026-09-28 added 203 packages (Qt libraries among them) and
ros-humble-slam-toolbox 2.6.10-1jammy.20260909.214512. The held L4T package count must not drop (it was 45
then; chapter 20 adds one more hold). The installed programs, read on 2026-10-06:On the robot:
async_slam_toolbox_node localization_slam_toolbox_node map_and_localization_slam_toolbox_node merge_maps_kinematic sync_slam_toolbox_nodeThis robot uses two of them:
async_slam_toolbox_nodeto make a map from scratch, and
map_and_localization_slam_toolbox_nodefor the kept map.
The configuration is slam_toolbox's own mapper_params_online_async.yaml from the apt package
(/opt/ros/humble/share/slam_toolbox/config/) with six lines changed, each marked # rosorin:
| Setting | Value | Why |
|---|---|---|
base_frame |
base_link |
this robot has no base_footprint frame |
min_laser_range |
0.12 | same as the LiDAR reader's range_min: ignore the robot's own body (self-returns at 5-11 cm, chapter 14) |
max_laser_range |
8.0 | the COIN-D6 reaches about 7.94 m |
enable_interactive_mode |
false | no RViz on the robot |
minimum_travel_distance |
0.2 | add a node after 0.2 m of travel: small rooms, slow robot |
minimum_travel_heading |
0.3 | or after 0.3 rad of turning |
A few stock values worth knowing: resolution: 0.05 (5 cm cells); transform_publish_period: 0.02 (map →
odom at 50 Hz); map_update_interval: 5.0 (the /map picture is republished every 5 s, changed or not);
do_loop_closing: true; mode: mapping, which one of the two nodes ignores (see the traps). Create
~/ros2_ws/src/rosorin_base/config/slam.yaml:
On the robot:
# slam_toolbox 2.6.10 mapper_params_online_async.yaml (apt) with ROSOrin changes marked '# rosorin'
slam_toolbox:
ros__parameters:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link # rosorin: no base_footprint frame
scan_topic: /scan
use_map_saver: true
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
# map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
min_laser_range: 0.12 # rosorin: = lidar_reader range_min (self-return mask)
max_laser_range: 8.0 # rosorin: COIN-D6 max ~7.94 m (hardware.md)
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: false # rosorin: no RViz on robot
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.2 # rosorin: small rooms, slow robot
minimum_travel_heading: 0.3 # rosorin
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
min_pass_through: 2
occupancy_threshold: 0.1
A launch file that starts only the mapping node, on top of the running base (/scan from the LiDAR reader,
odom → base_link from the EKF). Create ~/ros2_ws/src/rosorin_base/launch/slam.launch.py:
On the robot:
"""Online async SLAM (slam_toolbox, apt) on top of base.launch.py (scan + EKF odom->base_link).
Publishes map->odom and /map. Not started by rosorin-base.service: run on demand."""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
share = get_package_share_directory('rosorin_base')
return LaunchDescription([
Node(package='slam_toolbox', executable='async_slam_toolbox_node', name='slam_toolbox',
output='screen', parameters=[os.path.join(share, 'config', 'slam.yaml')]),
])
setup.py (chapter 11) already installs launch/slam.launch.py and config/slam.yaml. Rebuild:
On the robot:
cd ~/ros2_ws && source /opt/ros/humble/setup.bash && colcon build --packages-select rosorin_base
With the robot standing on its wheels or on the stand, wheels disabled, run the mapper for 40 s and look at what it
publishes. Standing still it can only map what one scan sees, which is enough to prove the chain works.
Safety
The wheels stay disabled for this. If navigation is installed (chapter 18), stop it first with
sudo systemctl stop rosorin-nav: it runs its own slam_toolbox node under the same name. Start it again
afterwards.
On the robot:
source /opt/ros/humble/setup.bash; source ~/ros2_ws/install/setup.bash
timeout 40 ros2 launch rosorin_base slam.launch.py
In a second session while it runs:
On the robot:
source /opt/ros/humble/setup.bash
timeout 8 ros2 topic echo --once /map --field info
timeout 6 ros2 run tf2_ros tf2_echo map base_link 2>&1 | grep -m2 -E "Translation|RPY \(deg"
Check
On 2026-09-28 the standing check gave a map of84x227cells atres 0.05with 58 occupied, 1115 free and
17895 unknown cells (one scan's worth), and a transform:On the robot:
- Translation: [0.003, 0.226, 0.000] - Rotation: in RPY (degree) [0.000, -0.000, 6.647]
/maparrives withresolution: 0.05, andtf2_echo map base_linkprints a transform:map→odom→
base_linkis complete.
If it fails
tf2_echosays framemapdoes not exist. slam_toolbox is not running or not receiving/scanor
odom→base_link. Checkros2 topic hz /scan(chapter 14) and/odometry/filtered(chapter 16).- Two slam_toolbox processes.
pgrep -af slam_toolbox. On 2026-09-28 a mapper started in the background
from a non-interactive shell ignoredpkill -INT(docs/lessons.md); run it in the foreground, or stop the
old one by PID withkill -TERM.
The first map of this room was driven by the owner with the gamepad on 2026-09-28, at most 0.2 m/s and 0.8 rad/s,
with slam_toolbox in mapping mode. Do this only after chapter 19's stand proof: the wheels drive on the floor.
Safety
- Robot on the floor, charger unplugged, stand removed. Mapping on the stand or on the charger records motion
that did not happen (chapter 16); on 2026-10-06 ten seconds of spinning wheels on the stand put phantom nodes
into the kept graph.- Nothing else may publish
cmd_vel(the driver latches an e-stop on two publishers): stoprosorin-navif it
is installed, and do not use the driver's own gamepad drive mode at the same time (leave the padlocked; do
not press MODE).- Any gamepad button is an e-stop. Keep a hand near it.
- Enabling the wheels moves the arm to the drive pose first. Keep clear of the arm.
- Keep people out of the LiDAR's view as far as you can. The floor odometry tests of 2026-09-28 failed while a
person moved in it.- Do not press a stick down: a stick click is a button (bits 5 and 6) and trips the e-stop like any other.
Three ssh sessions on the robot. In each, first:
On the robot:
source /opt/ros/humble/setup.bash; source ~/ros2_ws/install/setup.bash
Session 1, the mapper (foreground):
On the robot:
ros2 launch rosorin_base slam.launch.py
Session 2, the gamepad to cmd_vel node (rosorin_base/gamepad_teleop.py, chapter 11; factory stick layout: left
stick forward/back and sideways, right stick turn):
On the robot:
ros2 run rosorin_base gamepad_teleop
Check
The teleop node announces its limits:On the robot:
[INFO] [1790578240.814702738] [gamepad_teleop]: gamepad teleop: max 0.2 m/s, 0.8 rad/s; any button = e-stop (board_driver)and the mapper's output contains
Registering sensoronce.
Session 3: with the wheels still disabled, watch cmd_vel and move each stick once, to see that each stick
does what you expect before anything can move:
On the robot:
ros2 topic echo /cmd_vel
Then enable the wheels, drive, and disable them again:
On the robot:
ros2 service call /rosorin_board_driver/enable std_srvs/srv/SetBool "{data: true}"
ros2 topic echo --once /rosorin_board_driver/state | head -1 | cut -c1-100
Check
On the robot:data: '{"enabled": true, "estop": null, "arm_enabled": false, "arm_moving": false, "arm_pose": null,and the base's journal says
enable(True) -> enabled.
Drive slowly round the room. Go along every wall, come back past places you have already seen (that is what lets
loop closure correct the map), and finish where you started. Then disable the wheels and save the graph and the
image while the mapper is still running:
On the robot:
ros2 service call /rosorin_board_driver/enable std_srvs/srv/SetBool "{data: false}"
mkdir -p ~/maps
ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph "{filename: /home/burgerbarn/maps/room_20260928}"
ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap "{name: {data: /home/burgerbarn/maps/room_20260928}}"
ls -la ~/maps
Stop the teleop node and the mapper with Ctrl-C.
Check
The map of 2026-09-28:map 90x140 cells = 4.5 x 7.0 m, res 0.05, origin -1.36 -3.11; occupied 1371 free 6262,
and the files:On the robot:
-rw-rw-r-- 1 burgerbarn burgerbarn 609055 Sep 28 06:57 room_20260928.data -rw-rw-r-- 1 burgerbarn burgerbarn 12614 Sep 28 06:57 room_20260928.pgm -rw-rw-r-- 1 burgerbarn burgerbarn 4016097 Sep 28 06:57 room_20260928.posegraph -rw-rw-r-- 1 burgerbarn burgerbarn 135 Sep 28 06:57 room_20260928.yamlThe owner looked at the image and confirmed it matched the room. Do the same: copy the
.pgmto your laptop
and open it.
Not test-built
- The name
room_20260928is the nameinstall_live_map.sh(next section) looks for. A map you make today keeps
that name so the script finds it; the date in it is then only a label.- On 2026-09-28 the image and YAML were written by a short Python script that read
/map; the pose graph was
saved with theserialize_mapcall above. Saving the image with slam_toolbox'ssave_mapis what the
navigation supervisor has done since 2026-10-06 (logged asimage ok), but not by hand for a first map.- The driver has changed since 2026-09-28 (drive-pose interlock, scan gate, gamepad modes; chapter 11).
gamepad_teleopwith the current driver has not been used for a mapping drive. The driver's own gamepad
drive mode (MODE button, chapter 11) can drive the robot instead ofgamepad_teleopand needs noenable
call, but it allows up to 0.35 m/s, uses the owner's swapped stick layout, and has not been used to map.
If it fails
- The wheels do not move and the state shows an
estop. A gamepad button, or a secondcmd_vel
publisher. Clear it withros2 service call /rosorin_board_driver/reset_estop std_srvs/srv/Trigger(motion
stays disabled; enable again).- Walls appear twice. On 2026-10-02 a graph continued from a wrong start pose had doubled walls; it was set
aside as~/maps/corrupt_20261002/. A first map from scratch starts where the robot stands, so this happens
when an existing graph is continued from the wrong place. Map again.
The repo has the 2026-09-28 map image and YAML (maps/room_20260928.pgm, .yaml), its keepout mask and the
saved home pose (maps/home_pose.json). It does not have the pose graph (room_20260928.posegraph and
.data, about 4.6 MB), and without the graph slam_toolbox cannot continue the map. On 2026-10-07 the graph was on
the robot in ~/maps, and the rootfs archive of every golden set made after the map (chapter 8) contains
/home/burgerbarn/maps. If you rebuild this robot for the same room, copy the graph off before you wipe anything:
On your laptop:
mkdir -p ~/rosorin-maps
scp rosorin-wifi:maps/room_20260928.posegraph rosorin-wifi:maps/room_20260928.data ~/rosorin-maps/
and copy both back into ~/maps on the new install. Then the repo's keepout mask and home pose fit, because they
were made in that map's frame.
Not test-built
This copy has not been made. If you make a new map instead, do not use the repo's
maps/room_20260928_keepout.*ormaps/home_pose.json: a new map has its own frame (its origin is where the
new mapping drive started), and the old mask and home pose would sit in the wrong place. Make a new mask (below)
and set home again ("yo buddy, this is your home", chapter 24).
The always-on navigation continues one graph, ~/maps/room.posegraph + .data. scripts/install_live_map.sh
creates it once from the first map and never overwrites it:
On the robot:
#!/bin/bash
# The kept map the robot continues on every drive (nav.launch.py slam:=true, default): ~/maps/room.posegraph + .data.
# First time: from the 2026-09-28 pose graph (the map every run has used; the frame of the keepout mask and the home
# pose). Afterwards nav_supervisor.keep_map rewrites it after drives, previous copies in ~/maps/history/.
set -e
M=/home/burgerbarn/maps
[ -f $M/room.posegraph ] && { echo "kept graph exists: $(ls -la $M/room.posegraph | awk '{print $6,$7,$8}')"; exit 0; }
cp -v $M/room_20260928.posegraph $M/room.posegraph
cp -v $M/room_20260928.data $M/room.data
echo "kept graph installed from room_20260928"
On your laptop:
cd ~/CCode/rosorin-pro
scp scripts/install_live_map.sh rosorin-wifi:setup/
On the robot:
bash ~/setup/install_live_map.sh
Check
From 2026-10-06:On the robot:
'/home/burgerbarn/maps/room_20260928.posegraph' -> '/home/burgerbarn/maps/room.posegraph' '/home/burgerbarn/maps/room_20260928.data' -> '/home/burgerbarn/maps/room.data' kept graph installed from room_20260928
room_20260928.*stays as the untouched original.
If it fails
kept graph exists: ...and nothing copied. That is the script protecting the graph the robot has been
growing. To start over from the original, move~/maps/room.posegraphandroom.dataaside yourself first.cp: cannot stat '/home/burgerbarn/maps/room_20260928.posegraph'. There is no first map yet. Make one
(above) or copy it (previous section).
Chapter 18 builds the always-on navigation. The map part of it, in launch/nav.launch.py, is one slam_toolbox
node on the kept graph:
On the robot:
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')]),
map_file_name ~/maps/room: it loads room.posegraph + room.data.map_start_pose: where it believes it stands, from ~/maps/last_pose.json (the supervisor rewrites it while~/maps/home_pose.json, else (0, 0, 0).pose is renamed amcl_pose, the topic AMCL used before this node replaced it on 2026-10-06, so every programnav_supervisor (chapter 18) decides the mode. It places the robot in localization mode first, then switches to
mapping only when the scan confirms the pose and the robot is not plugged in:
On the robot:
def set_mapping(self, on):
"""slam_toolbox: mapping continues the kept graph with the scans of this drive; localization only tracks the
pose on it. Mapping only while the pose is confirmed (a wrong start pose spoils the map: whole_project_review
section 4); back to localization the moment the pose is lost."""
if not self.slam or self.mapping == on:
return True
# the installed node (2.6.10) starts in mapping whatever `mode` says, and only this service creates its
# /initialpose subscriber (checked live 2026-10-06): the first call is always made explicitly
r = self.call(self.slam_mode, SetBool.Request(data=not on), 5.0)
if r is None:
return False
self.mapping = on
self.get_logger().info('slam_toolbox: ' + ('mapping - the kept map grows with this drive' if on else 'localization only'))
return True
On the robot:
# the map grows only on real drives: plugged in (charger, stand) the wheels are not meant to move and any
# odometry is false (2026-10-06 stand test: 10 s of spinning wheels put phantom nodes into the graph)
self.set_mapping(not self.plugged)
The service is /slam_toolbox/set_localization_mode (std_srvs/SetBool): true = localization, false =
mapping. "Plugged" is the driver's belief from the battery voltage step (chapter 11).
Check (after chapter 18)
ros2 topic echo --once /nav/status --field datashows"mapping". On the charger, 2026-10-06 15:39 UTC,
right after the plugged-in rule was added:On the robot:
15:39:21 {"state": "ready", "ready": true, "heals": 0, "t": 1791301159.5, "pose": [-0.261, 0.18, 14.1], "fit": 0.82, "probe_fails": 0, "mapping": false}Off the charger the same evening (
scripts/ops/readback.sh, driver"plugged": false):On the robot:
nav: {"state": "ready", "ready": true, "heals": 0, "t": 1791325779.2, "pose": [1.288, -0.742, -157.2], "fit": 0.86, "probe_fails": 0, "mapping": true}The journal of
rosorin-navshowsslam_toolbox: localization onlywhile it places the robot, then a line
such asplaced by remembered (nothing else fits clearly better): x -0.33 y 0.22 yaw 12 deg, fit 0.73(15:36
UTC). That 15:36 start came before the plugged-in rule existed: the supervisor switched to mapping with the
robot on the stand, a test goal spun the wheels, and the graph got its phantom nodes.
While mapping, the supervisor saves the graph and the image after every drive (wheels were on, now off, and the
robot has stood for more than 5 s) and every 10 minutes. Before each save it copies the current files into
~/maps/history/<date_time>/ and keeps the newest 10:
On the robot:
def keep_map(self, why):
"""Copies of the map the robot carries: the pose graph (what it continues from) and the image + yaml (GPU
localizer, keepout mask alignment). The previous copies go to history/<stamp>/ (10 kept). My part of the map:
the robot makes it, I keep the copies."""
self.save_t = time.time()
stamp = time.strftime('%Y%m%d_%H%M%S')
hist = os.path.join(self.map_dir, 'history', stamp)
try:
os.makedirs(hist, exist_ok=True)
for f in ('room.posegraph', 'room.data', 'room_live.pgm', 'room_live.png', 'room_live.yaml'):
src = os.path.join(self.map_dir, f)
if os.path.exists(src):
shutil.copy2(src, hist)
old = sorted(d for d in glob.glob(os.path.join(self.map_dir, 'history', '2*')) if os.path.isdir(d))
for d in old[:-10]:
shutil.rmtree(d, ignore_errors=True)
except OSError as e:
self.get_logger().warn(f'map history copy failed: {e}')
r1 = self.call(self.serialize, SerializePoseGraph.Request(filename=os.path.join(self.map_dir, 'room')), 30.0)
r2 = self.call(self.savemap, SaveMap.Request(name=String(data=os.path.join(self.map_dir, 'room_live'))), 30.0)
ok = r1 is not None and r1.result == 0 and r2 is not None and r2.result == 0
self.get_logger().info(f'map kept ({why}): graph {"ok" if r1 is not None and r1.result == 0 else "FAILED"}, '
f'image {"ok" if r2 is not None and r2.result == 0 else "FAILED"}, previous in {hist}')
return ok
So ~/maps holds the graph the robot continues (room.*), its current picture (room_live.*, used by the GPU
localizer and by the keepout mask's file name), the original (room_20260928.*) and up to ten older versions.
Check (after chapter 18)
After a drive the journal ofrosorin-navshows a line like the one from 2026-10-06:On the robot:
[nav_supervisor-16] [INFO] [nav_supervisor]: map kept (after a drive): graph ok, image ok, previous in /home/burgerbarn/maps/history/20261006_153709and
ls ~/maps/historylists dated folders (10 on 2026-10-07). That particular save wrote the phantom graph
from the stand; its history folder held the good graph from before, which is what the restore below used.
To go back to an older version, stop navigation, copy the files from a history folder, and start it again. This is
how the graph spoiled by the stand test of 2026-10-06 (phantom nodes, 4.0 → 8.5 MB) was repaired:
On the robot:
sudo systemctl stop rosorin-nav
H=~/maps/history/20261006_153709
cp -v $H/room.posegraph $H/room.data ~/maps/
cp -v $H/room_live.yaml $H/room_live.png ~/maps/ && rm -f ~/maps/room_live.pgm
md5sum ~/maps/room.posegraph ~/maps/room_20260928.posegraph | cut -c1-12 | tr "\n" " "; echo
sudo systemctl start rosorin-nav
Check
The record of 2026-10-06 (copied while navigation ran, then navigation restarted):On the robot:
'/home/burgerbarn/maps/history/20261006_153709/room.posegraph' -> '/home/burgerbarn/maps/room.posegraph' '/home/burgerbarn/maps/history/20261006_153709/room.data' -> '/home/burgerbarn/maps/room.data' '/home/burgerbarn/maps/history/20261006_153709/room_live.yaml' -> '/home/burgerbarn/maps/room_live.yaml' '/home/burgerbarn/maps/history/20261006_153709/room_live.png' -> '/home/burgerbarn/maps/room_live.png' 8ae26e8b9314 8ae26e8b9314Equal checksums: the kept graph was back to the 2026-09-28 original.
Seen on the robot, not resolved
On 2026-10-07~/maps/room_live.yamlnamedroom_live.pgm(written bysave_mapat 17:21 UTC) and the kept
graph was 13.5 MB, its picture 169 x 297 cells. On 2026-10-05 the GPU localizer could not read a PGM map and a
PNG copy was made for it (docs/status.md). Since 2026-10-06 19:00 UTC the localizer's container has died at
start withGxfGraphActivate Error: GXF_FAILUREten times (journal ofrosorin-nav, read 2026-10-07), while
the supervisor kept confirming poses (~/maps/last_pose.jsonat fit 0.9 that evening). The cause of the
localizer failure is not established. Chapter 18 covers the localizer.
New idea: a keepout mask
A keepout mask is a second map image, the same size and position as a map, that marks where the robot must
never plan to go. Nav2'sKeepoutFilterreads it and makes those cells lethal in the costmap (chapter 18), so
no path ever crosses them. It does not stop the robot by itself; it shapes the paths.
Why this step
On 2026-10-01 the robot's first exploration chose a goal through the open door, because the LiDAR had cleared
the cells beyond the doorway into free space. The owner closed the door and the robot froze in front of it.
The owner's decision: the room's boundary is a keepout zone on the static map, not a learned model.
The mask was made on the Mac from the 2026-09-28 image with a flood fill: start from home (−0.17, 0.07), spread
through free cells, and everything not reached (walls, the space behind them, beyond the door) is keepout. This is
the code that was run on 2026-10-01; it is not a script in the repo. It was pasted into
cd ~/CCode/rosorin-pro && python3 - <<'EOF' ... EOF on the Mac (numpy needed):
On your laptop:
import numpy as np
from collections import deque
b=open('maps/room_20260928.pgm','rb').read(); hdr=b.split(b'\n',3); w,h=map(int,hdr[1].split()); M=np.frombuffer(b[-w*h:],np.uint8).reshape(h,w)
r=0.05; ox,oy=-1.358,-3.109
free=M>=250; s=(h-1-int((0.07-oy)/r), int((-0.17-ox)/r))
seen=np.zeros_like(free); q=deque([s]); seen[s]=1
while q:
j,i=q.popleft()
for dj,di in ((1,0),(-1,0),(0,1),(0,-1)):
a,c=j+dj,i+di
if 0<=a<h and 0<=c<w and not seen[a,c] and free[a,c]: seen[a,c]=1; q.append((a,c))
mask=np.where(seen,254,0).astype(np.uint8) # 0 = keepout (occupied), 254 = allowed
open('maps/room_20260928_keepout.pgm','wb').write(b'P5\n%d %d\n255\n'%(w,h)+mask.tobytes())
open('maps/room_20260928_keepout.yaml','w').write("""# Keepout mask (owner 2026-10-01): everything outside the room's walls on room_20260928 (flood fill of free
# space from home (-0.17, 0.07)); an open door leads into keepout. Nav2 KeepoutFilter (nav2.yaml, nav.launch.py).
image: room_20260928_keepout.pgm
mode: scale
resolution: 0.050
origin: [-1.358, -3.109, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
""")
print('allowed cells', seen.sum(), 'm2', seen.sum()*r*r)
It reads the image, picks the seed cell from the home position and the map's origin, flood-fills the free cells
(value 250 and up), writes black (0) for keepout and near-white (254) for allowed in an image of the same size, and
writes the YAML (shown again below). It printed
allowed cells 6093 m2 15.232500000000002: 15.2 m² of floor the robot may plan through. For a new map, change the seed point
to a free spot in that map (where the mapping drive started is free), ox, oy to its origin, and the file
names. The PGM written by save_map has the same three-line header (P5, size, 255) that this code expects
(checked on the robot 2026-10-07).
The mask's YAML, maps/room_20260928_keepout.yaml:
On your laptop:
# Keepout mask (owner 2026-10-01): everything outside the room's walls on room_20260928 (flood fill of free
# space from home (-0.17, 0.07)); an open door leads into keepout. Nav2 KeepoutFilter (nav2.yaml, nav.launch.py).
image: room_20260928_keepout.pgm
mode: scale
resolution: 0.050
origin: [-1.358, -3.109, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
mode: scale keeps the grey levels as costs; with negate: 0 black means "occupied", which the filter turns into
keepout. resolution and origin must be the map's.
Navigation finds the mask by name: nav.launch.py takes the map file (default ~/maps/room_live.yaml), cuts off
.yaml and looks for room_live_keepout.yaml next to it. If that file exists it starts the mask server and the
filter; if not, it runs without. Put the mask in place under that name (the commands used on the robot on
2026-10-02 and 2026-10-05):
On your laptop:
cd ~/CCode/rosorin-pro
scp maps/room_20260928_keepout.pgm maps/room_20260928_keepout.yaml rosorin-wifi:maps/
On the robot:
cd ~/maps
cp room_20260928_keepout.pgm room_live_keepout.pgm; sed 's/room_20260928_keepout.pgm/room_live_keepout.pgm/' room_20260928_keepout.yaml > room_live_keepout.yaml
head -5 room_live_keepout.yaml
Check
The copy on the robot (read 2026-10-07) names its own image and keeps the original's frame:On the robot:
# Keepout mask (owner 2026-10-01): everything outside the room's walls on room_20260928 (flood fill of free # space from home (-0.17, 0.07)); an open door leads into keepout. Nav2 KeepoutFilter (nav2.yaml, nav.launch.py). image: room_live_keepout.pgm mode: scale resolution: 0.050
In config/nav2.yaml the filter is on the global costmap only, so every planned path respects it:
On the robot:
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}
On the robot:
# Room keepout mask (owner 2026-10-01): maps/room_20260928_keepout.yaml served on /keepout_filter_mask
filter_mask_server:
ros__parameters:
use_sim_time: False
topic_name: keepout_filter_mask
frame_id: map
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
If it fails
- The filter sees no mask.
mask_topicandfilter_info_topicmust start with/. Without it the local
costmap subscribed under/local_costmap/...(2026-10-01).- Errors about
odom -> mapfrom the filter right after start. Expected until the robot is placed on the
map.- In-place turns refused with "Collision Ahead" right after start. That was the keepout in the local costmap:
before localization settled, the mask landed under the robot (run 2, 2026-10-01). That is why it is on the
global costmap only.- The robot plans out of the room. A mapping run from scratch has no keepout (
nav_slam.launch.pysets
keepout = False). On 2026-10-05 such a run drove 9 m out of the room into space it had never seen.
Not test-built
The mask was drawn for the 4.5 x 7.0 m map of 2026-09-28. On 2026-10-07 the kept map had grown to 169 x 297
cells (8.45 x 14.85 m). How the filter treats areas outside the mask's own image was not tested on this robot.
A mask made by hand in an image editor instead of the flood fill has not been tried either.
Chapter 24's scripts/explore_run.sh can also make a map from scratch: the robot explores on its own while
slam_toolbox maps. It does that when there is no ~/maps/room_live.yaml yet, or when REMAP=1 is set:
On the robot:
{ [ ! -f $M/room_live.yaml ] || [ "$REMAP" = 1 ]; } && MAPPING=1
On the robot:
if [ $MAPPING = 1 ]; then
sudo systemctl stop rosorin-nav
ros2 launch rosorin_base nav_slam.launch.py > /tmp/explore_nav.log 2>&1 &
On the robot:
# keep the new map only from a run that finished and got home (a run cut short leaves a partial map); old one -> history
if [ $MAPPING = 1 ] && [ $RC = 0 ]; then
[ -f $M/room_live.yaml ] && { H=$M/history/$(date +%Y%m%d_%H%M%S); mkdir -p $H && mv $M/room_live.* $H/; }
ros2 run nav2_map_server map_saver_cli --fmt png -f $M/room_live 2>&1 | tail -1
ls -la $M/room_live.*
fi
launch/nav_slam.launch.py is the navigation stack of chapter 18 with async_slam_toolbox_node in mapping mode,
no keepout, and Nav2 started straight away (autostart: True). rosorin-explore.service does not pass REMAP, so
a remap means running the script by hand with REMAP=1 in its environment.
Not test-built
- Mapping runs with
nav_slam.launch.pyhappened on 2026-10-02 (several) and 2026-10-05 (two).
docs/decisions.md(2026-10-02, night) lists "a mapping run saved by the script" as not verified, and the
2026-10-05 run that left the room was stopped with "the map was not saved". The save step in its current form
(map_saver_cli) has no recorded run.- From reading the code (not from a run): the script saves only the image (
room_live.png+.yaml) with
map_saver_cli. It does not write~/maps/room.posegraph, which is what the always-on navigation continues.
After a remap, navigation starts again on the old graph, and the supervisor's next copy overwrites
room_live.*with the old graph's picture. Also from the code: a fresh map becomes the kept map only when its
pose graph is written as~/maps/room.posegraphand.datawhile navigation is stopped. That has not been
done.REMAP=1 bash ~/setup/explore_run.shhas never been run.
Every one of these cost a map or a day on this robot (docs/lessons.md, 2026-10-02 and 2026-10-06):
mode: says. Only a call to/slam_toolbox/set_localization_mode with true puts it in localization mode, and only then does it subscribe/initialpose. Call the service first, seed the pose second. The supervisor always makes that first call.history/ brought the real one back. "Plugged in" is itself a belief from one voltage step~/battery/plugged.json until the next step corrects it.pose topic is VOLATILE. A subscriber with TRANSIENT_LOCAL ("latched") durability never receives aincompatible QoS ... DURABILITY. The supervisorcome_here.py both had that bug on 2026-10-06. Subscribe with the default (volatile) QoS./map every 5 s, unchanged. Anything that rebuilds on every map message pays for it each time;~/maps/corrupt_20261002/). That is why mapping starts only once the scan confirms the pose.map_start_at_dock is not supported in localization mode (2.6.10 logs it and starts at the odometry pose).map_start_pose.ls ~/maps on the robot shows room_20260928.{posegraph,data,pgm,yaml} (yours or copied) and room.posegraph,room.data from install_live_map.sh.ros2 launch rosorin_base slam.launch.py standing still gives a /map and a map → base_link transform.~/maps/room_live_keepout.yaml exists and names room_live_keepout.pgm, made for the same map frame as the keptWhere this comes from
ros2/rosorin_base/config/slam.yaml,launch/slam.launch.py,launch/nav.launch.py(slam node, keepout file
name),launch/nav_slam.launch.py,rosorin_base/nav_supervisor.py(set_mapping,keep_map, plugged-in
rule, volatile pose reader),config/nav2.yaml(keepout filter),scripts/install_live_map.sh,
scripts/explore_run.sh,systemd/rosorin-explore.service,maps/room_20260928*.yaml,maps/home_pose.json;
docs/odometry.md(SLAM, first room map),docs/decisions.md2026-10-01 (keepout), 2026-10-02 (map keeping,
mapping runs), 2026-10-06 (the map that updates itself),docs/lessons.md2026-10-01 (keepout topics, door),
2026-10-02 (corrupt graph) and 2026-10-06 (slam_toolbox traps, phantom nodes, plug belief, volatile pose),
docs/status.md2026-10-05 (keepout copied, localizer needs PNG, run left the room). Command log: slam_toolbox
install 2026-09-28 06:09 UTC, stand check 06:11, mapping drive and save 06:50-06:57 UTC, keepout mask
2026-10-01 17:15 UTC (Mac), keepout copy on the robot 2026-10-02,install_live_map.shand supervisor tests
2026-10-06 15:21-15:39 UTC, graph restore 2026-10-06. Read from the robot on 2026-10-07 (read-only):~/maps
listing,room_live.yaml, PGM headers,room_live_keepout.yaml, localizer errors in therosorin-navjournal.
survey_live_robot.mditem 13: the repo lacks the pose graph.
← Odometry without wheel encoders · Contents · Always-on navigation with Nav2 →