Merge pull request #1 from NVIDIA-ISAAC-ROS/release-3.0

Isaac ROS 3.0.0

Author: hemalshahNV

.gitattributes

@@ -0,0 +1,30 @@
+# 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
+
+# FIXME: Only for DNN packages
+# DNN Model files
+*.onnx filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,7 @@
+# Ignore all pycache files
+**/__pycache__/**
+
+# FIXME: Only for DNN-based packages
+# Ignore TensorRT plan files
+*.plan
+*.engine

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)

LICENSE

@@ -1,2 +1,14 @@
-# isaac_perceptor
-Perception workflows
+# Isaac Perceptor
+
+This repository contains the integrated Isaac Perceptor launch files.
+
+## Setup and Documentation
+
+Visit [Isaac Perceptor](https://nvidia-isaac-ros.github.io/reference_workflows/isaac_perceptor/index.html) to learn how to use this repository.
+
+## Performance
+
+| Sample Graph<br/><br/>                                                                                                                                                                                                                                                                    | Input Size<br/><br/>                | Nova Carter<br/><br/>                                                                                                                                                                                                               |
+|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [DNN Stereo Disparity Live Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_ess_nova_benchmark/scripts/isaac_ros_hawk_1f_2lt_ess_depth_graph.py)<br/><br/><br/>3 Hawk Cameras<br/><br/><br/>1x Full ESS and 2x Throttled Light ESS<br/><br/> | 1200p<br/><br/><br/><br/><br/><br/> | Full: [30.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_hawk_1f_2lt_ess_depth_graph-carter_v2.4.json)<br/><br/><br/>Light: 15.2 fps (avg)<br/><br/><br/><br/>                          |
+| [Perceptor Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_perceptor_nova_benchmark/scripts/isaac_ros_perceptor_graph.py)<br/><br/><br/>3 Hawk Cameras<br/><br/><br/><br/>                                                                  | 1200p<br/><br/><br/><br/><br/><br/> | Nvblox ESDF: [9.46 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_perceptor_graph-carter_v2.4.json)<br/><br/><br/>Nvblox Mesh: 9.77 fps<br/><br/><br/>Visual Odometry: 30.1 fps<br/><br/> |

README.md

@@ -0,0 +1,28 @@
+# 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
+
+cmake_minimum_required(VERSION 3.22.1)
+project(isaac_ros_perceptor_bringup)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_package(INSTALL_TO_SHARE launch params)

isaac_ros_perceptor_bringup/CMakeLists.txt

@@ -0,0 +1,140 @@
+# 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 isaac_ros_launch_utils.all_types import *
+import isaac_ros_launch_utils as lu
+import isaac_ros_perceptor_constants as pc
+
+
+def create_imager_pipeline(stereo_camera_name: str, identifier: str,
+                           args: lu.ArgumentContainer) -> list[Action]:
+    stereo_camera_config = getattr(args, stereo_camera_name)
+
+    actions: list[Action] = []
+
+    rectify_composable_node = ComposableNode(
+        name='rectify_node',
+        package='isaac_ros_image_proc',
+        plugin='nvidia::isaac_ros::image_proc::RectifyNode',
+        namespace=f'{stereo_camera_name}/{identifier}',
+        parameters=[{
+            'output_width': pc.HAWK_IMAGE_WIDTH,
+            'output_height': pc.HAWK_IMAGE_HEIGHT,
+        }],
+    )
+    rectify_condition = IfCondition(lu.has_substring(stereo_camera_config, 'rectify'))
+    actions.append(
+        lu.load_composable_nodes(
+            args.container_name,
+            [rectify_composable_node],
+            condition=rectify_condition,
+        ))
+
+    return actions
+
+
+def create_hawk_pipeline(stereo_camera_name: str, args: lu.ArgumentContainer) -> list[Action]:
+    actions = []
+
+    # Load the configuration for the stereo camera
+    stereo_camera_config = getattr(args, stereo_camera_name)
+    run_ess_light = lu.has_substring(stereo_camera_config, 'ess_light')
+    run_ess_full = lu.has_substring(stereo_camera_config, 'ess_full')
+    run_ess = OrSubstitution(run_ess_light, run_ess_full)
+
+    actions.append(
+        lu.assert_condition(
+            'Camera config invalid. Can not run ess_light and ess_full at the same time.',
+            IfCondition(AndSubstitution(run_ess_light, run_ess_full))),
+    )
+    engine_file_path = lu.if_else_substitution(run_ess_light, args.ess_light_engine_file_path,
+                                               args.ess_full_engine_file_path)
+    actions.append(lu.assert_path_exists(
+        engine_file_path,
+        condition=IfCondition(run_ess),
+    ))
+    ess_skip_frames = lu.has_substring(stereo_camera_config, 'ess_skip_frames')
+    throttler_skip = lu.if_else_substitution(ess_skip_frames, args.ess_number_of_frames_to_skip,
+                                             '0')
+
+    # Run the left/right imager pipelines (rectify)
+    actions.extend(create_imager_pipeline(stereo_camera_name, 'left', args))
+    actions.extend(create_imager_pipeline(stereo_camera_name, 'right', args))
+
+    # Run the depth estimation
+    ess_composable_node = ComposableNode(
+        name='ess_node',
+        package='isaac_ros_ess',
+        plugin='nvidia::isaac_ros::dnn_stereo_depth::ESSDisparityNode',
+        namespace=stereo_camera_name,
+        parameters=[{
+            'engine_file_path': engine_file_path,
+            'threshold': 0.4,
+            'throttler_skip': throttler_skip,
+        }],
+        remappings=[
+            ('left/camera_info', 'left/camera_info_rect'),
+            ('left/image_raw', 'left/image_rect'),
+            ('right/camera_info', 'right/camera_info_rect'),
+            ('right/image_raw', 'right/image_rect'),
+        ],
+    )
+    disparity_composable_node = ComposableNode(
+        name='disparity_to_depth',
+        package='isaac_ros_stereo_image_proc',
+        plugin='nvidia::isaac_ros::stereo_image_proc::DisparityToDepthNode',
+        namespace=stereo_camera_name,
+    )
+    depth_action = lu.load_composable_nodes(
+        args.container_name, [ess_composable_node, disparity_composable_node],
+        condition=IfCondition(run_ess))
+    actions.append(depth_action)
+
+    return actions
+
+
+def generate_launch_description() -> LaunchDescription:
+    args = lu.ArgumentContainer()
+
+    # Config strings for all stereo cameras.
+    # The config string must be a subset of:
+    # - driver,rectify,resize,ess_full,ess_light,ess_skip_frames,cuvslam,nvblox
+    args.add_arg('front_stereo_camera')
+    args.add_arg('back_stereo_camera')
+    args.add_arg('left_stereo_camera')
+    args.add_arg('right_stereo_camera')
+
+    # Additional arguments
+    args.add_arg('container_name', 'nova_container')
+    args.add_arg('ess_number_of_frames_to_skip', 1)
+    args.add_arg('ess_full_engine_file_path',
+                 lu.get_isaac_ros_ws_path() +
+                 '/isaac_ros_assets/models/dnn_stereo_disparity/dnn_stereo_disparity_v4.0.0/ess.engine')
+    args.add_arg('ess_light_engine_file_path',
+                 lu.get_isaac_ros_ws_path() +
+                 '/isaac_ros_assets/models/dnn_stereo_disparity/dnn_stereo_disparity_v4.0.0/light_ess.engine')
+
+    # Create pipelines for each camera according to the camera config
+    actions = args.get_launch_actions()
+    actions.extend(create_hawk_pipeline('front_stereo_camera', args))
+    actions.extend(create_hawk_pipeline('back_stereo_camera', args))
+    actions.extend(create_hawk_pipeline('left_stereo_camera', args))
+    actions.extend(create_hawk_pipeline('right_stereo_camera', args))
+
+    return LaunchDescription(actions)

isaac_ros_perceptor_bringup/launch/algorithms/hawks_processing.launch.py

@@ -0,0 +1,124 @@
+# 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_utils.all_types import *
+import isaac_ros_launch_utils as lu
+import isaac_ros_perceptor_constants as pc
+
+from nvblox_ros_python_utils.nvblox_launch_utils import NvbloxPeopleSegmentation
+import nvblox_ros_python_utils.nvblox_constants as nc
+
+from typing import Any, List, Tuple
+
+
+def get_nvblox_remappings(camera_names: List[str],
+                          enable_people_segmentation: bool) -> List[Tuple[str, str]]:
+    remappings = []
+    for i, name in enumerate(camera_names):
+        remappings.append((f'camera_{i}/depth/image', f'{name}/depth'))
+        remappings.append((f'camera_{i}/depth/camera_info', f'{name}/camera_info'))
+        # NOTE(alexmillane): If in people mapping mode the first camera subscribes to color
+        #                    image coming from the segmentation graph.
+        if enable_people_segmentation and name == 'front_stereo_camera':
+            remappings.append((f'camera_{i}/color/image', f'/segmentation/image_resized'))
+            remappings.append((f'camera_{i}/color/camera_info', f'/segmentation/camera_info_resized'))
+        else:
+            remappings.append((f'camera_{i}/color/image', f'{name}/left/image_rect'))
+            remappings.append((f'camera_{i}/color/camera_info', f'{name}/left/camera_info_rect'))
+    if enable_people_segmentation:
+        remappings.append((f'mask/image', f'/unet/raw_segmentation_mask'))
+        remappings.append((f'mask/camera_info', f'/segmentation/camera_info_resized'))
+    return remappings
+
+
+def get_nvblox_params(camera_names: List[str], enable_people_segmentation: bool,
+                      global_frame: str) -> List[Any]:
+    parameters = []
+    parameters.append(lu.get_path('nvblox_examples_bringup', 'config/nvblox/nvblox_base.yaml'))
+    parameters.append(lu.get_path('isaac_ros_perceptor_bringup', 'params/nvblox_perceptor.yaml'))
+    parameters.append({'num_cameras': len(camera_names)})
+    parameters.append({'global_frame': global_frame})
+    if enable_people_segmentation:
+        parameters.append(
+            lu.get_path('nvblox_examples_bringup',
+                        'config/nvblox/specializations/nvblox_segmentation.yaml'))
+    return parameters
+
+
+def check_valid_nvblox_configuration(args: lu.ArgumentContainer, enable_people_segmentation: bool):
+    if enable_people_segmentation:
+        # For now, we only allow people segmentation on the front camera.
+        segmentation_camera_names = args.enabled_stereo_cameras_for_nvblox_people.split(',')
+        assert len(
+            segmentation_camera_names) <= 1, 'People segmentation is only possible for one camera.'
+        assert segmentation_camera_names[0] == 'front_stereo_camera', \
+            'People segmentation is only possible for the front stereo camera.'
+
+
+def add_nvblox(args: lu.ArgumentContainer) -> List[Action]:
+    camera_names = args.enabled_stereo_cameras_for_nvblox.split(',')
+    enable_people_segmentation = len(args.enabled_stereo_cameras_for_nvblox_people) > 0
+
+    remappings = get_nvblox_remappings(camera_names, enable_people_segmentation)
+    parameters = get_nvblox_params(camera_names, enable_people_segmentation, args.global_frame)
+    check_valid_nvblox_configuration(args, enable_people_segmentation)
+
+    # Add the nvblox node.
+    if enable_people_segmentation:
+        nvblox_node_name = 'nvblox_human_node'
+        nvblox_plugin_name = 'nvblox::NvbloxHumanNode'
+    else:
+        nvblox_node_name = 'nvblox_node'
+        nvblox_plugin_name = 'nvblox::NvbloxNode'
+
+    nvblox_node = ComposableNode(
+        name=nvblox_node_name,
+        package='nvblox_ros',
+        plugin=nvblox_plugin_name,
+        remappings=remappings,
+        parameters=parameters,
+    )
+
+    actions = []
+    actions.append(lu.load_composable_nodes(args.container_name, [nvblox_node]))
+    actions.append(
+        lu.log_info(["Enabling nvblox for cameras '", args.enabled_stereo_cameras_for_nvblox, "'"]))
+
+    # Add people segmentation if enabled.
+    if enable_people_segmentation:
+        actions.append(
+            lu.include(
+                'nvblox_examples_bringup',
+                'launch/perception/segmentation.launch.py',
+                launch_arguments={
+                    'container_name': args.container_name,
+                    'input_topic': '/front_stereo_camera/left/image_rect',
+                    'input_camera_info_topic': '/front_stereo_camera/left/camera_info_rect',
+                    'people_segmentation': NvbloxPeopleSegmentation.peoplesemsegnet_shuffleseg,
+                }))
+
+    return actions
+
+
+def generate_launch_description() -> LaunchDescription:
+    args = lu.ArgumentContainer()
+    args.add_arg('enabled_stereo_cameras_for_nvblox')
+    args.add_arg('enabled_stereo_cameras_for_nvblox_people')
+    args.add_arg('container_name', 'nova_container')
+    args.add_arg('global_frame', 'odom')
+    args.add_opaque_function(add_nvblox)
+    return LaunchDescription(args.get_launch_actions())