Isaac_ros_foundationpose running on the jetson NX 16G is too SLOW

Autonomous Machines Robotics - Isaac Isaac ROS
1

I have already deployed isaac_ros_foundationpose using Docker on Jetson NX 16G. I have tested two launch. py files, which are:
isaac_ros_foundationpose_orbbec.launch.py ----(from isaac_ros_foundationpose_realsense.launch.py)

import os
from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode

# Number of Realsense messages to be dropped in 1 second
HAWK_EXPECT_FREQ = 10 #28
# Expected number of Realsense messages in 1 second
INPUT_IMAGES_DROP_FREQ = 30

RT_DETR_MODEL_INPUT_SIZE = 640  # RT-DETR models expect 640x640 encoded image size
RT_DETR_MODEL_NUM_CHANNELS = 3  # RT-DETR models expect 3 image channels

REALSENSE_IMAGE_WIDTH = 640
REALSENSE_IMAGE_HEIGHT = 480

VISUALIZATION_DOWNSCALING_FACTOR = 10

REALSENSE_TO_RT_DETR_RATIO = REALSENSE_IMAGE_WIDTH / RT_DETR_MODEL_INPUT_SIZE

REFINE_MODEL_PATH = '/tmp/refine_model.onnx'
SCORE_MODEL_PATH = '/tmp/score_model.onnx'
MESH_FILE_PATH = '/data/AR-Code-Object-Capture-app-1730803003.obj'
TEXTURE_MAP_PATH = '/data/baked_mesh_34b42ceb_tex0.png'
REFINE_ENGINE_PATH = '/data/isaac_ros-dev/isaac_ros_assets/models/foundationpose/refine_trt_engine.plan'
SCORE_ENGINE_PATH = '/data/isaac_ros-dev/isaac_ros_assets/models/foundationpose/score_trt_engine.plan'

def generate_launch_description():
    """Generate launch description for testing relevant nodes."""
    rviz_config_path = os.path.join(
        get_package_share_directory('isaac_ros_foundationpose'),
        'rviz', 'foundationpose_realsense.rviz')

    launch_args = [
        DeclareLaunchArgument(
            'hawk_expect_freq',
            default_value=str(HAWK_EXPECT_FREQ),
            description='Number of Realsense messages to be dropped in 1 second'),

        DeclareLaunchArgument(
            'input_images_drop_freq',
            default_value=str(INPUT_IMAGES_DROP_FREQ),
            description='Expected number of Realsense messages in 1 second'),

        DeclareLaunchArgument(
            'mesh_file_path',
            default_value=MESH_FILE_PATH,
            description='The absolute file path to the mesh file'),

        DeclareLaunchArgument(
            'texture_path',
            default_value=TEXTURE_MAP_PATH,
            description='The absolute file path to the texture map'),

        DeclareLaunchArgument(
            'refine_model_file_path',
            default_value=REFINE_MODEL_PATH,
            description='The absolute file path to the refine model'),

        DeclareLaunchArgument(
            'refine_engine_file_path',
            default_value=REFINE_ENGINE_PATH,
            description='The absolute file path to the refine trt engine'),

        DeclareLaunchArgument(
            'score_model_file_path',
            default_value=SCORE_MODEL_PATH,
            description='The absolute file path to the score model'),

        DeclareLaunchArgument(
            'score_engine_file_path',
            default_value=SCORE_ENGINE_PATH,
            description='The absolute file path to the score trt engine'),

        DeclareLaunchArgument(
            'rt_detr_model_file_path',
            default_value='',
            description='The absolute file path to the RT-DETR ONNX file'),

        DeclareLaunchArgument(
            'rt_detr_engine_file_path',
            default_value='',
            description='The absolute file path to the RT-DETR TensorRT engine file'),

        DeclareLaunchArgument(
            'launch_rviz',
            default_value='False',
            description='Flag to enable Rviz2 launch'),

        DeclareLaunchArgument(
            'container_name',
            default_value='foundationpose_container',
            description='Name for ComposableNodeContainer'),
    ]

    hawk_expect_freq = LaunchConfiguration('hawk_expect_freq')
    input_images_drop_freq = LaunchConfiguration('input_images_drop_freq')
    mesh_file_path = LaunchConfiguration('mesh_file_path')
    texture_path = LaunchConfiguration('texture_path')
    refine_model_file_path = LaunchConfiguration('refine_model_file_path')
    refine_engine_file_path = LaunchConfiguration('refine_engine_file_path')
    score_model_file_path = LaunchConfiguration('score_model_file_path')
    score_engine_file_path = LaunchConfiguration('score_engine_file_path')
    rt_detr_model_file_path = LaunchConfiguration('rt_detr_model_file_path')
    rt_detr_engine_file_path = LaunchConfiguration('rt_detr_engine_file_path')
    launch_rviz = LaunchConfiguration('launch_rviz')
    container_name = LaunchConfiguration('container_name')

    # Drops hawk_expect_freq out of input_images_drop_freq RealSense messages
    drop_node = ComposableNode(
        name='drop_node',
        package='isaac_ros_nitros_topic_tools',
        plugin='nvidia::isaac_ros::nitros::NitrosCameraDropNode',
        parameters=[{
            'X': hawk_expect_freq,
            'Y': input_images_drop_freq,
            'mode': 'mono+depth',
            'depth_format_string': 'nitros_image_mono16'
        }],
        remappings=[
            ('image_1', '/weng_camera/color/image_raw'),
            ('camera_info_1', '/weng_camera/color/camera_info'),
            ('depth_1', '/weng_camera/depth/image_raw'),
            ('image_1_drop', 'rgb/image_rect_color'),
            ('camera_info_1_drop', 'rgb/camera_info'),
            ('depth_1_drop', 'depth_uint16'),
        ]
    )

    # Realsense depth is in uint16 and millimeters. Convert to float32 and meters
    convert_metric_node = ComposableNode(
        package='isaac_ros_depth_image_proc',
        plugin='nvidia::isaac_ros::depth_image_proc::ConvertMetricNode',
        remappings=[
            ('image_raw', 'depth_uint16'),
            ('image', 'depth_image')
        ]
    )

    foundationpose_node = ComposableNode(
        name='foundationpose_node',
        package='isaac_ros_foundationpose',
        plugin='nvidia::isaac_ros::foundationpose::FoundationPoseNode',
        parameters=[{
            'mesh_file_path': mesh_file_path,
            'texture_path': texture_path,

            'refine_model_file_path': refine_model_file_path,
            'refine_engine_file_path': refine_engine_file_path,
            'refine_input_tensor_names': ['input_tensor1', 'input_tensor2'],
            'refine_input_binding_names': ['input1', 'input2'],
            'refine_output_tensor_names': ['output_tensor1', 'output_tensor2'],
            'refine_output_binding_names': ['output1', 'output2'],

            'score_model_file_path': score_model_file_path,
            'score_engine_file_path': score_engine_file_path,
            'score_input_tensor_names': ['input_tensor1', 'input_tensor2'],
            'score_input_binding_names': ['input1', 'input2'],
            'score_output_tensor_names': ['output_tensor'],
            'score_output_binding_names': ['output1'],
        }],
        remappings=[
            ('pose_estimation/depth_image', 'depth_image'),
            ('pose_estimation/image', 'rgb/image_rect_color'),
            ('pose_estimation/camera_info', 'rgb/camera_info'),
            ('pose_estimation/segmentation', 'weng_mask'),
            ('pose_estimation/output', 'output')])

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_path],
        condition=IfCondition(launch_rviz))

    foundationpose_container = ComposableNodeContainer(
        name=container_name,
        namespace='',
        package='rclcpp_components',
        executable='component_container_mt',
        composable_node_descriptions=[
            # realsense_node,
            drop_node,
            convert_metric_node,
            foundationpose_node,
            # resize_left_viz
        ],
        output='screen'
    )

    return launch.LaunchDescription(launch_args + [foundationpose_container,
                                                   rviz_node])

isaac_ros_foundationpose_orbbec_tracking.launch.py ---- (from isaac_ros_foundationpose_realsense_tracking.launch.py)

import os
from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode

# Number of Realsense messages to be dropped in 1 second
HAWK_EXPECT_FREQ = 6

# Expected number of Realsense messages in 1 second
INPUT_IMAGES_DROP_FREQ = 30

RT_DETR_MODEL_INPUT_SIZE = 640  # RT-DETR models expect 640x640 encoded image size
RT_DETR_MODEL_NUM_CHANNELS = 3  # RT-DETR models expect 3 image channels

REALSENSE_IMAGE_WIDTH = 640
REALSENSE_IMAGE_HEIGHT = 480

VISUALIZATION_DOWNSCALING_FACTOR = 10

REALSENSE_TO_RT_DETR_RATIO = REALSENSE_IMAGE_WIDTH / RT_DETR_MODEL_INPUT_SIZE

REFINE_MODEL_PATH = '/tmp/refine_model.onnx'
SCORE_MODEL_PATH = '/tmp/score_model.onnx'
MESH_FILE_PATH = '/data/AR-Code-Object-Capture-app-1730803003.obj'
TEXTURE_MAP_PATH = '/data/baked_mesh_34b42ceb_tex0.png'
REFINE_ENGINE_PATH = '/data/isaac_ros-dev/isaac_ros_assets/models/foundationpose/refine_trt_engine.plan'
SCORE_ENGINE_PATH = '/data/isaac_ros-dev/isaac_ros_assets/models/foundationpose/score_trt_engine.plan'


def generate_launch_description():
    """Generate launch description for testing relevant nodes."""
    rviz_config_path = os.path.join(
        get_package_share_directory('isaac_ros_foundationpose'),
        'rviz', 'foundationpose_realsense.rviz')

    launch_args = [
        DeclareLaunchArgument(
            'hawk_expect_freq',
            default_value=str(HAWK_EXPECT_FREQ),
            description='Number of Realsense messages to be dropped in 1 second'),

        DeclareLaunchArgument(
            'input_images_drop_freq',
            default_value=str(INPUT_IMAGES_DROP_FREQ),
            description='Expected number of Realsense messages in 1 second'),

        DeclareLaunchArgument(
            'mesh_file_path',
            default_value=MESH_FILE_PATH,
            description='The absolute file path to the mesh file'),

        DeclareLaunchArgument(
            'texture_path',
            default_value=TEXTURE_MAP_PATH,
            description='The absolute file path to the texture map'),

        DeclareLaunchArgument(
            'refine_model_file_path',
            default_value=REFINE_MODEL_PATH,
            description='The absolute file path to the refine model'),

        DeclareLaunchArgument(
            'refine_engine_file_path',
            default_value=REFINE_ENGINE_PATH,
            description='The absolute file path to the refine trt engine'),

        DeclareLaunchArgument(
            'score_model_file_path',
            default_value=SCORE_MODEL_PATH,
            description='The absolute file path to the score model'),

        DeclareLaunchArgument(
            'score_engine_file_path',
            default_value=SCORE_ENGINE_PATH,
            description='The absolute file path to the score trt engine'),

        DeclareLaunchArgument(
            'rt_detr_model_file_path',
            default_value='',
            description='The absolute file path to the RT-DETR ONNX file'),

        DeclareLaunchArgument(
            'rt_detr_engine_file_path',
            default_value='',
            description='The absolute file path to the RT-DETR TensorRT engine file'),

        DeclareLaunchArgument(
            'launch_rviz',
            default_value='False',
            description='Flag to enable Rviz2 launch'),

        DeclareLaunchArgument(
            'container_name',
            default_value='foundationpose_container',
            description='Name for ComposableNodeContainer'),
    ]

    hawk_expect_freq = LaunchConfiguration('hawk_expect_freq')
    input_images_drop_freq = LaunchConfiguration('input_images_drop_freq')
    mesh_file_path = LaunchConfiguration('mesh_file_path')
    texture_path = LaunchConfiguration('texture_path')
    refine_model_file_path = LaunchConfiguration('refine_model_file_path')
    refine_engine_file_path = LaunchConfiguration('refine_engine_file_path')
    score_model_file_path = LaunchConfiguration('score_model_file_path')
    score_engine_file_path = LaunchConfiguration('score_engine_file_path')
    rt_detr_model_file_path = LaunchConfiguration('rt_detr_model_file_path')
    rt_detr_engine_file_path = LaunchConfiguration('rt_detr_engine_file_path')
    launch_rviz = LaunchConfiguration('launch_rviz')
    container_name = LaunchConfiguration('container_name')

    # Drops hawk_expect_freq out of input_images_drop_freq RealSense messages
    drop_node = ComposableNode(
        name='drop_node',
        package='isaac_ros_nitros_topic_tools',
        plugin='nvidia::isaac_ros::nitros::NitrosCameraDropNode',
        parameters=[{
            'X': hawk_expect_freq, 
            'Y': input_images_drop_freq, 
            'mode': 'mono+depth', 
            'depth_format_string': 'nitros_image_mono16' 
        }],
        remappings=[
            ('image_1', '/weng_camera/color/image_raw'),
            ('camera_info_1', '/weng_camera/color/camera_info'),
            ('depth_1', '/weng_camera/depth/image_raw'),
            ('image_1_drop', 'rgb/image_rect_color'),
            ('camera_info_1_drop', 'rgb/camera_info'),
            ('depth_1_drop', 'depth_uint16'),
        ]
    )

    # Realsense depth is in uint16 and millimeters. Convert to float32 and meters
    convert_metric_node = ComposableNode(
        package='isaac_ros_depth_image_proc',
        plugin='nvidia::isaac_ros::depth_image_proc::ConvertMetricNode',
        remappings=[
            ('image_raw', 'depth_uint16'),
            ('image', 'depth_image')
        ]
    )

    # Launch isaac_ros_foundationpose
    selector_node = ComposableNode(
        name='selector_node',
        package='isaac_ros_foundationpose',
        plugin='nvidia::isaac_ros::foundationpose::Selector',
        parameters=[{
            'reset_period': 20000
        }],
        remappings=[
            ('depth_image', 'depth_image'),
            ('image', 'rgb/image_rect_color'),
            ('camera_info', 'rgb/camera_info'),
            ('segmentation', 'weng_mask')
            ])

    foundationpose_node = ComposableNode(
        name='foundationpose_node',
        package='isaac_ros_foundationpose',
        plugin='nvidia::isaac_ros::foundationpose::FoundationPoseNode',
        parameters=[{
            'mesh_file_path': mesh_file_path,
            'texture_path': texture_path,

            'refine_model_file_path': refine_model_file_path,
            'refine_engine_file_path': refine_engine_file_path,
            'refine_input_tensor_names': ['input_tensor1', 'input_tensor2'],
            'refine_input_binding_names': ['input1', 'input2'],
            'refine_output_tensor_names': ['output_tensor1', 'output_tensor2'],
            'refine_output_binding_names': ['output1', 'output2'],

            'score_model_file_path': score_model_file_path,
            'score_engine_file_path': score_engine_file_path,
            'score_input_tensor_names': ['input_tensor1', 'input_tensor2'],
            'score_input_binding_names': ['input1', 'input2'],
            'score_output_tensor_names': ['output_tensor'],
            'score_output_binding_names': ['output1'],
        }])

    foundationpose_tracking_node = ComposableNode(
        name='foundationpose_tracking_node',
        package='isaac_ros_foundationpose',
        plugin='nvidia::isaac_ros::foundationpose::FoundationPoseTrackingNode',
        parameters=[{
            'mesh_file_path': mesh_file_path,
            'texture_path': texture_path,

            'refine_model_file_path': refine_model_file_path,
            'refine_engine_file_path': refine_engine_file_path,
            'refine_input_tensor_names': ['input_tensor1', 'input_tensor2'],
            'refine_input_binding_names': ['input1', 'input2'],
            'refine_output_tensor_names': ['output_tensor1', 'output_tensor2'],
            'refine_output_binding_names': ['output1', 'output2'],
        }])
    foundationpose_container = ComposableNodeContainer(
        name=container_name,
        namespace='',
        package='rclcpp_components',
        executable='component_container_mt',
        composable_node_descriptions=[
            # realsense_node,
            drop_node,
            convert_metric_node,
            selector_node,
            foundationpose_node,
            foundationpose_tracking_node,
        ],
        output='screen'
    )

    return launch.LaunchDescription(launch_args + [foundationpose_container])

Jetson NX 16G:

20241129102000
202411291020002071×1253 104 KB

when running isaac_ros_foundationpose_orbbec.launch.py

ros2 topic hz  /output

average rate: 0.369
        min: 0.001s max: 5.962s std dev: 0.35987s window: 890
average rate: 0.369
        min: 0.001s max: 5.962s std dev: 0.35967s window: 891
average rate: 0.369
        min: 0.001s max: 5.962s std dev: 0.35947s window: 892
average rate: 0.369
        min: 0.001s max: 5.962s std dev: 0.35937s window: 893
average rate: 0.369
        min: 0.001s max: 5.962s std dev: 0.35929s window: 894

isaac_ros_foundationpose_orbbec_tracking.launch.py

ros2 topic hz  /tracking/output

average rate: 2.758
        min: 0.001s max: 2606.389s std dev: 26.06432s window: 10000
average rate: 2.758
        min: 0.001s max: 2606.389s std dev: 26.06432s window: 10000
average rate: 2.759
        min: 0.001s max: 2606.389s std dev: 26.06431s window: 10000
average rate: 2.758
        min: 0.001s max: 2606.389s std dev: 26.06432s window: 10000
average rate: 2.759
        min: 0.001s max: 2606.389s std dev: 26.06431s window: 10000

No matter which launch file is used to start, there is a huge delay in max time. I would like to know which parameters need to be adjusted to avoid delay, and I hope to achieve a stable 5FPS.