Latency when subscribing to VSLAM topics.

[javieryu]

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 Causes/Solutions for "Delta between current and previous frame [] is above threshold []" #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.

[javieryu]

@swapnesh-wani-nvidia Any information on this?

[swapnesh-wani-nvidia]

You are running your Real|Sense D455 camera at 30 fps as indicated here - 'depth_module.profile': '640x360x30',
Could you verify what is the image topic fps you are getting? I can also see that you are enabling the rgb module, try turning it off as well.

[javieryu]

I have the same behavior using the realsense launch file so I don't think it's related to the modified launch file.

Using that launch file I can confirm the same latency behavior with the following setup.

In one terminal attached to the docker container on the Orin run ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py. Wait until everything has initialized.

In a second terminal attached to the docker container on the Orin run ros2 topic hz /camera/infra1/image_rect_raw --window 100. This sometimes triggers a few of the latency "Delta between current ... " messages, but reads 90 Hz +/- 1 Hz consistently.

Finally, in a third terminal attached to the docker container on the Orin run ros2 topic hz /visual_slam/tracking/odometry --window 100. First this starts with a diminished frame rate of 5-20 Hz where we would expect 90 Hz. Terminal 1 (vslam) shows more "Delta between current ..." messages, and Terminal 2 shows no change in frequency from the nominal 90 Hz. After about 2-3 seconds vslam odometry message frequency rises to 90 Hz, and remains stable there.

Hope that clears up the "needs info".

[swapnesh-wani-nvidia]

Please clarify if the odometry topic stays at 90fps (desired). Is this not the expected behavior you are seeing?

[javieryu]

The FPS drops initially then returns to 90Hz stable after several seconds. This is an issue because a variety of different ROS operations cause this drop in framerate to occur. For example, checking the frequency of realsense topics (while attached to the running container) with hz seems to also temporarily drop the slam frequency.

[javieryu]

An update on this issue. Running with intra-process communication instead of separate nodes seems to fix the issue, and generally things seem to run smoother and more reliably with this setup.

Launch file that is tested on the setup in my original comment:

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


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

    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,
            realsense_camera_node
        ],
        output='screen'
    )

    return launch.LaunchDescription([visual_slam_launch_container])