Navigation & Path Planning
Week 11 • CMPSC 304 Robotic Agents
Press Space or → to advance
From Mapping to Navigation
Wednesday
SLAM & Mapping
→
Today
Navigation
→
P4 Parts 1-2
Map & Navigate
Wednesday: SLAM
- Built maps from lidar data
- Occupancy grids, scan matching
- Output: a map file
Today: Navigation
- Use the map to navigate
- Localization, path planning, obstacle avoidance
- Output: autonomous movement
The Navigation Problem
Given: a map and a goal position
Find: a safe path from the robot's current position to the goal
Execute: follow the path while avoiding obstacles
- Where am I? → Localization
- Where do I want to go? → Goal specification
- How do I get there? → Path planning
- How do I avoid obstacles? → Local planning / obstacle avoidance
Localization: Where Am I?
AMCL (Adaptive Monte Carlo Localization) is a particle filter that estimates the robot's position on a known map.
- Particles = many guesses of where the robot might be
- Each particle is a possible (x, y, θ) pose
- Compare each particle's expected lidar scan to the actual scan
- Particles that match well survive; poor matches are eliminated
- Over time, particles converge on the true position
In RViz: You'll see a cloud of green arrows around the robot. Initially spread out, they converge as the robot moves and gets more sensor data.
Setting the Initial Pose
Why you must set the initial pose: AMCL doesn't know where the robot starts. You give it a hint by clicking in RViz.
- Click the "2D Pose Estimate" button in the RViz toolbar
- Click on the map where the robot is
- Drag to set the direction the robot is facing
- The particle cloud should jump to that location and then converge
What if you get it wrong? AMCL may still converge to the correct position as the robot moves and gets more scan data — but it takes longer. Set it as accurately as you can.
Costmaps: The Planner's View
A costmap is a grid where each cell has a cost — how expensive it is for the robot to be there.
| Cost | Meaning | Visual |
|---|
| 0 | Free space — safe to traverse | No color |
| 1–252 | Increasing cost (near obstacles) | Blue → red gradient |
| 253 | Inscribed — robot center would hit obstacle | Cyan ring |
| 254 | Lethal — occupied cell | Purple/pink |
| 255 | Unknown | No data |
Inflation layer: Obstacles are "inflated" — expanded by the robot's radius — so the planner can treat the robot as a point. A cell 30 cm from a wall gets a higher cost than one 1 m away.
Global vs. Local Costmap
Global Costmap
- Covers the entire map
- Based on the static map you saved
- Updated slowly (mostly static)
- Used for long-range planning
- Finds the overall route
Local Costmap
- Small area around the robot
- Updated in real-time with lidar
- Includes dynamic obstacles
- Used for immediate decisions
- Adjusts the path on the fly
Why two costmaps? The global costmap gives the "big picture" route. The local costmap handles the messy reality of moment-to-moment driving.
Path Planning: Global Planner
The global planner finds a path from the robot's current position to the goal on the global costmap.
- Nav2 uses NavFn (navigation function) by default — a variant of Dijkstra's algorithm
- Treats the costmap as a graph where each cell is a node
- Edge weights = cell costs
- Finds the lowest-cost path (not just shortest distance — avoids getting close to obstacles)
In RViz: The global plan appears as a thin line from robot to goal. It may curve around obstacles and stay away from walls due to the inflation layer.
Path Following: Local Planner
The local planner (controller) generates velocity commands to follow the global path while reacting to real-time sensor data.
- Nav2 uses DWB (Dynamic Window Based) controller by default
- Simulates many possible velocity commands forward in time
- Scores each trajectory: does it follow the path? avoid obstacles? reach the goal?
- Picks the best-scoring trajectory and sends velocity commands
Global
Path
→
Local
Planner
→
/cmd_vel
Commands
→
Robot
Moves
Nav2: The Full Picture
Nav2 (Navigation 2) is the ROS2 navigation framework that combines all these components.
| Component | Role | ROS2 Node |
|---|
| Map Server | Loads and serves the static map | map_server |
| AMCL | Localizes robot on the map | amcl |
| Global Costmap | Full-map cost grid | global_costmap |
| Local Costmap | Real-time local cost grid | local_costmap |
| Planner Server | Global path planning | planner_server |
| Controller Server | Local path following | controller_server |
| BT Navigator | Coordinates everything | bt_navigator |
Sending Navigation Goals
You can send goals two ways:
RViz (Manual)
- Click "Nav2 Goal" button
- Click where you want to go
- Drag to set orientation
- Robot plans and drives
Great for testing and exploration
Code (Programmatic)
- Use
NavigateToPose action
- Send goals from a Python node
- Chain multiple waypoints
- React to results automatically
Required for P4 Part 3
What Happens When Things Go Wrong?
Nav2 has built-in recovery behaviors for when the robot gets stuck.
- Clear costmap: Reset the costmap to remove ghost obstacles from old sensor data
- Spin: Rotate in place to get new sensor readings
- Back up: Drive backwards to recover from a tight space
- Wait: Pause and hope a dynamic obstacle moves
If all recovery behaviors fail: Nav2 aborts the goal and reports failure. Your code needs to handle this — goals don't always succeed! This is very common with physical robots.
Using Nav2 with TurtleBot3
# Terminal 1: Launch Gazebo
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
# Terminal 2: Launch Nav2 with your map
ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=$HOME/map.yaml
- Wait for Nav2 to fully start (watch terminal for "Creating bond timer")
- Set initial pose with "2D Pose Estimate" in RViz
- Send a goal with "Nav2 Goal" in RViz
- Watch the robot plan and navigate!
Important: You must use the map you built in the SLAM exercise. Nav2 needs a pre-built map — it does not build one itself.
Textbook Connection: IAR Chapter 14
- Section 14.1: The path planning problem — configuration space, free space, obstacles
- Section 14.2–14.3: Graph-based planners — Dijkstra, A*, which Nav2's planner builds on
- Section 14.5: Sensor-based planning — the local planner's job, reacting in real-time
Key insight: Path planning balances optimality (find the best path) with feasibility (can the robot actually execute it?) and reactivity (can it handle surprises?). Nav2 uses a layered approach: optimal global plan + reactive local controller.
Summary
| Concept | What It Does |
|---|
| AMCL | Particle filter — localizes robot on a known map |
| Costmap | Grid with obstacle costs + inflation for safe planning |
| Global Planner | Finds optimal path on the full map (Dijkstra/A*) |
| Local Planner | Follows path reactively, avoids dynamic obstacles |
| Nav2 | Framework that combines all navigation components |
| Recovery | Built-in behaviors for when the robot gets stuck |