Build a Mapping Robot With SLAM From Scratch

A mapping robot needs three things: a clean odometry estimate, a calibrated range sensor, and a SLAM back-end that fuses them into an occupancy grid. This guide builds a differential-drive robot on ROS 2 Jazzy. It uses a 2D LiDAR, wheel encoders, an IMU, and slam_toolbox. You get the wiring, the kinematic math, working code, and the tuning steps that decide whether your map closes loops cleanly.

Quick Takeaways

• Hardware stack: Raspberry Pi 5 (8GB), RPLIDAR C1 (10Hz, 12m range), BNO055 IMU, two 12V DC gear motors with 600 CPR encoders, an ESP32 motor controller.
• Software stack: ROS 2 Jazzy, robot_localization (EKF), slam_toolbox (online async), nav2_map_server.
• Critical constraint: SLAM quality is bounded by odometry quality. Calibrate wheel radius and track width first. Tune the SLAM parameters second.
• Result: A 2D occupancy grid saved as .pgm + .yaml, ready for Nav2 localization.

System Architecture

The data flow is linear. Each stage depends on the one before it.

The TF tree must read map → odom → base_link → laser_link. Each frame has exactly one publisher. Two publishers on odom→base_link (raw odometry and the EKF) is the most common cause of a jittering map.

Hardware Selection and Wiring

Sensor Comparison for Indoor Mapping

Drivetrain Choice

Wiring Layout

• Battery: 3S LiPo (11.1V) feeds the motor driver directly.
• Regulator: A 5V/5A buck converter powers the Raspberry Pi 5. Do not share the motor rail with the Pi. Motor current spikes brown out the SBC.
• ESP32: Drives PWM at 20kHz on two pins plus two direction pins. Encoder channels A/B go to interrupt-capable GPIOs.
• LiDAR: USB to the Pi (CP2102 bridge, 460800 baud for the C1).
• IMU: I²C at 400kHz on the Pi, or UART if you use the BNO055 in UART mode.
• Grounds: Join all grounds at a single star point.

Kinematic Foundations

A differential-drive robot has wheel radius r, track width L, and encoder resolution CPR (counts per wheel revolution, after gearing).

Distance per tick:

d_tick = (2 * pi * r) / CPR

Per-interval wheel displacements (Δticks since last update):

dL = d_tick * dticks_left
dR = d_tick * dticks_right

Body displacement and heading change:

ds     = (dR + dL) / 2
dtheta = (dR - dL) / L

Pose update using the midpoint (second-order) approximation:

x     += ds * cos(theta + dtheta / 2)
y     += ds * sin(theta + dtheta / 2)
theta += dtheta

Velocities for the Odometry message:

v = ds / dt
w = dtheta / dt

Systematic errors in r and L produce drift that scales with distance. A 1% error in L gives about 1% error in rotation. After 10 full turns, that is 36° of heading error.

Software Setup

Install on Ubuntu 24.04 with ROS 2 Jazzy:

sudo apt update
sudo apt install -y ros-jazzy-slam-toolbox ros-jazzy-robot-localization \
  ros-jazzy-nav2-map-server ros-jazzy-nav2-lifecycle-manager \
  ros-jazzy-rplidar-ros ros-jazzy-robot-state-publisher \
  ros-jazzy-joint-state-publisher ros-jazzy-teleop-twist-keyboard \
  ros-jazzy-rviz2

mkdir -p ~/mapbot_ws/src && cd ~/mapbot_ws/src
ros2 pkg create mapbot --build-type ament_python \
  --dependencies rclpy nav_msgs geometry_msgs tf2_ros std_msgs

Robot Description (URDF)

The static transforms must match the physical robot. Measure the LiDAR offset with calipers.

<?xml version="1.0"?>
<robot name="mapbot">
  <link name="base_link"/>

  <!-- LiDAR mounted 0.10m forward, 0.12m above base_link origin -->
  <link name="laser_link"/>
  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <!-- rpy yaw: set to 3.14159 if the LiDAR connector faces forward -->
    <origin xyz="0.10 0 0.12" rpy="0 0 0"/>
  </joint>

  <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
  </joint>
</robot>

Wheel Odometry Node

This node converts raw tick counts into /odom. It leaves TF publishing to the EKF.

#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion


class WheelOdom(Node):
    def __init__(self):
        super().__init__('wheel_odom')
        # Calibrated physical parameters (meters, counts)
        self.declare_parameter('wheel_radius', 0.033)   # r
        self.declare_parameter('track_width', 0.160)    # L
        self.declare_parameter('cpr', 600.0)            # encoder counts/rev

        self.r = self.get_parameter('wheel_radius').value
        self.L = self.get_parameter('track_width').value
        self.cpr = self.get_parameter('cpr').value
        self.d_tick = 2.0 * math.pi * self.r / self.cpr

        self.x = self.y = self.th = 0.0
        self.prev = None          # previous cumulative ticks [left, right]
        self.prev_t = None

        self.sub = self.create_subscription(
            Int32MultiArray, '/wheel_ticks', self.cb, 50)
        self.pub = self.create_publisher(Odometry, '/odom', 50)

    def cb(self, msg):
        now = self.get_clock().now()
        ticks = list(msg.data)    # data[0]=left, data[1]=right (cumulative)
        if self.prev is None:
            self.prev, self.prev_t = ticks, now
            return

        dt = (now - self.prev_t).nanoseconds * 1e-9
        if dt <= 0.0:
            return

        dL = (ticks[0] - self.prev[0]) * self.d_tick   # left wheel travel (m)
        dR = (ticks[1] - self.prev[1]) * self.d_tick   # right wheel travel (m)
        ds = 0.5 * (dR + dL)                           # forward displacement
        dth = (dR - dL) / self.L                       # heading change (rad)

        # Midpoint integration
        self.x += ds * math.cos(self.th + 0.5 * dth)
        self.y += ds * math.sin(self.th + 0.5 * dth)
        self.th += dth

        o = Odometry()
        o.header.stamp = now.to_msg()
        o.header.frame_id = 'odom'
        o.child_frame_id = 'base_link'
        o.pose.pose.position.x = self.x
        o.pose.pose.position.y = self.y
        o.pose.pose.orientation = Quaternion(
            z=math.sin(self.th / 2.0), w=math.cos(self.th / 2.0))
        o.twist.twist.linear.x = ds / dt               # v (m/s)
        o.twist.twist.angular.z = dth / dt             # w (rad/s)

        # Covariance diagonal: x, y, z, roll, pitch, yaw (pose)
        o.pose.covariance[0] = 0.01
        o.pose.covariance[7] = 0.01
        o.pose.covariance[35] = 0.05
        o.twist.covariance[0] = 0.01
        o.twist.covariance[35] = 0.05

        self.pub.publish(o)
        self.prev, self.prev_t = ticks, now


def main():
    rclpy.init()
    rclpy.spin(WheelOdom())


if __name__ == '__main__':
    main()

Sensor Fusion: EKF Configuration

Wheel odometry slips on turns. The IMU gyro (yaw rate) corrects heading. Feed position and velocity from wheels, and yaw rate from the IMU.

# config/ekf.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true              # constrain z, roll, pitch to zero
    publish_tf: true              # EKF owns odom -> base_link
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /odom
    # [x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az]
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  false, false,
                   false, false, false,
                   false, false, false]

    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, true,    # fuse yaw rate only
                  false, false, false]
    imu0_remove_gravitational_acceleration: true

SLAM Configuration: slam_toolbox

slam_toolbox builds a pose graph. Each scan becomes a node. Scan matching (a correlation-based matcher) creates edges. Loop closure adds long-range constraints. The back-end (Ceres) solves:

X* = argmin_X  sum_ij  || f(x_i, x_j) - z_ij ||^2_(Omega_ij)

where x_i are poses, z_ij are relative-pose measurements, and Omega_ij is the information matrix. Loop closure corrects accumulated drift in one global optimization.

# config/slam.yaml
slam_toolbox:
  ros__parameters:
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan
    mode: mapping

    resolution: 0.05                    # 5cm grid cells
    max_laser_range: 10.0               # trim below the C1's 12m for cleaner returns
    minimum_time_interval: 0.5
    transform_publish_period: 0.02
    map_update_interval: 2.0

    # Keyframe gating: new node every 0.3m or ~0.2rad of motion
    minimum_travel_distance: 0.3
    minimum_travel_heading: 0.2

    # Loop closure
    do_loop_closing: true
    loop_search_maximum_distance: 3.0
    loop_match_minimum_response_coarse: 0.45
    loop_match_minimum_response_fine: 0.55

    # Scan matcher
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    angle_variance_penalty: 1.0
    distance_variance_penalty: 0.5

    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY

Launch File

# launch/mapping.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    pkg = get_package_share_directory('mapbot')
    urdf = os.path.join(pkg, 'urdf', 'mapbot.urdf')
    with open(urdf) as f:
        robot_desc = f.read()

    return LaunchDescription([
        Node(package='robot_state_publisher', executable='robot_state_publisher',
             parameters=[{'robot_description': robot_desc}]),   # static TF tree
        Node(package='mapbot', executable='wheel_odom'),
        Node(package='robot_localization', executable='ekf_node',
             parameters=[os.path.join(pkg, 'config', 'ekf.yaml')]),
        Node(package='rplidar_ros', executable='rplidar_node',
             parameters=[{'serial_port': '/dev/ttyUSB0',
                          'serial_baudrate': 460800,
                          'frame_id': 'laser_link',
                          'scan_mode': 'Standard'}]),
        Node(package='slam_toolbox', executable='async_slam_toolbox_node',
             parameters=[os.path.join(pkg, 'config', 'slam.yaml')]),
    ])

Calibration Before You Map

Skip this and no SLAM parameter set will save you.

Linear Calibration (wheel radius)

1. Mark a 3.000m straight line.
2. Command the robot to drive until odometry reads 3.000m.
3. Measure the real distance traveled, d_real.
4. Correct: r_new = r_old * (d_real / d_odom).

Rotational Calibration (track width)

1. Command 10 full rotations in place (odometry reads 20π rad).
2. Measure the real rotation with a floor mark.
3. Correct: L_new = L_old * (theta_odom / theta_real).

Repeat both tests clockwise and counterclockwise. Asymmetry points to unequal wheel diameters or a gearbox problem.

Practical Workflow: Mapping a Room End to End

1. Start the stack and the visualizer.

cd ~/mapbot_ws && colcon build --symlink-install && source install/setup.bash
ros2 launch mapbot mapping.launch.py
rviz2   # Fixed Frame: map | Add: Map (/map), LaserScan (/scan), TF

2. Verify TF and scan health before driving.

ros2 run tf2_tools view_frames           # confirm map->odom->base_link->laser_link
ros2 topic hz /scan                      # expect ~10Hz
ros2 topic echo /odometry/filtered --once

3. Drive slowly. Teleoperate at 0.15 to 0.2 m/s linear and under 0.5 rad/s angular:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

4. Close loops deliberately. Return to your start point along the same walls. Watch RViz: a successful loop closure snaps the map into alignment.

5. Save the map.

ros2 run nav2_map_server map_saver_cli -f ~/maps/living_room \
  --ros-args -p save_map_timeout:=10000.0

This writes living_room.pgm (the grid) and living_room.yaml (resolution, origin, thresholds). You can also serialize the full pose graph with the slam_toolbox service for later continuation of mapping.

Troubleshooting Matrix

Tuning the Matcher for Your Environment

• Small rooms (under 20 m²): Reduce minimum_travel_distance to 0.2 for denser keyframes. Cut max_laser_range to 6m.
• Large open halls: Raise max_laser_range to 12m. Increase correlation_search_space_dimension to 0.8 to absorb larger odometry errors between scans.
• Long corridors: Rely on a tight odometry prior. Raise distance_variance_penalty so the matcher cannot slide the robot along the corridor axis.
• CPU load on the Pi: Increase map_update_interval to 5.0. Switch the solver linear solver to SPARSE_SCHUR if memory grows on large maps.

Evaluating Map Quality

Measure instead of eyeballing:

• Wall thickness: A well-built map shows walls 1 to 2 cells thick (5 to 10cm at 0.05m resolution). Thicker walls indicate residual drift.
• Closure error: Drive a loop and compare the pose at the start and end in /odometry/filtered vs. the map→base_link transform. A calibrated robot should land within 5cm after a 20m loop with closure active.
• Ground truth: Measure two wall-to-wall distances with a laser meter, then measure the same distances in the saved map using the .yaml resolution. Target under 2% error.

FAQ

What is the best SLAM algorithm for a beginner mapping robot?

Use slam_toolbox in online async mode. It ships with ROS 2, handles loop closure, and runs on a Raspberry Pi 5. Cartographer offers stronger multi-sensor fusion but has a steeper configuration curve. Gmapping is a lighter particle-filter option for tiny CPUs, though it has no maintained ROS 2 path in most distributions.

Do I need an IMU for SLAM?

No, but you should add one. 2D LiDAR SLAM works with wheel odometry alone. An IMU gyro corrects wheel slip during turns, which is where wheel odometry fails most. Fusing yaw rate with an EKF typically cuts heading drift enough to improve loop closure success.

Why does my SLAM map drift or double up walls?

The cause is almost always odometry error or a wrong LiDAR transform. Check three items in order: wheel radius and track width calibration, the laser_joint yaw offset in the URDF, and duplicate TF publishers on odom→base_link. Then slow the robot to under 0.2 m/s.

Can I run SLAM on a Raspberry Pi?

Yes. A Raspberry Pi 5 (8GB) runs slam_toolbox at 5cm resolution for rooms up to about 200 m². Raise map_update_interval, keep resolution at 0.05m or coarser, and run RViz on a separate laptop over the network instead of on the Pi.