
## Autonomous Navigation with Nav2

This is the final part of the Project Ogre series. We've built the [robot](/blog/project-ogre-part1-building-robot), created a [digital twin](/blog/project-ogre-part2-isaac-sim), built [maps with SLAM](/blog/project-ogre-part3-slam-mapping), and trained an [RL policy](/blog/project-ogre-part4-isaac-lab-training). Now we bring it all together with Nav2 for autonomous waypoint navigation.

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>Project Goal:</strong> Navigate autonomously to user-specified waypoints while avoiding obstacles, using the saved map and trained policy.
</div>

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>📦 Full Source Code:</strong> Nav2 configuration and launch files are available at <a href="https://github.com/protomota/ogre-slam" style={{ color: '#ff8c29', textDecoration: 'underline' }}>github.com/protomota/ogre-slam</a>
</div>

The video below shows Nav2 autonomous navigation in action. On the left is RViz displaying the map, costmaps, and planned path. On the right is Isaac Sim where the robot navigates autonomously through the maze using the trained RL policy to execute velocity commands.

<div style={{ position: 'relative', paddingBottom: '56.25%', height: 0, overflow: 'hidden', maxWidth: '100%', marginTop: '1.5rem' }}>
  <iframe
    style={{ position: 'absolute', top: 0, left: 0, width: '100%', height: '100%' }}
    src="https://www.youtube.com/embed/IfBRDBTknWg"
    title="Nav2 Autonomous Navigation Demonstration"
    frameBorder="0"
    allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture"
    allowFullScreen
  />
</div>

## The Nav2 Stack

Nav2 (Navigation 2) is ROS2's navigation framework. It handles:

1. **Localization**: Where is the robot on the map?
2. **Global Planning**: What's the best path to the goal?
3. **Local Planning**: How do we follow that path while avoiding new obstacles?
4. **Recovery**: What to do when stuck?

For Project Ogre, the architecture looks like:

```
Saved Map → AMCL (localization)
                    ↓
User Goal → Global Planner → Local Planner → /cmd_vel
                                                  ↓
                                          Policy Controller
                                                  ↓
                                           Wheel Commands
```

The RL policy sits between Nav2's velocity commands and the actual wheel control, translating ideal commands into optimal wheel velocities.

## Understanding Costmaps

Nav2 uses **costmaps** to represent navigable space. Each cell in the grid has a cost (0-254) indicating traversal difficulty:

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Cost</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Name</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Meaning</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>0</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>FREE_SPACE</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Safe to traverse</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>1-252</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>INFLATED</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Near obstacles (increasingly expensive)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>253</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>INSCRIBED</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Inside robot's radius (collision likely)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>254</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>LETHAL</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Obstacle (certain collision)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}>255</td>
      <td style={{ padding: '12px 16px' }}>NO_INFORMATION</td>
      <td style={{ padding: '12px 16px' }}>Unknown space</td>
    </tr>
  </tbody>
</table>

### Two Costmaps

**Global Costmap** (`/global_costmap/costmap`)
- Covers the entire map
- Used for path planning
- Updates from static map + sensors

**Local Costmap** (`/local_costmap/costmap`)
- Rolling 3m × 3m window around robot
- Used for reactive obstacle avoidance
- Updates continuously from sensors

### Inflation

Obstacles are "inflated" by the robot's radius to prevent collision. The key parameters:

```yaml
robot_radius: 0.20        # Inscribed radius
inflation_radius: 0.35    # How far to spread costs
cost_scaling_factor: 3.0  # How quickly costs drop off
```

If the robot clips walls, increase `inflation_radius`. If it can't navigate narrow passages, decrease it.

## Nav2 Configuration

The full configuration lives in `config/nav2_params.yaml`. Here are the key sections:

### AMCL (Localization)

AMCL (Adaptive Monte Carlo Localization) tracks the robot's position on the map using laser scan matching:

```yaml
amcl:
  ros__parameters:
    base_frame_id: "base_link"
    odom_frame_id: "odom"
    scan_topic: /scan
    min_particles: 500
    max_particles: 2000
    update_min_d: 0.1  # Update after 10cm movement
    update_min_a: 0.2  # Update after ~11° rotation
```

### Global Planner

We use Smac 2D for global path planning - it handles tight spaces better than the default NavFn:

```yaml
planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2D"
      tolerance: 0.50  # Search 50cm around goal
      max_planning_time: 2.0
```

The `tolerance` parameter is crucial - it defines how close the robot needs to get to the exact goal point. Higher tolerance helps when goals are near obstacles.

### Local Controller

The local controller follows the global path while avoiding obstacles. For mecanum robots, we use a velocity-based approach:

```yaml
controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      max_vel_x: 0.3
      max_vel_y: 0.3     # Strafing enabled for mecanum
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      min_speed_theta: 0.0
```

Note `max_vel_y` - this enables strafing, which is what makes mecanum navigation special.

### Behavior Server (Recovery)

When the robot gets stuck, recovery behaviors kick in:

```yaml
behavior_server:
  ros__parameters:
    behavior_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
```

The robot will try spinning, backing up, or waiting before giving up on a goal.

## Running Navigation

### In Isaac Sim

**Terminal 1: Start Isaac Sim**
- Load `usds/ogre.usd`
- Verify ROS2 Context has a matching Domain ID
- Press Play

**Terminal 2: Launch Navigation**
```bash
cd ~/ros2_ws && source install/setup.bash

ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/isaac_sim_map.yaml \
  use_sim_time:=true
```

The `use_sim_time:=true` flag is critical - without it, TF transforms will time out because the simulation clock doesn't match wall clock.

**Terminal 3: Launch Policy Controller**
```bash
conda deactivate
source ~/ros2_ws/install/setup.bash

ros2 launch ogre_policy_controller policy_controller.launch.py use_sim_time:=true
```

**Terminal 4: Launch RViz (optional, for remote visualization)**
```bash
./scripts/launch_isaac_sim_rviz.sh
```

### On the Real Robot

```bash
cd ~/ros2_ws && source install/setup.bash
ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/my_map.yaml
```

This launches:
- AMCL localization with the saved map
- RPLIDAR + RealSense D435 sensors
- Full Nav2 stack
- Web teleop for manual override

## Navigating in RViz

With navigation running, RViz becomes your command interface:

### Step 1: Set Initial Pose

Click **2D Pose Estimate** in the toolbar, then click and drag on the map to indicate:
- Where the robot is
- Which direction it's facing

AMCL needs this initial hint to start tracking correctly. As the robot moves, localization will refine automatically.

### Step 2: Send Navigation Goals

Click **Nav2 Goal** (or **2D Goal Pose**), then click on the map where you want the robot to go. Drag to set the final orientation.

The robot will:
1. Plan a global path (green line in RViz)
2. Follow the path using local planning
3. Avoid obstacles detected in real-time
4. Execute recovery behaviors if stuck

### Manual Override

On the real robot, the web teleop interface at `http://10.21.21.45:8080` provides manual control. Any teleop commands override Nav2's autonomous control.

## The Complete Data Flow

Let's trace a navigation command through the entire system:

```
1. User clicks goal in RViz
         ↓
2. Global Planner computes path using costmap
         ↓
3. Local Controller generates /cmd_vel following path
         ↓
4. Policy Controller receives velocity command
         ↓
5. Neural network computes optimal wheel velocities
         ↓
6. /joint_command sent to robot/sim
         ↓
7. Robot moves, sensors update, localization refines
         ↓
   (loop continues until goal reached)
```

The trained RL policy adds value at step 5 - it learns compensation for wheel slip, motor delays, and mechanical imperfections that pure kinematics can't handle.

## 3D Obstacle Avoidance

The RealSense D435 provides depth data that the RPLIDAR can't see - obstacles below the 2D scan plane like:
- Table legs
- Stairs
- Low furniture
- Pets

The depth camera publishes a PointCloud2 to `/camera_points`, which feeds into the local costmap's voxel layer:

```yaml
local_costmap:
  voxel_layer:
    plugin: "nav2_costmap_2d::VoxelLayer"
    observation_sources: scan camera
    camera:
      topic: /camera_points
      sensor_frame: camera_link
      max_obstacle_height: 0.5
      min_obstacle_height: 0.0
```

Now the robot can detect and avoid 3D obstacles in real-time.

## Troubleshooting Navigation

### "Legal potential but no path"

**Cause**: Goal is in an inflated zone near obstacles

**Solutions**:
1. Click goals further from walls
2. Increase `tolerance` in planner config
3. Switch to SmacPlanner2D (handles this better than NavFn)

### Robot Clips Walls

**Cause**: Inflation radius too small

**Solution**: Increase `inflation_radius` in both costmaps (try 0.40-0.50m)

### Robot Can't Enter Narrow Spaces

**Cause**: Inflation radius too large

**Solution**: Decrease `inflation_radius` (try 0.30m)

### Localization Drifts

**Cause**: AMCL losing track, often from rapid motion or featureless areas

**Solutions**:
1. Move slower during navigation
2. Increase `max_particles`
3. Ensure good initial pose estimate

### TF_OLD_DATA Errors

**Cause**: Clock synchronization issues

**Solutions**:
- In simulation: Ensure `use_sim_time:=true` on ALL nodes
- On real robot: Check that odometry and SLAM are publishing current timestamps

### Robot Spins Forever

**Cause**: Recovery behavior stuck in loop

**Solution**: Cancel the goal and try a different destination. Check if there's an obstacle blocking all paths.

## Performance on Jetson

Running the full Nav2 stack on Jetson Orin Nano requires some optimization:

### Memory Management

```yaml
global_costmap:
  resolution: 0.10  # Coarser than SLAM (0.05) to save memory
local_costmap:
  resolution: 0.05  # Finer for local avoidance
```

### CPU Optimization

```bash
# Enable max performance mode
sudo /usr/bin/jetson_clocks

# Monitor resources
tegrastats
```

### Disable Unnecessary Visualization

Don't run RViz on the Jetson - use remote visualization instead. Save those CPU cycles for navigation.

## What We Built

Over these five posts, we created a complete autonomous mobile robot:

1. **Hardware**: Mecanum drive robot with Jetson Orin Nano, RPLIDAR, and RealSense
2. **Odometry**: Calibrated encoders with EKF sensor fusion
3. **Mapping**: SLAM-generated occupancy grids
4. **Learning**: RL-trained velocity controller
5. **Navigation**: Full Nav2 stack with 3D obstacle avoidance

The robot can now navigate autonomously to any point on the map, avoiding both static obstacles (walls from the map) and dynamic obstacles (detected in real-time by sensors).

## Future Improvements

The system works, but there's always room to improve:

- **IMU Integration**: Adding an IMU would dramatically improve odometry, especially during rotation
- **Dynamic Obstacle Tracking**: Currently obstacles are binary - tracking moving obstacles would enable smarter planning
- **Sim-to-Real Transfer**: Fine-tuning the RL policy on real-world data

---

**Project Ogre Summary:**

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Component</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Technology</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Computing</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Jetson Orin Nano</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Drive</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Mecanum wheels (omnidirectional)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>2D Sensing</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RPLIDAR A1</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>3D Sensing</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RealSense D435</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>SLAM</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>slam_toolbox</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Localization</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>AMCL</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Planning</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Nav2 (Smac2D + DWB)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Control</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RL-trained policy</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}>Simulation</td>
      <td style={{ padding: '12px 16px' }}>NVIDIA Isaac Sim</td>
    </tr>
  </tbody>
</table>

**Key Launch Commands:**

```bash
# Mapping (simulation)
ros2 launch ogre_slam mapping.launch.py use_sim_time:=true

# Mapping (real robot)
./scripts/launch_mapping_session.sh

# Navigation (simulation)
ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/isaac_sim_map.yaml \
  use_sim_time:=true

# Navigation (real robot)
ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/my_map.yaml

# Policy controller
ros2 launch ogre_policy_controller policy_controller.launch.py
```

Thanks for following along with Project Ogre. If you build something similar or have questions, let me know in the comments.

## Credits

- **slam_toolbox**: Steve Macenski
- **robot_localization**: Tom Moore
- **RPLIDAR ROS**: SLAMTEC
- **Isaac Lab**: NVIDIA
- **Nav2**: Open Robotics & Nav2 maintainers


Bringing it all together - autonomous waypoint navigation using ROS2 Nav2 with SLAM maps and RL-trained velocity control.

Posts tagged "nav2"

Project Ogre Part 5: Autonomous Navigation with Nav2