SLAM with ROS 2 — Shahid Kamal

If you've ever wondered how a robot "knows" where it is in a room it's never seen before — that's SLAM. Simultaneous Localization and Mapping is one of the foundational problems in robotics, and it's deceptively hard. The robot needs to build a map of an unknown environment while simultaneously figuring out its own position within that map. It's a chicken-and-egg problem: you need a map to localize, but you need to localize to build a map.

In this post, I'll walk through how I implemented a full SLAM pipeline on a mobile robot using ROS 2, a 2D LiDAR sensor, and wheel odometry — covering the theory, the code, and the real-world lessons learned.

1. The SLAM Problem, Simply Explained

Imagine you're blindfolded in a house you've never visited. You can reach out and feel nearby walls and furniture. As you walk around, you're doing two things at once: building a mental map of the house, and tracking where you are inside it. That's SLAM.

Formally, SLAM estimates two things simultaneously:

The robot's pose (position + orientation) over time
A map of the environment (occupancy grid, landmarks, etc.)

The challenge is that both are uncertain. Sensors are noisy, wheels slip, and LiDAR readings have measurement error. Everything is probabilistic — and that's where things get interesting.

2. Hardware Setup

For this project, I used a differential-drive mobile robot with the following sensor stack:

RPLiDAR A1 — 360° 2D LiDAR, 8000 samples/sec, 12m range
Wheel encoders — attached to both drive motors for odometry
IMU (MPU-6050) — 6-axis inertial measurement for angular velocity and acceleration
Jetson Nano — onboard compute running Ubuntu 22.04 + ROS 2 Humble

The LiDAR gives us range measurements (how far away obstacles are), the wheel encoders give us a rough estimate of how far we've moved, and the IMU helps correct rotational drift. None of these alone is accurate enough — but fused together, they become powerful.

3. Sensor Fusion with the Extended Kalman Filter

Raw odometry from wheel encoders drifts badly over time. After a few meters of driving, the estimated position can be off by a significant margin — especially during turns where wheel slip is common. This is where sensor fusion comes in.

I used an Extended Kalman Filter (EKF) to fuse wheel odometry and IMU data into a more accurate pose estimate. The EKF operates in two steps:

Predict where you think you are based on motion (odometry), then correct that prediction using independent measurements (IMU). Repeat forever.

The state vector tracks the robot's x, y position, heading (θ), and their velocities. Here's the core prediction-update loop:

# EKF Prediction Step
def predict(self, dt, v, omega):
    """Predict next state from control inputs."""
    theta = self.state[2]
    
    # State transition (motion model)
    self.state[0] += v * np.cos(theta) * dt  # x
    self.state[1] += v * np.sin(theta) * dt  # y
    self.state[2] += omega * dt               # theta
    
    # Jacobian of the motion model
    F = np.eye(3)
    F[0, 2] = -v * np.sin(theta) * dt
    F[1, 2] =  v * np.cos(theta) * dt
    
    # Update covariance
    self.P = F @ self.P @ F.T + self.Q

# EKF Update Step
def update(self, z, H, R):
    """Correct state with measurement z."""
    y = z - H @ self.state          # Innovation
    S = H @ self.P @ H.T + R        # Innovation covariance
    K = self.P @ H.T @ np.linalg.inv(S)  # Kalman gain
    
    self.state = self.state + K @ y
    self.P = (np.eye(3) - K @ H) @ self.P

In ROS 2, the robot_localization package provides a production-ready EKF node that handles all of this. I configured it to subscribe to /odom (wheel encoders) and /imu/data (IMU), and it publishes a fused /odometry/filtered topic:

# ekf_config.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    odom0: /odom
    odom0_config: [true, true, false,
                   false, false, true,
                   true, false, false,
                   false, false, true,
                   false, false, false]
    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  false, false, false]

The boolean arrays tell the EKF which dimensions of each sensor to trust. For a 2D ground robot, we fuse x, y, yaw from odometry and yaw rate from the IMU. The result is dramatically smoother than raw odometry — especially during sharp turns.

4. Running SLAM with Nav2 and slam_toolbox

With reliable fused odometry, the next step is actual map building. I used slam_toolbox, which is the recommended SLAM package in the ROS 2 Nav2 stack. It implements an optimized pose-graph SLAM approach.

The core idea: as the robot moves and takes LiDAR scans, slam_toolbox builds a graph where each node is a pose (robot position + LiDAR scan) and edges represent the spatial relationship between poses. When the robot revisits a previously seen area — a loop closure — the entire graph is optimized to correct accumulated drift.

# Launch slam_toolbox in async mode
ros2 launch slam_toolbox online_async_launch.py \
    slam_params_file:=./mapper_params.yaml \
    use_sim_time:=false

Key parameters I tuned in mapper_params.yaml:

resolution: 0.05 — 5cm grid cells. Smaller = more detail but more memory
max_laser_range: 8.0 — ignore readings beyond 8m (noise increases with distance)
minimum_travel_distance: 0.3 — only add a new scan after moving 30cm to avoid redundant data
loop_search_maximum_distance: 3.0 — search for loop closures within 3m of current position

5. The TF Tree: ROS 2's Coordinate Frame System

One thing that tripped me up early on was the TF (transform) tree. In ROS 2, every sensor and component lives in its own coordinate frame, and the system needs to know how they relate to each other spatially.

For SLAM to work correctly, you need these transforms:

map → odom → base_link → laser_frame
 ↑        ↑          ↑
slam    EKF     static TF

base_link → laser_frame: A static transform describing where the LiDAR is mounted on the robot (e.g., 10cm above center). Published via static_transform_publisher
odom → base_link: Published by the EKF node based on fused sensor data. This is the robot's estimated pose relative to where it started
map → odom: Published by slam_toolbox. This corrects the accumulated drift in odometry by anchoring it to the global map

If any of these transforms are missing or stale, the entire pipeline breaks silently. My debugging tip: always run ros2 run tf2_tools view_frames to visualize your TF tree early in the process.

6. Real-World Challenges

Theory is clean. Reality isn't. Here are the problems I hit:

Glass walls and mirrors. LiDAR uses infrared light, and glass surfaces either absorb or reflect the beams unpredictably. My lab had a glass partition that created phantom readings, causing the map to show a wall where there was only glass. The fix: I added a range filter node that discards readings at the exact distance of known glass surfaces, and tuned max_laser_range down to reduce long-range noise.

Featureless hallways. Long corridors where both walls look identical from the LiDAR's perspective are a SLAM nightmare. The scan matcher can't distinguish one section from another, leading to incorrect loop closures that warp the entire map. I mitigated this by increasing minimum_travel_distance in corridors and relying more heavily on odometry in these areas.

Dynamic obstacles. People walking through the environment create transient scan features. The map shouldn't include a person standing in a doorway, but if the robot scans that spot while they're there, it might. slam_toolbox handles this reasonably well because it averages multiple observations, but it's still an issue in high-traffic areas.

Compute constraints. On the Jetson Nano, running the LiDAR driver, EKF, slam_toolbox, and visualization simultaneously pushed CPU usage to ~85%. I had to reduce the LiDAR scan rate from 10Hz to 5Hz and lower the slam_toolbox processing rate. On a Jetson Orin, this wouldn't be an issue — but for a resource-constrained setup, optimization matters.

7. Results

After tuning, the system reliably mapped our lab environment — roughly 15m × 10m with multiple rooms, a corridor, and open spaces. The occupancy grid was accurate to within ~3cm compared to hand measurements, and loop closures corrected cumulative drift to under 5cm over a full traversal.

The entire map-building run took about 4 minutes of manual teleoperation (driving the robot around via joystick). Once built, the map was saved and reused for autonomous navigation using Nav2's path planner and controller.

Key Takeaways

Sensor fusion is non-negotiable. Raw odometry alone will always drift. An EKF fusing wheel encoders + IMU is the minimum viable setup for reliable SLAM
The TF tree is your best friend and worst enemy. Get it right before debugging anything else. Most "SLAM isn't working" problems are actually transform problems
Tune for your environment. Default slam_toolbox parameters work for demo rooms but not for real spaces with glass, corridors, and dynamic objects
Start with teleop, then automate. Build the map manually first. Once you trust the map, add autonomous exploration

What's Next

In a follow-up post, I'll cover autonomous navigation with Nav2 — using the SLAM-generated map for path planning, obstacle avoidance, and waypoint following. I'll also explore the jump from 2D to 3D SLAM using depth cameras and how that changes the pipeline.

If you're working on SLAM or ROS 2 projects, I'd love to hear about your setup. Reach out via email or find me on GitHub.