M7-T7 — SLAM and Nav2 Integration

Learning objectives

After this lesson, you should be able to:

• List what Nav2 requires from the SLAM or localisation system and which topics, frames and QoS are involved.
• Describe the global and local costmaps, their layers and the role of the inflation layer.
• Explain how to run Nav2 with live SLAM Toolbox instead of AMCL and map_server.
• Diagnose integration faults: wrong frames, missing map, lifecycle not active, wrong topic remaps and use_sim_time.

The problem and the intuition

Nav2 is a planner and a controller around a cost picture of the world. The global costmap is built on the map and answers “which route avoids all the walls?”; the local costmap is built from the live lidar in a small window that moves with the robot and answers “what is in my way right now?”. The map frame gives the global picture its coordinates; the odom frame keeps the local picture smooth. SLAM supplies the first, odometry supplies the second.

When the map comes from a live SLAM node, the map changes while Nav2 is using it, and the map-to-odom transform occasionally jumps. A robust integration lets the planner handle a changing map and the controller work in the smooth odom frame, as REP 105 intends (M7-T2). The failures are mostly start-up and naming: a costmap waiting for a map that never arrives, a node still unconfigured, a velocity command sent to the wrong topic.

Nav2 asks SLAM for three things: /map, the map → odom → base_link chain, and a clock everyone agrees on. Everything else is configuration.

The interfaces, the costmaps, the bring-up

Problem. Make a robot navigate to goals in an environment that is being mapped, or has been mapped, using SLAM output as the source of map and localisation.

Assumptions. A working SLAM node with correct TF and timing (M7-T2 to M7-T4); a base that accepts geometry_msgs/Twist (or TwistStamped in newer releases); a footprint for the robot; Nav2 for the same ROS 2 distribution.

What Nav2 needs

Costmaps and their layers

Each costmap is a stack of layers, combined by taking the maximum cost per cell.

• Static layer: the map from /map. With subscribe_to_updates: true it follows live map updates from SLAM.
• Obstacle (or voxel) layer: live sensor data marked and cleared with ray tracing; sees what the map does not (people, moved chairs).
• Inflation layer: grows costs around obstacles so that paths keep a margin. Cost decays exponentially with the distance d from the obstacle:

Cells closer than the inscribed radius of the robot are lethal or inscribed (cost 253–254): any robot centre there collides. Beyond the inflation radius cost is zero. Global costmap: frame map, whole environment, used by the planner. Local costmap: frame odom, rolling_window: true, a few metres around the robot, used by the controller.

Replacing AMCL and map_server with live SLAM

With SLAM Toolbox publishing /map and map → odom, do not start map_server or amcl: two publishers of map → odom conflict. Use Nav2’s navigation launch (navigation_launch.py), which starts the planner, controller, behaviour tree navigator and costmaps, without localisation. In the parameters set the global costmap’s static layer to the live map, global_frame: map and robot_base_frame to your base frame.

Lifecycle bring-up

Nav2 servers are lifecycle nodes: unconfigured → inactive → active. The lifecycle_manager (with autostart: true) brings them up in order. If a server is not active, goals are rejected or ignored:

ros2 lifecycle get /planner_server            # expect: active [3]
ros2 topic echo /global_costmap/costmap --once    # does the costmap have the right size and origin?
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: map}, pose: {position: {x: 1.0, y: 0.0}}}}"

costmap

a grid of traversal costs derived from the map and sensors.

inscribed radius

the radius of the largest circle inside the robot footprint.

lifecycle node

node with explicit configure/activate states, managed by the lifecycle manager.

behaviour tree

the logic (compute path, follow, recover) run by bt_navigator.

Integration failures

A worked example: inflation cost and a failed bring-up

1. Inflation numbers. A round robot with inscribed radius r = 0.22 m, cost_scaling_factor: 3.0 and inflation_radius: 0.55 m. A cell 0.30 m from a wall has cost 252 × exp(−3 × (0.30 − 0.22)) = 252 × exp(−0.24) = 198. At 0.40 m: 252 × exp(−0.54) = 147. At 0.55 m: 252 × exp(−0.99) = 94. Beyond 0.55 m: 0. A planner minimising cost therefore prefers the middle of a corridor, where the cost on both sides is lower.
2. A door too narrow? The doorway is 0.9 m wide. A robot centred in it is 0.45 m from each jamb: more than the inscribed radius of 0.22 m, so it is not in collision, but inside the inflation radius of 0.55 m, so the cells there cost 252 × exp(−3 × (0.45 − 0.22)) = 126 and the planner treats the door as costly but passable. With inflation_radius: 0.40 the centre line would be free. If the footprint was entered as a 0.5 m radius circle by mistake, the doorway centre is lethal and the robot refuses to go through.
3. A failed bring-up. Goals sent to /navigate_to_pose are accepted, but the robot does not move. ros2 lifecycle get /controller_server reports inactive. The lifecycle manager’s node_names list lacks controller_server: it was never activated. Adding it to the list (or fixing the typo contoller_server) fixes it.
4. Two localisers. The launch starts SLAM Toolbox and also bringup_launch.py (which includes AMCL and map_server). Both publish map → odom; tf2_echo map odom alternates between two values every few milliseconds, and the robot’s pose flickers. The fix is to start only navigation_launch.py (no localisation) next to SLAM Toolbox.
5. Live map size. The global costmap resizes with the SLAM map. As the map grows from 20 m to 60 m, the global costmap cells grow from 400 × 400 to 1 200 × 1 200 at 0.05 m, 1.44 M cells: planning slows. Using rolling_window for the local map and a coarser global resolution keeps the cost down (M7-T8).

Engineering implications and limitations

Engineering implications

• One publisher for map → odom. Decide which node owns it for each mode (SLAM when mapping; SLAM localization or AMCL when navigating) and launch only that one.
• Set the footprint carefully and the inflation to match the robot and the narrowest passage you need; look at the costmaps in RViz before field tests.
• Plan in map, control in odom. Let the planner replan when the map shifts and the local controller work in the smooth frame.
• Verify lifecycle states in the bring-up checklist and use the lifecycle manager’s autostart; log failures at configuration time.
• Test navigation on a recorded run first where possible; then test with a safe stop and a conservative speed.
• Mind the unknown space. Decide whether unknown cells are traversable (exploration) or not (operation) and set track_unknown_space and the planner’s allow_unknown accordingly.

Limits

Nav2 plans on a 2D cost picture. Overhanging obstacles, stairs, glass and negative obstacles need other sensors and layers. A live SLAM map can change under the planner, so plans are only as stable as the mapping; production robots usually navigate on a validated saved map and use SLAM for update. Parameter names and launch structure change between ROS 2 releases, so take them from the documentation of your version.

Common misconceptions

Engineering takeaways

1. Nav2 needs /map (transient local), map → odom → base_link, sensor data, a cmd_vel sink and a common clock.
2. The global costmap lives in map for planning; the local costmap in odom for control.
3. Inflation cost decays as 252·exp(−k(d − r_inscribed)); set footprint and radius to fit your passages.
4. With live SLAM, do not also run AMCL and map_server: only one publisher of map → odom.
5. Check lifecycle states, QoS on /map, frame names and use_sim_time first when navigation does not start.

References and further reading

Primary papers and authoritative documentation.

• Macenski, S., Martín, F., White, R. and Ginés Clavero, J. (2020). The Marathon 2: A navigation system. IEEE/RSJ IROS.
• Macenski, S., Moore, T., Lu, D. V., Merzlyakov, A. and Ferguson, M. (2023). From the desks of ROS maintainers: A survey of modern and capable mobile robotics algorithms in the Robot Operating System 2. Robotics and Autonomous Systems 168.
• Open Navigation. Nav2 documentation: Configuration guide, Costmap 2D, Setting up with SLAM. docs.nav2.org.
• Lu, D. V., Hershberger, D. and Smart, W. D. (2014). Layered costmaps for context-sensitive navigation. IEEE/RSJ IROS.
• Macenski, S. and Jambrecic, I. (2021). SLAM Toolbox: SLAM for the dynamic world. Journal of Open Source Software 6(61), 2783.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

A navigation system works only if the computer keeps up. The final lesson of the module covers latency, CPU and memory budgets and what to tune when a small robot struggles.

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T6: Map Saving and Reuse

  Distinguish a saved occupancy map from a serialised pose graph and reuse each correctly.

• Lab
  M7-L5: Debug SLAM-to-Nav2 Integration

  Make Nav2 navigate on a SLAM map: fix costmap frames, topic remaps and lifecycle bring-up.