Skip to content

About

ROS2 Humble stack for TurtleBot3 mapping and navigation using FastSLAM in C++/Python, MCL localization, A* path planning, and pure pursuit control.

Resources

Stars

1 star

Watchers

0 watching

Forks

Latest commit

 

History

99 Commits

Folders and files

NameName
Last commit message
Last commit date
 
 
 
 
 
 
 
 
 
 

Repository files navigation

FastSLAM & Autonomous Navigation in ROS2

ROS2 Humble Python C++17 License

Final project for I402 — Principios de la Robótica Autónoma, Universidad de San Andrés. Authors: Jeremías Figueiredo Paschmann and Francisco Nattero.

A full autonomous robotics stack for TurtleBot3 in Gazebo. The system implements Grid-based FastSLAM to build an occupancy map of an unknown environment, then uses that map for fully autonomous navigation — combining Monte Carlo Localization, A* path planning, and a pure pursuit controller with dynamic obstacle avoidance.


Demo

Mapping (FastSLAM) Autonomous Navigation
Mapping demo Navigation demo

System Overview

The project is divided into two independent phases that can be run sequentially or separately (a pre-built map is already included for Phase 2):

Phase 1 — Mapping
  Gazebo Sim ──► delta_publisher ──► FastSLAM node (C++ core) ──► map_dump ──► ~/ros2_fastslam_maps/latest_map.pkl

Phase 2 — Navigation  (works with the Phase 1 map or the included pre-built one)
  map_loader ──► navigation_node ──► /cmd_vel ──► Robot
                      │
              ┌───────┴────────┐
              MCL localization  A* planner
              Pure pursuit      Dynamic replanning

How It Works

Part 1 — FastSLAM (Mapping)

SLAM is inherently a chicken-and-egg problem: to build a map you need to know where you are, and to know where you are you need a map. FastSLAM resolves this with a particle filter where each particle is a complete independent hypothesis — it carries its own pose estimate and its own occupancy grid built from that pose's perspective.

At every step:

  1. Motion update — Odometry is decomposed into a rotation-translation-rotation sequence (r1, t, r2). Each particle's pose is propagated through this motion model with injected Gaussian noise, capturing the uncertainty of wheel odometry.

  2. Measurement update — Each LIDAR ray is traced against the particle's own grid using an inverse sensor model: cells along the ray have their log-odds decremented (more likely free), and the endpoint cell has its log-odds incremented (more likely occupied). The particle's weight is then set to p(scan | pose, map) — how well the observed scan matches what the particle's map would predict at that pose.

  3. Resampling — Stochastic Universal Sampling selects a new set of particles proportional to their weights. High-weight particles (consistent pose-map pairs) are duplicated; low-weight ones are discarded. This concentrates compute on the most plausible hypotheses.

  4. Map output — The best particle's grid is published as the current map. The final map is the average across all particle grids, which smooths out individual particle noise.

The inner update loop (grid ray tracing for 240 particles) runs in C++ and is exposed to the Python ROS2 node via pybind11, keeping the 10 Hz update rate achievable:

grid_fast_slam_node.py  ←→  pybind_module.cpp  ←→  fastslam_core.cpp
      ROS2 I/O               Python bindings         Log-odds grid updates

Part 2 — Autonomous Navigation

Localization (Monte Carlo Localization)

Given a pre-built map, the robot continuously estimates its pose using 500 particles. Rather than matching raw laser readings against the occupancy grid cell by cell (expensive and brittle), the system uses a likelihood field:

  • A distance transform is precomputed over the map, giving every cell its distance to the nearest obstacle.
  • A Gaussian is applied to that distance to produce a smooth probability surface.
  • For each particle, every LIDAR ray endpoint is looked up in this field. The particle's weight is the product of the per-ray likelihoods — a high weight means the scan endpoints cluster near the obstacles predicted by the particle's pose hypothesis.

The pose estimate is the weighted mean of all particles. After each update, Stochastic Universal Sampling resamples to keep the distribution focused on the most probable poses.

Path Planning (A*)

Before running A*, the map goes through two preprocessing steps:

  • Inflation — obstacles are dilated by the robot's radius, so the planner only needs to treat the robot as a point. Any planned path is guaranteed to keep the robot body clear of walls.
  • Likelihood field cost layer — the same distance-to-obstacle field used in localization is added as an edge cost modifier. Paths that pass close to obstacles pay a higher cost, so the planner naturally prefers the centers of corridors without needing explicit clearance rules.

A* runs on an 8-connected grid (diagonal moves allowed) with an octile distance heuristic. When a dynamic obstacle is detected — a scan point that falls in a free cell of the static map — the robot merges the live obstacle grid with the static map and reruns A*. Replanning is throttled to at most once per second to avoid oscillation when obstacles appear intermittently.

Path Following (Pure Pursuit)

The pure pursuit controller finds a lookahead point on the planned path at a fixed distance ahead of the robot (0.3 m). It then computes the arc that brings the robot from its current pose to that point, producing:

  • Angular velocity — proportional to the curvature of the required arc (heading error to the lookahead point).
  • Linear velocity — capped by the max speed and reduced to zero as the robot approaches the goal within tolerance.

If the heading error exceeds 60°, translation is suspended and the robot rotates in place first, preventing it from swinging wide around turns. Near obstacles, linear speed is scaled down proportionally so the robot slows before tight passages.

Navigation State Machine

Map, Pose and Goal received                          Goal reached               
┌───────────────────────┌──────────┐◄──────────────────────────────────────┐
│                       │   IDLE   │                                       │
│                       └──────────┘                                       │
│                                                                          │
▼                                                                          │
┌──────────────┐   The robot encountered a dynamic obstacle    ┌──────────────┐
│   PLANNING   │◄──────────────────────────────────────────────│    MOVING    │
└──────────────┘──────────────────────────────────────────────►└──────────────┘
                      Path to the goal computed via A*

Prerequisites

  • Ubuntu 22.04 + ROS2 Humble
  • Gazebo Classic
  • TurtleBot3 packages
  • pybind11
  • Python: numpy, scipy
# ROS2 and TurtleBot3
sudo apt install ros-humble-turtlebot3 ros-humble-turtlebot3-simulations ros-humble-turtlebot3-teleop
sudo apt install ros-humble-gazebo-ros-pkgs

# C++ build dependency
sudo apt install pybind11-dev

# Python dependencies
pip install numpy scipy

Set your TurtleBot3 model before running anything:

export TURTLEBOT3_MODEL=burger

Installation & Build

git clone https://github.com/fnattero/ROS2-FastSLAM-Navigation.git
cd ROS2-FastSLAM-Navigation/ros2_ws
colcon build
source install/setup.bash

Phase 1: Mapping

Run each command in a separate terminal. Source the workspace in each one before running.

Terminal 1 — Launch the Simulation

source install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_custom_simulation custom_casa.launch.py

Other available worlds:

ros2 launch turtlebot3_custom_simulation custom_casa_obs.launch.py   # house with obstacles
ros2 launch turtlebot3_custom_simulation custom_maze.launch.py        # maze
ros2 launch turtlebot3_custom_simulation custom_room.launch.py        # single room

Terminal 2 — Launch FastSLAM

source install/setup.bash
ros2 launch mapping_pkg mapping.launch.py

RViz opens automatically showing the occupancy grid being built in real time. The map is saved automatically to ~/ros2_fastslam_maps/latest_map.pkl every 5 seconds.

Terminal 3 — Teleoperate the Robot

ros2 run turtlebot3_teleop teleop_keyboard

Drive the robot around the environment to build the map. Once you are satisfied with the coverage, close the mapping launch.

Transfer the Map to Navigation

cp ~/ros2_fastslam_maps/latest_map.pkl src/navigation_pkg/maps/high_res.pkl
colcon build --packages-select navigation_pkg
source install/setup.bash

Phase 2: Navigation

A pre-built map (navigation_pkg/maps/high_res.pkl) is already included — you can skip Phase 1 and test navigation immediately.

Terminal 1 — Launch the Simulation

source install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_custom_simulation custom_casa.launch.py

Terminal 2 — Launch the Navigation Stack

source install/setup.bash
ros2 launch navigation_pkg navigation.launch.py

RViz opens showing the static map and the particle cloud converging around the robot's estimated pose.

Set Initial Pose and Goal in RViz

  1. Use the 2D Pose Estimate tool to click the robot's starting position on the map and drag to set its heading
  2. Watch the particle cloud collapse around that pose as the laser scan confirms the location
  3. Use the 2D Goal Pose tool to click a destination — the robot will plan and navigate autonomously, replanning around any dynamic obstacles it encounters

Configuration

All parameters live in plain Python config files — no recompilation needed, just source and relaunch.

Config file Parameter Default Effect
mapping_pkg/config/config.py slam.num_particles 240 SLAM accuracy vs. CPU cost
mapping_pkg/config/config.py grid.resolution 0.065 m/cell Map resolution
mapping_pkg/config/config.py map_saving.interval_sec 5.0 s Map autosave frequency
navigation_pkg/config/navigation_config.py localization.num_particles 500 Localization accuracy vs. CPU cost
navigation_pkg/config/navigation_config.py control.lookahead_dist 0.3 m Pure pursuit lookahead distance
navigation_pkg/config/navigation_config.py control.max_linear 0.1 m/s Max robot speed
navigation_pkg/config/navigation_config.py planning.goal_tolerance 0.15 m Distance at which goal is considered reached
navigation_pkg/config/navigation_config.py obstacle.decay_per_sec 0.1 Dynamic obstacle fade rate (how fast detected obstacles are forgotten)

Project Structure

ros2_ws/src/
├── custom_msgs/                  # Custom message types (DeltaOdom, Belief)
├── mapping_cpp_core/             # C++ FastSLAM particle filter (pybind11 bindings)
├── mapping_pkg/                  # ROS2 SLAM nodes, config, launch files
│   ├── mapping_pkg/
│   │   ├── grid_fast_slam_node.py  # Main FastSLAM node
│   │   ├── delta_publisher.py      # Odometry → motion delta converter
│   │   └── map_dump.py             # Periodic map serializer
│   └── tools/
│       └── view_latest_map.py      # Utility to visualize a saved map
├── navigation_pkg/               # ROS2 navigation nodes, config, launch, pre-built map
│   ├── navigation_pkg/
│   │   ├── navigation_node.py    # Main navigation coordinator (state machine)
│   │   ├── localization.py       # MCL particle filter
│   │   ├── planner.py            # A* path planner with likelihood field
│   │   └── path_follower.py      # Pure pursuit controller
│   └── maps/
│       └── high_res.pkl          # Pre-built map (ready to use, no mapping needed)
├── slam_utils/                   # Shared library (geometry, resampling, map I/O, types)
└── turtlebot3_custom_simulation/ # Gazebo worlds, robot model, simulation plugin

Authors

License

MIT

About

ROS2 Humble stack for TurtleBot3 mapping and navigation using FastSLAM in C++/Python, MCL localization, A* path planning, and pure pursuit control.

Resources

Stars

1 star

Watchers

0 watching

Forks

Releases

Packages

Contributors

Languages