Skip to main content

Nav2 Navigation System

Learning Objectives​

By the end of this chapter, you will be able to:

  • Understand the Navigation2 (Nav2) framework and its architecture
  • Configure and launch Nav2 for mobile robot navigation
  • Implement localization, mapping, and path planning with Nav2
  • Integrate Nav2 with Isaac Sim and Isaac ROS
  • Customize Nav2 for specific robot platforms

Introduction to Navigation2 (Nav2)​

Navigation2 is the next-generation navigation framework for ROS 2, designed to provide robust, reliable, and flexible navigation capabilities for mobile robots. Key features include:

  • Behavior tree-based architecture for flexible behavior control
  • Plugin-based system for custom components
  • Support for multiple robot types and sensors
  • Advanced path planning algorithms
  • Comprehensive lifecycle management
  • Integration with SLAM and localization systems

Core Components​

# Example: Nav2 core components overview
"""
Nav2 System Architecture:

┌─────────────────┐    ┌──────────────────┐    ┌─────────────────┐
│   Navigation    │    │   Controller     │    │   Planner       │
│   Server        │◄──►│   Server         │◄──►│   Server        │
└─────────────────┘    └──────────────────┘    └─────────────────┘
         │                       │                       │
         ▼                       ▼                       ▼
┌─────────────────┐    ┌──────────────────┐    ┌─────────────────┐
│   BT Navigator  │    │   Velocity       │    │   Global        │
│   (Behavior     │    │   Smoother       │    │   Planner       │
│   Tree)         │    └──────────────────┘    └─────────────────┘
└─────────────────┘                                    │
         │                                             ▼
         ▼                                    ┌─────────────────┐
┌─────────────────┐                    ┌─────►│   Local         │
│   Recovery      │                    │      │   Planner       │
│   Server        │◄───────────────────┼──────┤   (DWB, TEB,    │
└─────────────────┘                   │      │   etc.)         │
                                      │      └─────────────────┘
                                      │                │
                                      │                ▼
                                      │      ┌─────────────────┐
                                      └──────┤   Costmap       │
                                               │   2D/3D       │
                                               └─────────────────┘
"""

Behavior Tree Implementation​

# Example: Nav2 Behavior Tree for navigation
"""
Basic Navigation Behavior Tree:

Sequence
├── ComputePathToPose
├── SmoothPath
├── FollowPath
├── Spin (if needed)
└── BackUp (if needed)

Recovery Sequence:
Sequence
├── RecoveryNode
│   └── Spin
└── PipelineSequence
    ├── ComputePathToPose
    └── FollowPath
"""

Installing and Setting Up Nav2​

Prerequisites​

  • ROS 2 Humble Hawksbill
  • Ubuntu 22.04 LTS
  • Python 3.8+
  • Navigation2 packages
  • Compatible robot platform

Installation​

# Install Nav2 packages
sudo apt update
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
sudo apt install ros-humble-nav2-rviz-plugins
sudo apt install ros-humble-nav2-console-tools
sudo apt install ros-humble-nav2-system-tests
sudo apt install ros-humble-nav2-interfaces

Basic Configuration File​

# Example: nav2_params.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_footprint"
    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

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    # Specify the path to the Behavior Tree XML file
    default_nav_through_poses_bt_xml: "nav2_bt_xml_v0.14/navigate_w_replanning_and_recovery.xml"
    default_navigate_to_pose_bt_xml: "nav2_bt_xml_v0.14/navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_smooth_path_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_assisted_teleop_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_drive_on_heading_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_globally_updated_goal_condition_bt_node
    - nav2_is_path_valid_condition_bt_node
    - nav2_initial_pose_received_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_truncate_path_local_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
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node
    - nav2_controller_cancel_bt_node
    - nav2_path_longer_on_approach_bt_node
    - nav2_wait_cancel_bt_node
    - nav2_spin_cancel_bt_node
    - nav2_back_up_cancel_bt_node
    - nav2_assisted_teleop_cancel_bt_node
    - nav2_drive_on_heading_cancel_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

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"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    # Goal checker parameters
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True

    # DWB parameters
    FollowPath:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      speed_limit_topic: "/speed_limit"
      min_vel_x: 0.0
      max_vel_x: 0.5
      min_vel_y: -0.5
      max_vel_y: 0.5
      min_speed_xy: 0.0
      max_speed_xy: 0.5
      min_speed_theta: 0.0
      max_speed_theta: 1.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: 5
      vtheta_samples: 20
      sim_time: 1.7
      path_distance_bias: 32.0
      goal_distance_bias: 24.0
      occdist_scale: 0.02
      forward_point_distance: 0.325
      stop_time_buffer: 0.2
      scaling_speed: 0.25
      max_scaling_factor: 0.2

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: 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: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: 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: True
      robot_radius: 0.22
      resolution: 0.05
      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: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          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
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    recovery_plugin_types: ["nav2_recoveries::Spin", "nav2_recoveries::BackUp", "nav2_recoveries::Wait"]
    spin:
      plugin: "nav2_recoveries::Spin"
      ideal_linear_velocity: 0.0
      ideal_angular_velocity: 1.0
      time_allowance: 10.0
    backup:
      plugin: "nav2_recoveries::BackUp"
      ideal_linear_velocity: -0.1
      ideal_angular_velocity: 0.0
      time_allowance: 10.0
    wait:
      plugin: "nav2_recoveries::Wait"
      time_allowance: 5.0
      sim_frequency: 20.0

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: true
      waypoint_pause_duration: 200

Basic Nav2 Launch File​

# Example: Basic Nav2 launch file
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, SetParameter
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # Launch Arguments
    use_sim_time = LaunchConfiguration('use_sim_time', default='True')
    params_file = LaunchConfiguration('params_file')
    autostart = LaunchConfiguration('autostart', default='True')
    map_topic = LaunchConfiguration('map_topic', default='map')
    use_lifecycle_manager = LaunchConfiguration('use_lifecycle_manager', default='True')

    # Launch configurations for robot description
    robot_description = LaunchConfiguration('robot_description')

    # Create the launch description and populate
    ld = LaunchDescription()

    # Declare launch arguments
    ld.add_action(
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='True',
            description='Use simulation (Gazebo) clock if true'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'params_file',
            default_value=PathJoinSubstitution([
                FindPackageShare('nav2_bringup'),
                'params',
                'nav2_params.yaml'
            ]),
            description='Full path to the ROS2 parameters file to use for all launched nodes'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'autostart',
            default_value='True',
            description='Automatically startup the nav2 stack'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'map_topic',
            default_value='map',
            description='Topic name for the map'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'use_lifecycle_manager',
            default_value='True',
            description='Use the lifecycle manager to startup components'
        )
    )

    # Set parameters
    ld.add_action(
        SetParameter('use_sim_time', use_sim_time)
    )

    # Lifecycle Manager
    lifecycle_manager = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_navigation',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time},
                    {'autostart': autostart},
                    {'node_names': ['map_server',
                                   'planner_server',
                                   'controller_server',
                                   'recoveries_server',
                                   'bt_navigator',
                                   'waypoint_follower']}]
    )

    # Navigation Server
    navigation_server = Node(
        package='nav2_navfn_planner',
        executable='navfn_planner',
        name='planner_server',
        output='screen',
        parameters=[params_file, {'use_sim_time': use_sim_time}],
        remappings=[('cmd_vel', 'cmd_vel_nav')]
    )

    # Controller Server
    controller_server = Node(
        package='nav2_controller',
        executable='controller_server',
        name='controller_server',
        output='screen',
        parameters=[params_file, {'use_sim_time': use_sim_time}],
        remappings=[('cmd_vel', 'cmd_vel_nav'),
                    ('odom', 'odom')]
    )

    # Local and Global Costmap Server
    local_costmap = Node(
        package='nav2_costmap_2d',
        executable='nav2_costmap_2d',
        name='local_costmap',
        output='screen',
        parameters=[params_file, {'use_sim_time': use_sim_time}],
        remappings=[('scan', 'scan'),
                    ('tf', 'tf')]
    )

    global_costmap = Node(
        package='nav2_costmap_2d',
        executable='nav2_costmap_2d',
        name='global_costmap',
        output='screen',
        parameters=[params_file, {'use_sim_time': use_sim_time}],
        remappings=[('scan', 'scan'),
                    ('tf', 'tf')]
    )

    # Behavior Tree Navigator
    bt_navigator = Node(
        package='nav2_bt_navigator',
        executable='bt_navigator',
        name='bt_navigator',
        output='screen',
        parameters=[params_file, {'use_sim_time': use_sim_time}],
        remappings=[('cmd_vel', 'cmd_vel_nav')]
    )

    # Recovery Server
    recovery_server = Node(
        package='nav2_recoveries',
        executable='recoveries_server',
        name='recoveries_server',
        output='screen',
        parameters=[params_file, {'use_sim_time': use_sim_time}]
    )

    # Waypoint Follower
    waypoint_follower = Node(
        package='nav2_waypoint_follower',
        executable='waypoint_follower',
        name='waypoint_follower',
        output='screen',
        parameters=[params_file, {'use_sim_time': use_sim_time}]
    )

    # Add nodes to launch description
    ld.add_action(lifecycle_manager)
    ld.add_action(navigation_server)
    ld.add_action(controller_server)
    ld.add_action(local_costmap)
    ld.add_action(global_costmap)
    ld.add_action(bt_navigator)
    ld.add_action(recovery_server)
    ld.add_action(waypoint_follower)

    return ld

Custom Behavior Tree Implementation​

# Example: Custom Nav2 Behavior Tree Node
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import QoSProfile
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from std_msgs.msg import String

class CustomNav2Node(Node):
    def __init__(self):
        super().__init__('custom_nav2_node')

        # Create action client for navigation
        self.nav_to_pose_client = ActionClient(
            self,
            NavigateToPose,
            'navigate_to_pose'
        )

        # Create publisher for status
        self.status_pub = self.create_publisher(String, 'nav2_status', 10)

        # Timer for navigation tasks
        self.nav_timer = self.create_timer(5.0, self.navigation_task)

        self.get_logger().info('Custom Nav2 Node initialized')

    def navigation_task(self):
        """Example navigation task"""
        # Check if action server is available
        if not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0):
            self.get_logger().error('NavigateToPose action server not available')
            return

        # Create goal message
        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 = 1.0
        goal_msg.pose.pose.position.y = 1.0
        goal_msg.pose.pose.orientation.w = 1.0

        # Send navigation goal
        future = self.nav_to_pose_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_callback
        )

        future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        """Handle goal response"""
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected')
            return

        self.get_logger().info('Goal accepted')
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        """Handle navigation result"""
        result = future.result().result
        status = String()
        status.data = f'Navigation completed with status: {result}'
        self.status_pub.publish(status)

    def feedback_callback(self, feedback_msg):
        """Handle navigation feedback"""
        feedback = feedback_msg.feedback
        self.get_logger().info(f'Navigation feedback: {feedback}')

def main(args=None):
    rclpy.init(args=args)
    nav_node = CustomNav2Node()

    try:
        rclpy.spin(nav_node)
    except KeyboardInterrupt:
        pass
    finally:
        nav_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Path Planning with Nav2​

Global and Local Planners​

# Example: Custom Path Planner Integration
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Point
from visualization_msgs.msg import Marker, MarkerArray
import numpy as np

class CustomPathPlanner(Node):
    def __init__(self):
        super().__init__('custom_path_planner')

        # Create subscription for goal poses
        self.goal_sub = self.create_subscription(
            PoseStamped,
            'goal_pose',
            self.goal_callback,
            10
        )

        # Create publisher for planned path
        self.path_pub = self.create_publisher(Path, 'plan', 10)

        # Create publisher for visualization
        self.vis_pub = self.create_publisher(MarkerArray, 'path_visualization', 10)

        # Initialize planner
        self.initialize_planner()

    def initialize_planner(self):
        """Initialize the path planner"""
        self.get_logger().info('Custom Path Planner initialized')
        self.map_resolution = 0.05  # meters per cell
        self.map_width = 100  # cells
        self.map_height = 100  # cells

    def goal_callback(self, msg):
        """Handle incoming goal pose"""
        try:
            # Plan path from current position to goal
            path = self.plan_path_to_goal(msg.pose)

            # Publish the planned path
            self.publish_path(path)

            # Visualize the path
            self.visualize_path(path)

        except Exception as e:
            self.get_logger().error(f'Error in path planning: {str(e)}')

    def plan_path_to_goal(self, goal_pose):
        """Plan path to goal using Nav2-compatible approach"""
        # In real implementation, this would use Nav2's planning algorithms
        # For demonstration, we'll create a straight-line path

        # Get current position (in real implementation, get from TF or odometry)
        current_x, current_y = 0.0, 0.0  # Starting position
        goal_x, goal_y = goal_pose.position.x, goal_pose.position.y

        # Create path points
        path = Path()
        path.header.frame_id = 'map'
        path.header.stamp = self.get_clock().now().to_msg()

        # Generate intermediate points (simplified for demo)
        steps = max(10, int(np.sqrt((goal_x - current_x)**2 + (goal_y - current_y)**2) / 0.1))
        for i in range(steps + 1):
            t = i / steps
            x = current_x + t * (goal_x - current_x)
            y = current_y + t * (goal_y - current_y)

            pose_stamped = PoseStamped()
            pose_stamped.header.frame_id = 'map'
            pose_stamped.header.stamp = self.get_clock().now().to_msg()
            pose_stamped.pose.position.x = x
            pose_stamped.pose.position.y = y
            pose_stamped.pose.position.z = 0.0
            pose_stamped.pose.orientation.w = 1.0

            path.poses.append(pose_stamped)

        return path

    def publish_path(self, path):
        """Publish the planned path"""
        self.path_pub.publish(path)

    def visualize_path(self, path):
        """Visualize the path in RViz"""
        marker_array = MarkerArray()

        # Create marker for the path
        path_marker = Marker()
        path_marker.header.frame_id = 'map'
        path_marker.header.stamp = self.get_clock().now().to_msg()
        path_marker.ns = 'planned_path'
        path_marker.id = 0
        path_marker.type = Marker.LINE_STRIP
        path_marker.action = Marker.ADD

        # Set scale
        path_marker.scale.x = 0.05  # Line width

        # Set color (green)
        path_marker.color.r = 0.0
        path_marker.color.g = 1.0
        path_marker.color.b = 0.0
        path_marker.color.a = 1.0

        # Add path points to marker
        for pose_stamped in path.poses:
            point = Point()
            point.x = pose_stamped.pose.position.x
            point.y = pose_stamped.pose.position.y
            point.z = pose_stamped.pose.position.z
            path_marker.points.append(point)

        marker_array.markers.append(path_marker)

        # Create markers for path waypoints
        for i, pose_stamped in enumerate(path.poses[::5]):  # Every 5th point
            waypoint_marker = Marker()
            waypoint_marker.header.frame_id = 'map'
            waypoint_marker.header.stamp = self.get_clock().now().to_msg()
            waypoint_marker.ns = 'waypoints'
            waypoint_marker.id = i
            waypoint_marker.type = Marker.SPHERE
            waypoint_marker.action = Marker.ADD

            # Set position
            waypoint_marker.pose = pose_stamped.pose

            # Set scale
            waypoint_marker.scale.x = 0.1
            waypoint_marker.scale.y = 0.1
            waypoint_marker.scale.z = 0.1

            # Set color (blue)
            waypoint_marker.color.r = 0.0
            waypoint_marker.color.g = 0.0
            waypoint_marker.color.b = 1.0
            waypoint_marker.color.a = 1.0

            marker_array.markers.append(waypoint_marker)

        self.vis_pub.publish(marker_array)

def main(args=None):
    rclpy.init(args=args)
    planner = CustomPathPlanner()

    try:
        rclpy.spin(planner)
    except KeyboardInterrupt:
        pass
    finally:
        planner.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Costmap Integration​

# Example: Costmap processing with Nav2
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker, MarkerArray
import numpy as np

class Nav2CostmapProcessor(Node):
    def __init__(self):
        super().__init__('nav2_costmap_processor')

        # Create subscription for costmap
        self.costmap_sub = self.create_subscription(
            OccupancyGrid,
            'global_costmap/costmap',
            self.costmap_callback,
            10
        )

        # Create publisher for costmap visualization
        self.vis_pub = self.create_publisher(MarkerArray, 'costmap_visualization', 10)

        # Initialize costmap data
        self.costmap_data = None
        self.costmap_info = None

    def costmap_callback(self, msg):
        """Process incoming costmap data"""
        self.costmap_data = np.array(msg.data).reshape((msg.info.height, msg.info.width))
        self.costmap_info = msg.info

        # Process and visualize costmap
        self.process_costmap()
        self.visualize_costmap()

    def process_costmap(self):
        """Process costmap data for navigation"""
        if self.costmap_data is None:
            return

        # Example: Find safe paths (areas with low cost)
        safe_threshold = 50  # Occupancy grid values: 0=free, 100=occupied
        safe_areas = np.where(self.costmap_data < safe_threshold)

        self.get_logger().info(f'Found {len(safe_areas[0])} safe cells in costmap')

        # Example: Find obstacles
        obstacle_threshold = 70
        obstacles = np.where(self.costmap_data > obstacle_threshold)

        self.get_logger().info(f'Found {len(obstacles[0])} obstacle cells in costmap')

    def visualize_costmap(self):
        """Visualize costmap in RViz"""
        if self.costmap_data is None or self.costmap_info is None:
            return

        marker_array = MarkerArray()

        # Create markers for costmap cells
        for y in range(0, self.costmap_info.height, 10):  # Sample every 10th cell for performance
            for x in range(0, self.costmap_info.width, 10):
                cost = self.costmap_data[y, x]

                if cost > 0:  # Only visualize non-free cells
                    marker = Marker()
                    marker.header.frame_id = 'map'
                    marker.header.stamp = self.get_clock().now().to_msg()
                    marker.ns = 'costmap_cells'
                    marker.id = y * self.costmap_info.width + x
                    marker.type = Marker.CUBE
                    marker.action = Marker.ADD

                    # Convert grid coordinates to world coordinates
                    world_x = self.costmap_info.origin.position.x + x * self.costmap_info.resolution
                    world_y = self.costmap_info.origin.position.y + y * self.costmap_info.resolution
                    world_z = self.costmap_info.origin.position.z

                    marker.pose.position.x = world_x
                    marker.pose.position.y = world_y
                    marker.pose.position.z = world_z
                    marker.pose.orientation.w = 1.0

                    # Set scale based on resolution
                    marker.scale.x = self.costmap_info.resolution
                    marker.scale.y = self.costmap_info.resolution
                    marker.scale.z = 0.1  # Height of visualization

                    # Set color based on cost value
                    if cost > 70:  # Obstacle
                        marker.color.r = 1.0
                        marker.color.g = 0.0
                        marker.color.b = 0.0
                    elif cost > 30:  # Inflated obstacle
                        marker.color.r = 1.0
                        marker.color.g = 0.5
                        marker.color.b = 0.0
                    else:  # Free space with some cost
                        marker.color.r = 1.0
                        marker.color.g = 1.0
                        marker.color.b = 0.0

                    marker.color.a = 0.5  # Transparency

                    marker_array.markers.append(marker)

        self.vis_pub.publish(marker_array)

def main(args=None):
    rclpy.init(args=args)
    processor = Nav2CostmapProcessor()

    try:
        rclpy.spin(processor)
    except KeyboardInterrupt:
        pass
    finally:
        processor.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Isaac Sim Nav2 Launch Configuration​

# Example: Isaac Sim with Nav2 integration launch file
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():
    # Launch configuration variables
    use_sim_time = LaunchConfiguration('use_sim_time')
    params_file = LaunchConfiguration('params_file')
    autostart = LaunchConfiguration('autostart')

    # Isaac Sim specific configurations
    isaac_sim_world = LaunchConfiguration('isaac_sim_world', default='small_room')
    robot_name = LaunchConfiguration('robot_name', default='turtlebot3_waffle')

    # Create launch description
    ld = LaunchDescription()

    # Declare launch arguments
    ld.add_action(
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='True',
            description='Use simulation (Isaac Sim) clock if true'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'params_file',
            default_value=PathJoinSubstitution([
                FindPackageShare('nav2_bringup'),
                'params',
                'nav2_params.yaml'
            ]),
            description='Full path to the ROS2 parameters file to use for all launched nodes'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'autostart',
            default_value='True',
            description='Automatically startup the nav2 stack'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'isaac_sim_world',
            default_value='small_room',
            description='Isaac Sim world to load'
        )
    )

    ld.add_action(
        DeclareLaunchArgument(
            'robot_name',
            default_value='turtlebot3_waffle',
            description='Name of the robot to spawn in Isaac Sim'
        )
    )

    # Include Isaac Sim launch (assuming Isaac Sim ROS bridge is available)
    # This would typically be provided by Isaac Sim packages
    """
    isaac_sim_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('isaac_ros_launch'),
                'launch',
                'isaac_sim.launch.py'
            ])
        ]),
        launch_arguments={
            'world': isaac_sim_world,
            'robot_name': robot_name,
            'use_sim_time': use_sim_time
        }.items()
    )
    ld.add_action(isaac_sim_launch)
    """

    # Include Nav2 bringup
    nav2_bringup_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('nav2_bringup'),
                'launch',
                'bringup_launch.py'
            ])
        ]),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'params_file': params_file,
            'autostart': autostart
        }.items()
    )
    ld.add_action(nav2_bringup_launch)

    # Additional nodes specific to Isaac Sim integration
    # Isaac Sim to Nav2 bridge for sensor data
    isaac_sim_bridge = Node(
        package='isaac_ros_launch',
        executable='isaac_ros_bridge',
        name='isaac_sim_bridge',
        parameters=[
            {'use_sim_time': use_sim_time},
            {'sensor_topics': ['/front_camera/image_raw', '/lidar/points']}
        ],
        remappings=[
            ('/front_camera/image_raw', '/camera/image_raw'),
            ('/lidar/points', '/scan')
        ]
    )
    ld.add_action(isaac_sim_bridge)

    # Isaac Sim localization bridge
    localization_bridge = Node(
        package='nav2_localization',
        executable='localization_bridge',
        name='isaac_localization_bridge',
        parameters=[
            {'use_sim_time': use_sim_time}
        ]
    )
    ld.add_action(localization_bridge)

    return ld
# Example: Custom Nav2 Recovery Behaviors
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.action import CancelResponse
from rclpy.action import GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.qos import QoSProfile
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import threading
import time

class Nav2RecoveryNode(Node):
    def __init__(self):
        super().__init__('nav2_recovery_node')

        # Create publisher for velocity commands
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)

        # Create publisher for recovery status
        self.status_pub = self.create_publisher(String, 'recovery_status', 10)

        # Recovery behaviors
        self.recovery_behaviors = {
            'spin': self.spin_recovery,
            'backup': self.backup_recovery,
            'wait': self.wait_recovery
        }

        self.get_logger().info('Nav2 Recovery Node initialized')

    def spin_recovery(self, duration=5.0):
        """Spin recovery behavior"""
        self.get_logger().info(f'Executing spin recovery for {duration} seconds')

        start_time = time.time()
        while time.time() - start_time < duration:
            twist = Twist()
            twist.angular.z = 0.5  # Spin at 0.5 rad/s
            self.cmd_vel_pub.publish(twist)
            time.sleep(0.1)

        # Stop robot
        stop_twist = Twist()
        self.cmd_vel_pub.publish(stop_twist)

        self.get_logger().info('Spin recovery completed')

    def backup_recovery(self, duration=3.0):
        """Backup recovery behavior"""
        self.get_logger().info(f'Executing backup recovery for {duration} seconds')

        start_time = time.time()
        while time.time() - start_time < duration:
            twist = Twist()
            twist.linear.x = -0.2  # Move backward at 0.2 m/s
            self.cmd_vel_pub.publish(twist)
            time.sleep(0.1)

        # Stop robot
        stop_twist = Twist()
        self.cmd_vel_pub.publish(stop_twist)

        self.get_logger().info('Backup recovery completed')

    def wait_recovery(self, duration=2.0):
        """Wait recovery behavior"""
        self.get_logger().info(f'Executing wait recovery for {duration} seconds')

        time.sleep(duration)

        self.get_logger().info('Wait recovery completed')

    def execute_recovery(self, behavior_name, duration=None):
        """Execute a specific recovery behavior"""
        if behavior_name in self.recovery_behaviors:
            # Publish status
            status_msg = String()
            status_msg.data = f'Executing {behavior_name} recovery'
            self.status_pub.publish(status_msg)

            # Execute recovery behavior
            if duration:
                self.recovery_behaviors[behavior_name](duration)
            else:
                self.recovery_behaviors[behavior_name]()

            # Publish completion status
            status_msg.data = f'{behavior_name} recovery completed'
            self.status_pub.publish(status_msg)
        else:
            self.get_logger().error(f'Unknown recovery behavior: {behavior_name}')

def main(args=None):
    rclpy.init(args=args)
    recovery_node = Nav2RecoveryNode()

    # Example: Execute different recovery behaviors
    try:
        # Wait a bit for publishers to be ready
        time.sleep(1.0)

        # Execute a sequence of recovery behaviors
        recovery_node.execute_recovery('spin', 3.0)
        time.sleep(1.0)  # Pause between behaviors
        recovery_node.execute_recovery('backup', 2.0)
        time.sleep(1.0)
        recovery_node.execute_recovery('wait', 1.0)

        # Keep node alive to continue publishing status
        rclpy.spin(recovery_node)

    except KeyboardInterrupt:
        pass
    finally:
        recovery_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Best Practices for Nav2​

1. Parameter Tuning​

  1. Costmap Resolution: Balance between accuracy and performance
  2. Inflation Radius: Set appropriately for robot size and safety margin
  3. Controller Frequency: Match with robot capabilities
  4. Tolerance Values: Set based on application requirements

2. Performance Optimization​

  1. Update Frequencies: Optimize for your robot's capabilities
  2. Sensor Fusion: Combine multiple sensors for robust navigation
  3. Path Smoothing: Apply smoothing to improve robot motion
  4. Recovery Behaviors: Configure appropriate recovery strategies

3. Safety Considerations​

  1. Emergency Stops: Implement emergency stop mechanisms
  2. Safety Controllers: Use velocity limits and collision avoidance
  3. Monitor Systems: Implement navigation monitoring and alerts
  4. Graceful Degradation: Handle sensor failures gracefully

Troubleshooting Common Issues​

1. Navigation Failures​

  • Check costmap inflation parameters
  • Verify sensor data quality and frequency
  • Ensure proper TF tree configuration
  • Validate robot footprint

2. Performance Issues​

  • Monitor CPU and memory usage
  • Check update frequencies
  • Optimize costmap resolution
  • Profile individual components

3. Localization Problems​

  • Verify initial pose estimation
  • Check map quality and coverage
  • Validate sensor calibration
  • Ensure proper coordinate frames

Exercises​

  1. Set up Nav2 with a simulated robot in Isaac Sim
  2. Configure Nav2 parameters for your specific robot
  3. Implement a custom behavior tree for your navigation scenario
  4. Create a recovery behavior for your specific application
  5. Test navigation in various environments and scenarios

Summary​

Navigation2 provides a comprehensive and flexible framework for mobile robot navigation with ROS 2. Its behavior tree architecture, plugin-based design, and lifecycle management make it suitable for a wide range of navigation applications. When integrated with Isaac Sim and Isaac ROS, Nav2 enables sophisticated simulation and testing of navigation systems before deployment on real hardware. Proper configuration and tuning of Nav2 parameters are crucial for achieving reliable and efficient navigation performance.