Skip to content

CMP3103 Week 9

Jump to bottom Edit New page
Athanasios Polydoros edited this page Apr 22, 2026 · 5 revisions

LIMO Robot Workshop: SLAM Mapping & A* Navigation with ROS 2

Table of Contents

  1. Overview
  2. Architecture Diagram
  3. Prerequisites
  4. Repository Setup
  5. Part 1 — Launch the Gazebo Simulation
  6. Part 2 — SLAM Mapping with slam_toolbox
  7. Part 3 — Save the Map
  8. Part 4 — Autonomous Navigation with A*
  9. Part 5 — Programmatic Goal Sending
  10. Understanding the A* Planner
  11. Exercises
  12. Troubleshooting
  13. Dependency Reference

Overview

In this workshop you will:

Step What you do Key ROS 2 component
1 Spawn the LIMO differential-drive robot in Gazebo limo_gazebosim
2 Drive the robot and build a 2-D occupancy-grid map slam_toolbox
3 Save the finished map to disk nav2_map_server
4 Load the map and navigate autonomously using A* nav2_smac_planner
5 Send navigation goals from Python code nav2_msgs/NavigateToPose

The LIMO Robot

LIMO (AgileX Robotics) is a small differential-drive ground robot equipped with:

  • 2-D LiDAR for obstacle detection and SLAM
  • Wheel odometry
  • IMU (optional, used for EKF odometry fusion)

In this workshop we run it entirely inside Gazebo, so no physical robot is needed.

Architecture Diagram

┌────────────────────────────────────────────────────────────────┐
│                         ROS 2 Node Graph                        │
│                                                                  │
│  Gazebo ──/scan──► slam_toolbox ──/map──► RViz2                 │
│     │                   │                                        │
│     └──/odom─────────► tf (map→odom→base_link)                  │
│                                                                  │
│  [After map is saved]                                            │
│                                                                  │
│  map_server ─/map─►  amcl (localisation)                        │
│                         │                                        │
│  /scan ────────────────►│                                        │
│                         ▼                                        │
│               global_costmap ─► SmacPlanner2d (A*)              │
│                                        │                         │
│                                        ▼                         │
│                               bt_navigator (BT)                  │
│                                        │                         │
│                                        ▼                         │
│                              controller_server (DWB)             │
│                                        │                         │
│                                        ▼                         │
│                              /cmd_vel ──► Gazebo                 │
└────────────────────────────────────────────────────────────────┘

Prerequisites

Software (already installed in the dev container)

Package Purpose
ros-humble-slam-toolbox Online/offline SLAM
ros-humble-navigation2 Nav2 stack
ros-humble-nav2-smac-planner A* global planner
ros-humble-nav2-bringup Nav2 launch helpers
ros-humble-gazebo-ros-pkgs Gazebo–ROS bridge
uol_tidybot Course simulation — provides tidybot.launch.py and level_2_1.world

To verify all required packages are present, run:

ros2 pkg list | grep -E "slam_toolbox|nav2_smac_planner|uol_tidybot"

Expected output (at minimum):

uol_tidybot
nav2_smac_planner
slam_toolbox

Installing missing dependencies (if any)

sudo apt-get update
sudo apt-get install -y \
    ros-humble-slam-toolbox \
    ros-humble-navigation2 \
    ros-humble-nav2-bringup \
    ros-humble-nav2-smac-planner \
    ros-humble-teleop-twist-keyboard

Repository Setup

1. Clone the repository

Clone the limo_navigation package directly into the src/ folder of a ROS 2 workspace:

mkdir -p ~/limo_ws/src
cd ~/limo_ws/src
git clone https://github.com/athapoly/limo_navigation.git
cd ~/limo_ws

To update later with the latest changes:

cd ~/limo_ws/src/limo_navigation
git pull origin main

2. Source ROS 2 and the LCAS overlay

source /opt/ros/humble/setup.bash
source /opt/ros/lcas/install/setup.bash

3. Build the workspace

cd ~/limo_ws
colcon build --symlink-install --allow-overriding limo_navigation
source install/setup.bash

Note: The workshop package is named limo_navigation, which overrides the underlay package already present in /opt/ros/lcas/install. The --allow-overriding limo_navigation flag is required for this reason.

4. Run the tests

colcon test --packages-select limo_navigation --event-handlers console_direct+
colcon test-result --verbose

All 6 tests should pass:

  • test_package_xml_exists
  • test_nav_params_have_astar_plugin
  • test_slam_params_are_mapping_mode
  • test_launch_files_exist
  • test_nav_client_importable

Part 1 — Launch the Gazebo Simulation

This workshop uses the existing course simulation provided by uol_tidybot.

Open a new terminal (Terminal 1) and run:

source /opt/ros/humble/setup.bash
source /opt/ros/lcas/install/setup.bash

ros2 launch uol_tidybot tidybot.launch.py world:=level_2_1.world

You should see:

  • Gazebo window open with the LIMO robot in the level_2_1 environment
  • No red errors in the terminal

World used in this workshop:

World Description
level_2_1.world Course environment with rooms and corridors, provided by uol_tidybot

Verify the robot is publishing sensor data

Open a second terminal (Terminal 2):

source /opt/ros/humble/setup.bash
source /opt/ros/lcas/install/setup.bash
ros2 topic list

You should see /scan, /odom, /tf, /tf_static, /cmd_vel among others.

Verify the LiDAR is working:

ros2 topic hz /scan

Expected: ~10 Hz.

Part 2 — SLAM Mapping with slam_toolbox

SLAM stands for Simultaneous Localisation and Mapping. slam_toolbox builds a 2-D occupancy-grid map in real time by matching successive LiDAR scans and closing loops when the robot revisits areas.

Launch SLAM

Open Terminal 2 (keep the tidybot simulation running in Terminal 1):

source /opt/ros/humble/setup.bash
source /opt/ros/lcas/install/setup.bash
source ~/limo_ws/install/setup.bash

ros2 launch limo_navigation slam_mapping.launch.py

This launches:

  1. slam_toolbox (async online mode) – builds the map
  2. RViz2 – pre-configured to display the occupancy map (/map), LiDAR scan (/scan), robot model, and TF frames

Drive the robot to build a map

Open Terminal 3 and launch the keyboard teleoperation:

source /opt/ros/humble/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard

Controls:

Moving around:
   u    i    o
   j    k    l
   m    ,    .

i = forward    , = backward
j = rotate left   l = rotate right
k = stop

q/z : increase/decrease max speeds

Drive the robot slowly around the environment, making sure to:

  • Cover all open areas
  • Return to the start position at least once (helps loop closure)
  • Move slowly (< 0.3 m/s) for better scan quality

Watch RViz2 — the grey occupancy grid will grow as you explore.

What is happening

LiDAR scan (t)  ─┐
                  ├──► Scan matcher ──► Pose graph ──► Optimised map
LiDAR scan (t-1)─┘         │
                            └──► Loop detector
                                     │
                            Periodic loop closure
                            correction applied

slam_toolbox builds a pose graph: each node is a robot pose, each edge is a scan-match constraint. When the graph is optimised (especially after loop closure), the global map becomes consistent.

Part 3 — Save the Map

Once you are happy with the map, open Terminal 4 and save it:

# Create a directory for maps
mkdir -p ~/maps

# Save the map (generates .pgm image + .yaml metadata)
ros2 run nav2_map_server map_saver_cli -f ~/maps/workshop_map

This creates two files:

  • ~/maps/workshop_map.pgm — grayscale image (white=free, black=occupied, grey=unknown)
  • ~/maps/workshop_map.yaml — metadata (resolution, origin, thresholds)

Inspect the map metadata

cat ~/maps/workshop_map.yaml

Example output:

image: workshop_map.pgm
mode: trinary
resolution: 0.05       # metres per pixel
origin: [-3.44, -2.53, 0]   # map frame origin (x, y, yaw)
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

Note: The map resolution is 0.05 m/cell. Each pixel represents a 5 × 5 cm area in the real world.

Stop the SLAM launch (Ctrl+C in Terminal 2) and the teleop (Ctrl+C in Terminal 3).

Part 4 — Autonomous Navigation with A*

With a map saved, we can now run Nav2 to navigate the robot autonomously.

The A* Algorithm

A* is a graph-search algorithm that finds the shortest path from a start node to a goal node. It uses the evaluation function:

$$f(n) = g(n) + h(n)$$

where:

  • $g(n)$ = exact cost from start to node $n$
  • $h(n)$ = heuristic estimate from $n$ to goal (e.g. Euclidean distance)
  • $f(n)$ = estimated total cost through $n$

A* expands the node with the lowest $f(n)$ at each step. When $h(n)$ is admissible (never overestimates), A* is guaranteed to find the optimal path.

In Nav2, the SmacPlanner2d plugin implements A* on the 2-D occupancy costmap grid using an 8-connected Moore neighbourhood:

┌───┬───┬───┐
│ ↖ │ ↑ │ ↗ │
├───┼───┼───┤
│ ← │ X │ → │   8 neighbours of cell X
├───┼───┼───┤
│ ↙ │ ↓ │ ↘ │
└───┴───┴───┘

Launch Navigation

Open Terminal 2 (Gazebo should still be running in Terminal 1):

source /opt/ros/humble/setup.bash
source /opt/ros/lcas/install/setup.bash
source ~/limo_ws/install/setup.bash

# Using the map you just saved:
ros2 launch limo_navigation navigation_astar.launch.py \
    map:=$HOME/maps/workshop_map.yaml \
    use_sim_time:=true

# OR use the bundled demo map:
ros2 launch limo_navigation navigation_astar.launch.py use_sim_time:=true

Wait until you see in the terminal:

[lifecycle_manager_navigation]: All systems are active

Do not try to send a goal before this message appears. If Nav2 is still starting up, RViz2 may show: navigate_to_pose action server is not available. Is the initial pose set?

Set the initial pose in RViz2

Nav2 needs to know where the robot starts on the map:

  1. In RViz2, click "2D Pose Estimate" in the toolbar
  2. Click on the map at the robot's current location in Gazebo
  3. Drag the arrow in the direction the robot is facing
  4. Release – you should see the AMCL particles converge

Send a navigation goal

Before using the goal tool, make sure you have already set the initial pose in the previous step.

  1. In RViz2, click "Nav2 Goal" in the toolbar
  2. Click a reachable location on the map
  3. Drag the arrow to set the goal heading
  4. Release – Nav2 will plan and execute the path

In the terminal you will see the A* planner log:

[planner_server]: Planning...
[smac_planner]: Found path with cost X.XX

Observe the planned path in RViz2

Enable these display panels in RViz2 if not already visible:

  • Global Costmap (Map topic: /global_costmap/costmap)
  • Local Costmap (Map topic: /local_costmap/costmap)
  • Global Plan (Path topic: /plan)
  • Local Plan (Path topic: /local_plan)

The green line is the A* global plan. The yellow line is the DWB local plan (short-horizon trajectory optimisation).

Part 5 — Programmatic Goal Sending

Instead of clicking in RViz2, we can send goals from Python code.

Send a single goal

Open Terminal 3:

source /opt/ros/humble/setup.bash
source /opt/ros/lcas/install/setup.bash
source ~/limo_ws/install/setup.bash

# Navigate to (2.0, 0.0) facing East (yaw = 0 rad)
ros2 run limo_navigation astar_nav_client -- --x 2.0 --y 0.0 --yaw 0.0

Expected terminal output:

[astar_nav_client]: Waiting for NavigateToPose action server…
[astar_nav_client]: Sending goal → x=2.00  y=0.00  yaw=0.0°
[astar_nav_client]: Goal ACCEPTED – robot is navigating…
[astar_nav_client]:   Distance remaining: 1.87 m
[astar_nav_client]:   Distance remaining: 1.42 m
...
[astar_nav_client]: ✔  Navigation SUCCEEDED!

How the action client works

# 1. Create an ActionClient connected to the navigate_to_pose server
client = ActionClient(node, NavigateToPose, 'navigate_to_pose')

# 2. Build the goal message (PoseStamped in the map frame)
goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = 'map'
goal_msg.pose.pose.position.x = 2.0

# 3. Send goal asynchronously, register callbacks
future = client.send_goal_async(goal_msg, feedback_callback=...)

The NavigateToPose action follows the ROS 2 action protocol:

  • Goal → sent by client
  • Feedback → streamed by server (distance remaining)
  • Result → returned when done (succeeded / aborted / cancelled)

Try different goal positions

# Top-left corner
ros2 run limo_navigation astar_nav_client -- --x -1.5 --y 1.5 --yaw 1.57

# Back to origin
ros2 run limo_navigation astar_nav_client -- --x 0.0 --y 0.0 --yaw 0.0

Understanding the A* Planner

NavfnPlanner vs SmacPlanner2d

Feature NavfnPlanner (use_astar: true) SmacPlanner2d
Algorithm A* on Dijkstra grid A* with configurable heuristic
Connectivity 4-connected 8-connected (Moore)
Path smoother No Yes (configurable)
Heading awareness No Optional
Use case Simple, fast planning Better path quality

Key parameters in nav2_params_astar.yaml 

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2d"
      w_heuristic_cost: 1.0   # 1.0 = standard A*, >1 = weighted A* (faster but suboptimal)
      allow_unknown: true      # plan through unknown map areas
      tolerance: 0.5           # goal tolerance in metres

Heuristic weight effect

w_heuristic_cost Behaviour
0.0 Dijkstra (exhaustive, always optimal)
1.0 Standard A* (optimal if heuristic is admissible)
> 1.0 Weighted A* (faster, potentially suboptimal)

Costmap inflation

The inflation_layer expands obstacles outwards by inflation_radius metres, creating a gradient cost field. This gives A* a "keep-away" signal that results in smoother, safer paths.

     Wall
      │
      │▓▓▓▓░░░░          ▓ = high cost (near wall)
      │                  ░ = medium cost (inflation zone)
      │          robot   · = free space
      ·····················

Exercises

Exercise 1: Compare planners

Modify nav2_params_astar.yaml to switch between:

  • nav2_smac_planner/SmacPlanner2d (current, A*)
  • nav2_navfn_planner/NavfnPlanner with use_astar: false (Dijkstra)

Rebuild and compare the planned paths in RViz2. Which planner produces smoother paths?

Exercise 2: Heuristic weight

Change w_heuristic_cost from 1.0 to 5.0. Observe:

  • Does the robot still reach the goal?
  • Is the path shorter / longer?
  • Does planning complete faster?

Use ros2 topic echo /plan to inspect the path length.

Exercise 3: Map exploration script

Write a Python node that:

  1. Subscribes to /map (OccupancyGrid)
  2. Finds a random unvisited (unknown, value = -1) cell
  3. Sends a navigation goal to that cell using astar_nav_client
  4. Repeats until the map is fully explored

Hint: Unknown cells have value -1 in the OccupancyGrid.data array.

Exercise 4: Obstacle avoidance

Launch the simulation and SLAM as described in Parts 1 and 2:

# Terminal 1 – simulation
ros2 launch uol_tidybot tidybot.launch.py world:=level_2_1.world

# Terminal 2 – SLAM
ros2 launch limo_navigation slam_mapping.launch.py

After mapping, navigate through the environment. Observe how the costmap inflation affects path planning around obstacles.

Exercise 5: Custom world

Create a new Gazebo world file with custom obstacles (walls, boxes, cylinders). Add it to the limo_gazebosim/worlds/ folder and launch SLAM in it.

Troubleshooting

"No transform from odom to map"

The SLAM node has not started yet. Wait a few seconds or check:

ros2 topic echo /map --once

Robot doesn't move after sending goal

  1. Check AMCL has initialised – use 2D Pose Estimate in RViz2 first
  2. Check the goal is in a free area of the map (not inside an obstacle)
  3. Check Nav2 lifecycle nodes are active: 
    ros2 lifecycle list /planner_server

Map looks blurry or has drift

  • Drive more slowly (< 0.2 m/s)
  • Increase scan_buffer_size in slam_toolbox_online.yaml
  • Ensure the robot returns to the start for loop closure

"Action server not available"

Usually this means one of these conditions is not met yet:

  • Nav2 is still starting up — wait for [lifecycle_manager_navigation]: All systems are active
  • The initial pose has not been set in RViz2 with 2D Pose Estimate

Then check:

ros2 action list

You should see /navigate_to_pose.

Gazebo crashes on launch

killall gzserver gzclient

Then relaunch.

"Could not find package limo_navigation"

Rebuild and re-source:

cd ~/limo_ws
colcon build --packages-select limo_navigation --allow-overriding limo_navigation
source install/setup.bash

Dependency Reference

Install all dependencies at once

sudo apt-get update && sudo apt-get install -y \
    ros-humble-slam-toolbox \
    ros-humble-navigation2 \
    ros-humble-nav2-bringup \
    ros-humble-nav2-smac-planner \
    ros-humble-nav2-navfn-planner \
    ros-humble-nav2-amcl \
    ros-humble-nav2-map-server \
    ros-humble-nav2-lifecycle-manager \
    ros-humble-gazebo-ros-pkgs \
    ros-humble-teleop-twist-keyboard \
    ros-humble-tf2-tools \
    ros-humble-rqt-graph

Useful ROS 2 debugging commands

# View the full TF tree
ros2 run tf2_tools view_frames

# Plot the node graph
ros2 run rqt_graph rqt_graph

# Check active topics and their types
ros2 topic list -t

# View Nav2 action servers
ros2 action list

# Echo the A* path
ros2 topic echo /plan --once

# Check costmap
ros2 topic echo /global_costmap/costmap --once

# Monitor Nav2 planner server lifecycle
ros2 topic echo /planner_server/transition_event

Package Structure Reference

src/limo_navigation/
├── package.xml                    # ROS 2 package manifest
├── setup.py                       # Python build configuration
├── setup.cfg                      # Script install paths
├── resource/limo_navigation       # ament resource index marker
├── limo_navigation/
│   ├── __init__.py
│   ├── astar_nav_client.py        # Single-goal A* navigation client
│   └── waypoint_tour.py           # Multi-waypoint tour client
├── launch/
│   ├── slam_mapping.launch.py     # Gazebo + slam_toolbox + RViz2
│   └── navigation_astar.launch.py # Nav2 (A*) + RViz2
├── params/
│   ├── nav2_params_astar.yaml     # Nav2 config with SmacPlanner2d (A*)
│   └── slam_toolbox_online.yaml   # slam_toolbox online-async config
├── rviz/
│   ├── slam_mapping.rviz          # RViz config for SLAM phase
│   └── navigation.rviz            # RViz config for navigation phase
├── maps/
│   └── simple_map.*               # Demo map (pgm + yaml)
└── test/
    └── test_workshop.py           # Automated tests (6 test cases)

Repository

The workshop package is publicly available at:

https://github.com/athapoly/limo_navigation

To contribute improvements or report issues, open a pull request or issue on that repository.

Workshop developed for the LIMO Robot AMR course. Questions? Open an issue on GitHub.

Copyright by Lincoln Centre for Autonomous Systems

Clone this wiki locally