Isaac ROS 3.2

Author: jaiveersinghNV

.gitattributes

@@ -25,6 +25,5 @@
 **/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

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

README.md

@@ -8,7 +8,7 @@ Visit [Isaac Perceptor](https://nvidia-isaac-ros.github.io/reference_workflows/i
 
 ## 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/> |
+| 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-jp6.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-jp6.json)<br/><br/><br/>Nvblox Mesh: 1.01 fps<br/><br/><br/>Visual Odometry: 30.0 fps<br/><br/> |

isaac_ros_perceptor_bringup/CMakeLists.txt

@@ -25,4 +25,15 @@ find_package(rclpy REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
+install(PROGRAMS
+  scripts/create_map.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
+# Embed versioning information into installed files
+ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)
+include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake")
+generate_version_info(${PROJECT_NAME})
+
 ament_auto_package(INSTALL_TO_SHARE launch params)

isaac_ros_perceptor_bringup/launch/algorithms/hawks_processing.launch.py

@@ -15,11 +15,10 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-import os
-
+# flake8: noqa: F403,F405
 from isaac_ros_launch_utils.all_types import *
 import isaac_ros_launch_utils as lu
-import isaac_ros_perceptor_constants as pc
+import isaac_ros_perceptor_python_utils.constants as pc
 
 
 def create_imager_pipeline(stereo_camera_name: str, identifier: str,
@@ -34,7 +33,6 @@ def create_imager_pipeline(stereo_camera_name: str, identifier: str,
         plugin='nvidia::isaac_ros::image_proc::RectifyNode',
         namespace=f'{stereo_camera_name}/{identifier}',
         parameters=[{
-            'input_qos': 'SENSOR_DATA',
             'output_width': pc.HAWK_IMAGE_WIDTH,
             'output_height': pc.HAWK_IMAGE_HEIGHT,
         }],
@@ -126,10 +124,12 @@ def generate_launch_description() -> LaunchDescription:
     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')
+                 '/isaac_ros_assets/models/dnn_stereo_disparity/dnn_stereo_disparity_v4.1.0_onnx'
+                 '/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')
+                 '/isaac_ros_assets/models/dnn_stereo_disparity/dnn_stereo_disparity_v4.1.0_onnx/'
+                 'light_ess.engine')
 
     # Create pipelines for each camera according to the camera config
     actions = args.get_launch_actions()

isaac_ros_perceptor_bringup/launch/algorithms/lidar_processing.launch.py

@@ -0,0 +1,82 @@
+# 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
+# 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 math
+
+from isaac_ros_launch_utils.all_types import *
+import isaac_ros_launch_utils as lu
+
+
+def generate_launch_description() -> LaunchDescription:
+    args = lu.ArgumentContainer()
+    args.add_arg('container_name', 'nova_container')
+
+    pointcloud_to_flatscan_node = ComposableNode(
+        package='isaac_ros_pointcloud_utils',
+        plugin='nvidia::isaac_ros::pointcloud_utils::PointCloudToFlatScanNode',
+        name='pointcloud_to_flatscan',
+        namespace='front_3d_lidar',
+        parameters=[{
+            'min_z': -0.2,
+            'input_qos': 'SENSOR_DATA',
+        }],
+        remappings=[
+            ('pointcloud', 'lidar_points'),
+            ('pointcloud/nitros', 'lidar_points/nitros'),
+        ],
+    )
+
+    flatscan_to_laserscan_node = ComposableNode(
+        package='isaac_ros_pointcloud_utils',
+        plugin='nvidia::isaac_ros::pointcloud_utils::FlatScantoLaserScanNode',
+        name='flatscan_to_laserscan',
+        namespace='front_3d_lidar',
+        parameters=[{
+            'angle_min': -math.pi,
+            'angle_max': math.pi,
+            'angle_increment': math.pi / 720,
+        }],
+    )
+
+    load_lidar_processing_pipeline = LoadComposableNodes(
+        target_container=args.container_name,
+        composable_node_descriptions=[
+            pointcloud_to_flatscan_node,
+            flatscan_to_laserscan_node,
+        ],
+    )
+
+    actions = args.get_launch_actions()
+    actions.append(load_lidar_processing_pipeline)
+
+    return LaunchDescription(actions)

isaac_ros_perceptor_bringup/launch/algorithms/nvblox.launch.py

@@ -15,12 +15,10 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-from isaac_ros_launch_utils.all_types import *
+import isaac_ros_launch_utils.all_types as lut
 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
 
@@ -33,92 +31,111 @@ def get_nvblox_remappings(camera_names: List[str],
         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'))
+        if enable_people_segmentation:
+            remappings.append((f'camera_{i}/color/image', f'/{name}/segmentation/image_resized'))
+            remappings.append((f'camera_{i}/color/camera_info',
+                               f'/{name}/segmentation/camera_info_resized'))
+            remappings.append((f'camera_{i}/mask/image', f'/{name}/segmentation/people_mask'))
+            remappings.append((f'camera_{i}/mask/camera_info',
+                               f'/{name}/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]:
+def get_nvblox_params(
+    camera_names: List[str],
+    enable_people_segmentation: bool,
+    global_frame: str,
+    param_filename: 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})
+
+    # Dynamics is always enabled, unless we're running in people segmentation mode
     if enable_people_segmentation:
         parameters.append(
             lu.get_path('nvblox_examples_bringup',
                         'config/nvblox/specializations/nvblox_segmentation.yaml'))
-    return parameters
+    else:
+        parameters.append(
+            lu.get_path('nvblox_examples_bringup',
+                        'config/nvblox/specializations/nvblox_dynamics.yaml'))
 
+    parameters.append(lu.get_path('isaac_ros_perceptor_bringup', param_filename))
+    parameters.append({'num_cameras': len(camera_names)})
+    parameters.append({'global_frame': global_frame})
 
-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.'
+    return parameters
 
 
-def add_nvblox(args: lu.ArgumentContainer) -> List[Action]:
+def add_nvblox(args: lu.ArgumentContainer) -> List[lut.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)
+    parameters = get_nvblox_params(camera_names, enable_people_segmentation,
+                                   args.nvblox_global_frame, args.param_filename)
+    parameters.append({'after_shutdown_map_save_path': args.after_shutdown_map_save_path})
 
     # 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,
+    nvblox_node = lut.ComposableNode(
+        name='nvblox_node',
         package='nvblox_ros',
-        plugin=nvblox_plugin_name,
+        plugin='nvblox::NvbloxNode',
         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, "'"]))
+        lu.log_info(["Enabling nvblox for cameras '",
+                     args.enabled_stereo_cameras_for_nvblox, "'"]))
 
     # Add people segmentation if enabled.
     if enable_people_segmentation:
+        camera_namespaces = []
+        camera_input_topics = []
+        input_camera_info_topics = []
+        output_resized_image_topics = []
+        output_resized_camera_info_topics = []
+        for _, name in enumerate(camera_names):
+            camera_namespaces.append(name)
+            camera_input_topics.append(f'/{name}/left/image_rect')
+            input_camera_info_topics.append(f'/{name}/left/camera_info_rect')
+            output_resized_image_topics.append(f'/{name}/segmentation/image_resized')
+            output_resized_camera_info_topics.append(f'/{name}/segmentation/camera_info_resized')
+
         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,
+                    'namespace_list': camera_namespaces,
+                    'input_topic_list': camera_input_topics,
+                    'input_camera_info_topic_list': input_camera_info_topics,
+                    'output_resized_image_topic_list': output_resized_image_topics,
+                    'output_resized_camera_info_topic_list': output_resized_camera_info_topics,
+                    'num_cameras': len(camera_names),
+                    # Isaac 3.0 enabled people seg only on front camera, and fps drops during
+                    # replay is not reported.
+                    'one_container_per_camera': False,
+                    'people_segmentation': NvbloxPeopleSegmentation.peoplesemsegnet_vanilla,
                 }))
 
     return actions
 
 
-def generate_launch_description() -> LaunchDescription:
+def generate_launch_description() -> lut.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_arg('param_filename', 'params/nvblox_perceptor.yaml')
+    args.add_arg('after_shutdown_map_save_path', '')
+    args.add_arg('nvblox_global_frame', 'odom')
     args.add_opaque_function(add_nvblox)
-    return LaunchDescription(args.get_launch_actions())
+    return lut.LaunchDescription(args.get_launch_actions())