Nav2 Introduction - EEET2610 - Mecanum Robot Guide

Overview

Nav2 (Navigation 2) is the ROS2 navigation stack. It enables robots to autonomously navigate from point A to point B while avoiding obstacles.

Nav2 combines localization, mapping, path planning, and obstacle avoidance into a complete autonomous navigation system.

Nav2 Architecture

Goal (2D Nav Goal)
       ↓
┌──────────────────┐
│  Behavior Tree   │ ← Decision making
└────────┬─────────┘
         ↓
┌──────────────────┐
│  Planner Server  │ ← Global path (A to B)
└────────┬─────────┘
         ↓
┌──────────────────┐
│ Controller Server│ ← Local control (follow path, avoid obstacles)
└────────┬─────────┘
         ↓
    /cmd_vel (robot moves)

Components

Planner Server

Computes global path from start to goal using map. Algorithms: NavFn, Smac Planner.

Controller Server

Follows path while avoiding dynamic obstacles. Algorithms: DWB, TEB, MPPI.

Behavior Server

Executes recovery behaviors (backup, spin, wait) when stuck.

BT Navigator

Coordinates all servers using Behavior Trees (decision logic).

Waypoint Follower

Navigates through multiple waypoints in sequence.

Lifecycle Manager

Manages server lifecycles (startup, shutdown, recovery).

Map Server

Provides occupancy grid map (from SLAM or pre-built).

AMCL / Localization

Localizes robot in map (corrects odometry drift).

Prerequisites

Before using Nav2:

Map created (using SLAM Toolbox or Cartographer)
Localization working (EKF fusion or AMCL)
Odometry accurate (<5% error)
LiDAR functioning (publishes /scan)
Robot control (accepts /cmd_vel)
TF tree complete (map → odom → base_link)

Installation

# Install Nav2
sudo apt install ros-jazzy-navigation2 ros-jazzy-nav2-bringup

# Install AMCL for localization
sudo apt install ros-jazzy-nav2-amcl

Basic Configuration

Minimal Nav2 Parameters

File: config/nav2_params.yaml

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odometry/filtered
    bt_loop_duration: 10
    default_server_timeout: 20

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 20.0  # Hz
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"]
    controller_plugins: ["FollowPath"]

    # DWB controller (good for Mecanum)
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      min_vel_x: -0.5
      max_vel_x: 0.5
      min_vel_y: -0.5  # Mecanum strafing
      max_vel_y: 0.5
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.55
      min_speed_theta: 0.0
      acc_lim_x: 2.5
      acc_lim_y: 2.5
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: -2.5
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 20
      vtheta_samples: 40

planner_server:
  ros__parameters:
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22  # Adjust for your robot
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: False
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

Launch File

File: launch/navigation.launch.py

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node

def generate_launch_description():
    # Paths
    pkg_share = FindPackageShare('mecanum_description').find('mecanum_description')
    nav2_params = PathJoinSubstitution([pkg_share, 'config', 'nav2_params.yaml'])
    map_yaml = PathJoinSubstitution([pkg_share, 'maps', 'my_map.yaml'])

    # Arguments
    use_sim_time = LaunchConfiguration('use_sim_time')

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) clock if true')

    # Map server
    map_server_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('mecanum_description'),
                'launch',
                'map_server.launch.py'
            ])
        ]),
        launch_arguments={'map': map_yaml}.items()
    )

    # AMCL localization
    amcl_cmd = Node(
        package='nav2_amcl',
        executable='amcl',
        name='amcl',
        output='screen',
        parameters=[nav2_params]
    )

    # Nav2 bringup
    nav2_bringup_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('nav2_bringup'),
                'launch',
                'navigation_launch.py'
            ])
        ]),
        launch_arguments={
            'params_file': nav2_params,
            'use_sim_time': use_sim_time
        }.items()
    )

    return LaunchDescription([
        declare_use_sim_time_cmd,
        map_server_cmd,
        amcl_cmd,
        nav2_bringup_cmd,
    ])

Complete System Launch

File: launch/robot_navigation.launch.py

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # Robot control (motors, odometry, EKF)
    robot_control = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('mecanum_description'),
                'launch',
                'robot_full.launch.py'
            ])
        ])
    )

    # LiDAR
    lidar = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('mecanum_description'),
                'launch',
                'lidar.launch.py'
            ])
        ])
    )

    # Navigation
    navigation = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('mecanum_description'),
                'launch',
                'navigation.launch.py'
            ])
        ])
    )

    return LaunchDescription([
        robot_control,
        lidar,
        navigation,
    ])

Usage:

ros2 launch mecanum_description robot_navigation.launch.py

RViz Configuration

Launch RViz

rviz2

Essential Displays

Fixed Frame: map
Map - Static map
- Topic: /map
- Color Scheme: map
Global Costmap - Planning costmap
- Topic: /global_costmap/costmap
- Color Scheme: costmap
Local Costmap - Local obstacle avoidance
- Topic: /local_costmap/costmap
- Color Scheme: costmap
Global Plan - Computed path
- Topic: /plan
- Color: Green
Local Plan - Trajectory being followed
- Topic: /local_plan
- Color: Red
LaserScan - LiDAR data
- Topic: /scan
RobotModel - Robot visualization
TF - Coordinate frames
2D Nav Goal - Set navigation goals
- Toolbar → 2D Nav Goal button

Save RViz Config

File → Save Config As → rviz/navigation.rviz

Method 1: RViz (Interactive)

Click “2D Nav Goal” button in toolbar
Click and drag on map:
- Click: Goal position
- Drag direction: Goal orientation
Release mouse
Robot navigates to goal!

Method 2: Command Line

ros2 topic pub --once /goal_pose geometry_msgs/msg/PoseStamped \
  "{header: {stamp: {sec: 0}, frame_id: 'map'},
    pose: {position: {x: 2.0, y: 1.0, z: 0.0},
           orientation: {w: 1.0}}}"

Method 3: Python Script

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped

class NavGoalPublisher(Node):
    def __init__(self):
        super().__init__('nav_goal_publisher')
        self.publisher = self.create_publisher(PoseStamped, '/goal_pose', 10)

    def send_goal(self, x, y, yaw=0.0):
        goal = PoseStamped()
        goal.header.frame_id = 'map'
        goal.header.stamp = self.get_clock().now().to_msg()

        goal.pose.position.x = x
        goal.pose.position.y = y
        goal.pose.position.z = 0.0

        # Convert yaw to quaternion
        goal.pose.orientation.z = np.sin(yaw / 2.0)
        goal.pose.orientation.w = np.cos(yaw / 2.0)

        self.publisher.publish(goal)
        self.get_logger().info(f'Sent goal: ({x}, {y}, {yaw})')

def main():
    rclpy.init()
    node = NavGoalPublisher()
    node.send_goal(2.0, 1.0, 0.0)  # Navigate to (2, 1) facing 0°
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Launch Complete System

ros2 launch mecanum_description robot_navigation.launch.py

Verify:

All nodes running (ros2 node list)
Map loaded (/map topic)
Localization active (AMCL or EKF)
Costmaps publishing

Localize Robot

If using AMCL:

Open RViz
Click “2D Pose Estimate”
Click robot’s actual position on map
Drag to set orientation
Watch particles converge

If using SLAM localization mode:

Already localized automatically

Send Navigation Goal

Click “2D Nav Goal” in RViz
Click goal position on map
Drag to set orientation
Release mouse

Monitor Navigation

Watch in RViz:

Green path (global plan) from robot to goal
Red trajectory (local plan) adjusting around obstacles
Robot moving toward goal
Costmaps updating in real-time

Monitor status:

ros2 topic echo /behavior_tree_log

Goal Reached

Success indicators:

Robot stops at goal
Status: “Goal succeeded”
Velocity commands stop (/cmd_vel = 0)

If failed:

Check logs: ros2 node logs /controller_server
Common: Path blocked, timeout, robot stuck

Behavior Tree

Nav2 uses Behavior Trees for decision logic:

NavigateToPoseSequence
├── ComputePathToPose
├── FollowPath
└── [Recovery behaviors if stuck]
    ├── Backup
    ├── Spin
    └── Wait

Custom behavior trees possible (advanced topic).

Troubleshooting

No path found

Causes:

Goal in occupied space
Goal too close to obstacle
No valid path exists

Solutions:

Choose different goal
Increase planner tolerance
Check global costmap (goal might appear blocked)

Robot won't move

Debug:

Check /cmd_vel published:
```
ros2 topic echo /cmd_vel
```
Check controller server status
Verify robot control stack running
Check velocity limits not too restrictive

Robot oscillates / unstable

Cause: Controller gains too aggressiveSolutions:

Reduce max velocities
Increase controller frequency
Tune DWB parameters
Switch controller plugin (try TEB or MPPI)

Gets stuck near obstacles

Cause: Costmap inflation too largeSolution:

Reduce inflation_radius
Reduce cost_scaling_factor
Check robot_radius accurate

Next Steps

Path Planning

Deep dive into planners and costmaps

Behavior Trees

Customize navigation behavior

Navigation Testing

Systematic navigation validation

AMCL Localization

Localize robot in pre-built map

References

[1] Nav2 Documentation: https://docs.nav2.org/ [2] Nav2 Tutorials: https://docs.nav2.org/tutorials/index.html [3] Nav2 Configuration Guide: https://docs.nav2.org/configuration/index.html

WP1: MATLAB Simulation

WP2: Prototyping

WP3: Embedded Systems

WP4: ROS2 Setup

WP4: Robot Description

WP4: Control

WP4: Localization

WP4: Mapping

WP4: Navigation

WP4: Sensors

WP5: Integration & Presentation

​Overview

​Nav2 Architecture

​Components

Planner Server

Controller Server

Behavior Server

BT Navigator

Waypoint Follower

Lifecycle Manager

Map Server

AMCL / Localization

​Prerequisites

​Installation

​Basic Configuration

​Minimal Nav2 Parameters

​Launch File

​Complete System Launch

​RViz Configuration

​Launch RViz

​Essential Displays

​Save RViz Config

​Sending Navigation Goals

​Method 1: RViz (Interactive)

​Method 2: Command Line

​Method 3: Python Script

​Navigation Workflow

​Behavior Tree

​Troubleshooting

​Next Steps

Path Planning

Behavior Trees

Navigation Testing

AMCL Localization

​References

Overview

Nav2 Architecture

Components

Prerequisites

Installation

Basic Configuration

Minimal Nav2 Parameters

Launch File

Complete System Launch

RViz Configuration

Launch RViz

Essential Displays

Save RViz Config

Sending Navigation Goals

Method 1: RViz (Interactive)

Method 2: Command Line

Method 3: Python Script

Navigation Workflow

Behavior Tree

Troubleshooting

Next Steps

References