Build a ROS 2 Rover: Raspberry Pi 5, Motors, and LiDAR

A rover that maps its own environment needs three things working together: a motor controller that holds wheel speed, an odometry estimate that turns encoder ticks into a pose, and a LiDAR feed that a SLAM node can match against. This guide builds all three on a two-wheel differential-drive chassis. The Raspberry Pi 5 runs ROS 2, and a Raspberry Pi Pico closes the wheel-speed loop at 50Hz. You finish with a robot that you drive from a laptop while it builds an occupancy grid.

Quick Takeaways

• Split the compute. The Pi 5 runs ROS 2, SLAM, and Nav2. A Pico runs the 50Hz wheel PID. Linux scheduling jitter stays out of your control loop.
• Stack: Ubuntu 24.04 + ROS 2 Jazzy on the Pi 5, RPLIDAR C1 at 460800 baud, slam_toolbox in online async mode.
• Odometry: midpoint integration of encoder deltas. One 360° spin test calibrates wheel_separation.
• TF chain: map → odom → base_footprint → base_link → laser_link. slam_toolbox owns the first link, your base node owns the second, and the URDF owns the rest.

System Architecture and Bill of Materials

The data path is short. The laptop sends /cmd_vel. The Pi 5 converts it to wheel speeds and sends them over USB serial. The Pico drives the TB6612FNG H-bridge, reads the quadrature encoders, and returns tick counts. The Pi turns those counts into /odom and the odom → base_footprint transform. The LiDAR publishes /scan, and slam_toolbox publishes /map.

Match the motor voltage and stall current to your driver. A stalled 12V motor can exceed 1.2A and trip the TB6612FNG thermal limit. Pick motors with a stall current under 2A and cap PWM duty in firmware.

Why a Microcontroller for the Wheel Loop

The Pico plus a plain-text serial protocol is the most debuggable option. You can test it with a terminal before ROS 2 enters the picture.

Wiring Layout

All grounds are common: battery negative, buck converter, TB6612FNG, Pico, encoders.

Power the encoders from the Pico 3V3 pin, not 5V. This keeps the Hall signals within the RP2040’s 3.3V GPIO limit. Feed VM on the TB6612 from the battery. Plug the LiDAR into a Pi 5 USB-A port. A 5V/5A supply lets the Pi 5 allow its full USB current budget.

Firmware: Wheel-Speed PID on the Pico

Flash MicroPython to the Pico, then save this as main.py. The protocol is plain text. The Pi sends V <left_rad_s> <right_rad_s>. The Pico replies every 20ms with E <left_ticks> <right_ticks>, both cumulative counts.

# main.py - MicroPython on Raspberry Pi Pico (RP2040)
from machine import Pin, PWM
import sys, time, select, math

TICKS_PER_REV = 748      # 11 PPR x 2 edges (2x decode) x 34:1 gearbox
LOOP_MS = 20             # 50 Hz control loop
CMD_TIMEOUT_MS = 500     # stop motors if the Pi goes silent
MAX_WHEEL_RAD_S = 12.0   # used for feedforward scaling

class Encoder:
    def __init__(self, pin_a, pin_b, sign):
        self.a = Pin(pin_a, Pin.IN, Pin.PULL_UP)
        self.b = Pin(pin_b, Pin.IN, Pin.PULL_UP)
        self.count = 0
        self.sign = sign  # +1 or -1 to fix mirrored mounting
        # Fire on both edges of channel A (2x decoding)
        self.a.irq(trigger=Pin.IRQ_RISING | Pin.IRQ_FALLING, handler=self._isr)

    def _isr(self, _):
        # A != B after an A edge means forward rotation (A leads B)
        self.count += self.sign if self.a.value() != self.b.value() else -self.sign

class Motor:
    def __init__(self, pwm_pin, in1, in2):
        self.pwm = PWM(Pin(pwm_pin))
        self.pwm.freq(20000)             # 20 kHz: above audible range
        self.in1, self.in2 = Pin(in1, Pin.OUT), Pin(in2, Pin.OUT)

    def set(self, u):                    # u in [-1.0, 1.0]
        self.in1.value(1 if u > 0 else 0)
        self.in2.value(1 if u < 0 else 0)
        self.pwm.duty_u16(int(min(abs(u), 1.0) * 65535))

class PID:
    def __init__(self, kp, ki, kd, kff, i_max=0.5):
        self.kp, self.ki, self.kd, self.kff = kp, ki, kd, kff
        self.i, self.prev, self.i_max = 0.0, 0.0, i_max

    def step(self, target, measured, dt):
        err = target - measured
        self.i = max(-self.i_max, min(self.i_max, self.i + err * dt))  # anti-windup clamp
        d = (err - self.prev) / dt
        self.prev = err
        u = self.kff * target + self.kp * err + self.ki * self.i + self.kd * d
        return max(-1.0, min(1.0, u))

    def reset(self):
        self.i, self.prev = 0.0, 0.0

Pin(9, Pin.OUT).value(1)                       # TB6612 STBY high = enabled
enc_l, enc_r = Encoder(2, 3, +1), Encoder(4, 5, +1)
mot_l, mot_r = Motor(10, 11, 12), Motor(13, 14, 15)
pid_l = PID(0.05, 0.30, 0.0, 1.0 / MAX_WHEEL_RAD_S)
pid_r = PID(0.05, 0.30, 0.0, 1.0 / MAX_WHEEL_RAD_S)

poll = select.poll()
poll.register(sys.stdin, select.POLLIN)
buf, tgt_l, tgt_r = "", 0.0, 0.0
prev_l = prev_r = 0
last_cmd = last_loop = time.ticks_ms()
dt = LOOP_MS / 1000.0

while True:
    # Parse incoming "V <l> <r>" commands (non-blocking)
    while poll.poll(0):
        ch = sys.stdin.read(1)
        if ch == "\n":
            p = buf.split()
            if len(p) == 3 and p[0] == "V":
                try:
                    tgt_l, tgt_r = float(p[1]), float(p[2])
                    last_cmd = time.ticks_ms()
                except ValueError:
                    pass
            buf = ""
        else:
            buf += ch

    now = time.ticks_ms()
    if time.ticks_diff(now, last_loop) >= LOOP_MS:
        last_loop = now
        cl, cr = enc_l.count, enc_r.count
        # Wheel speed in rad/s from tick deltas over one loop period
        w_l = (cl - prev_l) / TICKS_PER_REV * 2 * math.pi / dt
        w_r = (cr - prev_r) / TICKS_PER_REV * 2 * math.pi / dt
        prev_l, prev_r = cl, cr

        if time.ticks_diff(now, last_cmd) > CMD_TIMEOUT_MS:
            tgt_l = tgt_r = 0.0                # watchdog: command stream lost
        if tgt_l == 0.0 and tgt_r == 0.0:
            pid_l.reset(); pid_r.reset()
            mot_l.set(0); mot_r.set(0)
        else:
            mot_l.set(pid_l.step(tgt_l, w_l, dt))
            mot_r.set(pid_r.step(tgt_r, w_r, dt))

        print("E %d %d" % (cl, cr))             # cumulative ticks to the Pi

The feedforward term kff = 1/12 supplies most of the duty cycle. The PID only corrects the remainder. This keeps kp small and the response free of oscillation.

Kinematic Foundations

Define the wheel radius r, wheel separation L, body linear velocity v, and angular velocity ω.

Inverse kinematics (/cmd_vel to wheel speeds, rad/s):

w_left  = (v - ω·L/2) / r
w_right = (v + ω·L/2) / r

Forward kinematics (encoder deltas to pose). With ticks per revolution N, the wheel arc lengths over one sample are:

Δs_l = Δticks_l · 2π·r / N
Δs_r = Δticks_r · 2π·r / N

Δs = (Δs_r + Δs_l) / 2          # distance traveled by the center
Δθ = (Δs_r - Δs_l) / L          # heading change

x  += Δs · cos(θ + Δθ/2)        # midpoint integration
y  += Δs · sin(θ + Δθ/2)
θ  += Δθ

Midpoint integration (using θ + Δθ/2) cuts the error that plain Euler integration adds during turns. With N = 748, r = 0.033m, one tick equals about 0.277mm of wheel travel.

Install the Software Stack

Flash Ubuntu Server 24.04 LTS (64-bit) to the Pi 5 with Raspberry Pi Imager. Jazzy is the matching distribution. The ROS docs cover installing on a Raspberry Pi with 64-bit Ubuntu and the normal binary install instructions. Follow the official apt repository setup, then install:

sudo apt update
sudo apt install -y ros-jazzy-ros-base ros-jazzy-slam-toolbox \
  ros-jazzy-nav2-map-server ros-jazzy-robot-state-publisher \
  ros-jazzy-teleop-twist-keyboard python3-serial \
  python3-colcon-common-extensions git
echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
sudo usermod -aG dialout $USER   # serial port access; log out and back in

Which distribution? Lyrical is the newer LTS and the standard choice for Ubuntu 26.04, while Jazzy remains supported through May 2029 and is the standard choice for Ubuntu 24.04. Pick the Ubuntu and ROS pair together. This guide uses Jazzy on 24.04. The Python nodes and launch files are distribution-agnostic, so you only change the ros-jazzy-* package names for Lyrical on 26.04.

Clone and build the Slamtec driver:

mkdir -p ~/rover_ws/src && cd ~/rover_ws/src
git clone https://github.com/Slamtec/sllidar_ros2.git
ros2 pkg create --build-type ament_python rover_bringup \
  --dependencies rclpy geometry_msgs nav_msgs tf2_ros
mkdir -p rover_bringup/{launch,urdf,config}

Add udev rules so device names stay stable across reboots. Find the IDs with ls -l /dev/serial/by-id/, then use the by-id paths in your launch arguments.

The Base Node: Serial Bridge and Odometry

Save as ~/rover_ws/src/rover_bringup/rover_bringup/base_node.py.

#!/usr/bin/env python3
import math
import serial
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, TransformStamped
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster


class BaseNode(Node):
    def __init__(self):
        super().__init__('base_node')
        self.declare_parameter('port', '/dev/ttyACM0')
        self.declare_parameter('wheel_radius', 0.033)       # r, meters
        self.declare_parameter('wheel_separation', 0.18)    # L, meters
        self.declare_parameter('ticks_per_rev', 748)        # must match firmware
        self.declare_parameter('max_wheel_speed', 12.0)     # rad/s clamp

        self.r = self.get_parameter('wheel_radius').value
        self.L = self.get_parameter('wheel_separation').value
        self.N = self.get_parameter('ticks_per_rev').value
        self.w_max = self.get_parameter('max_wheel_speed').value

        self.ser = serial.Serial(self.get_parameter('port').value, 115200, timeout=0)
        self.buf = ''
        self.w_cmd = (0.0, 0.0)
        self.prev_ticks = None
        self.prev_time = None
        self.x = self.y = self.th = 0.0

        self.create_subscription(Twist, 'cmd_vel', self.on_cmd, 10)
        self.odom_pub = self.create_publisher(Odometry, 'odom', 20)
        self.tf_pub = TransformBroadcaster(self)
        self.create_timer(0.01, self.read_serial)   # 100 Hz serial poll
        self.create_timer(0.05, self.send_cmd)      # 20 Hz resend keeps the MCU watchdog fed

    def on_cmd(self, msg: Twist):
        v, w = msg.linear.x, msg.angular.z
        wl = (v - w * self.L / 2.0) / self.r        # left wheel, rad/s
        wr = (v + w * self.L / 2.0) / self.r        # right wheel, rad/s
        self.w_cmd = (max(-self.w_max, min(self.w_max, wl)),
                      max(-self.w_max, min(self.w_max, wr)))

    def send_cmd(self):
        self.ser.write(f'V {self.w_cmd[0]:.3f} {self.w_cmd[1]:.3f}\n'.encode())

    def read_serial(self):
        self.buf += self.ser.read(self.ser.in_waiting or 1).decode(errors='ignore')
        while '\n' in self.buf:
            line, self.buf = self.buf.split('\n', 1)
            p = line.strip().split()
            if len(p) == 3 and p[0] == 'E':
                try:
                    self.update_odom(int(p[1]), int(p[2]))
                except ValueError:
                    pass                              # partial line after boot

    def update_odom(self, tl, tr):
        now = self.get_clock().now()
        if self.prev_ticks is None:                  # first sample sets the baseline
            self.prev_ticks, self.prev_time = (tl, tr), now
            return
        dt = (now - self.prev_time).nanoseconds * 1e-9
        k = 2.0 * math.pi * self.r / self.N          # meters per tick
        ds_l = (tl - self.prev_ticks[0]) * k
        ds_r = (tr - self.prev_ticks[1]) * k
        self.prev_ticks, self.prev_time = (tl, tr), now
        if dt <= 0.0:
            return

        ds = (ds_r + ds_l) / 2.0
        dth = (ds_r - ds_l) / self.L
        self.x += ds * math.cos(self.th + dth / 2.0)  # midpoint integration
        self.y += ds * math.sin(self.th + dth / 2.0)
        self.th += dth

        qz, qw = math.sin(self.th / 2.0), math.cos(self.th / 2.0)  # yaw-only quaternion
        stamp = now.to_msg()

        t = TransformStamped()
        t.header.stamp, t.header.frame_id = stamp, 'odom'
        t.child_frame_id = 'base_footprint'
        t.transform.translation.x, t.transform.translation.y = self.x, self.y
        t.transform.rotation.z, t.transform.rotation.w = qz, qw
        self.tf_pub.sendTransform(t)

        o = Odometry()
        o.header.stamp, o.header.frame_id = stamp, 'odom'
        o.child_frame_id = 'base_footprint'
        o.pose.pose.position.x, o.pose.pose.position.y = self.x, self.y
        o.pose.pose.orientation.z, o.pose.pose.orientation.w = qz, qw
        o.twist.twist.linear.x = ds / dt             # body-frame forward speed
        o.twist.twist.angular.z = dth / dt           # yaw rate
        o.pose.covariance[0] = o.pose.covariance[7] = 0.01
        o.pose.covariance[35] = 0.05                 # yaw drifts fastest
        self.odom_pub.publish(o)


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


if __name__ == '__main__':
    main()

Register the entry point in setup.py, and install the launch, URDF, and config folders:

# setup.py (relevant parts)
import os
from glob import glob

data_files=[
    ('share/ament_index/resource_index/packages', ['resource/rover_bringup']),
    ('share/rover_bringup', ['package.xml']),
    (os.path.join('share', 'rover_bringup', 'launch'), glob('launch/*.py')),
    (os.path.join('share', 'rover_bringup', 'urdf'), glob('urdf/*.urdf')),
    (os.path.join('share', 'rover_bringup', 'config'), glob('config/*.yaml')),
],
entry_points={'console_scripts': ['base_node = rover_bringup.base_node:main']},

Robot Description and TF Tree

Save as urdf/rover.urdf. The wheels are fixed joints because SLAM only needs the body and laser frames. Switch them to continuous once you publish joint_states.

<?xml version="1.0"?>
<robot name="rover">
  <link name="base_footprint"/>
  <link name="base_link">
    <visual><geometry><box size="0.25 0.18 0.08"/></geometry></visual>
  </link>
  <!-- base_link origin sits one wheel radius (0.033 m) above the ground -->
  <joint name="footprint_joint" type="fixed">
    <parent link="base_footprint"/><child link="base_link"/>
    <origin xyz="0 0 0.033"/>
  </joint>
  <link name="laser_link">
    <visual><geometry><cylinder radius="0.035" length="0.04"/></geometry></visual>
  </link>
  <!-- LiDAR 5 cm forward, 12 cm above base_link. Adjust yaw if scan 0 deg is not forward. -->
  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/><child link="laser_link"/>
    <origin xyz="0.05 0 0.12" rpy="0 0 0"/>
  </joint>
</robot>

Measure the real offsets with a ruler. A 1cm error in the laser offset corrupts every scan match.

SLAM Configuration and Launch

Save as config/slam.yaml:

slam_toolbox:
  ros__parameters:
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: /scan
    mode: mapping
    resolution: 0.05              # 5 cm grid cells
    max_laser_range: 8.0          # trim noisy far returns (C1 max is 12 m)
    minimum_travel_distance: 0.2  # meters between scan insertions
    minimum_travel_heading: 0.2   # radians between scan insertions
    minimum_time_interval: 0.5
    map_update_interval: 2.0      # lower = faster map, higher CPU
    transform_publish_period: 0.02
    do_loop_closing: true
    use_scan_matching: true
    stack_size_to_use: 40000000

Save as launch/rover.launch.py:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node


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

    lidar = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(
            get_package_share_directory('sllidar_ros2'), 'launch', 'sllidar_c1_launch.py')),
        launch_arguments={'serial_port': '/dev/ttyUSB0',
                          'frame_id': 'laser_link'}.items())

    slam = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(
            get_package_share_directory('slam_toolbox'), 'launch', 'online_async_launch.py')),
        launch_arguments={'slam_params_file': os.path.join(share, 'config', 'slam.yaml'),
                          'use_sim_time': 'false'}.items())

    return LaunchDescription([
        Node(package='robot_state_publisher', executable='robot_state_publisher',
             parameters=[{'robot_description': robot_desc}]),
        Node(package='rover_bringup', executable='base_node',
             parameters=[{'port': '/dev/ttyACM0'}]),
        lidar,
        slam,
    ])

Build and run:

cd ~/rover_ws && colcon build --symlink-install
source install/setup.bash
ros2 launch rover_bringup rover.launch.py

Bring-Up Checks, in Order

Test each layer before the next. Debugging a full SLAM stack with a dead encoder wastes hours.

Run RViz on your laptop, not the Pi. Set the same ROS_DOMAIN_ID on both machines. Set the Fixed Frame to odom, then add LaserScan on /scan. The scan should rotate rigidly with the rover and not smear.

Calibrating Odometry

Two parameters dominate error: wheel_radius and wheel_separation.

Radius check. Drive 2.0m by tape measure. If /odom reports a different distance, scale the radius:

r_new = r_old · (actual_distance / odom_distance)

Separation check. Command a slow in-place spin and mark the start heading on the floor. After the rover physically completes one full turn, read the odometry yaw from /odom (convert the quaternion with 2·atan2(z, w)). Then:

L_new = L_old · (θ_odom / θ_actual)

If odometry reports 380° for a true 360° turn, increase L by 380/360, about 5.6%. Repeat three times and average. Turn error matters more than distance error because heading error compounds into lateral position error.

Real-World Workflow: Mapping a Room with slam_toolbox

1. Place the rover in the room’s most feature-rich corner. Straight blank walls give the scan matcher little to lock onto.
2. Launch the stack and confirm /scan and the TF tree.
3. Open RViz on the laptop. Add a Map display on /map.
4. Drive at under 0.2m/s with turns under 0.5rad/s. Fast rotation smears a 10Hz scan.
5. Return to the start so slam_toolbox can close the loop.
6. Save the map:

mkdir -p ~/maps
ros2 run nav2_map_server map_saver_cli -f ~/maps/room

This writes room.pgm and room.yaml. Both load into Nav2 for localization later.

Tuning the Wheel PID

Log E lines over serial and plot the measured wheel speed against the target. A good response reaches 90% of a step within about 150ms with under 10% overshoot.

Scaling Up

Once mapping works, extend the rover in this order:

• Add an IMU (BNO055 or MPU-6050) and fuse yaw with robot_localization. Gyro yaw cuts the heading drift that wheel slip causes.
• Install Nav2. Set robot_radius or the footprint polygon from your chassis dimensions. Check whether your Nav2 release expects Twist or TwistStamped on cmd_vel, since that default has changed between distributions.
• Move to ros2_control. Wrap the serial protocol in a hardware interface and use diff_drive_controller. You gain standard odometry and joint states with no custom node.
• Add a real-time kernel only if you move wheel control back onto the Pi.

FAQ

Can a Raspberry Pi 5 run ROS 2 with SLAM and LiDAR?

Yes. Ubuntu 24.04 with ROS 2 Jazzy runs on the Pi 5, and projects using a Pi 5 with an RPLIDAR and slam_toolbox are well documented. Use active cooling and a 5V/5A supply. Run RViz and heavy visualization on a separate computer.

Which LiDAR works best with a Raspberry Pi 5 rover?

The RPLIDAR C1 and A1 have mature ROS 2 drivers in the sllidar_ros2 package and connect over USB. The C1 offers a 12m range and a 10Hz scan rate. The LDROBOT LD19 is a lower-cost alternative that uses a UART interface and a different driver package.

Do I need a microcontroller to drive the motors?

No, but it is the more reliable design. The Pi 5 runs a non-real-time OS, so GPIO encoder counting loses ticks and PWM timing jitters. A Pico handles interrupts and the PID loop deterministically. The Pi stays free for SLAM and navigation.

Why does my map drift or smear while driving?

Three causes dominate: wrong wheel_separation, a laser frame that is not where the URDF says, and turning too fast for a 10Hz scan. Run the spin calibration, verify laser_joint against the physical mount, and cap angular velocity at about 0.5rad/s during mapping.