Project Ogre Part 5: Autonomous Navigation with Nav2

Autonomous Navigation with Nav2

This is the final part of the Project Ogre series. We've built the robot, created a digital twin, built maps with SLAM, and trained an RL policy. Now we bring it all together with Nav2 for autonomous waypoint navigation.

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.

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:

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:

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:

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:

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:

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:

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

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

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)

./scripts/launch_isaac_sim_rviz.sh

On the Real Robot

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:

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

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

CPU Optimization

# 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:

Key Launch Commands:

# 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