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)