diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..735407e --- /dev/null +++ b/.gitattributes @@ -0,0 +1,27 @@ +# Ignore Python files in linguist +*.py linguist-detectable=false + +# Images +*.gif filter=lfs diff=lfs merge=lfs -text +*.jpg filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.psd filter=lfs diff=lfs merge=lfs -text + +# Archives +*.gz filter=lfs diff=lfs merge=lfs -text +*.tar filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text + +# Documents +*.pdf filter=lfs diff=lfs merge=lfs -text + +# Shared libraries +*.so filter=lfs diff=lfs merge=lfs -text +*.so.* filter=lfs diff=lfs merge=lfs -text + +# ROS Bags +**/resources/**/*.zstd filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.bag filter=lfs diff=lfs merge=lfs -text + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b793570 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +# Ignore all pycache files +**/__pycache__/** diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..a89cd42 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,14 @@ +# Isaac ROS Contribution Rules + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +> **5. Submission of Contributions.** Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). + +[//]: # (202201002) diff --git a/LICENSE b/LICENSE index 261eeb9..7a4a3ea 100644 --- a/LICENSE +++ b/LICENSE @@ -1,3 +1,4 @@ + Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ @@ -198,4 +199,4 @@ 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. + limitations under the License. \ No newline at end of file diff --git a/README.md b/README.md index 390a45e..c7d2f9a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,5 @@ -# isaac_ros_examples -Examples of using Isaac ROS GEMs together +# Isaac ROS Examples + +This repository contains example launch files and scripts to support Isaac ROS package quickstarts. + +Visit the [Isaac ROS Package Index](https://nvidia-isaac-ros.github.io/repositories_and_packages/index.html) for a list of packages that include quickstart examples. diff --git a/isaac_ros_examples/isaac_ros_examples/__init__.py b/isaac_ros_examples/isaac_ros_examples/__init__.py new file mode 100644 index 0000000..5e65927 --- /dev/null +++ b/isaac_ros_examples/isaac_ros_examples/__init__.py @@ -0,0 +1,25 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 + +from .isaac_ros_launch_fragment import IsaacROSLaunchFragment +from .isaac_ros_launch_fragment_spec import IsaacROSLaunchFragmentSpec, LAUNCH_FRAGMENT_SPECS + +__all__ = [ + 'IsaacROSLaunchFragment', + 'IsaacROSLaunchFragmentSpec', + 'LAUNCH_FRAGMENT_SPECS' +] diff --git a/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment.py b/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment.py new file mode 100644 index 0000000..200d367 --- /dev/null +++ b/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment.py @@ -0,0 +1,77 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 + +from typing import Any, Dict + +from launch import Action +from launch_ros.descriptions import ComposableNode + + +class IsaacROSLaunchFragment(): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + """ + Get the interface specifications for this fragment. + + Interface specifications are a way to define key-value pairs that this fragment + makes available to other fragments in the launch graph. This is useful for communicating + values that must be known prior to launch time, such as camera resolutions, etc. + + Returns + ------- + Dict[str, Any] + The interface specifications for this fragment. + + """ + return {} + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any] = {}) -> Dict[str, ComposableNode]: + """ + Get the composable nodes for this fragment. + + Parameters + ---------- + interface_specs : Dict[str, Any], optional + The accumulated interface specs from all fragments, by default {} + + Returns + ------- + Dict[str, ComposableNode] + The composable nodes for this fragment, labelled with unique names + + """ + return {} + + @staticmethod + def get_launch_actions(interface_specs: Dict[str, Any] = {}) -> Dict[str, Action]: + """ + Get the launch actions for this fragment. + + Parameters + ---------- + interface_specs : Dict[str, Any], optional + The accumulated interface specs from all fragments, by default {} + + Returns + ------- + Dict[str, Action] + The launch actions for this fragment, labelled with unique names + + """ + return {} diff --git a/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment_spec.py b/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment_spec.py new file mode 100644 index 0000000..a3f6d21 --- /dev/null +++ b/isaac_ros_examples/isaac_ros_examples/isaac_ros_launch_fragment_spec.py @@ -0,0 +1,184 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 + +from typing import Dict + + +class IsaacROSLaunchFragmentSpec: + + def __init__(self, package, class_name, filename=''): + self.package = package + self.class_name = class_name + self.filename = filename if filename != '' else f'{package}_core.launch.py' + + +LAUNCH_FRAGMENT_SPECS: Dict[str, IsaacROSLaunchFragmentSpec] = { + ################ + # Data Sources # + ################ + + # RealSense Camera + 'realsense_mono': IsaacROSLaunchFragmentSpec( + 'isaac_ros_realsense', 'IsaacROSRealSenseMonoLaunchFragment', + 'isaac_ros_realsense_mono_core.launch.py'), + 'realsense_mono_rect': IsaacROSLaunchFragmentSpec( + 'isaac_ros_realsense', 'IsaacROSRealSenseMonoRectLaunchFragment', + 'isaac_ros_realsense_mono_rect_core.launch.py'), + 'realsense_stereo_rect': IsaacROSLaunchFragmentSpec( + 'isaac_ros_realsense', 'IsaacROSRealSenseStereoRectLaunchFragment', + 'isaac_ros_realsense_stereo_rect_core.launch.py'), + 'realsense_stereo_rect_imu': IsaacROSLaunchFragmentSpec( + 'isaac_ros_realsense', 'IsaacROSRealSenseStereoRectImuLaunchFragment', + 'isaac_ros_realsense_stereo_rect_imu_core.launch.py'), + 'realsense_mono_rect_depth': IsaacROSLaunchFragmentSpec( + 'isaac_ros_realsense', 'IsaacROSRealSenseMonoRectDepthLaunchFragment', + 'isaac_ros_realsense_mono_rect_depth_core.launch.py'), + + # Argus Camera + 'argus_mono': IsaacROSLaunchFragmentSpec( + 'isaac_ros_argus_camera', 'IsaacROSArgusMonoLaunchFragment', + 'isaac_ros_argus_camera_mono_core.launch.py'), + 'argus_stereo': IsaacROSLaunchFragmentSpec( + 'isaac_ros_argus_camera', 'IsaacROSArgusStereoLaunchFragment', + 'isaac_ros_argus_camera_stereo_core.launch.py'), + 'argus_depth': IsaacROSLaunchFragmentSpec( + 'isaac_ros_argus_camera', 'IsaacROSArgusDepthLaunchFragment', + 'isaac_ros_argus_camera_depth_core.launch.py'), + + # USB Camera + 'usb_cam': IsaacROSLaunchFragmentSpec( + 'isaac_ros_usb_cam', 'IsaacROSUSBCameraLaunchFragment'), + + ########################### + # Preprocessing Utilities # + ########################### + + # Rectification + 'rectify_mono': IsaacROSLaunchFragmentSpec( + 'isaac_ros_image_proc', 'IsaacROSRectifyMonoLaunchFragment', + 'isaac_ros_image_rectify_mono_core.launch.py'), + 'rectify_stereo': IsaacROSLaunchFragmentSpec( + 'isaac_ros_image_proc', 'IsaacROSRectifyStereoLaunchFragment', + 'isaac_ros_image_rectify_stereo_core.launch.py'), + + # Resize + Rectification + 'resize_rectify_stereo': IsaacROSLaunchFragmentSpec( + 'isaac_ros_image_proc', 'IsaacROSResizeRectifyStereoLaunchFragment', + 'isaac_ros_image_resize_rectify_stereo_core.launch.py'), + + # Isaac ROS Depth Proc + # Convert depth to metric + 'convert_metric': IsaacROSLaunchFragmentSpec( + 'isaac_ros_depth_image_proc', 'IsaacROSConvertMetricLaunchFragment', + 'isaac_ros_depth_image_proc_convert_metric_core.launch.py'), + # Convert depth to pointcloud + 'point_cloud_xyz': IsaacROSLaunchFragmentSpec( + 'isaac_ros_depth_image_proc', 'IsaacROSPointCloudXyzLaunchFragment', + 'isaac_ros_depth_image_proc_point_cloud_xyz_core.launch.py'), + # Convert depth to colorized pointcloud + 'point_cloud_xyzrgb': IsaacROSLaunchFragmentSpec( + 'isaac_ros_depth_image_proc', 'IsaacROSPointCloudXyzrgbLaunchFragment', + 'isaac_ros_depth_image_proc_point_cloud_xyzrgb_core.launch.py'), + + # Isaac ROS Stereo Image Proc + # SGM disparity estimation + 'disparity': IsaacROSLaunchFragmentSpec( + 'isaac_ros_stereo_image_proc', 'IsaacROSDisparityLaunchFragment', + 'isaac_ros_stereo_image_proc_disparity_core.launch.py'), + # Convert disparity to colorized pointcloud + 'point_cloud': IsaacROSLaunchFragmentSpec( + 'isaac_ros_stereo_image_proc', 'IsaacROSPointCloudLaunchFragment', + 'isaac_ros_stereo_image_proc_point_cloud_core.launch.py'), + # Convert disparity to depth + 'disparity_to_depth': IsaacROSLaunchFragmentSpec( + 'isaac_ros_stereo_image_proc', 'IsaacROSDisparityToDepthLaunchFragment', + 'isaac_ros_stereo_image_proc_disparity_to_depth_core.launch.py'), + + ############### + # Core Graphs # + ############### + + # Isaac ROS AprilTag + 'apriltag': IsaacROSLaunchFragmentSpec( + 'isaac_ros_apriltag', 'IsaacROSAprilTagLaunchFragment'), + + # Isaac ROS Compression + 'stereo_h264_decoder': IsaacROSLaunchFragmentSpec( + 'isaac_ros_h264_decoder', 'IsaacROSStereoH264DecoderLaunchFragment'), + 'stereo_h264_encoder': IsaacROSLaunchFragmentSpec( + 'isaac_ros_h264_encoder', 'IsaacROSStereoH264EncoderLaunchFragment'), + + # Isaac ROS Depth Segmentation + 'bi3d': IsaacROSLaunchFragmentSpec( + 'isaac_ros_bi3d', 'IsaacROSBi3DLaunchFragment'), + + # Isaac ROS DNN Stereo Depth + 'ess_disparity': IsaacROSLaunchFragmentSpec( + 'isaac_ros_ess', 'IsaacROSEssLaunchFragment'), + + # Isaac ROS Freespace Segmentation + 'bi3d_freespace': IsaacROSLaunchFragmentSpec( + 'isaac_ros_bi3d_freespace', 'IsaacROSBi3DFreespaceLaunchFragment'), + + # Isaac ROS Image Pipeline + 'resize': IsaacROSLaunchFragmentSpec( + 'isaac_ros_image_proc', 'IsaacROSResizeLaunchFragment', + 'isaac_ros_image_resize_core.launch.py'), + 'flip': IsaacROSLaunchFragmentSpec( + 'isaac_ros_image_proc', 'IsaacROSFlipLaunchFragment', + 'isaac_ros_image_flip_core.launch.py'), + 'crop': IsaacROSLaunchFragmentSpec( + 'isaac_ros_image_proc', 'IsaacROSCropLaunchFragment', + 'isaac_ros_image_crop_core.launch.py'), + 'color_conversion': IsaacROSLaunchFragmentSpec( + 'isaac_ros_image_proc', 'IsaacROSColorConversionLaunchFragment', + 'isaac_ros_image_color_conversion_core.launch.py'), + + # Isaac ROS Image Segmentation + 'segformer': IsaacROSLaunchFragmentSpec( + 'isaac_ros_segformer', 'IsaacROSSegformerLaunchFragment'), + 'segment_anything': IsaacROSLaunchFragmentSpec( + 'isaac_ros_segment_anything', 'IsaacROSSegmentAnythingLaunchFragment'), + 'unet': IsaacROSLaunchFragmentSpec( + 'isaac_ros_unet', 'IsaacROSUNetLaunchFragment'), + + # Isaac ROS Object Detection + 'rtdetr': IsaacROSLaunchFragmentSpec( + 'isaac_ros_rtdetr', 'IsaacROSRtDetrLaunchFragment'), + 'detectnet': IsaacROSLaunchFragmentSpec( + 'isaac_ros_detectnet', 'IsaacROSDetectnetLaunchFragment'), + 'yolov8': IsaacROSLaunchFragmentSpec( + 'isaac_ros_yolov8', 'IsaacROSYolov8LaunchFragment'), + + # Isaac ROS Pose Estimation + 'centerpose': IsaacROSLaunchFragmentSpec( + 'isaac_ros_centerpose', 'IsaacROSCenterPoseLaunchFragment'), + 'centerpose_visualizer': IsaacROSLaunchFragmentSpec( + 'isaac_ros_centerpose', 'IsaacROSCenterPoseVisualizerLaunchFragment', + 'isaac_ros_centerpose_visualizer_core.launch.py'), + 'dope': IsaacROSLaunchFragmentSpec( + 'isaac_ros_dope', 'IsaacROSDopeLaunchFragment'), + 'foundationpose': IsaacROSLaunchFragmentSpec( + 'isaac_ros_foundationpose', 'IsaacROSFoundationPoseLaunchFragment'), + 'foundationpose_tracking': IsaacROSLaunchFragmentSpec( + 'isaac_ros_foundationpose', 'IsaacROSFoundationPoseTrackingLaunchFragment', + 'isaac_ros_foundationpose_tracking_core.launch.py'), + + # Isaac ROS Visual Slam + 'visual_slam': IsaacROSLaunchFragmentSpec( + 'isaac_ros_visual_slam', 'IsaacROSVisualSlamLaunchFragment'), +} diff --git a/isaac_ros_examples/launch/isaac_ros_examples.launch.py b/isaac_ros_examples/launch/isaac_ros_examples.launch.py new file mode 100644 index 0000000..90e66ba --- /dev/null +++ b/isaac_ros_examples/launch/isaac_ros_examples.launch.py @@ -0,0 +1,157 @@ +# 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 importlib.util +import json +import os + +from ament_index_python.packages import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment, IsaacROSLaunchFragmentSpec, \ + LAUNCH_FRAGMENT_SPECS +import launch +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +import rclpy + + +def load_launch_fragment(fragment_spec: IsaacROSLaunchFragmentSpec) -> IsaacROSLaunchFragment: + """ + Load a Launch Fragment as a Python class from a specific file in a different ROS package. + + Parameters + ---------- + fragment_spec : IsaacROSLaunchFragmentSpec + Launch Fragment specification that indicates the package, filename, and class name to load + + Returns + ------- + IsaacROSLaunchFragment + The loaded Launch Fragment class + + """ + path = os.path.join( + get_package_share_directory(fragment_spec.package), + 'launch', + fragment_spec.filename + ) + spec = importlib.util.spec_from_file_location(f'{fragment_spec.package}.module', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return getattr(module, fragment_spec.class_name) + + +def parse_launch_fragments(context): + """ + Parse the launch fragments specified by the user and prepare the composite launch graph. + + Parameters + ---------- + context : LaunchContext + The launch context, containing all arguments passed in by the user + + Returns + ------- + List[LaunchDescriptionEntity] + List of launch actions, including the composite node container and all composable nodes + + """ + logger = rclpy.logging.get_logger('isaac_ros_examples') + + launch_fragments_str: str = \ + context.perform_substitution(LaunchConfiguration('launch_fragments')) + + if launch_fragments_str == '': + logger.warning('No launch fragments specified.') + return [] + + # Parse string into fragments list + fragments = [] + for fragment_key in launch_fragments_str.split(','): + if not fragment_key: + continue + + assert fragment_key in LAUNCH_FRAGMENT_SPECS, \ + f'Invalid launch fragment key: {fragment_key}' + + fragment_spec = LAUNCH_FRAGMENT_SPECS[fragment_key] + fragment = load_launch_fragment(fragment_spec) + fragments.append(fragment) + + # Collect interface specs from all fragments + interface_specs = {} + + for fragment in fragments: + for key, value in fragment.get_interface_specs().items(): + if key in interface_specs: + logger.warning( + f'Warning: Interface spec "{key}" is specified multiple times. ' + 'The last value will be used' + ) + interface_specs[key] = value + + # If interface specs file is provided, override specs with values from file + interface_specs_file: str = \ + context.perform_substitution(LaunchConfiguration('interface_specs_file')) + + if interface_specs_file: + with open(interface_specs_file, 'r') as file: + override_specs = json.load(file) + for key, value in override_specs.items(): + if key in interface_specs: + logger.info( + f'Info: Interface spec "{key}" is overridden. ' + 'The value from the overriding file will be used' + ) + interface_specs[key] = value + + # Collect composable nodes and launch actions from all fragments + composable_nodes = [] + launch_actions = [] + for fragment in fragments: + composable_nodes.extend(fragment.get_composable_nodes(interface_specs).values()) + launch_actions.extend(fragment.get_launch_actions(interface_specs).values()) + + container = ComposableNodeContainer( + package='rclcpp_components', + name='container', + namespace='isaac_ros_examples', + executable='component_container_mt', + composable_node_descriptions=composable_nodes, + output='screen' + ) + + return launch_actions + [container] + + +def generate_launch_description(): + + launch_args = [ + DeclareLaunchArgument( + 'launch_fragments', + default_value='', + description='Comma separated list of fragment keys to launch', + ), + DeclareLaunchArgument( + 'interface_specs_file', + default_value='', + description='Path to JSON file containing manually-specified interface specs', + ) + ] + + return launch.LaunchDescription( + launch_args + [OpaqueFunction(function=parse_launch_fragments)]) diff --git a/isaac_ros_examples/package.xml b/isaac_ros_examples/package.xml new file mode 100644 index 0000000..12441d5 --- /dev/null +++ b/isaac_ros_examples/package.xml @@ -0,0 +1,21 @@ + + + + isaac_ros_examples + 3.0.0 + Utilities for launching example graphs of Isaac ROS packages + + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros/ + Jaiveer Singh + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_examples/resource/isaac_ros_examples b/isaac_ros_examples/resource/isaac_ros_examples new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_examples/setup.cfg b/isaac_ros_examples/setup.cfg new file mode 100644 index 0000000..86d98bb --- /dev/null +++ b/isaac_ros_examples/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_examples +[install] +install_scripts=$base/lib/isaac_ros_examples diff --git a/isaac_ros_examples/setup.py b/isaac_ros_examples/setup.py new file mode 100644 index 0000000..34a54bd --- /dev/null +++ b/isaac_ros_examples/setup.py @@ -0,0 +1,46 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_examples' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Utilities for launching example graphs of Isaac ROS packages', + license='NVIDIA Isaac ROS Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/isaac_ros_examples/test/test_copyright.py b/isaac_ros_examples/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/isaac_ros_examples/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_examples/test/test_flake8.py b/isaac_ros_examples/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_examples/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_examples/test/test_pep257.py b/isaac_ros_examples/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_examples/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_multicamera_vo/config/foxglove_bridge_launch.xml b/isaac_ros_multicamera_vo/config/foxglove_bridge_launch.xml new file mode 100644 index 0000000..05c6862 --- /dev/null +++ b/isaac_ros_multicamera_vo/config/foxglove_bridge_launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/isaac_ros_multicamera_vo/foxglove_layouts/foxglove_layout_hawk.json b/isaac_ros_multicamera_vo/foxglove_layouts/foxglove_layout_hawk.json new file mode 100644 index 0000000..4119bd8 --- /dev/null +++ b/isaac_ros_multicamera_vo/foxglove_layouts/foxglove_layout_hawk.json @@ -0,0 +1,407 @@ +{ + "configById": { + "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": 9.26582460309705, + "phi": 63.83432019391079, + "thetaOffset": 68.73179120726533, + "targetOffset": [ + 4.369434512403312, + -0.5063632291349903, + -8.588853224570202e-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:back_2d_lidar": { + "visible": false + }, + "frame:base_link": { + "visible": true + }, + "frame:back_2d_lidar_mount": { + "visible": false + }, + "frame:back_fisheye_camera": { + "visible": false + }, + "frame:back_fisheye_camera_mount": { + "visible": false + }, + "frame:back_fisheye_camera_optical": { + "visible": false + }, + "frame:back_stereo_camera_imu": { + "visible": false + }, + "frame:back_stereo_camera": { + "visible": false + }, + "frame:back_stereo_camera_left": { + "visible": false + }, + "frame:back_stereo_camera_left_optical": { + "visible": false + }, + "frame:back_stereo_camera_mount": { + "visible": false + }, + "frame:back_stereo_camera_right": { + "visible": false + }, + "frame:back_stereo_camera_right_optical": { + "visible": false + }, + "frame:chassis_imu": { + "visible": false + }, + "frame:front_2d_lidar": { + "visible": false + }, + "frame:front_2d_lidar_mount": { + "visible": false + }, + "frame:front_3d_lidar": { + "visible": false + }, + "frame:front_3d_lidar_mount": { + "visible": false + }, + "frame:front_fisheye_camera": { + "visible": false + }, + "frame:front_fisheye_camera_mount": { + "visible": false + }, + "frame:front_fisheye_camera_optical": { + "visible": false + }, + "frame:front_stereo_camera_imu": { + "visible": false + }, + "frame:front_stereo_camera": { + "visible": false + }, + "frame:front_stereo_camera_left": { + "visible": false + }, + "frame:front_stereo_camera_left_optical": { + "visible": false + }, + "frame:front_stereo_camera_mount": { + "visible": false + }, + "frame:front_stereo_camera_right": { + "visible": false + }, + "frame:front_stereo_camera_right_optical": { + "visible": false + }, + "frame:left_fisheye_camera": { + "visible": false + }, + "frame:left_fisheye_camera_mount": { + "visible": false + }, + "frame:left_fisheye_camera_optical": { + "visible": false + }, + "frame:left_stereo_camera_imu": { + "visible": false + }, + "frame:left_stereo_camera": { + "visible": false + }, + "frame:left_stereo_camera_left": { + "visible": false + }, + "frame:left_stereo_camera_left_optical": { + "visible": false + }, + "frame:left_stereo_camera_mount": { + "visible": false + }, + "frame:left_stereo_camera_right": { + "visible": false + }, + "frame:left_stereo_camera_right_optical": { + "visible": false + }, + "frame:nova_carter": { + "visible": false + }, + "frame:right_fisheye_camera": { + "visible": false + }, + "frame:right_fisheye_camera_mount": { + "visible": false + }, + "frame:right_fisheye_camera_optical": { + "visible": false + }, + "frame:right_stereo_camera_imu": { + "visible": false + }, + "frame:right_stereo_camera": { + "visible": false + }, + "frame:right_stereo_camera_left": { + "visible": false + }, + "frame:right_stereo_camera_left_optical": { + "visible": false + }, + "frame:right_stereo_camera_mount": { + "visible": false + }, + "frame:right_stereo_camera_right": { + "visible": false + }, + "frame:right_stereo_camera_right_optical": { + "visible": false + }, + "frame:nova_carter_caster_frame_base": { + "visible": false + }, + "frame:caster_wheel_left": { + "visible": false + }, + "frame:caster_swivel_left": { + "visible": false + }, + "frame:caster_wheel_right": { + "visible": false + }, + "frame:caster_swivel_right": { + "visible": false + }, + "frame:wheel_left": { + "visible": false + }, + "frame:wheel_right": { + "visible": false + }, + "frame:nova_developer_kit_mount": { + "visible": false + }, + "frame:nova_developer_kit_link": { + "visible": false + }, + "frame:rear_fisheye_camera": { + "visible": false + }, + "frame:rear_fisheye_camera_optical": { + "visible": false + }, + "frame:rear_stereo_camera": { + "visible": false + }, + "frame:rear_stereo_camera_imu": { + "visible": false + }, + "frame:rear_stereo_camera_left": { + "visible": false + }, + "frame:rear_stereo_camera_left_optical": { + "visible": false + }, + "frame:rear_stereo_camera_right": { + "visible": false + }, + "frame:rear_stereo_camera_right_optical": { + "visible": false + } + }, + "topics": { + "/robot_description": { + "visible": true + }, + "/pointcloud": { + "visible": false, + "colorField": "z", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/front_stereo_camera/left/image_raw": { + "visible": false, + "frameLocked": true, + "cameraInfoTopic": "/front_stereo_camera/left/camera_info", + "distance": 1, + "planarProjectionFactor": 0, + "color": "#ffffff" + }, + "/front_fisheye_camera/left/image_raw": { + "visible": false, + "frameLocked": true, + "cameraInfoTopic": "/front_fisheye_camera/left/camera_info", + "distance": 1, + "planarProjectionFactor": 0, + "color": "#ffffff" + }, + "/front_2d_lidar/scan": { + "visible": true, + "colorField": "intensity", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/back_2d_lidar/scan": { + "visible": true, + "colorField": "intensity", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/nvblox_node/static_esdf_pointcloud": { + "visible": true, + "colorField": "intensity", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/nvblox_node/tsdf_layer": { + "visible": false + }, + "/left_stereo_camera/left/image_raw": { + "visible": false, + "frameLocked": true, + "cameraInfoTopic": "/left_stereo_camera/left/camera_info", + "distance": 1, + "planarProjectionFactor": 0, + "color": "#ffffff" + }, + "/right_stereo_camera/left/image_raw": { + "visible": true, + "frameLocked": true, + "cameraInfoTopic": "/right_stereo_camera/left/camera_info", + "distance": 1, + "planarProjectionFactor": 0, + "color": "#ffffff" + }, + "/map": { + "visible": false + }, + "/local_costmap/costmap": { + "visible": true, + "colorMode": "costmap" + }, + "/amcl_pose": { + "visible": false + }, + "/front_3d_lidar/scan": { + "visible": false, + "colorField": "intensity", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/plan": { + "visible": true + }, + "/global_costmap/costmap": { + "visible": true, + "colorMode": "costmap" + }, + "/initialpose": { + "visible": true + }, + "/front_3d_lidar/pointcloud": { + "visible": false, + "colorField": "z", + "colorMode": "colormap", + "colorMap": "turbo" + }, + "/visual_slam/tracking/vo_path": { + "visible": true, + "lineWidth": 0.05, + "gradient": [ + "#17c426ff", + "#17c426ff" + ] + }, + "/left_stereo_camera/left/camera_info": { + "visible": false + }, + "/front_stereo_camera/left/camera_info": { + "visible": false + }, + "/visual_slam/vis/observations_cloud": { + "visible": true, + "colorField": "rgb", + "colorMode": "rgb", + "colorMap": "turbo", + "pointSize": 3 + }, + "/visual_slam/vis/landmarks_cloud": { + "visible": false + }, + "/front_stereo_camera/right/image_raw": { + "visible": true, + "frameLocked": true, + "cameraInfoTopic": "/front_stereo_camera/right/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": "3D!18i6zy7" +} \ No newline at end of file diff --git a/isaac_ros_multicamera_vo/isaac_ros_multicamera_vo/__init__.py b/isaac_ros_multicamera_vo/isaac_ros_multicamera_vo/__init__.py new file mode 100644 index 0000000..34d9bfe --- /dev/null +++ b/isaac_ros_multicamera_vo/isaac_ros_multicamera_vo/__init__.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 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.launch.py new file mode 100644 index 0000000..2305ad5 --- /dev/null +++ b/isaac_ros_multicamera_vo/launch/isaac_ros_visual_slam_multihawk.launch.py @@ -0,0 +1,325 @@ +# 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 Carter Hawk ids. For Nova DevKit Hawk use following ids: front: 3, left: 5, right: 4 + 'front_stereo_camera': 5, + 'back_stereo_camera': 6, + 'left_stereo_camera': 7, + 'right_stereo_camera': 2 +} + + +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_optical_frame_name': f'{camera_name}_left_optical', + 'right_optical_frame_name': f'{camera_name}_right_optical'}], + 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=False): + 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 [] + + +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_carter_description = lu.include( + 'nova_carter_description', + 'launch/nova_carter_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': False, + + # 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', + 'back_stereo_camera_left_optical', + 'back_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': 8, + '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', '/back_stereo_camera/left/image_raw'), + ('visual_slam/camera_info_2', '/back_stereo_camera/left/camera_info'), + ('visual_slam/image_3', '/back_stereo_camera/right/image_raw'), + ('visual_slam/camera_info_3', '/back_stereo_camera/right/camera_info'), + ('visual_slam/image_4', '/left_stereo_camera/left/image_raw'), + ('visual_slam/camera_info_4', '/left_stereo_camera/left/camera_info'), + ('visual_slam/image_5', '/left_stereo_camera/right/image_raw'), + ('visual_slam/camera_info_5', '/left_stereo_camera/right/camera_info'), + ('visual_slam/image_6', '/right_stereo_camera/left/image_raw'), + ('visual_slam/camera_info_6', '/right_stereo_camera/left/camera_info'), + ('visual_slam/image_7', '/right_stereo_camera/right/image_raw'), + ('visual_slam/camera_info_7', '/right_stereo_camera/right/camera_info') + ] + ) + + visual_slam_container = ComposableNodeContainer( + name='visual_slam_launch_container', + package='rclcpp_components', + executable='component_container', + 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] + ), + output='screen', + condition=LaunchConfigurationEquals('use_rectify', 'False') + ) + + visual_slam_rect_container = ComposableNodeContainer( + name='visual_slam_launch_container', + package='rclcpp_components', + executable='component_container', + 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], + ), + 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('back', 'left'), + hawk_decoder('back', '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('back_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=['/back_stereo_camera/left/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_2'], + dst=['/back_stereo_camera/left/camera_info_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/image_3'], + dst=['/back_stereo_camera/right/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_3'], + dst=['/back_stereo_camera/right/camera_info_rect'], + condition=use_rectify), + + SetRemap(src=['visual_slam/image_4'], + dst=['/left_stereo_camera/left/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_4'], + dst=['/left_stereo_camera/left/camera_info_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/image_5'], + dst=['/left_stereo_camera/right/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_5'], + dst=['/left_stereo_camera/right/camera_info_rect'], + condition=use_rectify), + + SetRemap(src=['visual_slam/image_6'], + dst=['/right_stereo_camera/left/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_6'], + dst=['/right_stereo_camera/left/camera_info_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/image_7'], + dst=['/right_stereo_camera/right/image_rect'], + condition=use_rectify), + SetRemap(src=['visual_slam/camera_info_7'], + 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_carter_description, + foxglove_bridge_launch, + hawk_image_capture, + group_action, + hawk_h264_decoders]) diff --git a/isaac_ros_multicamera_vo/package.xml b/isaac_ros_multicamera_vo/package.xml new file mode 100644 index 0000000..3e61aec --- /dev/null +++ b/isaac_ros_multicamera_vo/package.xml @@ -0,0 +1,23 @@ + + + + isaac_ros_multicamera_vo + 3.0.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_visual_slam + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_multicamera_vo/resource/isaac_ros_multicamera_vo b/isaac_ros_multicamera_vo/resource/isaac_ros_multicamera_vo new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_multicamera_vo/rviz/hawk_multicamera_vo.rviz b/isaac_ros_multicamera_vo/rviz/hawk_multicamera_vo.rviz new file mode 100644 index 0000000..b7e07d1 --- /dev/null +++ b/isaac_ros_multicamera_vo/rviz/hawk_multicamera_vo.rviz @@ -0,0 +1,764 @@ +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: 932 + - 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 + back_2d_lidar: + Value: true + back_2d_lidar_mount: + Value: true + back_fisheye_camera: + Value: true + back_fisheye_camera_mount: + Value: true + back_fisheye_camera_optical: + Value: true + back_stereo_camera: + Value: true + back_stereo_camera_imu: + Value: true + back_stereo_camera_left: + Value: true + back_stereo_camera_left_optical: + Value: true + back_stereo_camera_mount: + Value: true + back_stereo_camera_right: + Value: true + back_stereo_camera_right_optical: + Value: true + base_link: + Value: true + caster_swivel_left: + Value: true + caster_swivel_right: + Value: true + caster_wheel_left: + Value: true + caster_wheel_right: + Value: true + chassis_imu: + Value: true + front_2d_lidar: + Value: true + front_2d_lidar_mount: + Value: true + front_3d_lidar: + Value: true + front_3d_lidar_mount: + Value: true + front_fisheye_camera: + Value: true + front_fisheye_camera_mount: + Value: true + front_fisheye_camera_optical: + Value: true + front_stereo_camera: + Value: true + front_stereo_camera_imu: + Value: true + front_stereo_camera_left: + Value: true + front_stereo_camera_left_optical: + Value: true + front_stereo_camera_mount: + Value: true + front_stereo_camera_right: + Value: true + front_stereo_camera_right_optical: + Value: true + left_fisheye_camera: + Value: true + left_fisheye_camera_mount: + Value: true + left_fisheye_camera_optical: + Value: true + left_stereo_camera: + Value: true + left_stereo_camera_imu: + Value: true + left_stereo_camera_left: + Value: true + left_stereo_camera_left_optical: + Value: true + left_stereo_camera_mount: + Value: true + left_stereo_camera_right: + Value: true + left_stereo_camera_right_optical: + Value: true + map: + Value: true + nova_carter: + Value: true + nova_carter_caster_frame_base: + Value: true + odom: + Value: true + rear_fisheye_camera: + Value: true + rear_fisheye_camera_optical: + Value: true + rear_stereo_camera: + Value: true + rear_stereo_camera_imu: + Value: true + rear_stereo_camera_left: + Value: true + rear_stereo_camera_left_optical: + Value: true + rear_stereo_camera_right: + Value: true + rear_stereo_camera_right_optical: + Value: true + right_fisheye_camera: + Value: true + right_fisheye_camera_mount: + Value: true + right_fisheye_camera_optical: + Value: true + right_stereo_camera: + Value: true + right_stereo_camera_imu: + Value: true + right_stereo_camera_left: + Value: true + right_stereo_camera_left_optical: + Value: true + right_stereo_camera_mount: + Value: true + right_stereo_camera_right: + Value: true + right_stereo_camera_right_optical: + Value: true + wheel_left: + Value: true + wheel_right: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + back_2d_lidar: + back_2d_lidar_mount: + {} + back_fisheye_camera: + back_fisheye_camera_mount: + {} + back_fisheye_camera_optical: + {} + back_stereo_camera: + back_stereo_camera_imu: + {} + back_stereo_camera_left: + {} + back_stereo_camera_left_optical: + {} + back_stereo_camera_mount: + {} + back_stereo_camera_right: + {} + back_stereo_camera_right_optical: + {} + chassis_imu: + {} + front_2d_lidar: + front_2d_lidar_mount: + {} + front_3d_lidar: + front_3d_lidar_mount: + {} + front_fisheye_camera: + front_fisheye_camera_mount: + {} + front_fisheye_camera_optical: + {} + front_stereo_camera: + front_stereo_camera_imu: + {} + front_stereo_camera_left: + {} + front_stereo_camera_left_optical: + {} + front_stereo_camera_mount: + {} + front_stereo_camera_right: + {} + front_stereo_camera_right_optical: + {} + left_fisheye_camera: + left_fisheye_camera_mount: + {} + left_fisheye_camera_optical: + {} + left_stereo_camera: + left_stereo_camera_imu: + {} + left_stereo_camera_left: + {} + left_stereo_camera_left_optical: + {} + left_stereo_camera_mount: + {} + left_stereo_camera_right: + {} + left_stereo_camera_right_optical: + {} + nova_carter: + {} + rear_fisheye_camera: + rear_fisheye_camera_optical: + {} + rear_stereo_camera: + rear_stereo_camera_imu: + {} + rear_stereo_camera_left: + rear_stereo_camera_left_optical: + {} + rear_stereo_camera_right: + rear_stereo_camera_right_optical: + {} + right_fisheye_camera: + right_fisheye_camera_mount: + {} + right_fisheye_camera_optical: + {} + right_stereo_camera: + right_stereo_camera_imu: + {} + right_stereo_camera_left: + {} + right_stereo_camera_left_optical: + {} + right_stereo_camera_mount: + {} + right_stereo_camera_right: + {} + right_stereo_camera_right_optical: + {} + 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: /front_stereo_camera/left/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Left Stereo Camera (Left Image) + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /left_stereo_camera/right/image_raw + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Right Stereo Camera (Left Image) + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /right_stereo_camera/left/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: false + 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: /back_stereo_camera/left/image_raw + Value: false + 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: 2272 + Hide Left Dock: false + Hide Right Dock: true + Left Stereo Camera (Left Image): + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000003e30000084efc0200000014fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000004920000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650200000403000002880000037d00000228fb0000000a0049006d00610067006502000001fc000002110000058400000296fb0000000a0049006d00610067006503000006fe000005e200000433000001fffb0000002000460072006f006e00740020004c00650066007400200049006d00610067006500000004a8000001ae0000000000000000fb0000002200460072006f006e007400200052006900670068007400200049006d0061006700650000000658000000690000000000000000fb0000000a0049006d00610067006500000002d0000000c70000000000000000fb0000001e004200610063006b0020004c00650066007400200049006d00610067006500000006880000022f0000000000000000fb00000020004200610063006b00200052006900670068007400200049006d0061006700650000000809000000ae0000000000000000fb0000004000460072006f006e0074002000530074006500720065006f002000430061006d00650072006100200028004c00650066007400200049006d00610067006500290100000507000001c30000004500fffffffb0000003e004c006500660074002000530074006500720065006f002000430061006d00650072006100200028004c00650066007400200049006d0061006700650029000000066d000001500000004500fffffffb0000004000520069006700680074002000530074006500720065006f002000430061006d00650072006100200028004c00650066007400200049006d006100670065002901000006d6000001e10000004500fffffffb0000003e004200610063006b002000530074006500720065006f002000430061006d00650072006100200028004c00650066007400200049006d006100670065002900000007f7000000c00000004500ffffff00000001000001b40000035cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035c0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000a850000084e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Right Stereo Camera (Left Image): + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 3700 + X: 140 + Y: 54 diff --git a/isaac_ros_multicamera_vo/setup.cfg b/isaac_ros_multicamera_vo/setup.cfg new file mode 100644 index 0000000..16156f2 --- /dev/null +++ b/isaac_ros_multicamera_vo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_multicamera_vo +[install] +install_scripts=$base/lib/isaac_ros_multicamera_vo diff --git a/isaac_ros_multicamera_vo/setup.py b/isaac_ros_multicamera_vo/setup.py new file mode 100644 index 0000000..14ab02a --- /dev/null +++ b/isaac_ros_multicamera_vo/setup.py @@ -0,0 +1,48 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_multicamera_vo' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Isaac ROS Launch Fragment for cuVSLAM Visual Odomtry for multicamera cases', + license='NVIDIA Isaac ROS Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/isaac_ros_multicamera_vo/test/test_copyright.py b/isaac_ros_multicamera_vo/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/isaac_ros_multicamera_vo/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_multicamera_vo/test/test_flake8.py b/isaac_ros_multicamera_vo/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_multicamera_vo/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_multicamera_vo/test/test_pep257.py b/isaac_ros_multicamera_vo/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_multicamera_vo/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_realsense/config/realsense_mono.yaml b/isaac_ros_realsense/config/realsense_mono.yaml new file mode 100644 index 0000000..1882ac2 --- /dev/null +++ b/isaac_ros_realsense/config/realsense_mono.yaml @@ -0,0 +1,14 @@ +device_type: '' +serial_no: '' +usb_port_id: '' + +enable_accel: false +enable_color: true +enable_depth: false +enable_gyro: false +enable_infra1: false +enable_infra2: false + +rgb_camera: + profile: '640x480x15' +color_qos: "SYSTEM_DEFAULT" diff --git a/isaac_ros_realsense/config/realsense_mono_depth.yaml b/isaac_ros_realsense/config/realsense_mono_depth.yaml new file mode 100644 index 0000000..0694003 --- /dev/null +++ b/isaac_ros_realsense/config/realsense_mono_depth.yaml @@ -0,0 +1,4 @@ +enable_infra1: false +enable_infra2: false + +align_depth.enable: true diff --git a/isaac_ros_realsense/config/realsense_stereo.yaml b/isaac_ros_realsense/config/realsense_stereo.yaml new file mode 100644 index 0000000..0d79385 --- /dev/null +++ b/isaac_ros_realsense/config/realsense_stereo.yaml @@ -0,0 +1,16 @@ +device_type: '' +serial_no: '' +usb_port_id: '' + +enable_accel: false +enable_color: false +enable_depth: false +enable_gyro: false +enable_infra1: true +enable_infra2: true + +depth_qos: "SYSTEM_DEFAULT" +depth_module: + profile: '640x360x90' + emitter_enabled: 0 + emitter_on_off: false \ No newline at end of file diff --git a/isaac_ros_realsense/config/realsense_stereo_imu.yaml b/isaac_ros_realsense/config/realsense_stereo_imu.yaml new file mode 100644 index 0000000..468ef1f --- /dev/null +++ b/isaac_ros_realsense/config/realsense_stereo_imu.yaml @@ -0,0 +1,19 @@ +device_type: '' +serial_no: '' +usb_port_id: '' + +enable_accel: true +enable_color: false +enable_depth: false +enable_gyro: true +enable_infra1: true +enable_infra2: true +gyro_fps: 200 +accel_fps: 200 +unite_imu_method: 2 + +depth_qos: "SYSTEM_DEFAULT" +depth_module: + profile: '640x360x90' + emitter_enabled: 0 + emitter_on_off: false \ No newline at end of file diff --git a/isaac_ros_realsense/isaac_ros_realsense/__init__.py b/isaac_ros_realsense/isaac_ros_realsense/__init__.py new file mode 100644 index 0000000..34d9bfe --- /dev/null +++ b/isaac_ros_realsense/isaac_ros_realsense/__init__.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 diff --git a/isaac_ros_realsense/launch/isaac_ros_realsense_mono_core.launch.py b/isaac_ros_realsense/launch/isaac_ros_realsense_mono_core.launch.py new file mode 100644 index 0000000..e62a177 --- /dev/null +++ b/isaac_ros_realsense/launch/isaac_ros_realsense_mono_core.launch.py @@ -0,0 +1,76 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +class IsaacROSRealSenseMonoLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 640, 'height': 480}, + 'camera_frame': 'camera_color_optical_frame', + 'focal_length': { + # Approximation - most RealSense cameras should be close to this value + 'f_x': 380.0, + # Approximation - most RealSense cameras should be close to this value + 'f_y': 380.0 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + realsense_config_file_path = os.path.join( + get_package_share_directory('isaac_ros_realsense'), + 'config', 'realsense_mono.yaml' + ) + + return { + 'camera_node': ComposableNode( + package='realsense2_camera', + plugin='realsense2_camera::RealSenseNodeFactory', + name='realsense2_camera', + namespace='', + parameters=[ + realsense_config_file_path + ], + remappings=[('color/image_raw', 'image_raw'), + ('color/camera_info', 'camera_info')] + ) + } + + +def generate_launch_description(): + realsense_container = ComposableNodeContainer( + package='rclcpp_components', + name='realsense_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=IsaacROSRealSenseMonoLaunchFragment.get_composable_nodes(), + output='screen' + ) + + return launch.LaunchDescription( + [realsense_container] + IsaacROSRealSenseMonoLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_realsense/launch/isaac_ros_realsense_mono_rect_core.launch.py b/isaac_ros_realsense/launch/isaac_ros_realsense_mono_rect_core.launch.py new file mode 100644 index 0000000..4d07bd7 --- /dev/null +++ b/isaac_ros_realsense/launch/isaac_ros_realsense_mono_rect_core.launch.py @@ -0,0 +1,78 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +class IsaacROSRealSenseMonoRectLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 640, 'height': 480}, + 'camera_frame': 'camera_color_optical_frame', + 'focal_length': { + # Approximation - most RealSense cameras should be close to this value + 'f_x': 380.0, + # Approximation - most RealSense cameras should be close to this value + 'f_y': 380.0 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + realsense_config_file_path = os.path.join( + get_package_share_directory('isaac_ros_realsense'), + 'config', 'realsense_mono.yaml' + ) + + return { + 'camera_node': ComposableNode( + package='realsense2_camera', + plugin='realsense2_camera::RealSenseNodeFactory', + name='realsense2_camera', + namespace='', + parameters=[ + realsense_config_file_path + ], + remappings=[('color/image_raw', 'image_rect'), + ('color/camera_info', 'camera_info_rect')] + ) + } + + +def generate_launch_description(): + realsense_container = ComposableNodeContainer( + package='rclcpp_components', + name='realsense_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=( + IsaacROSRealSenseMonoRectLaunchFragment.get_composable_nodes() + ), + output='screen' + ) + + return launch.LaunchDescription( + [realsense_container] + IsaacROSRealSenseMonoRectLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_realsense/launch/isaac_ros_realsense_mono_rect_depth_core.launch.py b/isaac_ros_realsense/launch/isaac_ros_realsense_mono_rect_depth_core.launch.py new file mode 100644 index 0000000..32937e1 --- /dev/null +++ b/isaac_ros_realsense/launch/isaac_ros_realsense_mono_rect_depth_core.launch.py @@ -0,0 +1,91 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +WIDTH = 1280 +HEIGHT = 720 + + +class IsaacROSRealSenseMonoRectDepthLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 640, 'height': 480}, + 'camera_frame': 'camera_color_optical_frame', + 'focal_length': { + # Approximation - most RealSense cameras should be close to this value + 'f_x': 380.0, + # Approximation - most RealSense cameras should be close to this value + 'f_y': 380.0 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + realsense_config_file_path = os.path.join( + get_package_share_directory('isaac_ros_realsense'), + 'config', 'realsense_mono_depth.yaml' + ) + + return { + 'camera_node': ComposableNode( + package='realsense2_camera', + plugin='realsense2_camera::RealSenseNodeFactory', + name='realsense2_camera', + namespace='', + parameters=[ + realsense_config_file_path + ], + remappings=[('color/image_raw', 'image_rect'), + ('color/camera_info', 'camera_info_rect')] + ), + # 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', + name='convert_metric', + remappings=[ + ('image_raw', 'aligned_depth_to_color/image_raw'), + ('image', 'depth') + ] + ), + } + + +def generate_launch_description(): + realsense_container = ComposableNodeContainer( + package='rclcpp_components', + name='realsense_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=( + IsaacROSRealSenseMonoRectDepthLaunchFragment.get_composable_nodes() + ), + output='screen' + ) + + return launch.LaunchDescription( + [realsense_container] + IsaacROSRealSenseMonoRectDepthLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_realsense/launch/isaac_ros_realsense_stereo_rect_core.launch.py b/isaac_ros_realsense/launch/isaac_ros_realsense_stereo_rect_core.launch.py new file mode 100644 index 0000000..04862d0 --- /dev/null +++ b/isaac_ros_realsense/launch/isaac_ros_realsense_stereo_rect_core.launch.py @@ -0,0 +1,121 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +WIDTH = 1280 +HEIGHT = 720 + + +class IsaacROSRealSenseStereoRectLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': WIDTH, 'height': HEIGHT}, + 'camera_frame': 'camera_infra1_optical_frame', + 'focal_length': { + # Approximation - most RealSense cameras should be close to this value + 'f_x': 635.0, + # Approximation - most RealSense cameras should be close to this value + 'f_y': 635.0 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + realsense_config_file = LaunchConfiguration('realsense_config_file') + + return { + 'camera_node': ComposableNode( + package='realsense2_camera', + plugin='realsense2_camera::RealSenseNodeFactory', + name='realsense2_camera', + namespace='', + parameters=[ + realsense_config_file + ], + remappings=[('infra1/image_rect_raw', 'infra1/image_rect_raw_mono'), + ('infra1/camera_info', 'left/camera_info_rect'), + ('infra2/image_rect_raw', 'infra2/image_rect_raw_mono'), + ('infra2/camera_info', 'right/camera_info_rect')] + ), + 'image_format_converter_left_node': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_left', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': WIDTH, + 'image_height': HEIGHT + }], + remappings=[ + ('image_raw', 'infra1/image_rect_raw_mono'), + ('image', 'left/image_rect')] + ), + 'image_format_converter_right_node': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_right', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': WIDTH, + 'image_height': HEIGHT + }], + remappings=[ + ('image_raw', 'infra2/image_rect_raw_mono'), + ('image', 'right/image_rect')] + ) + } + + @staticmethod + def get_launch_actions(interface_specs: Dict[str, Any]) -> \ + Dict[str, launch.actions.OpaqueFunction]: + return { + 'realsense_config_file': DeclareLaunchArgument( + 'realsense_config_file', + default_value=os.path.join( + get_package_share_directory('isaac_ros_realsense'), + 'config', 'realsense_stereo.yaml' + ) + ), + } + + +def generate_launch_description(): + realsense_container = ComposableNodeContainer( + package='rclcpp_components', + name='realsense_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=( + IsaacROSRealSenseStereoRectLaunchFragment.get_composable_nodes() + ), + output='screen' + ) + + return launch.LaunchDescription( + [realsense_container] + IsaacROSRealSenseStereoRectLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_realsense/launch/isaac_ros_realsense_stereo_rect_imu_core.launch.py b/isaac_ros_realsense/launch/isaac_ros_realsense_stereo_rect_imu_core.launch.py new file mode 100644 index 0000000..063b2cf --- /dev/null +++ b/isaac_ros_realsense/launch/isaac_ros_realsense_stereo_rect_imu_core.launch.py @@ -0,0 +1,109 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 typing import Any, Dict + +from ament_index_python import get_package_share_directory +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +WIDTH = 1280 +HEIGHT = 720 + + +class IsaacROSRealSenseStereoRectImuLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': WIDTH, 'height': HEIGHT}, + 'camera_frame': 'camera_infra1_optical_frame', + 'focal_length': { + # Approximation - most RealSense cameras should be close to this value + 'f_x': 635.0, + # Approximation - most RealSense cameras should be close to this value + 'f_y': 635.0 + } + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + realsense_config_file_path = os.path.join( + get_package_share_directory('isaac_ros_realsense'), + 'config', 'realsense_stereo_imu.yaml' + ) + + return { + 'camera_node': ComposableNode( + package='realsense2_camera', + plugin='realsense2_camera::RealSenseNodeFactory', + name='realsense2_camera', + namespace='', + parameters=[ + realsense_config_file_path + ], + remappings=[('infra1/image_rect_raw', 'infra1/image_rect_raw_mono'), + ('infra1/camera_info', 'left/camera_info_rect'), + ('infra2/image_rect_raw', 'infra2/image_rect_raw_mono'), + ('infra2/camera_info', 'right/camera_info_rect')] + ), + 'image_format_converter_left_node': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_left', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': WIDTH, + 'image_height': HEIGHT + }], + remappings=[ + ('image_raw', 'infra1/image_rect_raw_mono'), + ('image', 'left/image_rect')] + ), + 'image_format_converter_right_node': ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_right', + parameters=[{ + 'encoding_desired': 'rgb8', + 'image_width': WIDTH, + 'image_height': HEIGHT + }], + remappings=[ + ('image_raw', 'infra2/image_rect_raw_mono'), + ('image', 'right/image_rect')] + ) + } + + +def generate_launch_description(): + realsense_container = ComposableNodeContainer( + package='rclcpp_components', + name='realsense_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=( + IsaacROSRealSenseStereoRectImuLaunchFragment.get_composable_nodes() + ), + output='screen' + ) + + return launch.LaunchDescription( + [realsense_container] + IsaacROSRealSenseStereoRectImuLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_realsense/package.xml b/isaac_ros_realsense/package.xml new file mode 100644 index 0000000..4c429f3 --- /dev/null +++ b/isaac_ros_realsense/package.xml @@ -0,0 +1,24 @@ + + + + isaac_ros_realsense + 3.0.0 + Isaac ROS Launch Fragment for Intel RealSense cameras + + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros/ + Jaiveer Singh + + realsense2_camera + isaac_ros_depth_image_proc + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_realsense/resource/isaac_ros_realsense b/isaac_ros_realsense/resource/isaac_ros_realsense new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_realsense/setup.cfg b/isaac_ros_realsense/setup.cfg new file mode 100644 index 0000000..991fb55 --- /dev/null +++ b/isaac_ros_realsense/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_realsense +[install] +install_scripts=$base/lib/isaac_ros_realsense diff --git a/isaac_ros_realsense/setup.py b/isaac_ros_realsense/setup.py new file mode 100644 index 0000000..5b175d7 --- /dev/null +++ b/isaac_ros_realsense/setup.py @@ -0,0 +1,48 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_realsense' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Isaac ROS Launch Fragment for Intel RealSense cameras', + license='NVIDIA Isaac ROS Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/isaac_ros_realsense/test/test_copyright.py b/isaac_ros_realsense/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/isaac_ros_realsense/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_realsense/test/test_flake8.py b/isaac_ros_realsense/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_realsense/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_realsense/test/test_pep257.py b/isaac_ros_realsense/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_realsense/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_usb_cam/isaac_ros_usb_cam/__init__.py b/isaac_ros_usb_cam/isaac_ros_usb_cam/__init__.py new file mode 100644 index 0000000..34d9bfe --- /dev/null +++ b/isaac_ros_usb_cam/isaac_ros_usb_cam/__init__.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 diff --git a/isaac_ros_usb_cam/launch/isaac_ros_usb_cam_core.launch.py b/isaac_ros_usb_cam/launch/isaac_ros_usb_cam_core.launch.py new file mode 100644 index 0000000..30fb835 --- /dev/null +++ b/isaac_ros_usb_cam/launch/isaac_ros_usb_cam_core.launch.py @@ -0,0 +1,59 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 + +from typing import Any, Dict + +from isaac_ros_examples import IsaacROSLaunchFragment +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +class IsaacROSUSBCameraLaunchFragment(IsaacROSLaunchFragment): + + @staticmethod + def get_interface_specs() -> Dict[str, Any]: + return { + 'camera_resolution': {'width': 640, 'height': 480} + } + + @staticmethod + def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: + return { + 'camera_node': ComposableNode( + package='usb_cam', + plugin='usb_cam::UsbCamNode', + name='usb_cam', + parameters=[{ + 'pixel_format': 'yuyv2rgb' + }] + ) + } + + +def generate_launch_description(): + usb_cam_container = ComposableNodeContainer( + package='rclcpp_components', + name='usb_cam_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=IsaacROSUSBCameraLaunchFragment.get_composable_nodes(), + output='screen' + ) + + return launch.LaunchDescription( + [usb_cam_container] + IsaacROSUSBCameraLaunchFragment.get_launch_actions()) diff --git a/isaac_ros_usb_cam/package.xml b/isaac_ros_usb_cam/package.xml new file mode 100644 index 0000000..49b50c3 --- /dev/null +++ b/isaac_ros_usb_cam/package.xml @@ -0,0 +1,23 @@ + + + + isaac_ros_usb_cam + 3.0.0 + Isaac ROS Launch Fragment for USB cameras + + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros/ + Jaiveer Singh + + usb_cam + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_usb_cam/resource/isaac_ros_usb_cam b/isaac_ros_usb_cam/resource/isaac_ros_usb_cam new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_usb_cam/setup.cfg b/isaac_ros_usb_cam/setup.cfg new file mode 100644 index 0000000..1b1a4ed --- /dev/null +++ b/isaac_ros_usb_cam/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_usb_cam +[install] +install_scripts=$base/lib/isaac_ros_usb_cam diff --git a/isaac_ros_usb_cam/setup.py b/isaac_ros_usb_cam/setup.py new file mode 100644 index 0000000..bd0de70 --- /dev/null +++ b/isaac_ros_usb_cam/setup.py @@ -0,0 +1,45 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 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 +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_usb_cam' + +setup( + name=package_name, + version='3.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Isaac ROS Launch Fragment for USB cameras', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/isaac_ros_usb_cam/test/test_copyright.py b/isaac_ros_usb_cam/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/isaac_ros_usb_cam/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_usb_cam/test/test_flake8.py b/isaac_ros_usb_cam/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_usb_cam/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_usb_cam/test/test_pep257.py b/isaac_ros_usb_cam/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_usb_cam/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'