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.
| Mapping (FastSLAM) | Autonomous Navigation |
|---|---|
![]() |
![]() |
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
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:
-
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.
-
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. -
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.
-
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
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.
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.
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.
Map, Pose and Goal received Goal reached
┌───────────────────────┌──────────┐◄──────────────────────────────────────┐
│ │ IDLE │ │
│ └──────────┘ │
│ │
▼ │
┌──────────────┐ The robot encountered a dynamic obstacle ┌──────────────┐
│ PLANNING │◄──────────────────────────────────────────────│ MOVING │
└──────────────┘──────────────────────────────────────────────►└──────────────┘
Path to the goal computed via A*
- 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 scipySet your TurtleBot3 model before running anything:
export TURTLEBOT3_MODEL=burgergit clone https://github.com/fnattero/ROS2-FastSLAM-Navigation.git
cd ROS2-FastSLAM-Navigation/ros2_ws
colcon build
source install/setup.bashRun each command in a separate terminal. Source the workspace in each one before running.
source install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_custom_simulation custom_casa.launch.pyOther 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 roomsource install/setup.bash
ros2 launch mapping_pkg mapping.launch.pyRViz 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.
ros2 run turtlebot3_teleop teleop_keyboardDrive the robot around the environment to build the map. Once you are satisfied with the coverage, close the mapping launch.
cp ~/ros2_fastslam_maps/latest_map.pkl src/navigation_pkg/maps/high_res.pkl
colcon build --packages-select navigation_pkg
source install/setup.bashA pre-built map (navigation_pkg/maps/high_res.pkl) is already included — you can skip Phase 1 and test navigation immediately.
source install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_custom_simulation custom_casa.launch.pysource install/setup.bash
ros2 launch navigation_pkg navigation.launch.pyRViz opens showing the static map and the particle cloud converging around the robot's estimated pose.
- Use the 2D Pose Estimate tool to click the robot's starting position on the map and drag to set its heading
- Watch the particle cloud collapse around that pose as the laser scan confirms the location
- Use the 2D Goal Pose tool to click a destination — the robot will plan and navigate autonomously, replanning around any dynamic obstacles it encounters
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) |
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
- Francisco Nattero
- Jeremías Figueiredo Paschmann

