Rectify node not producing any output using USB Camera for Stereo Disparity

I am using a ELP Stereo USB Camera on JetPack 5.1.2 on a Xavier NX Developer Kit.
As it’s a USB Camera with 1280 x 480 feed, I had to write my own ROS code using OpenCV and cv_bridge in C++ to split the video frame in half send camera info for each camera seperately. I have also verified (as best as I can), that all /left/image_raw, /right/image_raw, /left/camerainfo and /right/camerainfo have the same timestamps.

I have modified the default isaac_ros_argus_ess.launch.py to customize to my needs by changing the source of the images.

Here’s the code:

# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# SPDX-License-Identifier: Apache-2.0

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


def generate_launch_description():
    launch_args = [
        DeclareLaunchArgument(
            'engine_file_path',
            default_value='',
            description='The absolute path to the ESS engine plan.'),
        DeclareLaunchArgument(
            'threshold',
            default_value='0.9',
            description='Threshold value ranges between 0.0 and 1.0 '
                        'for filtering disparity with confidence.'),
    ]
    engine_file_path = LaunchConfiguration('engine_file_path')
    threshold = LaunchConfiguration('threshold')

    left_resize_node = ComposableNode(
        name='left_resize_node',
        package='isaac_ros_image_proc',
        plugin='nvidia::isaac_ros::image_proc::ResizeNode',
        parameters=[{
            'output_width': 960,
            'output_height': 576,
            'keep_aspect_ratio': True
        }],
        remappings=[
            ('camera_info', 'left/camerainfo'),
            ('image', 'left/image_raw'),
            ('resize/camera_info', 'left/camerainfo_resize'),
            ('resize/image', 'left/image_resize')]
    )

    right_resize_node = ComposableNode(
        name='right_resize_node',
        package='isaac_ros_image_proc',
        plugin='nvidia::isaac_ros::image_proc::ResizeNode',
        parameters=[{
            'output_width': 960,
            'output_height': 576,
            'keep_aspect_ratio': True
        }],
        remappings=[
            ('camera_info', 'right/camerainfo'),
            ('image', 'right/image_raw'),
            ('resize/camera_info', 'right/camerainfo_resize'),
            ('resize/image', 'right/image_resize')]
    )

    left_rectify_node = ComposableNode(
        name='left_rectify_node',
        package='isaac_ros_image_proc',
        plugin='nvidia::isaac_ros::image_proc::RectifyNode',
        parameters=[{
            'output_width': 960,
            'output_height': 576
        }],
        remappings=[
            ('image_raw', 'left/image_resize'),
            ('camera_info', 'left/camerainfo_resize'),
            ('image_rect', 'left/image_rect'),
            ('camera_info_rect', 'left/camera_info_rect')
        ]
    )

    right_rectify_node = ComposableNode(
        name='right_rectify_node',
        package='isaac_ros_image_proc',
        plugin='nvidia::isaac_ros::image_proc::RectifyNode',
        parameters=[{
            'output_width': 960,
            'output_height': 576
        }],
        remappings=[
            ('image_raw', 'right/image_resize'),
            ('camera_info', 'right/camerainfo_resize'),
            ('image_rect', 'right/image_rect'),
            ('camera_info_rect', 'right/camera_info_rect')
        ]
    )

    disparity_node = ComposableNode(
        name='disparity',
        package='isaac_ros_ess',
        plugin='nvidia::isaac_ros::dnn_stereo_depth::ESSDisparityNode',
        parameters=[{'engine_file_path': engine_file_path,
                     'threshold': threshold}],
        remappings=[
            ('left/camera_info', 'left/camera_info_rect'),
            ('right/camera_info', 'right/camera_info_rect')
        ]
    )

    point_cloud_node = ComposableNode(
        name='point_cloud_node',
        package='isaac_ros_stereo_image_proc',
        plugin='nvidia::isaac_ros::stereo_image_proc::PointCloudNode',
        parameters=[{
                'approximate_sync': False,
                'use_color': False,
                'use_system_default_qos': True,
        }],
        remappings=[
            ('left/image_rect_color', 'left/image_rect'),
            ('left/camera_info', 'left/camera_info_rect'),
            ('right/camera_info', 'right/camera_info_rect')
        ]
    )

    container = ComposableNodeContainer(
        name='disparity_container',
        namespace='disparity',
        package='rclcpp_components',
        executable='component_container_mt',
        composable_node_descriptions=[
            left_resize_node, right_resize_node,
            left_rectify_node, right_rectify_node,
            disparity_node, point_cloud_node
        ],
        output='screen'
    )
    # this is my own node that pulls image from /dev/video0 to split into a /left/image_raw and /right/image_raw
    elp_driver = Node(
        package='elp_usb_camera',
        executable='elp_driver',
        name='elp_stereo'
    )
    return (launch.LaunchDescription(launch_args + [container, elp_driver]))

However, I am facing an issue where, /left/image_rect and /right/image_rect does not show any image on rqt_image_view and using ros2 topic echo.
I see data from resize node, but no data output from recetify node.
There appears to be an error from left_rectify_node and right_rectify_node that says:
[component_container_mt-1] [INFO] [1712678786.466658471] [left_rectify_node]: Could not negotiate

Not sure if calibration related, here’s my left and right camera info .yaml files:
left:

image_width: 640
image_height: 480
camera_name: narrow_stereo/left
camera_matrix:
  rows: 3
  cols: 3
  data: [422.93297,   0.     , 324.53325,
           0.     , 423.3644 , 259.94948,
           0.     ,   0.     ,   1.     ]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.032311, -0.028469, 0.000387, -0.004180, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [ 0.86340319, -0.46438282, -0.19718907,
          0.46459561,  0.57946804,  0.66960266,
         -0.1966872 , -0.66975025,  0.71606477]
projection_matrix:
  rows: 3
  cols: 4
  data: [ 917.43248,    0.     ,  408.97915,    0.     ,
            0.     ,  917.43248, -571.51225,    0.     ,
            0.     ,    0.     ,    1.     ,    0.     ]

right:

image_width: 640
image_height: 480
camera_name: narrow_stereo/right
camera_matrix:
  rows: 3
  cols: 3
  data: [422.17658,   0.     , 325.25803,
           0.     , 422.58006, 260.18346,
           0.     ,   0.     ,   1.     ]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.031051, -0.027944, 0.000337, -0.003946, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [ 0.86334958, -0.46487664, -0.196258  ,
          0.46466436,  0.58076129,  0.6684335 ,
         -0.19676007, -0.66828588,  0.71741163]
projection_matrix:
  rows: 3
  cols: 4
  data: [ 917.43248,    0.     ,  408.97915,    0.     ,
            0.     ,  917.43248, -571.51225,   -2.04199,
            0.     ,    0.     ,    1.     ,    0.     ]

Attached is full logs from ros2 launch on modified launch file.
Issac ROS Log - Rectify not working.log (60.0 KB)

Hi @weicong1990

Thank you for your message.
Based on the error log you shared, it appears that there is an incompatibility issue between your node and the rectify node.

[component_container_mt-1] [INFO] [1712678782.996257445] [left_rectify_node]: [NitrosSubscriber] Received a message but the application receiver’s pointer is not yet set.

Have you checked if your node correctly spilt the stream for the rectify node?