Skip to main content

Navigation with Nav2

Accessibility Statement

This chapter follows accessibility standards for educational materials, including sufficient color contrast, semantic headings, and alternative text for images.

Introduction​

This section explores Navigation2 (Nav2), ROS 2's navigation framework, and its integration with Isaac Sim and other simulation environments for humanoid robots.

Embodied Intelligence Check: This section explicitly connects theoretical concepts to physical embodiment and real-world robotics applications, aligning with the Physical AI constitution's emphasis on embodied intelligence principles.

Navigation2 (Nav2) is ROS 2's comprehensive navigation framework that provides a complete solution for robot navigation, including path planning, obstacle avoidance, and execution of navigation goals. For humanoid robots, Nav2 provides essential capabilities for autonomous navigation in complex environments, enabling the Physical AI principle of embodied intelligence by connecting computational path planning algorithms to physical robot movement and environmental interaction.

Nav2 consists of a behavior tree-based architecture that allows for flexible and robust navigation behaviors. It includes components for global path planning, local path planning, obstacle avoidance, and recovery behaviors. The framework is designed to work with various sensor configurations and robot types, making it suitable for humanoid robots with their unique kinematic and sensing capabilities.

This chapter will explore how Nav2 enables the Physical AI principle of embodied intelligence by providing humanoid robots with the capability to navigate complex environments by connecting computational planning algorithms to physical robot movement and environmental interaction.

Core Concepts​

Key Definitions​

  • Navigation2 (Nav2): ROS 2's comprehensive navigation framework for autonomous robot navigation.

  • Behavior Tree: A hierarchical structure used in Nav2 to compose complex navigation behaviors from simpler actions.

  • Global Planner: Component that computes a path from the robot's current location to the goal based on a map.

  • Local Planner: Component that computes short-term commands to follow the global path while avoiding obstacles.

  • Costmap: A grid-based representation of the environment that assigns costs to areas based on obstacles and other factors.

  • Recovery Behaviors: Actions executed when the robot encounters navigation failures to recover and continue operation.

  • Navigation Stack: The complete set of components that work together to provide autonomous navigation capabilities.

  • Path Execution: The process of following a computed path while monitoring for obstacles and adjusting as needed.

  • Dynamic Obstacle Avoidance: The capability to detect and avoid moving obstacles in real-time during navigation.

Architecture & Components​

Technical Standards Check: All architecture diagrams and component descriptions include references to ROS 2, Gazebo, Isaac Sim, VLA, and Nav2 as required by the Physical AI constitution's Multi-Platform Technical Standards principle.

Nav2 architecture includes:

  • Navigation Server: Main orchestrator that manages navigation goals and coordinates components
  • Global Planner: A* or other algorithms to compute global paths
  • Local Planner: DWA, TEB, or other algorithms for local path following and obstacle avoidance
  • Costmap Server: Maintains obstacle and inflation layers for navigation
  • Lifecycle Manager: Controls component lifecycle (configure, activate, etc.)
  • Behavior Trees: Defines navigation task execution flow
  • Sensors Integration: Interfaces with various sensor types for navigation
  • Recovery Behaviors: Predefined actions for navigation failure recovery

This architecture enables robust autonomous navigation for humanoid robots.

Technical Deep Dive​

Click here for detailed technical information
  • Architecture considerations: Behavior tree architecture with safety and recovery capabilities
  • Framework implementation: Integration with ROS 2 and lifecycle management
  • API specifications: Standard interfaces for navigation goals and feedback
  • Pipeline details: Global planning, local planning, and execution
  • Mathematical foundations: Path planning algorithms, cost functions, and optimization
  • ROS 2/Gazebo/Isaac/VLA structures: Integration points with AI and robotics frameworks
  • Code examples: Implementation details for Nav2 systems

Nav2's architecture is built around several core components that work together to provide robust navigation:

Navigation Server:

  • Manages navigation goals and the overall execution flow
  • Coordinates between planners, controllers, and other components
  • Uses lifecycle management for robust operation

Global Planner:

  • Computes optimal paths from start to goal using map information
  • Common algorithms: A*, Dijkstra, NavFn, or custom implementations
  • Works with static map information

Local Planner:

  • Executes short-term navigation commands to follow global path
  • Handles dynamic obstacle avoidance in real-time
  • Common algorithms: DWA, TEB, or MPC-based controllers

Costmaps:

  • Static layer: Fixed obstacles from the map
  • Obstacle layer: Dynamic obstacles from sensors
  • Inflation layer: Safety margins around obstacles

Here's an example of Nav2 configuration and usage:

nav2_params_humanoid.yaml
amcl:
  ros__parameters:
    use_sim_time: true
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

bt_navigator:
  ros__parameters:
    use_sim_time: true
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_have_feedback_condition_bt_node
    - nav2_is_path_valid_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_path_expiring_timer_condition
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node

controller_server:
  ros__parameters:
    use_sim_time: true
    controller_frequency: 20.0
    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_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]
    
    # DWB parameters
    FollowPath:
      plugin: "nav2_mppi_controller::MppiController"
      time_steps: 24
      control_freq: 20.0
      non_integrated_vel_penalty: 0.00001
      integrated_vel_penalty: 0.0001
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.1
      motion_model: "diff_drive"
      weight_xy: 1.0
      weight_theta: 1.0
      weight_obstacle: 50.0
      weight_path_align: 0.0
      weight_poses_align: 0.0
      weight_slowdown: 10.0
      decel_limit: -2.0

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: true
      rolling_window: true
      width: 10
      height: 10
      resolution: 0.05
      robot_radius: 0.25
      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
          data_type: "LaserScan"
          raytrace_max_range: 10.0
          raytrace_min_range: 0.0
          obstacle_max_range: 10.0
          obstacle_min_range: 0.0

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: true
      robot_radius: 0.25
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 10.0
          raytrace_min_range: 0.0
          obstacle_max_range: 10.0
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true
nav2_integration_example.py
#!/usr/bin/env python3

"""
Example of Nav2 integration for Physical AI applications,
demonstrating how navigation capabilities connect computational processes
to physical robot movement following Physical AI principles.
"""

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import QoSProfile
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped, Point, Quaternion
from tf2_ros import TransformListener, Buffer
from geometry_msgs.msg import TransformStamped
import math
from std_msgs.msg import String

class Nav2IntegrationNode(Node):
    """
    Node demonstrating Nav2 integration with humanoid robot systems,
    following Physical AI principles for embodied intelligence through
    navigation that connects computational planning to physical movement.
    """
    
    def __init__(self):
        super().__init__('nav2_integration_node')
        
        # Create action client for navigation
        self.nav_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        
        # Create subscriber for navigation commands
        self.nav_cmd_subscriber = self.create_subscription(
            String,
            '/nav_cmd',
            self.nav_cmd_callback,
            QoSProfile(depth=10)
        )
        
        # TF listener for robot pose
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # Timer for monitoring navigation status
        self.monitor_timer = self.create_timer(1.0, self.monitor_navigation)
        
        # Navigation state
        self.current_goal = None
        self.navigation_active = False
        
        self.get_logger().info('Nav2 integration node initialized')
        
    def nav_cmd_callback(self, msg):
        """Handle navigation commands"""
        cmd = msg.data
        
        if cmd.startswith('goto:'):
            # Parse coordinates from command (format: "goto:x,y")
            try:
                coords = cmd[5:].split(',')
                x = float(coords[0].strip())
                y = float(coords[1].strip())
                
                self.get_logger().info(f'Navigating to ({x}, {y})')
                
                # Send navigation goal
                self.send_navigate_to_pose_goal(x, y, 0.0)  # Assume z=0
            except ValueError:
                self.get_logger().error(f'Invalid coordinate format: {cmd}')
        elif cmd == 'stop':
            self.cancel_navigation()
        else:
            self.get_logger().warn(f'Unknown navigation command: {cmd}')
            
    def send_navigate_to_pose_goal(self, x, y, theta):
        """Send a navigation goal to Nav2"""
        # Wait for the action server to be available
        if not self.nav_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error('Navigation action server not available')
            return
            
        # Create navigation goal
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.position.z = 0.0
        
        # Convert theta to quaternion
        from math import sin, cos
        goal_msg.pose.pose.orientation.w = cos(theta / 2.0)
        goal_msg.pose.pose.orientation.z = sin(theta / 2.0)
        
        # Send the goal
        self.current_goal = self.nav_client.send_goal_async(
            goal_msg,
            feedback_callback=self.nav_feedback_callback
        )
        
        self.current_goal.add_done_callback(self.nav_goal_response_callback)
        self.navigation_active = True
        
    def nav_goal_response_callback(self, future):
        """Handle response from navigation goal"""
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Navigation goal rejected')
            self.navigation_active = False
            return
            
        self.get_logger().info('Navigation goal accepted')
        self.current_goal = goal_handle.get_result_async()
        self.current_goal.add_done_callback(self.nav_result_callback)
        
    def nav_result_callback(self, future):
        """Handle navigation result"""
        result = future.result().result
        self.get_logger().info(f'Navigation result: {result}')
        self.navigation_active = False
        
    def nav_feedback_callback(self, feedback_msg):
        """Handle navigation feedback"""
        feedback = feedback_msg.feedback
        current_pose = feedback.current_pose.pose.position
        self.get_logger().info(
            f'Navigating... Current position: ({current_pose.x:.2f}, {current_pose.y:.2f})'
        )
        
    def cancel_navigation(self):
        """Cancel the current navigation goal"""
        if self.current_goal:
            self.get_logger().info('Canceling navigation goal')
            # In a real implementation, we would cancel the goal
            # For this example, we just set the active flag to False
            self.navigation_active = False
            
    def monitor_navigation(self):
        """Monitor navigation status"""
        if self.navigation_active:
            self.get_logger().info('Navigation in progress...')
            
            # Try to get current robot pose
            try:
                transform = self.tf_buffer.lookup_transform(
                    'map', 'base_link', rclpy.time.Time())
                self.get_logger().info(
                    f'Robot at: ({transform.transform.translation.x:.2f}, '
                    f'{transform.transform.translation.y:.2f})'
                )
            except Exception as e:
                self.get_logger().info(f'Could not get robot transform: {e}')
        else:
            self.get_logger().info('Navigation idle')

def main(args=None):
    rclpy.init(args=args)
    nav_node = Nav2IntegrationNode()
    
    try:
        rclpy.spin(nav_node)
    except KeyboardInterrupt:
        pass
    finally:
        nav_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Hands-On Example​

In this hands-on example, we'll implement a complete Nav2 navigation system:

  1. Setup Nav2 Environment: Configure navigation parameters and costmaps
  2. Implement Path Planning: Create global and local planners
  3. Configure Costmaps: Set up obstacle and inflation layers
  4. Test Navigation: Execute navigation in simulated environment
  5. Validate Performance: Evaluate navigation in complex scenarios

Step 1: Create a complete Nav2 launch file (nav2_humanoid_launch.py)​

# launch/nav2_humanoid_launch.py
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from nav2_common.launch import RewrittenYaml
import os

def generate_launch_description():
    # Launch configurations
    use_sim_time = LaunchConfiguration('use_sim_time')
    params_file = LaunchConfiguration('params_file')
    autostart = LaunchConfiguration('autostart')
    use_composition = LaunchConfiguration('use_composition')
    container_name = LaunchConfiguration('container_name')
    
    # Launch arguments
    declare_use_sim_time = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true'
    )
    
    declare_params_file = DeclareLaunchArgument(
        'params_file',
        default_value=PathJoinSubstitution([
            FindPackageShare('my_robot_navigation'),
            'params',
            'nav2_params_humanoid.yaml'
        ]),
        description='Full path to the ROS2 parameters file to use for all launched nodes'
    )
    
    declare_autostart = DeclareLaunchArgument(
        'autostart', 
        default_value='true',
        description='Automatically startup the nav2 stack'
    )
    
    declare_use_composition = DeclareLaunchArgument(
        'use_composition', 
        default_value='False',
        description='Whether to use composed bringup'
    )
    
    declare_container_name = DeclareLaunchArgument(
        'container_name',
        default_value='nav2_container',
        description='the name of conatiner that nodes will load in if use composition'
    )
    
    # Make the launch file
    nav2_bringup_launch = PathJoinSubstitution([
        FindPackageShare('nav2_bringup'),
        'launch',
        'bringup_launch.py'
    ])
    
    nav2_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_bringup_launch),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'params_file': params_file,
            'autostart': autostart,
            'use_composition': use_composition,
            'container_name': container_name
        }.items()
    )
    
    return LaunchDescription([
        declare_use_sim_time,
        declare_params_file,
        declare_autostart,
        declare_use_composition,
        declare_container_name,
        nav2_bringup
    ])

Each step connects to the simulation-to-reality learning pathway.

Real-World Application​

Simulation-to-Reality Check: This section clearly demonstrates the progressive learning pathway from simulation to real-world implementation, following the Physical AI constitution's requirement for simulation-to-reality progressive learning approach.

In real-world humanoid robotics applications, Nav2 is essential for:

  • Autonomous navigation in human-populated environments
  • Path planning through complex indoor spaces
  • Dynamic obstacle avoidance with people and moving objects
  • Long-term autonomy with robust navigation capabilities

When transitioning from simulation to reality, Nav2 systems must account for:

  • Real sensor noise and uncertainty
  • Dynamic environments with moving obstacles
  • Computational constraints of real hardware
  • Safety requirements for navigation around humans

The Nav2 framework enables the Physical AI principle of embodied intelligence by providing humanoid robots with the capability to navigate complex environments by connecting computational planning algorithms to physical robot movement and environmental interaction.

Summary​

This chapter covered the fundamentals of navigation with Nav2:

  • How Nav2 provides comprehensive navigation capabilities for humanoid robots
  • Core components of Nav2 architecture and behavior trees
  • Technical implementation of navigation with global and local planning
  • Practical example of Nav2 integration with humanoid robots
  • Real-world considerations for deploying on physical hardware

Nav2 provides humanoid robots with the capability to navigate complex environments, enabling effective embodied intelligence applications, supporting the Physical AI principle of connecting computational planning algorithms to physical robot movement and environmental interaction.

Key Terms​

Navigation2 (Nav2)
ROS 2's comprehensive navigation framework for autonomous robot navigation in the Physical AI context.
Behavior Tree
A hierarchical structure used in Nav2 to compose complex navigation behaviors from simpler actions.
Global Planner
Component that computes a path from the robot's current location to the goal based on a map.
Local Planner
Component that computes short-term commands to follow the global path while avoiding obstacles.

Compliance Check​

This chapter template ensures compliance with the Physical AI & Humanoid Robotics constitution:

  • ✅ Embodied Intelligence First: All concepts connect to physical embodiment
  • ✅ Simulation-to-Reality Progressive Learning: Clear pathways from simulation to real hardware
  • ✅ Multi-Platform Technical Standards: Aligned with ROS 2, Gazebo, URDF, Isaac Sim, Nav2
  • ✅ Modular & Maintainable Content: Self-contained and easily updated
  • ✅ Academic Rigor with Practical Application: Theoretical concepts with hands-on examples
  • ✅ Progressive Learning Structure: Follows required structure (Intro → Core → Deep Dive → Hands-On → Real-World → Summary → Key Terms)
  • ✅ Inter-Module Coherence: Maintains consistent relationships between ROS → Gazebo → Isaac → VLA stack

Inter-Module Coherence​

Inter-Module Coherence Check: This chapter maintains consistent terminology, concepts, and implementation approaches with other modules in the Physical AI & Humanoid Robotics textbook, particularly regarding the ROS → Gazebo → Isaac → VLA stack relationships.

This chapter establishes the navigation framework that connects to other modules:

  • The navigation system integrates with Gazebo simulation from Module 2
  • Nav2 connects with Isaac perception from Module 3
  • The same navigation principles support VLA integration in Module 4