Latency when subscribing to VSLAM topics.

javieryu commented 8 months ago

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.

javieryu commented 4 months ago

@swapnesh-wani-nvidia Any information on this?

swapnesh-wani-nvidia commented 4 months ago

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 commented 4 months ago

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 commented 4 months ago

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

javieryu commented 4 months ago

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 commented 2 months ago

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

NamTruongTran commented 1 week ago

@javieryu

Did you only change the launch file?

I have exactly the same problem. Running ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py causes the warning "Delta between current and previous frame [] is above threshold []".

The warning still appears and outputs the message very frequently after running the command above.

Thank you!

javieryu commented 1 week ago

@NamTruongTran

Did you try the launch file above? Using intra-process comms doesn't appear to completely fix the issue, but the overall performance of the package does seem to improve. However, annoyingly using compressed image topics appears to crash the realsense node when using intra-process communication so I really only use the intra-process launch file when I don't need to stream images to remote devices.

In general those messages seem to be coupled to latency from ROS communication, and so your issues could also be related to that as well.

Sorry if that's not a great response, but if you post a bit more about your setup it could help to clarify the issue.

NamTruongTran commented 1 week ago

Thank you for your response @javieryu ! I really appreciate your time.

My setup: -Jetson AGX Xavier with a D455 -D455 Firmware version : 5.13.0.50 -RealSense camera ROS driver version (ROS Wrapper) : 4.51.1 -SDK Version : 2.51.1

This is my situation: I trying to set up Isaac visual slam with Realsense D455.

I set up my realsense camera and the Isaac ROS docker container: https://web.archive.org/web/20240226211629/https://nvidia-isaac-ros.github.io/getting_started/hardware_setup/sensors/realsense_setup.html
I set up Isaac visual slam: https://web.archive.org/web/20240226202321/https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_visual_slam/isaac_ros_visual_slam/index.html#quickstart
I set up Isaac visual slam: https://web.archive.org/web/20240226202321/https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_visual_slam/isaac_ros_visual_slam/index.html#quickstart
I set up Visual SLAM with a RealSense Camera and Integrated IMU: https://web.archive.org/web/20240226195409/https://nvidia-isaac-ros.github.io/concepts/visual_slam/cuvslam/tutorial_realsense.html

In step 4 I launched: ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py and tried to output different frequency:

ros2 topic hz /camera/infra1/image_rect_raw --window 20
ros2 topic hz /camera/imu --window 20
ros2 topic hz /visual_slam/tracking/odometry --window 20

Unfortunately, none of these topics showed any messages, although there was a publisher. The launch command continuously showed this error message: WARNING [139667063494400] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11

After some research, I found these approaches: https://github.com/IntelRealSense/realsense-ros/issues/3121 https://github.com/IntelRealSense/realsense-ros/issues/3121 I thought it was a mismatch problem. At that time, my setup was: -D455 Firmware version : 5.16.0.1 -RealSense camera ROS driver version (ROS Wrapper) : 4.51.1 -SDK Version : 2.55.1 So, I downgraded to: -D455 Firmware version : 5.13.0.50 -RealSense camera ROS driver version (ROS Wrapper) : 4.51.1 -SDK Version : 2.51.1

After the downgrade, all three topics published messages, but I continuously received the warning message: Delta between current and previous frame [] is above threshold [] I found your solution and changed my launch file as follows:

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': False,
                'enable_depth': False,
                'depth_module.emitter_enabled': 0,
                'depth_module.profile': '640x360x30',
                '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='nvidia::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': True,
                     'enable_landmarks_view': True,
                     'enable_observations_view': True,
                     '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.00
                    ],
                    }],
        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])

The warning message not showing up anymore: Delta between current and previous frame [] is above threshold [] no longer appears, and all three topics are still publishing messages. However, the error message: WARNING [139667063494400] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11 has come back and continues to appear.

Thank you for any advice or help !

javieryu commented 1 week ago

and all three topics are still publishing messages.

So do you mean that all of the realsense topics are publishing or that SLAM is also working? I do also see that resource error from time to time, but I haven't found it to be an indication of any noticeable performance issues.

NVIDIA-ISAAC-ROS / isaac_ros_visual_slam