Configure GPS-denied SLAM

Configuring Isaac ROS Visual SLAM (isaac_ros_visual_slam) for indoor, GPS-denied drone navigation requires binding high-frequency stereo image feeds and IMU measurements into GPU-accelerated Visual-Inertial Odometry (VIO). The calculated odometry pose is then relayed over ROS 2 to your flight controller (such as PX4 running EKF2) to maintain position hold and execute trajectories without GPS.

System Dataflow Architecture

 [Stereo Camera + IMU] (e.g., RealSense D435i / ZED X)
           │
           ▼ (Stereo RGB + IMU Topics)
 [Isaac ROS Visual SLAM Container (cuVSLAM)]
           │
           ▼ Output Topic: /visual_slam/tracking/odometry (nav_msgs/Odometry)
 [Transform Node / Coordinate Reprojection]
           │
           ▼ Transforms ENU to NED / FRD
 [Micro-XRCE-DDS / MAVROS Bridge]
           │
           ▼ Topic: /fmu/in/vehicle_visual_odometry
 [PX4 EKF2 Estimator] ──► Enables Offboard Indoor Position Hold

Step 1: Install & Set Up Isaac ROS VSLAM

Run Isaac ROS inside the official isaac_ros_common Docker container on your companion computer (e.g., Jetson Orin) to ensure all CUDA, TensorRT, and VPI hardware accelerations are correctly bound.

Bash

# On host machine: Set up workspace
mkdir -p ~/workspaces/isaac_ros-dev/src
cd ~/workspaces/isaac_ros-dev/src

# Clone Isaac ROS Common & Visual SLAM packages
git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common.git
git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam.git

# Run the Isaac ROS Docker container
cd isaac_ros_common && ./scripts/run_dev.sh

Inside the container, build the package:

Bash

cd /workspaces/isaac_ros-dev
colcon build --symlink-install --packages-up-to isaac_ros_visual_slam
source install/setup.bash

Step 2: Configure Camera & Launch Node

Isaac ROS VSLAM requires hardware-synchronized stereo image streams (left/right) along with IMU data.

1. Launch your camera node (e.g., RealSense or ZED ROS 2 driver) emitting aligned topics:
   • Left Camera: /camera/infra1/image_rect_raw, /camera/infra1/camera_info
   • Right Camera: /camera/infra2/image_rect_raw, /camera/infra2/camera_info
   • IMU: /camera/imu
2. Launch the isaac_ros_visual_slam node configured for high-frequency indoor tracking:

Python

# Save as launch_indoor_vslam.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    vslam_node = Node(
        package='isaac_ros_visual_slam',
        executable='isaac_ros_visual_slam_node',
        name='visual_slam_node',
        parameters=[{
            'denoise_input_images': True,
            'rectified_images': True,
            'enable_imu_fusion': True,         # Crucial for rapid UAV pitch/roll dynamics
            'gyro_noise_density': 0.0002,       # Tune based on camera IMU specs
            'accel_noise_density': 0.003,
            'enable_slam_visualization': True,
            'enable_landmarks_view': True,
            'map_frame': 'map',
            'odom_frame': 'odom',
            'base_frame': 'base_link',
            'num_cameras': 2,
        }],
        remappings=[
            ('stereo_camera/left/image_rect', '/camera/infra1/image_rect_raw'),
            ('stereo_camera/left/camera_info', '/camera/infra1/camera_info'),
            ('stereo_camera/right/image_rect', '/camera/infra2/image_rect_raw'),
            ('stereo_camera/right/camera_info', '/camera/infra2/camera_info'),
            ('visual_slam/imu', '/camera/imu')
        ]
    )
    return LaunchDescription([vslam_node])

Step 3: Map Frames & Coordinate Frames (ROS ENU $\rightarrow$ PX4 NED)

ROS 2 uses ENU (East-North-Up / Forward-Left-Up) coordinate conventions, whereas flight controllers like PX4 use NED (North-East-Down / Forward-Right-Down).

Create a transformer node to convert ROS nav_msgs/msg/Odometry (/visual_slam/tracking/odometry) into PX4’s expected px4_msgs/msg/VehicleVisualOdometry topic:

Python

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from px4_msgs.msg import VehicleVisualOdometry

class VSLAMToPX4Bridge(Node):
    def __init__(self):
        super().__init__('vslam_px4_bridge')
        self.sub = self.create_subscription(
            Odometry, '/visual_slam/tracking/odometry', self.odom_cb, 10)
        self.pub = self.create_publisher(
            VehicleVisualOdometry, '/fmu/in/vehicle_visual_odometry', 10)

    def odom_cb(self, msg: Odometry):
        px4_odom = VehicleVisualOdometry()
        px4_odom.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        px4_odom.timestamp_sample = int(msg.header.stamp.sec * 1e6 + msg.header.stamp.nanosec / 1e3)
        
        # Frame Transformation: ROS ENU to PX4 NED
        # x_ned = y_enu, y_ned = x_enu, z_ned = -z_enu
        px4_odom.pose_frame = VehicleVisualOdometry.POSE_FRAME_NED
        px4_odom.position = [
            float(msg.pose.pose.position.y),
            float(msg.pose.pose.position.x),
            -float(msg.pose.pose.position.z)
        ]
        
        # Quaternion rotation adjustment (ENU -> NED)
        px4_odom.q = [
            float(msg.pose.pose.orientation.w),
            float(msg.pose.pose.orientation.y),
            float(msg.pose.pose.orientation.x),
            -float(msg.pose.pose.orientation.z)
        ]

        self.pub.publish(px4_odom)

def main(args=None):
    rclpy.init(args=args)
    node = VSLAMToPX4Bridge()
    rclpy.spin(node)
    rclpy.shutdown()

Step 4: Configure Flight Controller (PX4 EKF2) Parameters

To instruct PX4 to trust the Visual SLAM odometry coming from Isaac ROS over missing GPS signals, update these PX4 parameters in QGroundControl:

• EKF2_EV_CTRL: Enable Position, Velocity, and Yaw fusion (Set bits 0, 1, 3 $\rightarrow$ 11).
• EKF2_HGT_REF: Set to Vision (or Range Sensor if an LIDAR height sensor is equipped).
• EKF2_EV_DELAY: Set to your onboard visual processing latency (typically 10 to 30 ms for Isaac ROS).
• EKF2_EV_POS_X, Y, Z: Set the physical offset distance (in meters) from the drone’s center of gravity (CG) to the stereo camera focal center.

Tuning Guidelines for Indoor Flight

• Textureless Walls: Plain featureless walls cause visual tracking loss. Enable the camera’s IR emitter/projector (if using Intel RealSense) to project artificial texture onto smooth indoor surfaces.
• Lighting Variations: Set denoise_input_images: True in Isaac ROS parameter settings and lock camera exposure to prevent auto-gain brightness jumps during rapid turns.
• IMU Calibration: Ensure the camera’s internal IMU is calibrated using hardware calibration tools before flying; VIO drift accumulates rapidly if accelerometer biases are inaccurate.

Connecting Isaac ROS Visual SLAM to ROS 2 Nav2 for indoor drone navigation requires an intermediate 3D mapping and collision avoidance layer. Because drones operate in 3D space, standard 2D laser scans are insufficient.

NVIDIA provides isaac_ros_nvblox, a GPU-accelerated 3D Signed Distance Field (TSDF/ESDF) mapping engine. It consumes depth feeds and Visual SLAM poses to build a real-time 3D voxel grid map and projects a 2D/3D costmap into Nav2 for dynamic obstacle avoidance.

End-to-End System Architecture

                       [Stereo Depth Camera + IMU]
                                │          │
           ┌────────────────────┘          └──────────────────┐
           ▼ (Stereo RGB + IMU)                               ▼ (Depth / Point Cloud)
 [isaac_ros_visual_slam]                             [isaac_ros_nvblox]
           │                                                  │
           ▼ Pose (/tf: odom -> base_link)                    ▼ 3D Voxel Grid & ESDF Map
           └────────────────────┬─────────────────────────────┘
                                │
                                ▼ Costmap & Distance Maps
                          [ROS 2 Nav2]
                                │
                                ▼ Trajectory Commands (cmd_vel / Setpoints)
                  [Coordinate Transformer (ENU->NED)]
                                │
                                ▼
                       [PX4 Flight Controller]

Step 1: Install NVIDIA isaac_ros_nvblox

Inside your Isaac ROS Docker container on your Jetson or x86 GPU workspace:

Bash

cd /workspaces/isaac_ros-dev/src
git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox.git

# Build workspace with nvblox
cd /workspaces/isaac_ros-dev
colcon build --symlink-install --packages-up-to isaac_ros_nvblox isaac_ros_visual_slam
source install/setup.bash

Step 2: Configure Launch File (VSLAM + Nvblox Integration)

Create a unified ROS 2 launch file (drone_nav_pipeline.launch.py) that feeds Visual SLAM odometry and Depth images directly into Nvblox.

Python

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # 1. Isaac ROS Visual SLAM Node
    vslam_node = Node(
        package='isaac_ros_visual_slam',
        executable='isaac_ros_visual_slam_node',
        name='visual_slam',
        parameters=[{
            'tracking_mode': 1,               # VIO mode (Visual + IMU)
            'rectified_images': True,
            'map_frame': 'map',
            'odom_frame': 'odom',
            'base_frame': 'base_link',
            'publish_tf': True,               # Publishes map -> odom -> base_link TF
        }],
        remappings=[
            ('stereo_camera/left/image_rect', '/camera/infra1/image_rect_raw'),
            ('stereo_camera/left/camera_info', '/camera/infra1/camera_info'),
            ('stereo_camera/right/image_rect', '/camera/infra2/image_rect_raw'),
            ('stereo_camera/right/camera_info', '/camera/infra2/camera_info'),
            ('visual_slam/imu', '/camera/imu')
        ]
    )

    # 2. Isaac ROS Nvblox Node (3D Scene Reconstruction & Costmap Generation)
    nvblox_node = Node(
        package='isaac_ros_nvblox',
        executable='nvblox_node',
        name='nvblox_node',
        parameters=[{
            'global_frame': 'map',
            'voxel_size': 0.05,               # 5cm voxel resolution for indoor flight
            'use_tf_transforms': True,
            
            # Distance field settings for obstacle avoidance
            'esdf_mode': '3d',                # Set to 3D for full spatial drone clearance
            'esdf_slice_height': 0.0,         # Slices grid at drone altitude
            'esdf_slice_min_height': -1.0,    # Vertical clearance below camera
            'esdf_slice_max_height': 1.0,     # Vertical clearance above camera
            'map_clearing_radius_m': 5.0,
        }],
        remappings=[
            ('depth/image', '/camera/depth/image_rect_raw'),
            ('depth/camera_info', '/camera/depth/camera_info'),
            ('color/image', '/camera/color/image_raw'),
            ('color/camera_info', '/camera/color/camera_info'),
        ]
    )

    return LaunchDescription([vslam_node, nvblox_node])

Step 3: Configure Nav2 Costmaps for Drone Flight

Configure Nav2 to consume Nvblox’s dynamic distance map output (/nvblox_node/map_slice_pointcloud or static occupancy). In your nav2_params.yaml:

YAML

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: false
      robot_radius: 0.35  # Adjust according to physical drone diameter
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: nvblox_cloud
        nvblox_cloud:
          topic: /nvblox_node/map_slice_pointcloud  # Consumes live 3D sliced ESDF point cloud
          max_obstacle_height: 2.5
          min_obstacle_height: 0.1
          clearing: True
          marking: True
          data_type: "PointCloud2"

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 6
      height: 6
      resolution: 0.05
      robot_radius: 0.35
      plugins: ["obstacle_layer", "inflation_layer"]
      
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: nvblox_cloud
        nvblox_cloud:
          topic: /nvblox_node/map_slice_pointcloud
          clearing: True
          marking: True
          data_type: "PointCloud2"

Step 4: Bridge Nav2 Output to PX4 Drone Flight Controller

Nav2 outputs 2D/3D linear velocity trajectories (geometry_msgs/msg/Twist on topic /cmd_vel). Convert these planar and z-velocity commands into PX4 Trajectory Setpoints (px4_msgs/msg/TrajectorySetpoint) over Micro-XRCE-DDS.

Python

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from px4_msgs.msg import TrajectorySetpoint, OffboardControlMode

class Nav2ToPX4DroneBridge(Node):
    def __init__(self):
        super().__init__('nav2_px4_bridge')
        
        # Subscribe to Nav2 output
        self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_cb, 10)
        
        # Publish to PX4 Micro-XRCE-DDS
        self.setpoint_pub = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10)
        self.offboard_mode_pub = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', 10)

        # 20 Hz Heartbeat required by PX4 Offboard Mode
        self.create_timer(0.05, self.publish_offboard_heartbeat)
        
        self.vx = 0.0
        self.vy = 0.0
        self.vz = 0.0
        self.yaw_rate = 0.0

    def cmd_vel_cb(self, msg: Twist):
        # Convert ROS ENU velocities to PX4 NED frame
        # ROS: x = forward, y = left, z = up
        # PX4: x = forward, y = right, z = down
        self.vx = float(msg.linear.x)
        self.vy = -float(msg.linear.y)
        self.vz = -float(msg.linear.z)
        self.yaw_rate = -float(msg.angular.z)

    def publish_offboard_heartbeat(self):
        # 1. Heartbeat
        offboard_msg = OffboardControlMode()
        offboard_msg.position = False
        offboard_msg.velocity = True
        offboard_msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.offboard_mode_pub.publish(offboard_msg)

        # 2. Velocity Setpoint Target sent to PX4 Flight Controller
        sp = TrajectorySetpoint()
        sp.velocity = [self.vx, self.vy, self.vz]
        sp.yawspeed = self.yaw_rate
        sp.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.setpoint_pub.publish(sp)

def main(args=None):
    rclpy.init(args=args)
    node = Nav2ToPX4DroneBridge()
    rclpy.spin(node)
    rclpy.shutdown()

Step 5: Verify the Navigation Stack

1. Launch Isaac ROS Stack:Bashros2 launch drone_bringup drone_nav_pipeline.launch.py
2. Launch Nav2 Stack:Bashros2 launch nav2_bringup navigation_launch.py params_file:=/path/to/nav2_params.yaml
3. Inspect in RViz 2:
   • Add NvbloxMesh plugin to view the live 3D GPU-reconstructed map.
   • Add Map and Costmap plugins listening to /global_costmap/costmap and /local_costmap/costmap.
   • Send a 2D/3D Navigation Goal from RViz to verify trajectory execution.

Handling Visual SLAM (VIO) tracking loss during indoor flight is critical to prevent flyaways or uncontrolled drifts. Because GPS is unavailable indoors, losing vision data means PX4’s EKF2 estimator will immediately lose its primary position reference.

Configuring robust failsafes requires a two-tiered strategy: PX4 Estimator Failsafes (to handle loss of visual odometry gracefully) and ROS 2 Watchdog Monitoring (to trigger safe landing procedures if communication drops).

Phase 1: PX4 EKF2 Estimator Failsafe Parameters

Configure these parameters in QGroundControl to determine how PX4 reacts when vision data stops or becomes corrupted:

1. COM_ARM_WO_GPS → Allowed (1)Allows arming without GPS using vision/optical flow.
2. COM_POS_FS_DELAY → 1.0 (seconds)Sets the grace period PX4 waits when position data is lost before triggering an automatic position failsafe action.
3. COM_POSCTL_NAVL → Land (1) or Altitude Control (0)Determines what flight mode PX4 switches to if position estimation fails in Position/Offboard mode:
   • 0 (Altitude/ALTTCTL): Drone maintains altitude using a barometer or LiDAR rangefinder, requiring manual stick commands for horizontal control.
   • 1 (Land Mode): Recommended indoors. The drone immediately descends straight down at a safe rate.
4. EKF2_EV_NOISE_MD → 1Enables dynamic measurement noise weighting. If Isaac ROS reports high uncertainty, PX4 gradually reduces vision data weight rather than dropping state estimation abruptly.
5. EKF2_REQ_EPH → 0.5 – 1.0 (meters)Sets the maximum allowed horizontal position uncertainty from the vision system before EKF2 marks the visual estimate as invalid and triggers a position failover.

Phase 2: Hardware Backup (Laser Rangefinder / Downward LiDAR)

When VSLAM fails, optical height estimation fails along with horizontal positioning. To prevent sudden altitude drops:

• Equip a small downward-facing distance sensor (e.g., TFmini, LightWare SF11/C, or TeraRanger).
• Set EKF2_HGT_REF → Range sensor (or set EKF2_RNG_CTRL → Enabled).
• If VSLAM tracking dies, PX4 retains accurate ground-clearance measurement and lands smoothly without relying on visual depth.

Phase 3: ROS 2 Heartbeat & Vision Watchdog Node

If isaac_ros_visual_slam freezes or stops publishing frames, PX4 will continue using the last received setpoint unless setpoints stop arriving. A ROS 2 watchdog node monitors the /visual_slam/tracking/odometry topic frequency and actively revokes OFFBOARD mode or triggers a landing command if visual tracking drops.

Python

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from px4_msgs.msg import VehicleCommand

class VSLAMWatchdog(Node):
    def __init__(self):
        super().__init__('vslam_watchdog_node')

        # Subscribe to Isaac ROS VSLAM output
        self.sub_odom = self.create_subscription(
            Odometry, '/visual_slam/tracking/odometry', self.odom_callback, 10)

        # Publisher for PX4 Vehicle Commands
        self.cmd_pub = self.create_publisher(
            VehicleCommand, '/fmu/in/vehicle_command', 10)

        self.last_vslam_time = self.get_clock().now()
        self.vslam_timeout_sec = 0.5  # Trigger land if no VSLAM for >500ms
        self.failsafe_triggered = False

        # Watchdog loop running at 20 Hz
        self.create_timer(0.05, self.watchdog_check)

    def odom_callback(self, msg: Odometry):
        # Update timestamp whenever a valid pose is received
        self.last_vslam_time = self.get_clock().now()

    def watchdog_check(self):
        time_since_last_vslam = (self.get_clock().now() - self.last_vslam_time).nanoseconds / 1e9

        if time_since_last_vslam > self.vslam_timeout_sec and not self.failsafe_triggered:
            self.get_logger().error(
                f"VSLAM Tracking LOST for {time_since_last_vslam:.2f}s! Emergency LAND command sent."
            )
            self.trigger_px4_land()
            self.failsafe_triggered = True

    def trigger_px4_land(self):
        # Send NAV_LAND command directly to PX4
        cmd = VehicleCommand()
        cmd.command = VehicleCommand.VEHICLE_CMD_NAV_LAND
        cmd.param1 = 0.0
        cmd.param2 = 0.0
        cmd.target_system = 1
        cmd.target_component = 1
        cmd.from_external = True
        cmd.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        self.cmd_pub.publish(cmd)

def main(args=None):
    rclpy.init(args=args)
    node = VSLAMWatchdog()
    rclpy.spin(node)
    rclpy.shutdown()

Indoor Safety Checklist Summary

Potential Failure Point	Recommended Configuration / Mitigation
Complete VSLAM Node Crash	Watchdog node detects missing topics within 500 ms and issues VEHICLE_CMD_NAV_LAND.
Featureless Surface Drift	EKF2_REQ_EPH invalidates position when accuracy drops >1.0 m; PX4 switches to Land mode.
Loss of Altitude Estimation	Downward LiDAR rangefinder handles height sensing independently of VSLAM.
Offboard Node Disconnection	PX4 COM_OF_LOSS_T switches mode automatically if offboard setpoint heartbeat stops for >0.5 s.