Latency when subscribing to VSLAM topics.

Background

Hardware: Jetson Orin Nano Devkit 8Gb + Realsense d455
OS: Jetpack 5.1.2 + Isaac VSLAM Docker Image
Launch File:

import launch
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode


def generate_launch_description():
    """Launch file which brings up visual slam node configured for RealSense."""
    realsense_camera_node = Node(
        name='camera',
        namespace='camera',
        package='realsense2_camera',
        executable='realsense2_camera_node',
        parameters=[{
                'enable_infra1': True,
                'enable_infra2': True,
                'enable_color': True,
                'enable_depth': True,
                'clip_distance': 5.0,
                'align_depth.enable': False,
                'depth_module.emitter_enabled': 0,
                'depth_module.profile': '640x360x30',
                'rgb_camera.profile': '848x480x15',
                'enable_gyro': True,
                'enable_accel': True,
                'gyro_fps': 200,
                'accel_fps': 200,
                'unite_imu_method': 2
        }]
    )

    visual_slam_node = ComposableNode(
        name='visual_slam_node',
        package='isaac_ros_visual_slam',
        plugin='isaac_ros::visual_slam::VisualSlamNode',
        parameters=[{
                    'denoise_input_images': False,
                    'rectified_images': True,
                    'enable_debug_mode': False,
                    'debug_dump_path': '/tmp/cuvslam',
                    'enable_slam_visualization': False,
                    'enable_landmarks_view': False,
                    'enable_observations_view': False,
                    'map_frame': 'map',
                    'odom_frame': 'odom',
                    'base_frame': 'camera_link',
                    'input_imu_frame': 'camera_gyro_optical_frame',
                    'enable_imu_fusion': True,
                    'gyro_noise_density': 0.000244,
                    'gyro_random_walk': 0.000019393,
                    'accel_noise_density': 0.001862,
                    'accel_random_walk': 0.003,
                    'calibration_frequency': 200.0,
                    'img_jitter_threshold_ms': 60.0
                    }],
        remappings=[('stereo_camera/left/image', 'camera/infra1/image_rect_raw'),
                    ('stereo_camera/left/camera_info', 'camera/infra1/camera_info'),
                    ('stereo_camera/right/image', 'camera/infra2/image_rect_raw'),
                    ('stereo_camera/right/camera_info', 'camera/infra2/camera_info'),
                    ('visual_slam/imu', 'camera/imu')]
    )

    visual_slam_launch_container = ComposableNodeContainer(
        name='visual_slam_launch_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            visual_slam_node
        ],
        output='screen'
    )

    return launch.LaunchDescription([visual_slam_launch_container, realsense_camera_node])

Issue

I observe inconsistent performance when subscribing to certain topics indicated by the "Delta between current and previous frame [] is above threshold []" and unstable frequency of Isaac VSLAM ros topics. I can consistently trigger this latency by running the launch file above, and then running ros2 topic hz /visual_slam/tracking/odometry. I also (possibly not related) see "Delta between ..." messages when I move the camera around, but this is less consistent. See this video for an example of the issue latency_isaac_vslam.webm.

Attempted Solutions

1. Enabling jetson_clocks. At first I thought this might be clock throttling so I tried running sudo jetson_clocks, and this reduced some of the "Delta between ..." messages. However, the video above was recorded with jetson clocks running so I do not think it's related to clock throttling.

2. Switching to Cyclone DDS. As mentioned in #120, I tried to switch to Cyclone DDS with export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp, but found that Isaac VSLAM topics do not seem to publish anything but others do. For example, /camera/infra1/image_rect_raw has a frequency with ros2 topic hz, but /visual_slam/tracking/odometry has nothing.