From badf0b836d894f70ecf4bbc21cd76d53f66a2b36 Mon Sep 17 00:00:00 2001 From: Jaiveer Singh Date: Tue, 10 Dec 2024 17:44:44 -0800 Subject: [PATCH] Isaac ROS 3.2 --- isaac_ros_examples/package.xml | 3 +- .../config/multi_realsense.yaml | 45 ++ .../foxglove_layout_realsense.json | 240 ++++++++ ...os_visual_slam_multihawk_carter.launch.py} | 74 +-- ...ros_visual_slam_multihawk_devkit.launch.py | 286 +++++++++ ...c_ros_visual_slam_multirealsense.launch.py | 122 ++++ isaac_ros_multicamera_vo/package.xml | 3 +- .../rviz/realsense_multicamera_vo.rviz | 548 ++++++++++++++++++ isaac_ros_multicamera_vo/setup.py | 2 + .../urdf/2_realsense_calibration.urdf.xacro | 36 ++ .../config/realsense_mono_depth.yaml | 11 + isaac_ros_realsense/package.xml | 3 +- isaac_ros_usb_cam/package.xml | 3 +- isaac_ros_zed/config/zed_mono_depth.yaml | 2 +- .../launch/isaac_ros_zed_mono_core.launch.py | 83 +-- .../isaac_ros_zed_mono_rect_core.launch.py | 56 +- ...aac_ros_zed_mono_rect_depth_core.launch.py | 57 +- .../isaac_ros_zed_stereo_rect_core.launch.py | 69 ++- isaac_ros_zed/package.xml | 3 +- 19 files changed, 1476 insertions(+), 170 deletions(-) create mode 100644 isaac_ros_multicamera_vo/config/multi_realsense.yaml create mode 100644 isaac_ros_multicamera_vo/foxglove_layouts/foxglove_layout_realsense.json rename isaac_ros_multicamera_vo/launch/{isaac_ros_visual_slam_multihawk.launch.py => isaac_ros_visual_slam_multihawk_carter.launch.py} (86%) create mode 100644 isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk_devkit.launch.py create mode 100644 isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multirealsense.launch.py create mode 100644 isaac_ros_multicamera_vo/rviz/realsense_multicamera_vo.rviz create mode 100644 isaac_ros_multicamera_vo/urdf/2_realsense_calibration.urdf.xacro diff --git a/isaac_ros_examples/package.xml b/isaac_ros_examples/package.xml index fa0eada..bac41c8 100644 --- a/isaac_ros_examples/package.xml +++ b/isaac_ros_examples/package.xml @@ -2,13 +2,14 @@ isaac_ros_examples - 3.1.0 + 3.2.0 Utilities for launching example graphs of Isaac ROS packages Isaac ROS Maintainers Apache-2.0 https://developer.nvidia.com/isaac-ros/ Jaiveer Singh + isaac_ros_common ament_copyright ament_flake8 diff --git a/isaac_ros_multicamera_vo/config/multi_realsense.yaml b/isaac_ros_multicamera_vo/config/multi_realsense.yaml new file mode 100644 index 0000000..1d9368a --- /dev/null +++ b/isaac_ros_multicamera_vo/config/multi_realsense.yaml @@ -0,0 +1,45 @@ +%YAML 1.2 +# 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 +--- +common_params: + enable_infra1: True + enable_infra2: True + enable_color: False + enable_depth: False + enable_rgbd: False + enable_gyro: False + enable_accel: False + pointcloud.enable: False + publish_tf: True + enable_sync: True + initial_reset: True + rgb_camera.color_profile: '640,360,30' + depth_module.infra_profile: '640,360,30' + depth_module.enable_auto_exposure: True + depth_module.emitter_enabled: 0 + +cameras: + - camera_name: camera1 + serial_no: '243222071526' + usb_port_id: '' + depth_module.inter_cam_sync_mode: 1 + + - camera_name: camera2 + serial_no: '238222076390' + usb_port_id: '' + depth_module.inter_cam_sync_mode: 2 \ No newline at end of file diff --git a/isaac_ros_multicamera_vo/foxglove_layouts/foxglove_layout_realsense.json b/isaac_ros_multicamera_vo/foxglove_layouts/foxglove_layout_realsense.json new file mode 100644 index 0000000..605bc48 --- /dev/null +++ b/isaac_ros_multicamera_vo/foxglove_layouts/foxglove_layout_realsense.json @@ -0,0 +1,240 @@ +{ + "configById": { + "Image!3qezyf6": { + "cameraState": { + "distance": 20, + "perspective": true, + "phi": 60, + "target": [ + 0, + 0, + 0 + ], + "targetOffset": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "thetaOffset": 45, + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "scene": {}, + "transforms": {}, + "topics": {}, + "layers": {}, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": { + "imageTopic": "/camera1/infra1/image_rect_raw", + "calibrationTopic": "/camera1/infra1/camera_info" + } + }, + "Image!3rc5faz": { + "cameraState": { + "distance": 20, + "perspective": true, + "phi": 60, + "target": [ + 0, + 0, + 0 + ], + "targetOffset": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "thetaOffset": 45, + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "scene": {}, + "transforms": {}, + "topics": {}, + "layers": {}, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": { + "imageTopic": "/camera2/infra1/image_rect_raw", + "calibrationTopic": "/camera2/infra1/camera_info" + } + }, + "3D!18i6zy7": { + "layers": { + "845139cb-26bc-40b3-8161-8ab60af4baf5": { + "visible": true, + "frameLocked": true, + "label": "Grid", + "instanceId": "845139cb-26bc-40b3-8161-8ab60af4baf5", + "layerId": "foxglove.Grid", + "size": 10, + "divisions": 10, + "lineWidth": 1, + "color": "#248eff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ], + "order": 1 + } + }, + "cameraState": { + "perspective": true, + "distance": 8.802533372941202, + "phi": 63.834320193910614, + "thetaOffset": 68.73179120726562, + "targetOffset": [ + 1.564471296021874, + 0.7890669288162179, + -8.059051112281559e-16 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "followTf": "odom", + "scene": { + "meshUpAxis": "z_up", + "transforms": { + "labelSize": 0.1 + } + }, + "transforms": { + "frame:base_link": { + "visible": true + }, + "frame:camera1_infra1_frame": { + "visible": true + }, + "frame:camera1_infra2_frame": { + "visible": true + }, + "frame:camera2_infra1_frame": { + "visible": true + }, + "frame:camera2_infra2_frame": { + "visible": true + } + }, + "topics": { + "/robot_description": { + "visible": true + }, + "/pointcloud": { + "visible": false, + "colorField": "z", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/visual_slam/tracking/vo_path": { + "visible": true, + "lineWidth": 0.05, + "gradient": [ + "#17c426ff", + "#17c426ff" + ] + }, + "/visual_slam/vis/observations_cloud": { + "visible": true, + "colorField": "rgb", + "colorMode": "rgb", + "colorMap": "turbo", + "pointSize": 3 + }, + "/visual_slam/vis/landmarks_cloud": { + "visible": true + }, + "/camera1/infra1/image_rect_raw": { + "visible": true, + "frameLocked": true, + "cameraInfoTopic": "/camera1/infra1/camera_info", + "distance": 1, + "planarProjectionFactor": 0, + "color": "#ffffff" + }, + "/camera2/infra1/image_rect_raw": { + "visible": true, + "frameLocked": true, + "cameraInfoTopic": "/camera2/infra1/camera_info", + "distance": 1, + "planarProjectionFactor": 0, + "color": "#ffffff" + } + }, + "publish": { + "type": "pose", + "poseTopic": "/goal_pose", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": {} + } + }, + "globalVariables": {}, + "userNodes": {}, + "playbackConfig": { + "speed": 1 + }, + "layout": { + "direction": "row", + "first": { + "first": "Image!3qezyf6", + "second": "Image!3rc5faz", + "direction": "column" + }, + "second": "3D!18i6zy7", + "splitPercentage": 37.166666666666664 + } +} \ No newline at end of file diff --git a/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk.launch.py b/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk_carter.launch.py similarity index 86% rename from isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk.launch.py rename to isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk_carter.launch.py index 2305ad5..7e5c8d5 100644 --- a/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk.launch.py +++ b/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk_carter.launch.py @@ -30,7 +30,7 @@ MODULE_IDS = { - # Nova Carter Hawk ids. For Nova DevKit Hawk use following ids: front: 3, left: 5, right: 4 + # Nova Carter Hawk ids 'front_stereo_camera': 5, 'back_stereo_camera': 6, 'left_stereo_camera': 7, @@ -46,8 +46,8 @@ def hawk_capture(camera_name): namespace=f'{camera_name}', parameters=[{'module_id': MODULE_IDS[camera_name], 'camera_link_frame_name': f'{camera_name}', - 'left_optical_frame_name': f'{camera_name}_left_optical', - 'right_optical_frame_name': f'{camera_name}_right_optical'}], + 'left_camera_frame_name': f'{camera_name}_left', + 'right_camera_frame_name': f'{camera_name}_right'}], remappings=[ (f'/{camera_name}/correlated_timestamp', '/correlated_timestamp'), ], @@ -67,23 +67,19 @@ def hawk_decoder(name, identifier): return decoder -def hawk_processing(name, identifier, rectify=False): +def hawk_processing(name, identifier): rectify_node = ComposableNode( - name=f'{name}_{identifier}_rectify_node', - package='isaac_ros_image_proc', - plugin='nvidia::isaac_ros::image_proc::RectifyNode', - namespace=f'{name}_stereo_camera/{identifier}', - parameters=[{ - 'output_width': 1920, - 'output_height': 1200, - 'type_negotiation_duration_s': 1, - }], - ) - - if rectify: - return [rectify_node] - - return [] + name=f'{name}_{identifier}_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + namespace=f'{name}_stereo_camera/{identifier}', + parameters=[{ + 'output_width': 1920, + 'output_height': 1200, + 'type_negotiation_duration_s': 1, + }], + ) + return rectify_node def generate_launch_description(): @@ -118,7 +114,7 @@ def generate_launch_description(): 'use_sim_time': False, 'override_publishing_stamp': False, 'enable_ground_constraint_in_odometry': True, - 'enable_localization_n_mapping': False, + 'enable_localization_n_mapping': True, # frame params 'map_frame': 'map', @@ -186,19 +182,9 @@ def generate_launch_description(): visual_slam_container = ComposableNodeContainer( name='visual_slam_launch_container', package='rclcpp_components', - executable='component_container', + executable='component_container_mt', namespace='', - composable_node_descriptions=( - hawk_processing('front', 'left') + - hawk_processing('front', 'right') + - hawk_processing('back', 'left') + - hawk_processing('back', 'right') + - hawk_processing('left', 'left') + - hawk_processing('left', 'right') + - hawk_processing('right', 'left') + - hawk_processing('right', 'right') + - [visual_slam_node] - ), + composable_node_descriptions=([visual_slam_node]), output='screen', condition=LaunchConfigurationEquals('use_rectify', 'False') ) @@ -206,19 +192,19 @@ def generate_launch_description(): visual_slam_rect_container = ComposableNodeContainer( name='visual_slam_launch_container', package='rclcpp_components', - executable='component_container', + executable='component_container_mt', namespace='', - composable_node_descriptions=( - hawk_processing('front', 'left', True) + - hawk_processing('front', 'right', True) + - hawk_processing('back', 'left', True) + - hawk_processing('back', 'right', True) + - hawk_processing('left', 'left', True) + - hawk_processing('left', 'right', True) + - hawk_processing('right', 'left', True) + - hawk_processing('right', 'right', True) + - [visual_slam_node], - ), + composable_node_descriptions=([ + hawk_processing('front', 'left'), + hawk_processing('front', 'right'), + hawk_processing('back', 'left'), + hawk_processing('back', 'right'), + hawk_processing('left', 'left'), + hawk_processing('left', 'right'), + hawk_processing('right', 'left'), + hawk_processing('right', 'right'), + visual_slam_node + ]), output='screen', condition=LaunchConfigurationEquals('use_rectify', 'True') ) diff --git a/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk_devkit.launch.py b/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk_devkit.launch.py new file mode 100644 index 0000000..95feb06 --- /dev/null +++ b/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk_devkit.launch.py @@ -0,0 +1,286 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2021-2024 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 os + +from ament_index_python.packages import get_package_share_directory +import isaac_ros_launch_utils as lu +import launch +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription +from launch.conditions import IfCondition, LaunchConfigurationEquals +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.actions import SetParameter, SetRemap +from launch_ros.descriptions import ComposableNode +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource + + +MODULE_IDS = { + # Nova Devkit Hawk ids + 'front_stereo_camera': 3, + 'left_stereo_camera': 5, + 'right_stereo_camera': 4 +} + + +def hawk_capture(camera_name): + stereo_capture = ComposableNode( + name=f'{camera_name}_node', + package='isaac_ros_hawk', + plugin='nvidia::isaac_ros::hawk::HawkNode', + namespace=f'{camera_name}', + parameters=[{'module_id': MODULE_IDS[camera_name], + 'camera_link_frame_name': f'{camera_name}', + 'left_camera_frame_name': f'{camera_name}_left', + 'right_camera_frame_name': f'{camera_name}_right'}], + remappings=[ + (f'/{camera_name}/correlated_timestamp', '/correlated_timestamp'), + ], + ) + return stereo_capture + + +def hawk_decoder(name, identifier): + + decoder = ComposableNode( + name=f'{name}_{identifier}_decoder_node', + package='isaac_ros_h264_decoder', + plugin='nvidia::isaac_ros::h264_decoder::DecoderNode', + namespace=f'{name}_stereo_camera/{identifier}', + remappings=[('image_uncompressed', 'image_raw')], + ) + return decoder + + +def hawk_processing(name, identifier): + rectify_node = ComposableNode( + name=f'{name}_{identifier}_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + namespace=f'{name}_stereo_camera/{identifier}', + parameters=[{ + 'output_width': 1920, + 'output_height': 1200, + 'type_negotiation_duration_s': 1, + }], + ) + return rectify_node + + +def generate_launch_description(): + + use_rectify_arg = DeclareLaunchArgument('use_rectify', default_value='False', + description='Whether to use rectify nodes') + use_rectify = IfCondition(LaunchConfiguration('use_rectify', default='False')) + + use_rosbag_arg = DeclareLaunchArgument('use_rosbag', default_value='False', + description='Whether to execute on rosbag') + use_rosbag = IfCondition(LaunchConfiguration('use_rosbag', default='False')) + + nova_devkit_description = lu.include( + 'nova_developer_kit_description', + 'launch/nova_developer_kit_description.launch.py', + ) + + correlated_timestamp_driver_node = ComposableNode( + package='isaac_ros_correlated_timestamp_driver', + plugin='nvidia::isaac_ros::correlated_timestamp_driver::CorrelatedTimestampDriverNode', + name='correlated_timestamp_driver', + parameters=[{'use_time_since_epoch': False, + 'nvpps_dev_name': '/dev/nvpps0'}]) + + visual_slam_node = ComposableNode( + name='visual_slam_node', + package='isaac_ros_visual_slam', + plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode', + parameters=[{ + + # general params + 'use_sim_time': False, + 'override_publishing_stamp': False, + 'enable_ground_constraint_in_odometry': True, + 'enable_localization_n_mapping': True, + + # frame params + 'map_frame': 'map', + 'odom_frame': 'odom', + 'base_frame': 'base_link', + 'publish_odom_to_base_tf': True, + 'publish_map_to_odom_tf': True, + + # camera optical frames should have the same order as camera topics + 'input_camera_optical_frames': [ + 'front_stereo_camera_left_optical', + 'front_stereo_camera_right_optical', + 'left_stereo_camera_left_optical', + 'left_stereo_camera_right_optical', + 'right_stereo_camera_left_optical', + 'right_stereo_camera_right_optical', + ], + + # camera params + 'img_jitter_threshold_ms': 34.0, + 'denoise_input_images': False, + 'rectified_images': False, + + # multicamera params + 'num_cameras': 6, + 'min_num_images': 4, + 'sync_matching_threshold_ms': 5.0, + + # inertial odometry is not supported for multi-camera visual odometry + 'enable_imu_fusion': False, + + # visualization params + 'path_max_size': 1024, + 'enable_slam_visualization': False, + 'enable_landmarks_view': False, + 'enable_observations_view': False, + + # debug params + 'verbosity': 0, + 'enable_debug_mode': False, + 'debug_dump_path': '/tmp/cuvslam', + }], + remappings=[ + ('visual_slam/image_0', '/front_stereo_camera/left/image_raw'), + ('visual_slam/camera_info_0', '/front_stereo_camera/left/camera_info'), + ('visual_slam/image_1', '/front_stereo_camera/right/image_raw'), + ('visual_slam/camera_info_1', '/front_stereo_camera/right/camera_info'), + ('visual_slam/image_2', '/left_stereo_camera/left/image_raw'), + ('visual_slam/camera_info_2', '/left_stereo_camera/left/camera_info'), + ('visual_slam/image_3', '/left_stereo_camera/right/image_raw'), + ('visual_slam/camera_info_3', '/left_stereo_camera/right/camera_info'), + ('visual_slam/image_4', '/right_stereo_camera/left/image_raw'), + ('visual_slam/camera_info_4', '/right_stereo_camera/left/camera_info'), + ('visual_slam/image_5', '/right_stereo_camera/right/image_raw'), + ('visual_slam/camera_info_5', '/right_stereo_camera/right/camera_info') + ] + ) + + visual_slam_container = ComposableNodeContainer( + name='visual_slam_launch_container', + package='rclcpp_components', + executable='component_container_mt', + namespace='', + composable_node_descriptions=([visual_slam_node]), + output='screen', + condition=LaunchConfigurationEquals('use_rectify', 'False') + ) + + visual_slam_rect_container = ComposableNodeContainer( + name='visual_slam_launch_container', + package='rclcpp_components', + executable='component_container_mt', + namespace='', + composable_node_descriptions=([ + hawk_processing('front', 'left'), + hawk_processing('front', 'right'), + hawk_processing('left', 'left'), + hawk_processing('left', 'right'), + hawk_processing('right', 'left'), + hawk_processing('right', 'right'), + visual_slam_node + ]), + output='screen', + condition=LaunchConfigurationEquals('use_rectify', 'True') + ) + + hawk_h264_decoders = LoadComposableNodes( + target_container='visual_slam_launch_container', + condition=use_rosbag, + composable_node_descriptions=[ + hawk_decoder('front', 'left'), + hawk_decoder('front', 'right'), + hawk_decoder('left', 'left'), + hawk_decoder('left', 'right'), + hawk_decoder('right', 'left'), + hawk_decoder('right', 'right'), + ] + ) + + hawk_image_capture = LoadComposableNodes( + target_container='visual_slam_launch_container', + condition=LaunchConfigurationEquals('use_rosbag', 'False'), + composable_node_descriptions=[ + hawk_capture('front_stereo_camera'), + hawk_capture('left_stereo_camera'), + hawk_capture('right_stereo_camera'), + correlated_timestamp_driver_node, + ] + ) + + group_action = GroupAction([ + use_rectify_arg, + use_rosbag_arg, + + SetParameter(name='rectified_images', value=True, + condition=LaunchConfigurationEquals('use_rectify', 'True')), + + SetRemap(src=['visual_slam/image_0'], + dst=['/front_stereo_camera/left/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_0'], + dst=['/front_stereo_camera/left/camera_info_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/image_1'], + dst=['/front_stereo_camera/right/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_1'], + dst=['/front_stereo_camera/right/camera_info_rect'], + condition=use_rectify), + + SetRemap(src=['visual_slam/image_2'], + dst=['/left_stereo_camera/left/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_2'], + dst=['/left_stereo_camera/left/camera_info_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/image_3'], + dst=['/left_stereo_camera/right/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_3'], + dst=['/left_stereo_camera/right/camera_info_rect'], + condition=use_rectify), + + SetRemap(src=['visual_slam/image_4'], + dst=['/right_stereo_camera/left/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_4'], + dst=['/right_stereo_camera/left/camera_info_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/image_5'], + dst=['/right_stereo_camera/right/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_5'], + dst=['/right_stereo_camera/right/camera_info_rect'], + condition=use_rectify), + visual_slam_container, + visual_slam_rect_container + ]) + + config_directory = get_package_share_directory('isaac_ros_multicamera_vo') + foxglove_xml_config = os.path.join(config_directory, 'config', 'foxglove_bridge_launch.xml') + foxglove_bridge_launch = IncludeLaunchDescription( + XMLLaunchDescriptionSource([foxglove_xml_config]) + ) + + return launch.LaunchDescription([nova_devkit_description, + foxglove_bridge_launch, + hawk_image_capture, + group_action, + hawk_h264_decoders]) diff --git a/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multirealsense.launch.py b/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multirealsense.launch.py new file mode 100644 index 0000000..4974585 --- /dev/null +++ b/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multirealsense.launch.py @@ -0,0 +1,122 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2021-2024 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 +# flake8: noqa: F403,F405 + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.conditions import LaunchConfigurationEquals +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes, Node +from launch_ros.descriptions import ComposableNode +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource + + +def generate_launch_description(): + + use_rosbag_arg = DeclareLaunchArgument('use_rosbag', default_value='False', + description='Whether to execute on rosbag') + use_rosbag = LaunchConfiguration('use_rosbag') + + config_directory = get_package_share_directory('isaac_ros_multicamera_vo') + foxglove_xml_config = os.path.join(config_directory, 'config', 'foxglove_bridge_launch.xml') + foxglove_bridge_launch = IncludeLaunchDescription( + XMLLaunchDescriptionSource([foxglove_xml_config]) + ) + + urdf_file = os.path.join(config_directory, 'urdf', '2_realsense_calibration.urdf.xacro') + with open(urdf_file, 'r') as f: + robot_description = f.read() + + rs_config_path = os.path.join(config_directory, 'config', 'multi_realsense.yaml') + with open(rs_config_path) as rs_config_file: + rs_config = yaml.safe_load(rs_config_file) + + remapping_list, optical_frames = [], [] + num_cameras = 2*len(rs_config['cameras']) + + for idx in range(num_cameras): + infra_cnt = idx % 2+1 + camera_cnt = rs_config['cameras'][idx//2]['camera_name'] + optical_frames += [f'{camera_cnt}_infra{infra_cnt}_optical_frame'] + remapping_list += [(f'visual_slam/image_{idx}', + f'/{camera_cnt}/infra{infra_cnt}/image_rect_raw'), + (f'visual_slam/camera_info_{idx}', + f'/{camera_cnt}/infra{infra_cnt}/camera_info')] + + def realsense_capture(common_params, camera_params): + stereo_capture = ComposableNode( + name=camera_params['camera_name'], + namespace='', + package='realsense2_camera', + plugin='realsense2_camera::RealSenseNodeFactory', + parameters=[common_params | camera_params]) + return stereo_capture + + visual_slam_node = ComposableNode( + name='visual_slam_node', + package='isaac_ros_visual_slam', + plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode', + parameters=[{ + 'enable_image_denoising': False, + 'rectified_images': True, + 'enable_imu_fusion': False, + 'image_jitter_threshold_ms': 34.00, + 'base_frame': 'base_link', + 'enable_slam_visualization': False, + 'enable_landmarks_view': False, + 'enable_observations_view': False, + 'enable_ground_constraint_in_odometry': False, + 'enable_ground_constraint_in_slam': False, + 'enable_localization_n_mapping': True, + 'enable_debug_mode': False, + 'num_cameras': num_cameras, + 'min_num_images': num_cameras, + 'camera_optical_frames': optical_frames, + }], + remappings=remapping_list, + ) + + visual_slam_launch_container = ComposableNodeContainer( + name='visual_slam_launch_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=([visual_slam_node]), + output='screen', + ) + + realsense_image_capture = LoadComposableNodes( + target_container='visual_slam_launch_container', + condition=LaunchConfigurationEquals('use_rosbag', 'False'), + composable_node_descriptions=([realsense_capture(rs_config['common_params'], camera_config) + for camera_config in rs_config['cameras']]), + ) + + state_publisher = Node(package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[{'robot_description': robot_description}]) + + return LaunchDescription([use_rosbag_arg, + foxglove_bridge_launch, + state_publisher, + realsense_image_capture, + visual_slam_launch_container]) diff --git a/isaac_ros_multicamera_vo/package.xml b/isaac_ros_multicamera_vo/package.xml index ee12958..fcf2b9c 100644 --- a/isaac_ros_multicamera_vo/package.xml +++ b/isaac_ros_multicamera_vo/package.xml @@ -2,13 +2,14 @@ isaac_ros_multicamera_vo - 3.1.0 + 3.2.0 Isaac ROS Launch Fragment for cuVSLAM Visual Odomtry for multicamera cases Isaac ROS Maintainers Apache-2.0 https://developer.nvidia.com/isaac-ros/ Alexander Efitorov + isaac_ros_common foxglove_bridge isaac_ros_h264_decoder diff --git a/isaac_ros_multicamera_vo/rviz/realsense_multicamera_vo.rviz b/isaac_ros_multicamera_vo/rviz/realsense_multicamera_vo.rviz new file mode 100644 index 0000000..d566c1d --- /dev/null +++ b/isaac_ros_multicamera_vo/rviz/realsense_multicamera_vo.rviz @@ -0,0 +1,548 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Loop closure1/Topic1 + - /Localizer1/Localizer Map1 + - /Odometry1/Shape1 + Splitter Ratio: 0.5261569619178772 + Tree Height: 481 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + camera1_aligned_depth_to_infra1_frame: + Value: true + camera1_infra1_frame: + Value: true + camera1_infra1_optical_frame: + Value: true + camera1_infra2_frame: + Value: true + camera1_infra2_optical_frame: + Value: true + camera1_link: + Value: true + camera2_aligned_depth_to_infra1_frame: + Value: true + camera2_infra1_frame: + Value: true + camera2_infra1_optical_frame: + Value: true + camera2_infra2_frame: + Value: true + camera2_infra2_optical_frame: + Value: true + camera2_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + camera1_link: + camera1_aligned_depth_to_infra1_frame: + camera1_infra1_optical_frame: + {} + camera1_infra1_frame: + {} + camera1_infra2_frame: + camera1_infra2_optical_frame: + {} + camera2_link: + camera2_aligned_depth_to_infra1_frame: + camera2_infra1_optical_frame: + {} + camera2_infra1_frame: + {} + camera2_infra2_frame: + camera2_infra2_optical_frame: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Gravity + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/gravity + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Landmarks + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/landmarks_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Observations + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/observations_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Loop closure + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/loop_closure_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 25; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: VO Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/vo_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: SLAM Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/slam_path + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.10000000149011612 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PG Nodes + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/pose_graph_nodes + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: PG Edges2 + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/pose_graph_edges2 + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: PG Edges + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/pose_graph_edges + Value: true + Enabled: true + Name: Pose Graph + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Localizer + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/localizer + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Localizer Map + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/localizer_map_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: Localizer + - Angle Tolerance: 0.009999999776482582 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: false + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.009999999776482582 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/odometry + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Velocity + Namespaces: + angular velocity: true + linear velocity: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/velocity + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Front Stereo Camera (Left Image) + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera1/infra1/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Back Stereo Camera (Left Image) + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera2/infra1/image_rect_raw + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 17.10153579711914 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.814717411994934 + Y: 0.6073047518730164 + Z: -0.6894067525863647 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5047963857650757 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1453962326049805 + Saved: ~ +Window Geometry: + Back Stereo Camera (Left Image): + collapsed: false + Displays: + collapsed: false + Front Stereo Camera (Left Image): + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 3370 + X: 70 + Y: 27 diff --git a/isaac_ros_multicamera_vo/setup.py b/isaac_ros_multicamera_vo/setup.py index 1e2d315..74f2382 100644 --- a/isaac_ros_multicamera_vo/setup.py +++ b/isaac_ros_multicamera_vo/setup.py @@ -35,6 +35,8 @@ glob(os.path.join('config', '*'))), (os.path.join('share', package_name, 'rviz'), glob(os.path.join('rviz', '*'))), + (os.path.join('share', package_name, 'urdf'), + glob(os.path.join('urdf', '*'))), ], install_requires=['setuptools'], zip_safe=True, diff --git a/isaac_ros_multicamera_vo/urdf/2_realsense_calibration.urdf.xacro b/isaac_ros_multicamera_vo/urdf/2_realsense_calibration.urdf.xacro new file mode 100644 index 0000000..0d1eabb --- /dev/null +++ b/isaac_ros_multicamera_vo/urdf/2_realsense_calibration.urdf.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/isaac_ros_realsense/config/realsense_mono_depth.yaml b/isaac_ros_realsense/config/realsense_mono_depth.yaml index 0694003..eec5049 100644 --- a/isaac_ros_realsense/config/realsense_mono_depth.yaml +++ b/isaac_ros_realsense/config/realsense_mono_depth.yaml @@ -1,3 +1,14 @@ +rgb_camera: + profile: '640x480x15' +color_qos: "SYSTEM_DEFAULT" + +depth_module: + profile: '640x480x15' + emitter_enabled: 0 + emitter_on_off: false +depth_qos: "SYSTEM_DEFAULT" +depth_info_qos: "SYSTEM_DEFAULT" + enable_infra1: false enable_infra2: false diff --git a/isaac_ros_realsense/package.xml b/isaac_ros_realsense/package.xml index 5baedc5..5d7abb9 100644 --- a/isaac_ros_realsense/package.xml +++ b/isaac_ros_realsense/package.xml @@ -2,13 +2,14 @@ isaac_ros_realsense - 3.1.0 + 3.2.0 Isaac ROS Launch Fragment for Intel RealSense cameras Isaac ROS Maintainers Apache-2.0 https://developer.nvidia.com/isaac-ros/ Jaiveer Singh + isaac_ros_common isaac_ros_depth_image_proc diff --git a/isaac_ros_usb_cam/package.xml b/isaac_ros_usb_cam/package.xml index 7f367d4..02459ed 100644 --- a/isaac_ros_usb_cam/package.xml +++ b/isaac_ros_usb_cam/package.xml @@ -2,13 +2,14 @@ isaac_ros_usb_cam - 3.1.0 + 3.2.0 Isaac ROS Launch Fragment for USB cameras Isaac ROS Maintainers Apache-2.0 https://developer.nvidia.com/isaac-ros/ Jaiveer Singh + isaac_ros_common usb_cam diff --git a/isaac_ros_zed/config/zed_mono_depth.yaml b/isaac_ros_zed/config/zed_mono_depth.yaml index b429cf7..cbc7aea 100644 --- a/isaac_ros_zed/config/zed_mono_depth.yaml +++ b/isaac_ros_zed/config/zed_mono_depth.yaml @@ -46,7 +46,7 @@ qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE depth: - depth_mode: 'NEURAL' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_mode: 'ULTRA' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) diff --git a/isaac_ros_zed/launch/isaac_ros_zed_mono_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_mono_core.launch.py index ed2531e..b93c0c1 100644 --- a/isaac_ros_zed/launch/isaac_ros_zed_mono_core.launch.py +++ b/isaac_ros_zed/launch/isaac_ros_zed_mono_core.launch.py @@ -14,14 +14,14 @@ # limitations under the License. # # SPDX-License-Identifier: Apache-2.0 - import os from typing import Any, Dict from ament_index_python import get_package_share_directory from isaac_ros_examples import IsaacROSLaunchFragment from launch.substitutions import Command -from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode class IsaacROSZedMonoLaunchFragment(IsaacROSLaunchFragment): @@ -33,23 +33,15 @@ def get_interface_specs() -> Dict[str, Any]: 'camera_frame': 'zed2_base_link', 'camera_model': 'zed2', 'focal_length': { - # Approximation - most zed cameras should be close to this value 'f_x': 380.0, - # Approximation - most zed cameras should be close to this value 'f_y': 380.0 } } @staticmethod - def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: - # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm - camera_model = interface_specs['camera_model'] + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: - # URDF/xacro file to be loaded by the Robot State Publisher node - xacro_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'urdf', 'zed_descr.urdf.xacro' - ) + camera_model = interface_specs['camera_model'] # ZED Configurations to be loaded by ZED Node config_common = os.path.join( @@ -65,34 +57,59 @@ def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: ) return { - # Robot State Publisher node + 'image_format_converter_node': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_node_left', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': interface_specs['camera_resolution']['width'], + 'image_height': interface_specs['camera_resolution']['height'], + }], + remappings=[ + ('image_raw', '/zed_node/rgb_raw/image_raw_color'), + ('image', 'image_raw') + ] + ), + 'zed_wrapper_component': ComposableNode( + package='zed_components', + plugin='stereolabs::ZedCamera', + name='zed_node', + parameters=[ + config_common, # Common parameters + config_camera, # Camera related parameters + ], + remappings=[ + ('zed_node/left/camera_info', '/camera_info'), + ], + extra_arguments=[{'use_intra_process_comms': True}] + ) + } + + @staticmethod + def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: + camera_model = interface_specs['camera_model'] + xacro_path = os.path.join(get_package_share_directory('zed_wrapper'), 'urdf', + 'zed_descr.urdf.xacro') + + return { 'rsp_node': Node( package='robot_state_publisher', executable='robot_state_publisher', name='zed_state_publisher', output='screen', parameters=[{ - 'robot_description': Command( - [ - 'xacro', ' ', xacro_path, ' ', - 'camera_name:=', camera_model, ' ', - 'camera_model:=', camera_model - ]) + 'robot_description': Command(['xacro', ' ', xacro_path, ' ', + 'camera_name:=', camera_model, ' ', + 'camera_model:=', camera_model]) }] ), - # ZED node using manual composition - 'zed_node': Node( - package='zed_wrapper', - executable='zed_wrapper', - output='screen', - parameters=[ - config_common, # Common parameters - config_camera, # Camera related parameters - ], - arguments=[ - '--ros-args', - '--remap', '/zed_node/rgb_raw/image_raw_color:=/image_raw', - '--remap', '/zed_node/rgb_raw/camera_info:=/camera_info' - ] + 'zed_container': ComposableNodeContainer( + name='zed_container', + namespace='', + package='rclcpp_components', + executable='component_container_isolated', + composable_node_descriptions=[], + output='screen' ) } diff --git a/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_core.launch.py index 51d0424..e086806 100644 --- a/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_core.launch.py +++ b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_core.launch.py @@ -44,6 +44,22 @@ def get_interface_specs() -> Dict[str, Any]: @staticmethod def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + + camera_model = interface_specs['camera_model'] + + # ZED Configurations to be loaded by ZED Node + config_common = os.path.join( + get_package_share_directory('isaac_ros_zed'), + 'config', + 'zed_mono.yaml' + ) + + config_camera = os.path.join( + get_package_share_directory('zed_wrapper'), + 'config', + camera_model + '.yaml' + ) + return { 'image_format_converter_node_left': ComposableNode( package='isaac_ros_image_proc', @@ -57,6 +73,19 @@ def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, Composabl remappings=[ ('image_raw', 'zed_node/left/image_rect_color'), ('image', 'image_rect')] + ), + 'zed_wrapper_component': ComposableNode( + package='zed_components', + plugin='stereolabs::ZedCamera', + name='zed_node', + parameters=[ + config_common, # Common parameters + config_camera, # Camera related parameters + ], + remappings=[ + ('zed_node/left/camera_info', '/camera_info_rect'), + ], + extra_arguments=[{'use_intra_process_comms': True}] ) } @@ -71,19 +100,6 @@ def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: 'urdf', 'zed_descr.urdf.xacro' ) - # ZED Configurations to be loaded by ZED Node - config_common = os.path.join( - get_package_share_directory('isaac_ros_zed'), - 'config', - 'zed_mono.yaml' - ) - - config_camera = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - camera_model + '.yaml' - ) - return { # Robot State Publisher node 'rsp_node': Node( @@ -99,20 +115,6 @@ def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: 'camera_model:=', camera_model ]) }] - ), - # ZED node using manual composition - 'zed_node': Node( - package='zed_wrapper', - executable='zed_wrapper', - output='screen', - parameters=[ - config_common, # Common parameters - config_camera, # Camera related parameters - ], - arguments=[ - '--ros-args', - '--remap', 'zed_node/left/camera_info:=/camera_info_rect' - ] ) } diff --git a/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_depth_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_depth_core.launch.py index f288b37..040856e 100644 --- a/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_depth_core.launch.py +++ b/isaac_ros_zed/launch/isaac_ros_zed_mono_rect_depth_core.launch.py @@ -44,6 +44,21 @@ def get_interface_specs() -> Dict[str, Any]: @staticmethod def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + + camera_model = interface_specs['camera_model'] + + # ZED Configurations to be loaded by ZED Node + config_common = os.path.join( + get_package_share_directory('isaac_ros_zed'), + 'config', + 'zed_mono_depth.yaml' + ) + + config_camera = os.path.join( + get_package_share_directory('zed_wrapper'), + 'config', + camera_model + '.yaml' + ) return { 'image_format_converter_node_left': ComposableNode( package='isaac_ros_image_proc', @@ -57,6 +72,19 @@ def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, Composabl remappings=[ ('image_raw', 'zed_node/left/image_rect_color'), ('image', 'image_rect')] + ), + 'zed_wrapper_component': ComposableNode( + package='zed_components', + plugin='stereolabs::ZedCamera', + name='zed_node', + parameters=[ + config_common, # Common parameters + config_camera, # Camera related parameters + ], + remappings=[ + ('zed_node/left/camera_info', '/camera_info'), + ], + extra_arguments=[{'use_intra_process_comms': True}] ) } @@ -71,19 +99,6 @@ def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: 'urdf', 'zed_descr.urdf.xacro' ) - # ZED Configurations to be loaded by ZED Node - config_common = os.path.join( - get_package_share_directory('isaac_ros_zed'), - 'config', - 'zed_mono_depth.yaml' - ) - - config_camera = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - camera_model + '.yaml' - ) - return { # Robot State Publisher node 'rsp_node': Node( @@ -99,22 +114,6 @@ def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: 'camera_model:=', camera_model ]) }] - ), - # ZED node using manual composition - 'zed_node': Node( - package='zed_wrapper', - executable='zed_wrapper', - output='screen', - parameters=[ - config_common, # Common parameters - config_camera, # Camera related parameters - ], - arguments=[ - '--ros-args', - '--remap', '/zed_node/left/camera_info:=/camera_info_rect', - '--remap', '/zed_node/depth/camera_info:=/camera_info_depth', - '--remap', '/zed_node/depth/depth_registered:=/depth' - ] ) } diff --git a/isaac_ros_zed/launch/isaac_ros_zed_stereo_rect_core.launch.py b/isaac_ros_zed/launch/isaac_ros_zed_stereo_rect_core.launch.py index 3053437..d42d8ee 100644 --- a/isaac_ros_zed/launch/isaac_ros_zed_stereo_rect_core.launch.py +++ b/isaac_ros_zed/launch/isaac_ros_zed_stereo_rect_core.launch.py @@ -21,7 +21,8 @@ from ament_index_python import get_package_share_directory from isaac_ros_examples import IsaacROSLaunchFragment import launch -from launch.substitutions import Command +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode @@ -46,6 +47,24 @@ def get_interface_specs() -> Dict[str, Any]: def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm camera_model = interface_specs['camera_model'] + pub_frame_rate = LaunchConfiguration('pub_frame_rate') + + # ZED Configurations to be loaded by ZED Node + config_common = os.path.join( + get_package_share_directory('isaac_ros_zed'), + 'config', + 'zed_mono.yaml' + ) + + config_camera = os.path.join( + get_package_share_directory('zed_wrapper'), + 'config', + camera_model + '.yaml' + ) + base_parameters = [config_common, config_camera] + override_parameters = {'general.pub_frame_rate': pub_frame_rate} + parameters = base_parameters + [override_parameters] + return { 'image_format_converter_node_left': ComposableNode( package='isaac_ros_image_proc', @@ -86,8 +105,19 @@ def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, Composabl 'rotation.x': -0.5, 'rotation.y': 0.5, 'rotation.z': -0.5, - 'rotation.w': 0.5 - }]) + 'rotation.w': 0.5}] + ), + 'zed_wrapper_component': ComposableNode( + package='zed_components', + plugin='stereolabs::ZedCamera', + name='zed_node', + parameters=parameters, + remappings=[ + ('zed_node/left/camera_info', '/left/camera_info_rect'), + ('zed_node/right/camera_info', '/right/camera_info_rect'), + ], + extra_arguments=[{'use_intra_process_comms': True}] + ) } @staticmethod @@ -101,20 +131,12 @@ def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: 'urdf', 'zed_descr.urdf.xacro' ) - # ZED Configurations to be loaded by ZED Node - config_common = os.path.join( - get_package_share_directory('isaac_ros_zed'), - 'config', - 'zed.yaml' - ) - - config_camera = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - camera_model + '.yaml' - ) - return { + 'pub_frame_rate': DeclareLaunchArgument( + 'pub_frame_rate', # Name of the argument + default_value='15.0', # Default value as a string + description='Frame rate for publishing ZED camera data' + ), # Robot State Publisher node 'rsp_node': Node( package='robot_state_publisher', @@ -130,21 +152,6 @@ def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]: ]) }] ), - # ZED node using manual composition - 'zed_node': Node( - package='zed_wrapper', - executable='zed_wrapper', - output='screen', - parameters=[ - config_common, # Common parameters - config_camera, # Camera related parameters - ], - arguments=[ - '--ros-args', - '--remap', 'zed_node/left/camera_info:=/left/camera_info_rect', - '--remap', 'zed_node/right/camera_info:=/right/camera_info_rect', - ] - ) } diff --git a/isaac_ros_zed/package.xml b/isaac_ros_zed/package.xml index e7187ef..de3a0e4 100644 --- a/isaac_ros_zed/package.xml +++ b/isaac_ros_zed/package.xml @@ -2,13 +2,14 @@ isaac_ros_zed - 3.1.0 + 3.2.0 Isaac ROS Launch Fragment for Intel zed cameras Isaac ROS Maintainers Apache-2.0 https://developer.nvidia.com/isaac-ros/ Sameer Tangade + isaac_ros_common ament_copyright ament_flake8