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
Good matches survive; poor matches are removed
Over time, the cloud converges to the true pose

In RViz: Green arrows start spread out, then tighten as confidence increases.

AMCL: How Many Particles?

<<<<<<< HEAD

AMCL adapts its particle count between a minimum (confident) and maximum (uncertain). More particles = more hypotheses = more computation.

Few particles (confident)

Clustered tightly in one spot
Distinctive area with clear landmarks
Lidar scans match expected map well

Many particles (uncertain)

Spread over a large area
Multiple map locations look similar
e.g., long featureless corridor or just after startup

Rule of thumb: The harder it is to distinguish the robot's current location from other map locations using lidar, the more particles AMCL needs. =======

AMCL adapts its particle count between a minimum (when confident) and a maximum (when uncertain). More particles = more hypotheses tracked = more computation.

Few particles (confident)

Particles cluster tightly
Robot moves through a distinctive area
Lidar scans match the expected map well
Only a small pose neighborhood is tracked

Many particles (uncertain)

Particles spread over a larger area
Several map regions look similar
Long featureless corridors are ambiguous
Startup is uncertain until scans accumulate

Rule of thumb: Ambiguous scans require more particles. >>>>>>> 75af8b3a9148c651a7452130fe37a5b854aa9b86

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 as the robot moves and gets more scan data — but it takes longer.

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.

<<<<<<< HEAD =======

>>>>>>> 75af8b3a9148c651a7452130fe37a5b854aa9b86

Cost	Meaning	Visual
0	Free space — safe to traverse	No color
1–252	Increasing cost (near obstacles)	Blue → red
253	Inscribed — robot center would hit obstacle	Cyan ring
254	Lethal — occupied cell	Purple/pink
255	Unknown	No data

<<<<<<< HEAD

Inflation layer: Obstacles are expanded outward by inflation_radius. The closer to an obstacle, the higher the cost. The planner treats the robot as a point and avoids high-cost cells. =======

Inflation layer: Obstacles expand by inflation_radius; nearby cells get higher cost, so paths stay farther from walls. >>>>>>> 75af8b3a9148c651a7452130fe37a5b854aa9b86

Inflation: A Worked Example

<<<<<<< HEAD

Config: resolution: 0.05 m/cell | inflation_radius: 0.15 m → buffer = 3 cells

Raw map (`#` = wall)

  0 1 2 3 4 5
=======
                
                    Config: resolution: 0.05 m/cell  |  inflation_radius: 0.15 m

                    Buffer width: 0.15 ÷ 0.05 = 3 cells
                
                Start with a 6×6 map with a wall on the left:
                
                    
                        Raw map (# = wall)
                          0 1 2 3 4 5
>>>>>>> 75af8b3a9148c651a7452130fe37a5b854aa9b86
0 # . . . . .
1 # . . . . .
2 # . . . . .
3 # . . . . .
                    
                    
<<<<<<< HEAD
                        After inflation (I = inflated)
                          0 1 2 3 4 5
0 # I I I . .
1 # I I I . .
2 # I I I . .
3 # I I I . .
                    
                
                
                    Key rule: Measure straight-line distance (in cells) from each free cell to the nearest #. If distance ≤ buffer width, the cell is inflated. Diagonal neighbors are √2 ≈ 1.4 cells away.
=======
                        After inflation (I = inflated)
                          0 1 2 3 4 5
0 # I I I . .
1 # I I I . .
2 # I I I . .
3 # I I I . .
4 # I I I . .
5 # I I I . .
                        Cols 1–3 are within 3 cells of col 0; cols 4–5 remain cost 0.


                
                    Key rule: If a free cell is within the buffer width (in cells) of the nearest obstacle, mark it inflated.
>>>>>>> 75af8b3a9148c651a7452130fe37a5b854aa9b86



            
            
                How the Planner Uses Cost
                
                    The global planner finds the minimum-total-cost path from start to goal.
                
                
                    
                        Two routes, same length
                          0 1 2 3 4 5
0 . . . . . G
1 . I I . . .
2 . I # . . .
3 . I # . . .
4 . . . . . .
5 S . . . . .
                    
                    
                        Which path wins?
                        
                            Route A (left, through I cells): high cost
                            Route B (right, cost-0 cells): low cost
                            Planner picks Route B
                        
                    
                
                
                    This is why paths in RViz sometimes curve away from walls even when a straight line would work geometrically.
                
            

            
            
                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
                        
                    
                    
                        Local Costmap
                        
                            Small area around the robot
                            Updated in real-time with lidar
                            Includes dynamic obstacles
                            Used for immediate decisions
                        
                    
                
                
                    Why two costmaps? The global costmap gives the big-picture route. The local costmap handles moment-to-moment driving.
                
            

            
            
                Global Path Planning
                
                    The global planner finds a path from the robot's current position to the goal on the global costmap.
                
                
                    Treats the costmap as a graph — each cell is a node, edge weights = cell costs
                    Explores cells and builds up the lowest-cost route to the goal
                    High-cost (inflated) cells are allowed but penalized, so cheaper routes are preferred
                    If inflation completely fills a corridor, no path can pass through it
                
                
                    In RViz: The path often curves away from walls because inflation pushes it toward lower-cost cells.
                
            

            
            
<<<<<<< HEAD
                robot_radius: Matching Config to Hardware
                
                    robot_radius tells the costmap how close the robot center can get to a wall. Wrong value → bad plans.
                
                
                    
                        Too small
                        
                            Plans paths through gaps robot won't fit
                            Robot collides with walls
                            Common when reusing config on different platform
                        
                    
                    
                        Too large
                        
                            Avoids passages robot could navigate
                            May fail to find any path in tight spaces
                            Conservative — may fail unnecessarily
                        
                    
                
                
                    Both matter together: robot_radius = inscribed zone; inflation_radius = how far beyond that the planner prefers to stay away.
=======
                robot_radius and Safety
                
                    The costmap uses robot_radius to set how close the robot center can approach walls. If it mismatches hardware, planning fails.
                
                
                    
                        robot_radius too small
                        
                            Planner thinks the robot is smaller than it is
                            Plans gaps the real robot cannot fit through
                            Collision risk increases near walls
                            Common when reusing configs across robots
                        
                    
                    
                        robot_radius too large
                        
                            Planner thinks the robot is larger than it is
                            Avoids passages the robot could actually navigate
                            May fail to find any path in tight environments
                            Safe, but can fail unnecessarily
                        
                    
                
                
                    Together: robot_radius sets the inscribed collision zone; inflation_radius adds extra clearance.
>>>>>>> 75af8b3a9148c651a7452130fe37a5b854aa9b86
                
            

            
            
                Reaching the Goal: Tolerance
                
                    tolerance = how close robot must get before Nav2 declares succeeded.
                
                
                    Large (1.0 m): Fast arrival, imprecise — may stop in wrong places
                    Small (0.02 m): Very precise, but odometry drift → oscillation or timeout
                    Rule of thumb: General nav → 0.3–0.5 m. Precise positioning → smaller, expect more failures
                
                
                    Recovery behaviors trigger when goal can't be reached (including tight tolerance). Your code must handle goal failure gracefully.
                
            

            
            
                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 velocity commands forward in time, scores each trajectory
                    Picks the best-scoring trajectory and sends velocity commands
                
                
                    Global
Path
                    →
                    Local
Planner
                    →
                    /cmd_vel
                    →
                    Robot
Moves
                
            

            
            
                Nav2: The Full Picture
                
                    Nav2 = the ROS2 navigation framework combining all components.
                
                
                    Component Role Node
                    
                        Map Server Loads/serves static map map_server
                        AMCL Localizes robot on 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
                    
                
            

            
            
                Activity 7: Parameter Cheat Sheet
                
                    For Q5–Q8 — focus on what changes behavior, not memorizing defaults.
                
                
                    Parameter Controls If Increased
                    
                        min/max_particles AMCL confidence vs compute More robust, slower
                        odom_alpha1/2 Assumed odom noise AMCL trusts odom less
                        pf_err Target localization error Fewer particles needed
                        use_astar A* vs Dijkstra Often faster planning
                        tolerance Goal reached distance Easier completion, less precise
                        desired_linear_vel Speed cap Faster, less control margin
                        lookahead_dist Controller look-ahead Smoother, may overshoot turns
                        max_angular_vel Turn-rate cap Quicker turns, oscillation risk
                    
                
                
                    Rule of thumb: Tune speed, lookahead, and tolerance together for the robot platform.
                
            

            
            
                Sending Navigation Goals
                Two ways to send goals:
                
                    
                        RViz (Manual)
                        
                            Click "Nav2 Goal" button
                            Click destination, drag to set heading
                            Robot plans and drives
                        
                        Great for testing
                    
                    
                        Code (Programmatic)
                        
                            Use NavigateToPose action
                            Send goals from a Python node
                            Chain multiple waypoints
                        
                        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: Remove ghost obstacles from old sensor data
                    Spin: Rotate in place for new sensor readings
                    Back up: Drive backwards from tight space
                    Wait: Pause for a dynamic obstacle to move
                
                
                    If all fail: Nav2 aborts the goal. Your code must handle goal failure — this is common with physical robots.
                
            

            
            
                Using Nav2 with TurtleBot3
                ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py   # Terminal 1
ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=$HOME/map.yaml  # Terminal 2
                
                    Wait for Nav2 to start ("Creating bond timer" in terminal)
                    Set initial pose with "2D Pose Estimate" in RViz
                    Send a goal with "Nav2 Goal" in RViz
                
                
                    Important: Use the map you built in the SLAM exercise. Nav2 needs a pre-built map.
                
            

            
            
                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 Optimal path on full map (Dijkstra/A*)
                        Local Planner Follows path reactively, avoids dynamic obstacles
                        Nav2 Framework combining all navigation components
                        Recovery Built-in behaviors when robot gets stuck
                    
                
                
                    Next: Activity 7 — analyze costmap inflation and Nav2 parameters

Component	Role	Node
Map Server	Loads/serves static map	`map_server`
AMCL	Localizes robot on 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`

Parameter	Controls	If Increased
`min/max_particles`	AMCL confidence vs compute	More robust, slower
`odom_alpha1/2`	Assumed odom noise	AMCL trusts odom less
`pf_err`	Target localization error	Fewer particles needed
`use_astar`	A* vs Dijkstra	Often faster planning
`tolerance`	Goal reached distance	Easier completion, less precise
`desired_linear_vel`	Speed cap	Faster, less control margin
`lookahead_dist`	Controller look-ahead	Smoother, may overshoot turns
`max_angular_vel`	Turn-rate cap	Quicker turns, oscillation risk

Concept	What It Does
AMCL	Particle filter — localizes robot on a known map
Costmap	Grid with obstacle costs + inflation for safe planning
Global Planner	Optimal path on full map (Dijkstra/A*)
Local Planner	Follows path reactively, avoids dynamic obstacles
Nav2	Framework combining all navigation components
Recovery	Built-in behaviors when robot gets stuck