├── .gitattributes ├── .gitignore ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── gxf_isaac_occupancy_grid_projector ├── CMakeLists.txt ├── gxf │ ├── occupancy_grid_projector.cpp │ ├── occupancy_grid_projector.cu.cpp │ ├── occupancy_grid_projector.cu.hpp │ ├── occupancy_grid_projector.hpp │ └── occupancy_grid_projector_ext.cpp └── package.xml ├── isaac_ros_bi3d_freespace ├── CMakeLists.txt ├── config │ ├── freespace_segmentation_node.yaml │ ├── isaac_ros_bi3d_freespace_isaac_sim.rviz │ ├── isaac_ros_bi3d_freespace_zed.rviz │ └── zed.yaml ├── include │ └── isaac_ros_bi3d_freespace │ │ └── freespace_segmentation_node.hpp ├── launch │ ├── isaac_ros_bi3d_freespace.launch.py │ ├── isaac_ros_bi3d_freespace_core.launch.py │ ├── isaac_ros_bi3d_freespace_hawk.launch.py │ ├── isaac_ros_bi3d_freespace_isaac_sim.launch.py │ └── isaac_ros_bi3d_freespace_realsense.launch.py ├── package.xml ├── src │ └── freespace_segmentation_node.cpp └── test │ ├── freespace_segmentation_node_test.cpp │ └── isaac_ros_freespace_segmentation_pol.py └── resources └── rosbags └── bi3dnode_rosbag ├── bi3dnode_rosbag_0.db3 └── metadata.yaml /.gitattributes: -------------------------------------------------------------------------------- 1 | # Ignore Python files in linguist 2 | *.py linguist-detectable=false 3 | 4 | # Images 5 | *.gif filter=lfs diff=lfs merge=lfs -text 6 | *.jpg filter=lfs diff=lfs merge=lfs -text 7 | *.png filter=lfs diff=lfs merge=lfs -text 8 | *.psd filter=lfs diff=lfs merge=lfs -text 9 | 10 | # Archives 11 | *.gz filter=lfs diff=lfs merge=lfs -text 12 | *.tar filter=lfs diff=lfs merge=lfs -text 13 | *.zip filter=lfs diff=lfs merge=lfs -text 14 | 15 | # Documents 16 | *.pdf filter=lfs diff=lfs merge=lfs -text 17 | 18 | # Shared libraries 19 | *.so filter=lfs diff=lfs merge=lfs -text 20 | *.so.* filter=lfs diff=lfs merge=lfs -text 21 | 22 | # ROS Bags 23 | **/resources/**/*.zstd filter=lfs diff=lfs merge=lfs -text 24 | **/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text 25 | **/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text 26 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore all pycache files 2 | **/__pycache__/** 3 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Isaac ROS Contribution Rules 2 | 3 | Any contribution that you make to this repository will 4 | be under the Apache 2 License, as dictated by that 5 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 6 | 7 | > **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. 8 | 9 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 10 | line to commit messages to certify that they have the right to submit 11 | the code they are contributing to the project according to the 12 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 13 | 14 | [//]: # (202201002) 15 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Isaac ROS Freespace Segmentation 2 | 3 | NVIDIA-accelerated, deep-learned freespace segmentation 4 | 5 |
Isaac ROS Freespace Segmentation Sample Output
6 | 7 | ## Overview 8 | 9 | Isaac ROS Freespace Segmentation contains an ROS 2 package to produce 10 | occupancy grids for navigation. By processing a freespace segmentation 11 | mask with the pose of the robot relative to the ground, Bi3D Freespace 12 | produces an occupancy grid for 13 | [Nav2](https://github.com/ros-planning/navigation2), which is used to 14 | avoid obstacles during navigation. This package is GPU accelerated to 15 | provide real-time, low latency results in a robotics application. Bi3D 16 | Freespace provides an additional occupancy grid source for mobile robots 17 | (ground based). 18 | 19 |
Isaac ROS Freespace Segmentation Sample Output
20 | 21 | `isaac_ros_bi3d` is used in a graph of nodes to provide a freespace 22 | segmentation mask as one output from a time-synchronized input left and 23 | right stereo image pair. The freespace mask is used by 24 | `isaac_ros_bi3d_freespace` with TF pose of the camera relative to the 25 | ground to compute planar freespace into an occupancy grid as input to 26 | [Nav2](https://github.com/ros-planning/navigation2). 27 | 28 | There are multiple methods to predict the occupancy grid as an input to 29 | navigation. None of these methods are perfect; each has limitations on 30 | the accuracy of its estimate from the sensor providing measured 31 | observations. Each sensor has a unique field of view, range to provide 32 | its measured view of the world, and corresponding areas it does not 33 | measure. Bi3D Freespace provides a diverse approach to 34 | identifying obstacles from freespace. Stereo camera input used for this 35 | function is diverse relative to lidar, and has a better vertical field 36 | of view than most lidar units, allowing for perception of low lying 37 | obstacles that lidar can miss. Bi3D Freespace provides a 38 | robust, vision-based complement to lidar occupancy scanning. 39 | 40 | ## Isaac ROS NITROS Acceleration 41 | 42 | This package is powered by [NVIDIA Isaac Transport for ROS (NITROS)](https://developer.nvidia.com/blog/improve-perception-performance-for-ros-2-applications-with-nvidia-isaac-transport-for-ros/), which leverages type adaptation and negotiation to optimize message formats and dramatically accelerate communication between participating nodes. 43 | 44 | ## Performance 45 | 46 | | Sample Graph

| Input Size

| AGX Orin

| Orin NX

| Orin Nano Super 8GB

| x86_64 w/ RTX 4090

| 47 | |--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------| 48 | | [Freespace Segmentation Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_bi3d_freespace_benchmark/scripts/isaac_ros_bi3d_fs_node.py)



| 576p



| [3340 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_node-agx_orin.json)


1.7 ms @ 30Hz

| [2530 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_node-orin_nx.json)


1.5 ms @ 30Hz

| [2140 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_node-orin_nano.json)


1.9 ms @ 30Hz

| [3500 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_node-x86-4090.json)


0.44 ms @ 30Hz

| 49 | | [Freespace Segmentation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_bi3d_freespace_benchmark/scripts/isaac_ros_bi3d_fs_graph.py)



| 576p



| [40.3 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_graph-agx_orin.json)


79 ms @ 30Hz

| [27.6 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_graph-orin_nx.json)


98 ms @ 30Hz

| [31.8 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_graph-orin_nano.json)


55 ms @ 30Hz

| [102 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_fs_graph-x86-4090.json)


30 ms @ 30Hz

| 50 | 51 | --- 52 | 53 | ## Documentation 54 | 55 | Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_freespace_segmentation/index.html) to learn how to use this repository. 56 | 57 | --- 58 | 59 | ## Packages 60 | 61 | * [`isaac_ros_bi3d_freespace`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_freespace_segmentation/isaac_ros_bi3d_freespace/index.html) 62 | * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_freespace_segmentation/isaac_ros_bi3d_freespace/index.html#quickstart) 63 | * [Try More Examples](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_freespace_segmentation/isaac_ros_bi3d_freespace/index.html#try-more-examples) 64 | * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_freespace_segmentation/isaac_ros_bi3d_freespace/index.html#api) 65 | 66 | ## Latest 67 | 68 | Update 2024-12-10: Update to be compatible with JetPack 6.1 69 | -------------------------------------------------------------------------------- /gxf_isaac_occupancy_grid_projector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | cmake_minimum_required(VERSION 3.22.1) 19 | project(gxf_isaac_occupancy_grid_projector LANGUAGES C CXX CUDA) 20 | 21 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 22 | add_compile_options(-fPIC -w) 23 | endif() 24 | 25 | find_package(ament_cmake_auto REQUIRED) 26 | ament_auto_find_build_dependencies() 27 | 28 | # Dependencies 29 | find_package(CUDAToolkit REQUIRED) 30 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 31 | find_package(yaml-cpp) 32 | 33 | # Occupancy Grid Projector extension 34 | ament_auto_add_library(${PROJECT_NAME} SHARED 35 | gxf/occupancy_grid_projector_ext.cpp 36 | gxf/occupancy_grid_projector.cpp 37 | gxf/occupancy_grid_projector.cu.cpp 38 | gxf/occupancy_grid_projector.cu.hpp 39 | gxf/occupancy_grid_projector.hpp 40 | ) 41 | # Mark as CUDA files with non-standard extensions 42 | set_source_files_properties( 43 | gxf/occupancy_grid_projector.cu.cpp 44 | gxf/occupancy_grid_projector.cu.hpp 45 | PROPERTIES LANGUAGE CUDA 46 | ) 47 | 48 | target_link_libraries(${PROJECT_NAME} 49 | CUDA::cudart 50 | Eigen3::Eigen 51 | yaml-cpp 52 | ) 53 | 54 | target_include_directories(${PROJECT_NAME} PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/gxf") 55 | 56 | set_target_properties(${PROJECT_NAME} PROPERTIES 57 | BUILD_WITH_INSTALL_RPATH TRUE 58 | BUILD_RPATH_USE_ORIGIN TRUE 59 | INSTALL_RPATH_USE_LINK_PATH TRUE 60 | ) 61 | 62 | # Install the binary file 63 | install(TARGETS ${PROJECT_NAME} DESTINATION share/${PROJECT_NAME}/gxf/lib) 64 | 65 | 66 | # Embed versioning information into installed files 67 | ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common) 68 | include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake") 69 | generate_version_info(${PROJECT_NAME}) 70 | 71 | ament_auto_package(INSTALL_TO_SHARE) 72 | -------------------------------------------------------------------------------- /gxf_isaac_occupancy_grid_projector/gxf/occupancy_grid_projector.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include 19 | #include 20 | 21 | #include "occupancy_grid_projector.cu.hpp" 22 | #include "occupancy_grid_projector.hpp" 23 | 24 | #include "gxf/core/parameter_parser_std.hpp" 25 | #include "gxf/multimedia/video.hpp" 26 | #include "gxf/std/tensor.hpp" 27 | #include "gxf/std/timestamp.hpp" 28 | 29 | namespace nvidia { 30 | namespace isaac_ros { 31 | namespace freespace_segmentation { 32 | 33 | gxf_result_t OccupancyGridProjector::registerInterface(gxf::Registrar * registrar) { 34 | gxf::Expected result; 35 | 36 | result &= registrar->parameter( 37 | mask_receiver_, "mask_receiver", "Mask Receiver", 38 | "Input for segmentation mask from Bi3D postprocessor"); 39 | result &= registrar->parameter( 40 | output_transmitter_, "output_transmitter", "Output transmitter", 41 | "Output containing the projected ground-plane occupancy grid"); 42 | result &= registrar->parameter( 43 | allocator_, "allocator", "Allocator", 44 | "Allocator instance for output"); 45 | 46 | result &= registrar->parameter( 47 | projection_transform_param_, "projection_transform", 48 | "Projection transform", 49 | "Transform from camera frame to ground plane frame"); 50 | result &= registrar->parameter( 51 | intrinsics_param_, "intrinsics", 52 | "Intrinsics", 53 | "The camera intrinsics"); 54 | result &= registrar->parameter( 55 | grid_height_param_, "grid_height", 56 | "Grid height", 57 | "Number of rows in the occupancy grid"); 58 | result &= registrar->parameter( 59 | grid_width_param_, "grid_width", 60 | "Grid width", 61 | "Number of columns in the occupancy grid"); 62 | result &= registrar->parameter( 63 | grid_resolution_param_, "grid_resolution", 64 | "Grid resolution", 65 | "Occupancy grid resolution in meters per cell"); 66 | 67 | return gxf::ToResultCode(result); 68 | } 69 | 70 | gxf_result_t OccupancyGridProjector::start() { 71 | // Extract 3D transform from camera frame to ground frame from parameter 72 | auto projection = projection_transform_param_.get(); 73 | if (projection.size() != 7) { 74 | GXF_LOG_ERROR("Expected 3D transform vector to be length 7 but got %lu", projection.size()); 75 | return GXF_FAILURE; 76 | } 77 | auto translation = 78 | Eigen::Vector3f{static_cast(projection.at(0)), 79 | static_cast(projection.at(1)), static_cast(projection.at(2))}; 80 | 81 | // Allocate and copy translation to device 82 | if (cudaMalloc(&translation_device_, sizeof(float) * translation.size()) != cudaSuccess) { 83 | GXF_LOG_ERROR("Failed to allocate translation on device"); 84 | return GXF_FAILURE; 85 | } 86 | if (cudaMemcpy( 87 | translation_device_, translation.data(), 88 | sizeof(float) * translation.size(), cudaMemcpyHostToDevice) != cudaSuccess) { 89 | GXF_LOG_ERROR("Failed to copy translation to device"); 90 | return GXF_FAILURE; 91 | } 92 | 93 | // Parse quaternion (xyzw) as Eigen quaternion (wxyz) 94 | auto q = 95 | Eigen::Quaternionf{static_cast(projection.at(6)), static_cast(projection.at(3)), 96 | static_cast(projection.at(4)), static_cast(projection.at(5))}; 97 | 98 | // Convert quaternion to rotation matrix 99 | auto rotation_matrix = q.toRotationMatrix(); 100 | 101 | // Allocate and copy rotation matrix to device 102 | if (cudaMalloc(&rotation_matrix_device_, sizeof(float) * rotation_matrix.size()) != cudaSuccess) { 103 | GXF_LOG_ERROR("Failed to allocate rotation matrix on device"); 104 | return GXF_FAILURE; 105 | } 106 | if (cudaMemcpy( 107 | rotation_matrix_device_, rotation_matrix.data(), 108 | sizeof(float) * rotation_matrix.size(), cudaMemcpyHostToDevice) != cudaSuccess) { 109 | GXF_LOG_ERROR("Failed to copy rotation matrix to device"); 110 | return GXF_FAILURE; 111 | } 112 | 113 | // Extract camera intrinsics from parameter 114 | auto intrinsics = intrinsics_param_.get(); 115 | if (intrinsics.size() != 2) { 116 | GXF_LOG_ERROR("Expected intrinsics vector to be length 2 but got %lu", intrinsics.size()); 117 | return GXF_FAILURE; 118 | } 119 | f_x_ = static_cast(intrinsics.at(0)); 120 | f_y_ = static_cast(intrinsics.at(1)); 121 | 122 | // Extract occupancy grid dimension parameters 123 | grid_height_ = grid_height_param_.get(); 124 | grid_width_ = grid_width_param_.get(); 125 | grid_resolution_ = static_cast(grid_resolution_param_.get()); 126 | 127 | return GXF_SUCCESS; 128 | } 129 | 130 | gxf_result_t OccupancyGridProjector::stop() noexcept { 131 | if (cudaFree(rotation_matrix_device_) != cudaSuccess) { 132 | GXF_LOG_ERROR("Failed to free rotation matrix"); 133 | return GXF_FAILURE; 134 | } 135 | if (cudaFree(translation_device_) != cudaSuccess) { 136 | GXF_LOG_ERROR("Failed to free rotation matrix"); 137 | return GXF_FAILURE; 138 | } 139 | 140 | return GXF_SUCCESS; 141 | } 142 | 143 | gxf_result_t OccupancyGridProjector::tick() { 144 | // Retrieve segmentation mask from Bi3D postprocessor 145 | const auto maybe_mask_message = mask_receiver_->receive(); 146 | if (!maybe_mask_message) { 147 | return gxf::ToResultCode(maybe_mask_message); 148 | } 149 | 150 | const auto maybe_mask = maybe_mask_message.value().get("frame"); 151 | if (!maybe_mask) { 152 | return gxf::ToResultCode(maybe_mask); 153 | } 154 | 155 | auto segmentation_mask = maybe_mask.value(); 156 | 157 | // Allocate output message 158 | auto maybe_occupancy_grid_message = gxf::Entity::New(context()); 159 | if (!maybe_occupancy_grid_message) { 160 | GXF_LOG_ERROR("Failed to allocate occupancy grid message"); 161 | return gxf::ToResultCode(maybe_occupancy_grid_message); 162 | } 163 | auto occupancy_grid_message = maybe_occupancy_grid_message.value(); 164 | 165 | // Populate primitive metadata 166 | auto maybe_resolution = occupancy_grid_message.add("resolution"); 167 | if (!maybe_resolution) { 168 | GXF_LOG_ERROR("Failed to allocate resolution"); 169 | return gxf::ToResultCode(maybe_resolution); 170 | } 171 | *maybe_resolution.value() = grid_resolution_; 172 | 173 | auto maybe_width = occupancy_grid_message.add("width"); 174 | if (!maybe_width) { 175 | GXF_LOG_ERROR("Failed to allocate width"); 176 | return gxf::ToResultCode(maybe_width); 177 | } 178 | *maybe_width.value() = grid_width_; 179 | 180 | auto maybe_height = occupancy_grid_message.add("height"); 181 | if (!maybe_height) { 182 | GXF_LOG_ERROR("Failed to allocate height"); 183 | return gxf::ToResultCode(maybe_height); 184 | } 185 | *maybe_height.value() = grid_height_; 186 | 187 | // Allocate output tensor for the occupancy grid origin 188 | auto maybe_origin = occupancy_grid_message.add("origin"); 189 | if (!maybe_origin) { 190 | GXF_LOG_ERROR("Failed to allocate origin"); 191 | return gxf::ToResultCode(maybe_origin); 192 | } 193 | auto origin = maybe_origin.value(); 194 | 195 | // Initializing the origin 196 | auto result = origin->reshape( 197 | nvidia::gxf::Shape{7}, 198 | nvidia::gxf::MemoryStorageType::kDevice, allocator_); 199 | if (!result) { 200 | GXF_LOG_ERROR("Failed to reshape origin to (7,)"); 201 | return gxf::ToResultCode(result); 202 | } 203 | 204 | // Populate the origin with center of map 205 | std::array origin_pose{ 206 | -grid_width_ / 2.0 * grid_resolution_, // translation x 207 | -grid_height_ / 2.0 * grid_resolution_, // translation y 208 | 0, // translation z 209 | 0, 0, 0, 1 // rotation 210 | }; 211 | 212 | if (cudaMemcpy( 213 | origin->pointer(), origin_pose.data(), sizeof(double) * origin_pose.size(), 214 | cudaMemcpyHostToDevice) != cudaSuccess) { 215 | GXF_LOG_ERROR("Failed to copy origin to device"); 216 | return GXF_FAILURE; 217 | } 218 | 219 | // Allocate output tensor for the occupancy grid data 220 | auto maybe_occupancy_grid = occupancy_grid_message.add("data"); 221 | if (!maybe_occupancy_grid) { 222 | GXF_LOG_ERROR("Failed to allocate data"); 223 | return gxf::ToResultCode(maybe_occupancy_grid); 224 | } 225 | auto occupancy_grid = maybe_occupancy_grid.value(); 226 | 227 | // Initializing the occupancy grid 228 | result = occupancy_grid->reshape( 229 | nvidia::gxf::Shape{grid_height_, grid_width_}, 230 | nvidia::gxf::MemoryStorageType::kDevice, allocator_); 231 | if (!result) { 232 | GXF_LOG_ERROR( 233 | "Failed to reshape occupancy grid to (%d, %d)", 234 | grid_height_, grid_width_); 235 | return gxf::ToResultCode(result); 236 | } 237 | 238 | // Process segmentation map 239 | process_segmentation_mask( 240 | reinterpret_cast(segmentation_mask->pointer()), occupancy_grid->data().value(), 241 | segmentation_mask->video_frame_info().height, 242 | segmentation_mask->video_frame_info().width, 243 | grid_height_, grid_width_, grid_resolution_, f_x_, f_y_, rotation_matrix_device_, 244 | translation_device_); 245 | 246 | // Add timestamp 247 | std::string timestamp_name{"timestamp"}; 248 | auto maybe_mask_timestamp = maybe_mask_message->get(); 249 | if (!maybe_mask_timestamp) { 250 | GXF_LOG_ERROR("Failed to get a timestamp from Bi3D segmentation mask output"); 251 | } 252 | auto out_timestamp = occupancy_grid_message.add(timestamp_name.c_str()); 253 | if (!out_timestamp) {return GXF_FAILURE;} 254 | *out_timestamp.value() = *maybe_mask_timestamp.value(); 255 | 256 | // Publish message 257 | return gxf::ToResultCode( 258 | output_transmitter_->publish(std::move(occupancy_grid_message))); 259 | } 260 | 261 | } // namespace freespace_segmentation 262 | } // namespace isaac_ros 263 | } // namespace nvidia 264 | -------------------------------------------------------------------------------- /gxf_isaac_occupancy_grid_projector/gxf/occupancy_grid_projector.cu.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #include "occupancy_grid_projector.cu.hpp" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "gxf/core/gxf.h" 24 | #include "gxf/core/parameter.hpp" 25 | 26 | namespace nvidia 27 | { 28 | namespace isaac_ros 29 | { 30 | namespace freespace_segmentation 31 | { 32 | 33 | __global__ void cuda_process_segmentation_mask( 34 | const float * segmentation_mask, int8_t * occupancy_grid, int mask_height, int mask_width, 35 | int grid_height, int grid_width, float grid_resolution, float f_x_, float f_y_, 36 | float * rotation_matrix, float * translation) 37 | { 38 | const uint32_t mask_u = blockIdx.x * blockDim.x + threadIdx.x; 39 | const uint32_t mask_v = blockIdx.y * blockDim.y + threadIdx.y; 40 | const uint32_t mask_index = mask_v * mask_width + mask_u; 41 | 42 | // Ensure segmentation mask coordinates are in-bounds 43 | if (mask_u >= mask_width || mask_v >= mask_height) { 44 | return; 45 | } 46 | 47 | // Ensure pixel corresponds to ground (label of 0) 48 | if (segmentation_mask[mask_index] != 0.0) { 49 | return; 50 | } 51 | 52 | /** 53 | * p0: Camera center 54 | * p1: Center of pixel on image plane 55 | * p2: Intersection of ray from p0 through p1 with ground plane 56 | */ 57 | 58 | // Convert from pixel coordinates (u, v) in pixels to (x, y, z) in distance units 59 | // Offset to ensure that point passes through center of the pixel 60 | // f_x_ = s_x_ * f and f_y_ = s_y_ * f 61 | // p1_x_cam = (mask_u + 0.5f - mask_width / 2.0f) / s_x_; 62 | // p1_y_cam = (mask_v + 0.5f - mask_height / 2.0f) / s_y_; 63 | // p1_z_cam = f; // Point exists on image plane 64 | // Divide each component by f to get the following formulation 65 | 66 | const float p1_x_cam = (mask_u + 0.5f - mask_width / 2.0f) / f_x_; 67 | const float p1_y_cam = (mask_v + 0.5f - mask_height / 2.0f) / f_y_; 68 | const float p1_z_cam = 1.0; 69 | 70 | // Apply transformation from camera frame to global frame 71 | // Extract coefficients from column-major rotation matrix 72 | const float p1_x_global = 73 | rotation_matrix[0] * p1_x_cam + 74 | rotation_matrix[3] * p1_y_cam + 75 | rotation_matrix[6] * p1_z_cam + 76 | translation[0]; 77 | 78 | const float p1_y_global = 79 | rotation_matrix[1] * p1_x_cam + 80 | rotation_matrix[4] * p1_y_cam + 81 | rotation_matrix[7] * p1_z_cam + 82 | translation[1]; 83 | 84 | const float p1_z_global = 85 | rotation_matrix[2] * p1_x_cam + 86 | rotation_matrix[5] * p1_y_cam + 87 | rotation_matrix[8] * p1_z_cam + 88 | translation[2]; 89 | 90 | // Camera center given by translation 91 | const float p0_x_global = translation[0]; 92 | const float p0_y_global = translation[1]; 93 | const float p0_z_global = translation[2]; 94 | 95 | // Using parametric representation of ray from camera center p0 through pixel p1, 96 | // find value of parameter t that hits the ground plane to produce p2 with Z=0 97 | const float t = p0_z_global / (p0_z_global - p1_z_global); 98 | 99 | // Ensure that we only project rays in the 'forwards' direction, out of the camera 100 | if (t <= 0) { 101 | return; 102 | } 103 | 104 | // Calculate p2_global where ray intersects ground plane 105 | const float p2_x_global = p0_x_global + (p1_x_global - p0_x_global) * t; 106 | const float p2_y_global = p0_y_global + (p1_y_global - p0_y_global) * t; 107 | // p2_z_global = 0 108 | 109 | // Calculate grid indices, transforming from global frame to occupancy grid frame 110 | const uint32_t grid_x = static_cast(p2_x_global / grid_resolution + grid_width / 2.0f); 111 | const uint32_t grid_y = 112 | static_cast(p2_y_global / grid_resolution + grid_height / 2.0f); 113 | const uint32_t grid_index = grid_y * grid_width + grid_x; 114 | 115 | // Ensure grid index is within range 116 | if (grid_x > grid_width || grid_y > grid_height) { 117 | return; 118 | } 119 | 120 | // Indicate that the corresponding cell is certainly free (value of 0) 121 | occupancy_grid[grid_index] = 0; 122 | } 123 | 124 | #define CHECK_CUDA_ERRORS(result){checkCudaErrors(result, __FILE__, __LINE__); \ 125 | } 126 | inline void checkCudaErrors(cudaError_t result, const char * filename, int line_number) 127 | { 128 | if (result != cudaSuccess) { 129 | GXF_LOG_ERROR( 130 | ("CUDA Error: " + std::string(cudaGetErrorString(result)) + 131 | " (error code: " + std::to_string(result) + ") at " + 132 | std::string(filename) + " in line " + std::to_string(line_number)).c_str()); 133 | } 134 | } 135 | 136 | uint16_t ceil_div(uint16_t numerator, uint16_t denominator) 137 | { 138 | uint32_t accumulator = numerator + denominator - 1; 139 | return accumulator / denominator; 140 | } 141 | 142 | void process_segmentation_mask( 143 | const float * segmentation_mask, int8_t * occupancy_grid, int mask_height, int mask_width, 144 | int grid_height, int grid_width, float grid_resolution, float f_x, 145 | float f_y, float * rotation_matrix, float * translation) 146 | { 147 | // Initialize occupancy grid as all -1 (unknown occupancy status) 148 | CHECK_CUDA_ERRORS(cudaMemset(occupancy_grid, -1, grid_width * grid_height)); 149 | 150 | dim3 block(16, 16); 151 | dim3 grid(ceil_div(mask_width, 16), ceil_div(mask_height, 16), 1); 152 | cuda_process_segmentation_mask << < grid, block >> > 153 | (segmentation_mask, occupancy_grid, mask_height, mask_width, grid_height, grid_width, 154 | grid_resolution, f_x, f_y, rotation_matrix, translation); 155 | 156 | // Wait for CUDA to finish 157 | CHECK_CUDA_ERRORS(cudaDeviceSynchronize()); 158 | } 159 | 160 | } // namespace freespace_segmentation 161 | } // namespace isaac_ros 162 | } // namespace nvidia 163 | -------------------------------------------------------------------------------- /gxf_isaac_occupancy_grid_projector/gxf/occupancy_grid_projector.cu.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #ifndef NVIDIA_ISAAC_ROS_GXF_EXTENSIONS_SEGMENTATION_POSTPROCESSOR_CU_HPP_ 18 | #define NVIDIA_ISAAC_ROS_GXF_EXTENSIONS_SEGMENTATION_POSTPROCESSOR_CU_HPP_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cuda.h" 24 | #include "cuda_runtime.h" 25 | 26 | namespace nvidia 27 | { 28 | namespace isaac_ros 29 | { 30 | namespace freespace_segmentation 31 | { 32 | 33 | void process_segmentation_mask( 34 | const float * segmentation_mask, int8_t * occupancy_grid, int mask_height, int mask_width, 35 | int grid_height, int grid_width, float grid_resolution, float f_x, 36 | float f_y, float * rotation_matrix, float * translation); 37 | 38 | } // namespace freespace_segmentation 39 | } // namespace isaac_ros 40 | } // namespace nvidia 41 | 42 | #endif // NVIDIA_ISAAC_ROS_GXF_EXTENSIONS_SEGMENTATION_POSTPROCESSOR_CU_HPP_ 43 | -------------------------------------------------------------------------------- /gxf_isaac_occupancy_grid_projector/gxf/occupancy_grid_projector.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #ifndef NVIDIA_ISAAC_ROS_GXF_EXTENSIONS_OCCUPANCY_GRID_PROJECTOR_HPP_ 18 | #define NVIDIA_ISAAC_ROS_GXF_EXTENSIONS_OCCUPANCY_GRID_PROJECTOR_HPP_ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include "gxf/core/parameter_parser_std.hpp" 26 | #include "gxf/std/codelet.hpp" 27 | #include "gxf/std/memory_buffer.hpp" 28 | #include "gxf/std/receiver.hpp" 29 | #include "gxf/std/transmitter.hpp" 30 | 31 | #include "occupancy_grid_projector.cu.hpp" 32 | 33 | namespace nvidia { 34 | namespace isaac_ros { 35 | namespace freespace_segmentation { 36 | 37 | class OccupancyGridProjector : public gxf::Codelet { 38 | public: 39 | gxf_result_t registerInterface(gxf::Registrar * registrar) override; 40 | gxf_result_t start() override; 41 | gxf_result_t tick() override; 42 | gxf_result_t stop() noexcept override; 43 | 44 | private: 45 | gxf::Parameter> mask_receiver_; 46 | gxf::Parameter> output_transmitter_; 47 | gxf::Parameter> allocator_; 48 | 49 | // Parameters 50 | gxf::Parameter> projection_transform_param_; 51 | gxf::Parameter> intrinsics_param_; 52 | gxf::Parameter grid_height_param_; 53 | gxf::Parameter grid_width_param_; 54 | gxf::Parameter grid_resolution_param_; 55 | 56 | // Parsed parameters 57 | float * rotation_matrix_device_{}; 58 | float * translation_device_{}; 59 | float f_x_{}; 60 | float f_y_{}; 61 | int grid_height_{}; 62 | int grid_width_{}; 63 | float grid_resolution_{}; 64 | }; 65 | 66 | } // namespace freespace_segmentation 67 | } // namespace isaac_ros 68 | } // namespace nvidia 69 | 70 | #endif // NVIDIA_ISAAC_ROS_GXF_EXTENSIONS_OCCUPANCY_GRID_PROJECTOR_HPP_ 71 | -------------------------------------------------------------------------------- /gxf_isaac_occupancy_grid_projector/gxf/occupancy_grid_projector_ext.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #include "occupancy_grid_projector.hpp" 18 | 19 | #include "gxf/std/extension_factory_helper.hpp" 20 | 21 | GXF_EXT_FACTORY_BEGIN() 22 | 23 | GXF_EXT_FACTORY_SET_INFO( 24 | 0xef1862d43a7911ed, 0xa2610242ac120002, 25 | "OccupancyGridProjectorExtension", 26 | "Isaac ROS Occupancy Grid Projector Extension", "NVIDIA", "0.0.1", 27 | "LICENSE"); 28 | 29 | GXF_EXT_FACTORY_ADD( 30 | 0x0f0237783a7a11ed, 0xa2610242ac120002, 31 | nvidia::isaac_ros::freespace_segmentation::OccupancyGridProjector, nvidia::gxf::Codelet, 32 | "Projects a segmentation mask to the ground plane as an occupancy grid"); 33 | 34 | GXF_EXT_FACTORY_END() 35 | -------------------------------------------------------------------------------- /gxf_isaac_occupancy_grid_projector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 20 | 21 | 22 | 23 | gxf_isaac_occupancy_grid_projector 24 | 3.2.5 25 | Isaac ROS Occupancy Grid Projector Extension. 26 | 27 | Isaac ROS Maintainers 28 | Apache-2.0 29 | https://developer.nvidia.com/isaac-ros-gems/ 30 | CY Chen 31 | 32 | ament_cmake_auto 33 | 34 | isaac_ros_common 35 | isaac_ros_gxf 36 | 37 | ament_lint_auto 38 | ament_lint_common 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | cmake_minimum_required(VERSION 3.22.1) 19 | project(isaac_ros_bi3d_freespace LANGUAGES C CXX) 20 | 21 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 22 | add_compile_options(-Wall -Wextra -Wpedantic) 23 | endif() 24 | 25 | find_package(ament_cmake_auto REQUIRED) 26 | ament_auto_find_build_dependencies() 27 | 28 | # FreespaceSegmentation_node 29 | ament_auto_add_library(freespace_segmentation_node SHARED src/freespace_segmentation_node.cpp) 30 | rclcpp_components_register_nodes(freespace_segmentation_node "nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode") 31 | set(node_plugins "${node_plugins}nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode;$\n") 32 | set_target_properties(freespace_segmentation_node PROPERTIES 33 | BUILD_WITH_INSTALL_RPATH TRUE 34 | BUILD_RPATH_USE_ORIGIN TRUE 35 | INSTALL_RPATH_USE_LINK_PATH TRUE 36 | ) 37 | 38 | if(BUILD_TESTING) 39 | find_package(ament_lint_auto REQUIRED) 40 | ament_lint_auto_find_test_dependencies() 41 | 42 | # Gtest for freespace segmentation node 43 | ament_add_gtest(freespace_segmentation_node_test test/freespace_segmentation_node_test.cpp) 44 | target_link_libraries(freespace_segmentation_node_test freespace_segmentation_node) 45 | target_include_directories(freespace_segmentation_node_test PUBLIC include/isaac_ros_bi3d_freespace/) 46 | target_include_directories(freespace_segmentation_node_test PUBLIC /usr/src/googletest/googlemock/include/) 47 | ament_target_dependencies(freespace_segmentation_node_test rclcpp) 48 | ament_target_dependencies(freespace_segmentation_node_test isaac_ros_nitros) 49 | 50 | # The FindPythonInterp and FindPythonLibs modules are removed 51 | if(POLICY CMP0148) 52 | cmake_policy(SET CMP0148 OLD) 53 | endif() 54 | 55 | find_package(launch_testing_ament_cmake REQUIRED) 56 | add_launch_test(test/isaac_ros_freespace_segmentation_pol.py TIMEOUT "1000") 57 | endif() 58 | 59 | 60 | # Embed versioning information into installed files 61 | ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common) 62 | include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake") 63 | generate_version_info(${PROJECT_NAME}) 64 | 65 | ament_auto_package(INSTALL_TO_SHARE config launch) 66 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/config/freespace_segmentation_node.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | name: freespace_segmentation 20 | components: 21 | - name: mask_in 22 | type: nvidia::gxf::DoubleBufferReceiver 23 | parameters: 24 | capacity: 12 25 | - name: occupancy_grid_out 26 | type: nvidia::gxf::DoubleBufferTransmitter 27 | parameters: 28 | capacity: 12 29 | - name: allocator 30 | type: nvidia::gxf::UnboundedAllocator 31 | - name: occupancy_grid_projector 32 | type: nvidia::isaac_ros::freespace_segmentation::OccupancyGridProjector 33 | parameters: 34 | mask_receiver: mask_in 35 | output_transmitter: occupancy_grid_out 36 | allocator: allocator 37 | projection_transform: [] 38 | intrinsics: [] 39 | grid_height: 10 40 | grid_width: 10 41 | grid_resolution: 0.01 42 | - type: nvidia::gxf::MessageAvailableSchedulingTerm 43 | parameters: 44 | receiver: mask_in 45 | min_size: 1 46 | - type: nvidia::gxf::DownstreamReceptiveSchedulingTerm 47 | parameters: 48 | transmitter: occupancy_grid_out 49 | min_size: 1 50 | --- 51 | name: sink 52 | components: 53 | - name: signal 54 | type: nvidia::gxf::DoubleBufferReceiver 55 | parameters: 56 | capacity: 1 57 | - type: nvidia::gxf::MessageAvailableSchedulingTerm 58 | parameters: 59 | receiver: signal 60 | min_size: 1 61 | - name: sink 62 | type: nvidia::isaac_ros::MessageRelay 63 | parameters: 64 | source: signal 65 | --- 66 | components: 67 | - name: edge0 68 | type: nvidia::gxf::Connection 69 | parameters: 70 | source: freespace_segmentation/occupancy_grid_out 71 | target: sink/signal 72 | --- 73 | components: 74 | - name: clock 75 | type: nvidia::gxf::RealtimeClock 76 | - type: nvidia::gxf::EventBasedScheduler 77 | parameters: 78 | clock: clock 79 | stop_on_deadlock: false 80 | worker_thread_number: 2 81 | - type: nvidia::gxf::JobStatistics 82 | parameters: 83 | clock: clock 84 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/config/isaac_ros_bi3d_freespace_isaac_sim.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Map1 11 | Splitter Ratio: 0.5 12 | Tree Height: 966 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz_default_plugins/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 200 50 | Reference Frame: 51 | Value: true 52 | - Class: rviz_default_plugins/TF 53 | Enabled: true 54 | Frame Timeout: 15 55 | Frames: 56 | All Enabled: true 57 | back_2d_lidar: 58 | Value: true 59 | base_link: 60 | Value: true 61 | chassis_link: 62 | Value: true 63 | front_2d_lidar: 64 | Value: true 65 | front_3d_lidar: 66 | Value: true 67 | front_stereo_camera:imu: 68 | Value: true 69 | front_stereo_camera:left_rgb: 70 | Value: true 71 | front_stereo_camera:right_rgb: 72 | Value: true 73 | left_stereo_camera:imu: 74 | Value: true 75 | left_stereo_camera:left_rgb: 76 | Value: true 77 | left_stereo_camera:right_rgb: 78 | Value: true 79 | odom: 80 | Value: true 81 | rear_stereo_camera:imu: 82 | Value: true 83 | rear_stereo_camera:left_rgb: 84 | Value: true 85 | rear_stereo_camera:right_rgb: 86 | Value: true 87 | right_stereo_camera:imu: 88 | Value: true 89 | right_stereo_camera:left_rgb: 90 | Value: true 91 | right_stereo_camera:right_rgb: 92 | Value: true 93 | Marker Scale: 1 94 | Name: TF 95 | Show Arrows: true 96 | Show Axes: true 97 | Show Names: false 98 | Tree: 99 | odom: 100 | base_link: 101 | back_2d_lidar: 102 | {} 103 | chassis_link: 104 | {} 105 | front_2d_lidar: 106 | {} 107 | front_3d_lidar: 108 | {} 109 | front_stereo_camera:imu: 110 | {} 111 | front_stereo_camera:left_rgb: 112 | {} 113 | front_stereo_camera:right_rgb: 114 | {} 115 | left_stereo_camera:imu: 116 | {} 117 | left_stereo_camera:left_rgb: 118 | {} 119 | left_stereo_camera:right_rgb: 120 | {} 121 | rear_stereo_camera:imu: 122 | {} 123 | rear_stereo_camera:left_rgb: 124 | {} 125 | rear_stereo_camera:right_rgb: 126 | {} 127 | right_stereo_camera:imu: 128 | {} 129 | right_stereo_camera:left_rgb: 130 | {} 131 | right_stereo_camera:right_rgb: 132 | {} 133 | Update Interval: 0 134 | Value: true 135 | - Alpha: 1 136 | Class: rviz_default_plugins/Map 137 | Color Scheme: map 138 | Draw Behind: false 139 | Enabled: true 140 | Name: Map 141 | Topic: 142 | Depth: 5 143 | Durability Policy: Volatile 144 | Filter size: 10 145 | History Policy: Keep Last 146 | Reliability Policy: Reliable 147 | Value: /freespace_segmentation/occupancy_grid 148 | Update Topic: 149 | Depth: 5 150 | Durability Policy: Volatile 151 | History Policy: Keep Last 152 | Reliability Policy: Reliable 153 | Value: /freespace_segmentation/occupancy_grid_updates 154 | Use Timestamp: false 155 | Value: true 156 | Enabled: true 157 | Global Options: 158 | Background Color: 48; 48; 48 159 | Fixed Frame: odom 160 | Frame Rate: 30 161 | Name: root 162 | Tools: 163 | - Class: rviz_default_plugins/Interact 164 | Hide Inactive Objects: true 165 | - Class: rviz_default_plugins/MoveCamera 166 | - Class: rviz_default_plugins/Select 167 | - Class: rviz_default_plugins/FocusCamera 168 | - Class: rviz_default_plugins/Measure 169 | Line color: 128; 128; 0 170 | - Class: rviz_default_plugins/SetInitialPose 171 | Covariance x: 0.25 172 | Covariance y: 0.25 173 | Covariance yaw: 0.06853891909122467 174 | Topic: 175 | Depth: 5 176 | Durability Policy: Volatile 177 | History Policy: Keep Last 178 | Reliability Policy: Reliable 179 | Value: /initialpose 180 | - Class: rviz_default_plugins/SetGoal 181 | Topic: 182 | Depth: 5 183 | Durability Policy: Volatile 184 | History Policy: Keep Last 185 | Reliability Policy: Reliable 186 | Value: /goal_pose 187 | - Class: rviz_default_plugins/PublishPoint 188 | Single click: true 189 | Topic: 190 | Depth: 5 191 | Durability Policy: Volatile 192 | History Policy: Keep Last 193 | Reliability Policy: Reliable 194 | Value: /clicked_point 195 | Transformation: 196 | Current: 197 | Class: rviz_default_plugins/TF 198 | Value: true 199 | Views: 200 | Current: 201 | Class: rviz_default_plugins/Orbit 202 | Distance: 21.24007797241211 203 | Enable Stereo Rendering: 204 | Stereo Eye Separation: 0.05999999865889549 205 | Stereo Focal Distance: 1 206 | Swap Stereo Eyes: false 207 | Value: false 208 | Focal Point: 209 | X: 0.7114542126655579 210 | Y: -0.37252914905548096 211 | Z: 0 212 | Focal Shape Fixed Size: true 213 | Focal Shape Size: 0.05000000074505806 214 | Invert Z Axis: false 215 | Name: Current View 216 | Near Clip Distance: 0.009999999776482582 217 | Pitch: 0.73979651927948 218 | Target Frame: 219 | Value: Orbit (rviz_default_plugins) 220 | Yaw: 3.5823850631713867 221 | Saved: ~ 222 | Window Geometry: 223 | Displays: 224 | collapsed: false 225 | Height: 1257 226 | Hide Left Dock: false 227 | Hide Right Dock: false 228 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000044ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000044f000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000044ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000044f000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000072c0000003efc0100000002fb0000000800540069006d006501000000000000072c0000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004bb0000044f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 229 | Selection: 230 | collapsed: false 231 | Time: 232 | collapsed: false 233 | Tool Properties: 234 | collapsed: false 235 | Views: 236 | collapsed: false 237 | Width: 1836 238 | X: 2799 239 | Y: 67 240 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/config/isaac_ros_bi3d_freespace_zed.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 183 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Class: rviz_default_plugins/Image 51 | Enabled: true 52 | Max Value: 1 53 | Median window: 5 54 | Min Value: 0 55 | Name: Image 56 | Normalize Range: true 57 | Topic: 58 | Depth: 5 59 | Durability Policy: Volatile 60 | History Policy: Keep Last 61 | Reliability Policy: Reliable 62 | Value: /zed_node/left/image_rect_color_rgb 63 | Value: true 64 | - Class: rviz_default_plugins/TF 65 | Enabled: true 66 | Frame Timeout: 15 67 | Frames: 68 | All Enabled: true 69 | base_link: 70 | Value: true 71 | zed2i_base_link: 72 | Value: true 73 | zed2i_camera_center: 74 | Value: true 75 | zed2i_left_camera_frame: 76 | Value: true 77 | zed2i_left_camera_optical_frame: 78 | Value: true 79 | zed2i_right_camera_frame: 80 | Value: true 81 | zed2i_right_camera_optical_frame: 82 | Value: true 83 | Marker Scale: 1 84 | Name: TF 85 | Show Arrows: true 86 | Show Axes: true 87 | Show Names: false 88 | Tree: 89 | base_link: 90 | zed2i_base_link: 91 | zed2i_camera_center: 92 | zed2i_left_camera_frame: 93 | zed2i_left_camera_optical_frame: 94 | {} 95 | zed2i_right_camera_frame: 96 | zed2i_right_camera_optical_frame: 97 | {} 98 | Update Interval: 0 99 | Value: true 100 | - Alpha: 0.699999988079071 101 | Class: rviz_default_plugins/Map 102 | Color Scheme: map 103 | Draw Behind: false 104 | Enabled: true 105 | Name: Map 106 | Topic: 107 | Depth: 5 108 | Durability Policy: Volatile 109 | Filter size: 10 110 | History Policy: Keep Last 111 | Reliability Policy: Reliable 112 | Value: /freespace_segmentation/occupancy_grid 113 | Update Topic: 114 | Depth: 5 115 | Durability Policy: Volatile 116 | History Policy: Keep Last 117 | Reliability Policy: Reliable 118 | Value: /freespace_segmentation/occupancy_grid_updates 119 | Use Timestamp: false 120 | Value: true 121 | - Class: rviz_default_plugins/Image 122 | Enabled: true 123 | Max Value: 1 124 | Median window: 5 125 | Min Value: 0 126 | Name: Image 127 | Normalize Range: true 128 | Topic: 129 | Depth: 5 130 | Durability Policy: Volatile 131 | History Policy: Keep Last 132 | Reliability Policy: Reliable 133 | Value: /zed_node/right/image_rect_color_rgb 134 | Value: true 135 | Enabled: true 136 | Global Options: 137 | Background Color: 48; 48; 48 138 | Fixed Frame: base_link 139 | Frame Rate: 30 140 | Name: root 141 | Tools: 142 | - Class: rviz_default_plugins/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz_default_plugins/MoveCamera 145 | - Class: rviz_default_plugins/Select 146 | - Class: rviz_default_plugins/FocusCamera 147 | - Class: rviz_default_plugins/Measure 148 | Line color: 128; 128; 0 149 | - Class: rviz_default_plugins/SetInitialPose 150 | Covariance x: 0.25 151 | Covariance y: 0.25 152 | Covariance yaw: 0.06853891909122467 153 | Topic: 154 | Depth: 5 155 | Durability Policy: Volatile 156 | History Policy: Keep Last 157 | Reliability Policy: Reliable 158 | Value: /initialpose 159 | - Class: rviz_default_plugins/SetGoal 160 | Topic: 161 | Depth: 5 162 | Durability Policy: Volatile 163 | History Policy: Keep Last 164 | Reliability Policy: Reliable 165 | Value: /goal_pose 166 | - Class: rviz_default_plugins/PublishPoint 167 | Single click: true 168 | Topic: 169 | Depth: 5 170 | Durability Policy: Volatile 171 | History Policy: Keep Last 172 | Reliability Policy: Reliable 173 | Value: /clicked_point 174 | Transformation: 175 | Current: 176 | Class: rviz_default_plugins/TF 177 | Value: true 178 | Views: 179 | Current: 180 | Class: rviz_default_plugins/Orbit 181 | Distance: 10 182 | Enable Stereo Rendering: 183 | Stereo Eye Separation: 0.05999999865889549 184 | Stereo Focal Distance: 1 185 | Swap Stereo Eyes: false 186 | Value: false 187 | Focal Point: 188 | X: 0 189 | Y: 0 190 | Z: 0 191 | Focal Shape Fixed Size: true 192 | Focal Shape Size: 0.05000000074505806 193 | Invert Z Axis: false 194 | Name: Current View 195 | Near Clip Distance: 0.009999999776482582 196 | Pitch: 0.8603980541229248 197 | Target Frame: 198 | Value: Orbit (rviz) 199 | Yaw: 2.880399227142334 200 | Saved: ~ 201 | Window Geometry: 202 | Displays: 203 | collapsed: false 204 | Height: 1136 205 | Hide Left Dock: false 206 | Hide Right Dock: false 207 | Image: 208 | collapsed: false 209 | QMainWindow State: 000000ff00000000fd0000000400000000000002fd000003d6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000140000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003b000001e30000002800fffffffb0000000a0049006d0061006700650100000224000001ed0000002800ffffff000000010000010f000003d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000435000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 210 | Selection: 211 | collapsed: false 212 | Time: 213 | collapsed: false 214 | Tool Properties: 215 | collapsed: false 216 | Views: 217 | collapsed: false 218 | Width: 1848 219 | X: 72 220 | Y: 27 221 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/config/zed.yaml: -------------------------------------------------------------------------------- 1 | # config/common_yaml 2 | # Common parameters to Stereolabs ZED and ZED mini cameras 3 | # 4 | # Note: the parameter svo_file is passed as exe argumet 5 | --- 6 | /**: 7 | ros__parameters: 8 | 9 | general: 10 | svo_file: "" # usually overwritten by launch file 11 | svo_loop: false # Enable loop mode when using an SVO as input source 12 | svo_realtime: true # if true the SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting 13 | camera_timeout_sec: 5 14 | camera_max_reconnect: 5 15 | camera_flip: false 16 | zed_id: 0 # IMPORTANT: to be used in simulation to distinguish individual cameras im multi-camera configurations - usually overwritten by launch file 17 | serial_number: 0 # usually overwritten by launch file 18 | pub_resolution: 'MEDIUM' # The resolution used for output. 'HD2K', 'HD1080', 'HD1200', 'HD720', 'MEDIUM', 'SVGA', 'VGA', 'LOW' 19 | pub_frame_rate: 15.0 # [DYNAMIC] - frequency of publishing of visual images and depth images 20 | gpu_id: -1 21 | region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. 22 | #region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. 23 | #region_of_interest: '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. 24 | #region_of_interest: '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. 25 | sdk_verbose: 1 26 | 27 | video: 28 | brightness: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini 29 | contrast: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini 30 | hue: 0 # [DYNAMIC] Not available for ZED X/ZED X Mini 31 | saturation: 4 # [DYNAMIC] 32 | sharpness: 4 # [DYNAMIC] 33 | gamma: 8 # [DYNAMIC] 34 | auto_exposure_gain: true # [DYNAMIC] 35 | exposure: 80 # [DYNAMIC] 36 | gain: 80 # [DYNAMIC] 37 | auto_whitebalance: true # [DYNAMIC] 38 | whitebalance_temperature: 42 # [DYNAMIC] - [28,65] works only if `auto_whitebalance` is false 39 | qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL 40 | qos_depth: 1 # Queue size if using KEEP_LAST 41 | qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - 42 | qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE 43 | 44 | depth: 45 | depth_mode: 'NONE' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) 46 | depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] 47 | openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] 48 | point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) 49 | depth_confidence: 50 # [DYNAMIC] 50 | depth_texture_conf: 100 # [DYNAMIC] 51 | remove_saturated_areas: true # [DYNAMIC] 52 | qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL 53 | qos_depth: 1 # Queue size if using KEEP_LAST 54 | qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - 55 | qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE 56 | 57 | pos_tracking: 58 | pos_tracking_enabled: false # True to enable positional tracking from start 59 | imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. 60 | publish_tf: true # [usually overwritten by launch file] publish `odom -> base_link` TF 61 | publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF 62 | publish_imu_tf: true # [usually overwritten by launch file] enable/disable the IMU TF broadcasting 63 | base_frame: "base_link" # usually overwritten by launch file 64 | map_frame: "map" 65 | odometry_frame: "odom" 66 | area_memory_db_path: "" 67 | area_memory: true # Enable to detect loop closure 68 | depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation 69 | set_as_static: false # If 'true' the camera will be static and not move in the environment 70 | set_gravity_as_origin: true # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. 71 | floor_alignment: false # Enable to automatically calculate camera/floor offset 72 | initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `base_frame` in the map -> [X, Y, Z, R, P, Y] 73 | init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose 74 | path_pub_rate: 2.0 # [DYNAMIC] - Camera trajectory publishing frequency 75 | path_max_count: -1 # use '-1' for unlimited path size 76 | two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero 77 | fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true 78 | transform_time_offset: 0.0 # The value added to the timestamp of `map->odom` and `odom->base_link`` transform being generated 79 | qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL 80 | qos_depth: 1 # Queue size if using KEEP_LAST 81 | qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT 82 | qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE 83 | 84 | gnss_fusion: 85 | gnss_fusion_enabled: false # fuse 'sensor_msg/NavSatFix' message information into pose data 86 | gnss_fix_topic: "/gps/fix" # Name of the GNSS topic of type NavSatFix to subscribe [Default: "/gps/fix"] 87 | gnss_init_distance: 5.0 # The minimum distance traveled by the robot required to initilize the GNSS fusion 88 | gnss_zero_altitude: false # Set to `true` to ignore GNSS altitude information 89 | gnss_frame: "gnss_link" # [usually overwritten by launch file] The TF frame of the GNSS sensor 90 | publish_utm_tf: true # Publish `utm` -> `map` TF 91 | broadcast_utm_transform_as_parent_frame: false # if 'true' publish `utm` -> `map` TF, otherwise `map` -> `utm` 92 | 93 | mapping: 94 | mapping_enabled: false # True to enable mapping and fused point cloud pubblication 95 | resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f] 96 | max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] 97 | fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud 98 | clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection 99 | qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL 100 | qos_depth: 1 # Queue size if using KEEP_LAST 101 | qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - 102 | qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE 103 | 104 | sensors: 105 | sensors_image_sync: false # Synchronize Sensors messages with latest published video/depth message 106 | sensors_pub_rate: 200. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate 107 | qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL 108 | qos_depth: 1 # Queue size if using KEEP_LAST 109 | qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT - 110 | qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE 111 | 112 | object_detection: 113 | od_enabled: false # True to enable Object Detection 114 | model: 'MULTI_CLASS_BOX_MEDIUM' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' 115 | allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage 116 | max_range: 20.0 # [m] Defines a upper depth range for detections 117 | confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,99] 118 | prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions 119 | filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS 120 | mc_people: false # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models 121 | mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models 122 | mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models 123 | mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models 124 | mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models 125 | mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models 126 | mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models 127 | qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL 128 | qos_depth: 1 # Queue size if using KEEP_LAST 129 | qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT 130 | qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE 131 | 132 | body_tracking: 133 | bt_enabled: false # True to enable Body Tracking 134 | model: 'HUMAN_BODY_MEDIUM' # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE' 135 | body_format: 'BODY_38' # 'BODY_18','BODY_34','BODY_38','BODY_70' 136 | allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage 137 | max_range: 20.0 # [m] Defines a upper depth range for detections 138 | body_kp_selection: 'FULL' # 'FULL', 'UPPER_BODY' 139 | enable_body_fitting: false # Defines if the body fitting will be applied 140 | enable_tracking: true # Defines if the object detection will track objects across images flow 141 | prediction_timeout_s: 0.5 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions 142 | confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of skeleton key points [0,99] 143 | minimum_keypoints_threshold: 5 # [DYNAMIC] - Minimum number of skeleton key points to be detected for a valid skeleton 144 | qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL 145 | qos_depth: 1 # Queue size if using KEEP_LAST 146 | qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT 147 | qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE 148 | 149 | use_sim_time: false # EXPERIMENTAL (only for development) - Set to true to enable SIMULATION mode # 150 | sim_address: '192.168.1.90' # EXPERIMENTAL (only for development) - The local address of the machine running the simulator 151 | 152 | debug: 153 | debug_common: false 154 | debug_video_depth: false 155 | debug_camera_controls: false 156 | debug_point_cloud: false 157 | debug_positional_tracking: false 158 | debug_gnss: false 159 | debug_sensors: false 160 | debug_mapping : false 161 | debug_terrain_mapping : false 162 | debug_object_detection : false 163 | debug_body_tracking : false -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/include/isaac_ros_bi3d_freespace/freespace_segmentation_node.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | 19 | #ifndef ISAAC_ROS_BI3D_FREESPACE__FREESPACE_SEGMENTATION_NODE_HPP_ 20 | #define ISAAC_ROS_BI3D_FREESPACE__FREESPACE_SEGMENTATION_NODE_HPP_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "rclcpp/rclcpp.hpp" 29 | #include "isaac_ros_nitros/nitros_node.hpp" 30 | 31 | #include "tf2_ros/transform_listener.h" 32 | #include "tf2_ros/buffer.h" 33 | 34 | namespace nvidia 35 | { 36 | namespace isaac_ros 37 | { 38 | namespace bi3d_freespace 39 | { 40 | 41 | class FreespaceSegmentationNode : public nitros::NitrosNode 42 | { 43 | public: 44 | explicit FreespaceSegmentationNode(const rclcpp::NodeOptions &); 45 | ~FreespaceSegmentationNode(); 46 | 47 | void postLoadGraphCallback() override; 48 | 49 | private: 50 | std::unique_ptr tf_buffer_; 51 | std::shared_ptr transform_listener_; 52 | 53 | std::string base_link_frame_; 54 | std::string camera_frame_; 55 | std::vector projection_transform_param_; 56 | 57 | double f_x_{}; 58 | double f_y_{}; 59 | std::vector intrinsics_param_; 60 | 61 | int grid_height_; 62 | int grid_width_; 63 | double grid_resolution_; 64 | }; 65 | 66 | } // namespace bi3d_freespace 67 | } // namespace isaac_ros 68 | } // namespace nvidia 69 | 70 | #endif // ISAAC_ROS_BI3D_FREESPACE__FREESPACE_SEGMENTATION_NODE_HPP_ 71 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/launch/isaac_ros_bi3d_freespace.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 20 | from launch.substitutions import LaunchConfiguration 21 | from launch_ros.actions import ComposableNodeContainer 22 | from launch_ros.descriptions import ComposableNode 23 | 24 | 25 | def generate_launch_description(): 26 | launch_args = [ 27 | DeclareLaunchArgument( 28 | 'featnet_engine_file_path', 29 | default_value='', 30 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 31 | DeclareLaunchArgument( 32 | 'segnet_engine_file_path', 33 | default_value='', 34 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 35 | DeclareLaunchArgument( 36 | 'rosbag_path', 37 | default_value='src/isaac_ros_depth_segmentation/resources/rosbags/bi3dnode_rosbag', 38 | description='Path to the rosbag file'), 39 | DeclareLaunchArgument( 40 | 'max_disparity_values', 41 | default_value='64', 42 | description='The maximum number of disparity values given for Bi3D inference'), 43 | DeclareLaunchArgument( 44 | 'base_link_frame', 45 | default_value='base_link', 46 | description='The name of the tf2 frame corresponding to the origin of the robot base'), 47 | DeclareLaunchArgument( 48 | 'camera_frame', 49 | default_value='carter_camera_stereo_left', 50 | description='The name of the tf2 frame corresponding to the camera center'), 51 | DeclareLaunchArgument( 52 | 'f_x', 53 | default_value='732.999267578125', 54 | description='The number of pixels per distance unit in the x dimension'), 55 | DeclareLaunchArgument( 56 | 'f_y', 57 | default_value='734.1167602539062', 58 | description='The number of pixels per distance unit in the y dimension'), 59 | DeclareLaunchArgument( 60 | 'grid_height', 61 | default_value='2000', 62 | description='The desired height of the occupancy grid, in cells'), 63 | DeclareLaunchArgument( 64 | 'grid_width', 65 | default_value='2000', 66 | description='The desired width of the occupancy grid, in cells'), 67 | DeclareLaunchArgument( 68 | 'grid_resolution', 69 | default_value='0.01', 70 | description='The desired resolution of the occupancy grid, in m/cell'), 71 | ] 72 | 73 | rosbag_path = LaunchConfiguration('rosbag_path') 74 | 75 | # Bi3DNode parameters 76 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 77 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 78 | max_disparity_values = LaunchConfiguration('max_disparity_values') 79 | 80 | # FreespaceSegmentationNode parameters 81 | base_link_frame = LaunchConfiguration('base_link_frame') 82 | camera_frame = LaunchConfiguration('camera_frame') 83 | f_x_ = LaunchConfiguration('f_x') 84 | f_y_ = LaunchConfiguration('f_y') 85 | grid_height = LaunchConfiguration('grid_height') 86 | grid_width = LaunchConfiguration('grid_width') 87 | grid_resolution = LaunchConfiguration('grid_resolution') 88 | 89 | bi3d_node = ComposableNode( 90 | name='bi3d_node', 91 | package='isaac_ros_bi3d', 92 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 93 | parameters=[{ 94 | 'featnet_engine_file_path': featnet_engine_file_path, 95 | 'segnet_engine_file_path': segnet_engine_file_path, 96 | 'max_disparity_values': max_disparity_values, 97 | 'use_sim_time': True}], 98 | remappings=[('bi3d_node/bi3d_output', 'bi3d_mask'), 99 | ('left_image_bi3d', 'rgb_left'), 100 | ('right_image_bi3d', 'rgb_right'), 101 | ('left_camera_info_bi3d', 'camera_info_left'), 102 | ('right_camera_info_bi3d', 'camera_info_right')] 103 | ) 104 | 105 | freespace_segmentation_node = ComposableNode( 106 | name='freespace_segmentation_node', 107 | package='isaac_ros_bi3d_freespace', 108 | plugin='nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode', 109 | parameters=[{ 110 | 'base_link_frame': base_link_frame, 111 | 'camera_frame': camera_frame, 112 | 'f_x': f_x_, 113 | 'f_y': f_y_, 114 | 'grid_height': grid_height, 115 | 'grid_width': grid_width, 116 | 'grid_resolution': grid_resolution, 117 | 'use_sim_time': True 118 | }]) 119 | 120 | rosbag_play = ExecuteProcess( 121 | cmd=['ros2', 'bag', 'play', '--loop', rosbag_path], 122 | output='screen') 123 | 124 | container = ComposableNodeContainer( 125 | name='bi3d_freespace_container', 126 | namespace='bi3d_freespace', 127 | package='rclcpp_components', 128 | executable='component_container_mt', 129 | composable_node_descriptions=[ 130 | bi3d_node, 131 | freespace_segmentation_node, 132 | ], 133 | output='screen' 134 | ) 135 | 136 | final_launch_description = launch_args + [rosbag_play, container] 137 | return (launch.LaunchDescription(final_launch_description)) 138 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/launch/isaac_ros_bi3d_freespace_core.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | from typing import Any, Dict 19 | 20 | from isaac_ros_examples import IsaacROSLaunchFragment 21 | import launch 22 | from launch.actions import DeclareLaunchArgument 23 | from launch.conditions import IfCondition 24 | from launch.substitutions import LaunchConfiguration 25 | from launch_ros.descriptions import ComposableNode 26 | 27 | 28 | class IsaacROSBi3DFreespaceLaunchFragment(IsaacROSLaunchFragment): 29 | 30 | @staticmethod 31 | def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: 32 | 33 | base_link_frame = LaunchConfiguration('base_link_frame') 34 | grid_height = LaunchConfiguration('grid_height') 35 | grid_width = LaunchConfiguration('grid_width') 36 | grid_resolution = LaunchConfiguration('grid_resolution') 37 | 38 | publish_default_tf = LaunchConfiguration('publish_default_tf') 39 | 40 | return { 41 | 'freespace_segmentation_node': ComposableNode( 42 | package='isaac_ros_bi3d_freespace', 43 | plugin='nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode', 44 | name='freespace_segmentation_node', 45 | namespace='', 46 | parameters=[{ 47 | 'base_link_frame': base_link_frame, 48 | 'camera_frame': interface_specs['camera_frame'], 49 | 'f_x': interface_specs['focal_length']['f_x'], 50 | 'f_y': interface_specs['focal_length']['f_y'], 51 | 'grid_height': grid_height, 52 | 'grid_width': grid_width, 53 | 'grid_resolution': grid_resolution, 54 | 'use_sim_time': True 55 | }], 56 | remappings=[ 57 | ('bi3d_mask', 'bi3d_node/bi3d_output') 58 | ] 59 | ), 60 | 'tf_publisher': ComposableNode( 61 | name='static_transform_publisher', 62 | package='tf2_ros', 63 | plugin='tf2_ros::StaticTransformBroadcasterNode', 64 | parameters=[{ 65 | 'frame_id': base_link_frame, 66 | 'child_frame_id': interface_specs['camera_frame'], 67 | 'translation.x': 0.0, 68 | 'translation.y': 0.0, 69 | 'translation.z': 0.3, 70 | 'rotation.x': -0.5, 71 | 'rotation.y': 0.5, 72 | 'rotation.z': -0.5, 73 | 'rotation.w': 0.5 74 | }], 75 | condition=IfCondition(publish_default_tf) 76 | ) 77 | } 78 | 79 | @staticmethod 80 | def get_launch_actions(interface_specs: Dict[str, Any]) -> \ 81 | Dict[str, launch.actions.OpaqueFunction]: 82 | return { 83 | 'base_link_frame': DeclareLaunchArgument( 84 | 'base_link_frame', 85 | default_value='base_link', 86 | description='The name of the tf2 frame corresponding to the origin of the robot ' 87 | 'base' 88 | ), 89 | 'grid_height': DeclareLaunchArgument( 90 | 'grid_height', 91 | default_value='2000', 92 | description='The desired height of the occupancy grid, in cells'), 93 | 'grid_width': DeclareLaunchArgument( 94 | 'grid_width', 95 | default_value='2000', 96 | description='The desired width of the occupancy grid, in cells'), 97 | 'grid_resolution': DeclareLaunchArgument( 98 | 'grid_resolution', 99 | default_value='0.01', 100 | description='The desired resolution of the occupancy grid, in m/cell'), 101 | 'publish_default_tf': DeclareLaunchArgument( 102 | 'publish_default_tf', 103 | default_value='false', 104 | description='Whether to publish a default tf from the base_link_frame to the ' 105 | 'camera_frame. The default transform locates the camera 0.3m above ' 106 | 'the base_link_frame, with the optical frame Z-axis pointing straight ' 107 | 'forward.' 108 | ) 109 | } 110 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/launch/isaac_ros_bi3d_freespace_hawk.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument 20 | from launch.substitutions import LaunchConfiguration 21 | from launch_ros.actions import ComposableNodeContainer 22 | from launch_ros.descriptions import ComposableNode 23 | 24 | 25 | def generate_launch_description(): 26 | launch_args = [ 27 | DeclareLaunchArgument( 28 | 'featnet_engine_file_path', 29 | default_value='', 30 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 31 | DeclareLaunchArgument( 32 | 'segnet_engine_file_path', 33 | default_value='', 34 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 35 | DeclareLaunchArgument( 36 | 'max_disparity_values', 37 | default_value='64', 38 | description='The maximum number of disparity values given for Bi3D inference'), 39 | DeclareLaunchArgument( 40 | 'base_link_frame', 41 | default_value='base_link', 42 | description='The name of the tf2 frame corresponding to the origin of the robot base'), 43 | DeclareLaunchArgument( 44 | 'camera_frame', 45 | default_value='left_cam', 46 | description='The name of the tf2 frame corresponding to the camera center'), 47 | DeclareLaunchArgument( 48 | 'f_x', 49 | default_value='478.9057', 50 | description='The number of pixels per distance unit in the x dimension'), 51 | DeclareLaunchArgument( 52 | 'f_y', 53 | default_value='459.7495', 54 | description='The number of pixels per distance unit in the y dimension'), 55 | DeclareLaunchArgument( 56 | 'grid_height', 57 | default_value='2000', 58 | description='The desired height of the occupancy grid, in cells'), 59 | DeclareLaunchArgument( 60 | 'grid_width', 61 | default_value='2000', 62 | description='The desired width of the occupancy grid, in cells'), 63 | DeclareLaunchArgument( 64 | 'grid_resolution', 65 | default_value='0.01', 66 | description='The desired resolution of the occupancy grid, in m/cell'), 67 | DeclareLaunchArgument( 68 | 'module_id', 69 | default_value='2', 70 | description='Index specifying the stereo camera module to use.'), 71 | ] 72 | 73 | module_id = LaunchConfiguration('module_id') 74 | 75 | # Bi3DNode parameters 76 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 77 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 78 | max_disparity_values = LaunchConfiguration('max_disparity_values') 79 | 80 | # FreespaceSegmentationNode parameters 81 | base_link_frame = LaunchConfiguration('base_link_frame') 82 | camera_frame = LaunchConfiguration('camera_frame') 83 | f_x_ = LaunchConfiguration('f_x') 84 | f_y_ = LaunchConfiguration('f_y') 85 | grid_height = LaunchConfiguration('grid_height') 86 | grid_width = LaunchConfiguration('grid_width') 87 | grid_resolution = LaunchConfiguration('grid_resolution') 88 | 89 | argus_stereo_node = ComposableNode( 90 | name='argus_stereo', 91 | package='isaac_ros_argus_camera', 92 | plugin='nvidia::isaac_ros::argus::ArgusStereoNode', 93 | parameters=[{'module_id': module_id}], 94 | ) 95 | 96 | left_rectify_node = ComposableNode( 97 | name='left_rectify_node', 98 | package='isaac_ros_image_proc', 99 | plugin='nvidia::isaac_ros::image_proc::RectifyNode', 100 | parameters=[{ 101 | 'output_width': 960, 102 | 'output_height': 576, 103 | }], 104 | remappings=[ 105 | ('image_raw', 'left/image_raw'), 106 | ('camera_info', 'left/camera_info'), 107 | ('image_rect', 'left/image_rect'), 108 | ('camera_info_rect', 'left/camera_info') 109 | ] 110 | ) 111 | 112 | right_rectify_node = ComposableNode( 113 | name='right_rectify_node', 114 | package='isaac_ros_image_proc', 115 | plugin='nvidia::isaac_ros::image_proc::RectifyNode', 116 | parameters=[{ 117 | 'output_width': 960, 118 | 'output_height': 576, 119 | }], 120 | remappings=[ 121 | ('image_raw', 'right/image_raw'), 122 | ('camera_info', 'right/camera_info'), 123 | ('image_rect', 'right/image_rect'), 124 | ('camera_info_rect', 'right/camera_info') 125 | ] 126 | ) 127 | 128 | bi3d_node = ComposableNode( 129 | name='bi3d_node', 130 | package='isaac_ros_bi3d', 131 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 132 | parameters=[{ 133 | 'featnet_engine_file_path': featnet_engine_file_path, 134 | 'segnet_engine_file_path': segnet_engine_file_path, 135 | 'max_disparity_values': max_disparity_values, 136 | }], 137 | remappings=[('bi3d_node/bi3d_output', 'bi3d_mask'), 138 | ('left_image_bi3d', 'left/image_rect'), 139 | ('left_camera_info_bi3d', 'right/camera_info'), 140 | ('right_image_bi3d', 'right/image_rect'), 141 | ('right_camera_info_bi3d', 'right/camera_info')] 142 | ) 143 | 144 | freespace_segmentation_node = ComposableNode( 145 | name='freespace_segmentation_node', 146 | package='isaac_ros_bi3d_freespace', 147 | plugin='nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode', 148 | parameters=[{ 149 | 'base_link_frame': base_link_frame, 150 | 'camera_frame': camera_frame, 151 | 'f_x': f_x_, 152 | 'f_y': f_y_, 153 | 'grid_height': grid_height, 154 | 'grid_width': grid_width, 155 | 'grid_resolution': grid_resolution, 156 | 'use_sim_time': True 157 | }]) 158 | 159 | tf_publisher = ComposableNode( 160 | name='static_transform_publisher', 161 | package='tf2_ros', 162 | plugin='tf2_ros::StaticTransformBroadcasterNode', 163 | parameters=[{ 164 | 'frame_id': base_link_frame, 165 | 'child_frame_id': 'camera', 166 | 'translation.x': 0.0, 167 | 'translation.y': 0.0, 168 | 'translation.z': 0.1, 169 | 'rotation.x': 0.0, 170 | 'rotation.y': 0.0, 171 | 'rotation.z': 0.0, 172 | 'rotation.w': 1.0 173 | }]) 174 | 175 | container = ComposableNodeContainer( 176 | name='bi3d_freespace_container', 177 | namespace='bi3d_freespace', 178 | package='rclcpp_components', 179 | executable='component_container_mt', 180 | composable_node_descriptions=[ 181 | bi3d_node, 182 | freespace_segmentation_node, 183 | left_rectify_node, 184 | right_rectify_node, 185 | argus_stereo_node, 186 | tf_publisher 187 | ], 188 | output='screen', 189 | arguments=['--ros-args', '--log-level', 'info', 190 | '--log-level', 'color_format_convert:=info', 191 | '--log-level', 'NitrosImage:=info', 192 | '--log-level', 'NitrosNode:=info' 193 | ], 194 | ) 195 | 196 | final_launch_description = launch_args + [container] 197 | return (launch.LaunchDescription(final_launch_description)) 198 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/launch/isaac_ros_bi3d_freespace_isaac_sim.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import os 19 | 20 | from ament_index_python.packages import get_package_share_directory 21 | import launch 22 | from launch.actions import DeclareLaunchArgument 23 | from launch.substitutions import LaunchConfiguration 24 | from launch_ros.actions import ComposableNodeContainer, Node 25 | from launch_ros.descriptions import ComposableNode 26 | 27 | 28 | def generate_launch_description(): 29 | launch_args = [ 30 | DeclareLaunchArgument( 31 | 'featnet_engine_file_path', 32 | default_value='', 33 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 34 | DeclareLaunchArgument( 35 | 'segnet_engine_file_path', 36 | default_value='', 37 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 38 | DeclareLaunchArgument( 39 | 'max_disparity_values', 40 | default_value='64', 41 | description='The maximum number of disparity values given for Bi3D inference'), 42 | DeclareLaunchArgument( 43 | 'base_link_frame', 44 | default_value='base_link', 45 | description='The name of the tf2 frame corresponding to the origin of the robot base'), 46 | DeclareLaunchArgument( 47 | 'camera_frame', 48 | default_value='front_stereo_camera_left_optical', 49 | description='The name of the tf2 frame corresponding to the camera center'), 50 | 51 | # f(mm) / sensor width (mm) = f(pixels) / image width(pixels) 52 | 53 | DeclareLaunchArgument( 54 | 'f_x', 55 | default_value='478.9057', 56 | description='The number of pixels per distance unit in the x dimension'), 57 | DeclareLaunchArgument( 58 | 'f_y', 59 | default_value='459.7495', 60 | description='The number of pixels per distance unit in the y dimension'), 61 | DeclareLaunchArgument( 62 | 'grid_height', 63 | default_value='2000', 64 | description='The desired height of the occupancy grid, in cells'), 65 | DeclareLaunchArgument( 66 | 'grid_width', 67 | default_value='2000', 68 | description='The desired width of the occupancy grid, in cells'), 69 | DeclareLaunchArgument( 70 | 'grid_resolution', 71 | default_value='0.01', 72 | description='The desired resolution of the occupancy grid, in m/cell'), 73 | ] 74 | 75 | # Bi3DNode parameters 76 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 77 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 78 | max_disparity_values = LaunchConfiguration('max_disparity_values') 79 | 80 | # FreespaceSegmentationNode parameters 81 | base_link_frame = LaunchConfiguration('base_link_frame') 82 | camera_frame = LaunchConfiguration('camera_frame') 83 | f_x_ = LaunchConfiguration('f_x') 84 | f_y_ = LaunchConfiguration('f_y') 85 | grid_height = LaunchConfiguration('grid_height') 86 | grid_width = LaunchConfiguration('grid_width') 87 | grid_resolution = LaunchConfiguration('grid_resolution') 88 | 89 | image_resize_node_right = ComposableNode( 90 | package='isaac_ros_image_proc', 91 | plugin='nvidia::isaac_ros::image_proc::ResizeNode', 92 | name='image_resize_node_right', 93 | parameters=[{ 94 | 'output_width': 960, 95 | 'output_height': 576, 96 | 'encoding_desired': 'rgb8', 97 | }], 98 | remappings=[ 99 | ('camera_info', 'front_stereo_camera/right/camera_info'), 100 | ('image', 'front_stereo_camera/right/image_rect_color'), 101 | ('resize/camera_info', 'front_stereo_camera/right/camera_info_resize'), 102 | ('resize/image', 'front_stereo_camera/right/image_resize')] 103 | ) 104 | 105 | image_resize_node_left = ComposableNode( 106 | package='isaac_ros_image_proc', 107 | plugin='nvidia::isaac_ros::image_proc::ResizeNode', 108 | name='image_resize_node_left', 109 | parameters=[{ 110 | 'output_width': 960, 111 | 'output_height': 576, 112 | 'encoding_desired': 'rgb8', 113 | }], 114 | remappings=[ 115 | ('camera_info', 'front_stereo_camera/left/camera_info'), 116 | ('image', 'front_stereo_camera/left/image_rect_color'), 117 | ('resize/camera_info', 'front_stereo_camera/left/camera_info_resize'), 118 | ('resize/image', 'front_stereo_camera/left/image_resize')] 119 | ) 120 | 121 | bi3d_node = ComposableNode( 122 | name='bi3d_node', 123 | package='isaac_ros_bi3d', 124 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 125 | parameters=[{ 126 | 'featnet_engine_file_path': featnet_engine_file_path, 127 | 'segnet_engine_file_path': segnet_engine_file_path, 128 | 'max_disparity_values': max_disparity_values, 129 | 'disparity_values': [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 20, 30, 40, 50, 60], 130 | 'image_width': 960, 131 | 'image_height': 576 132 | }], 133 | remappings=[('bi3d_node/bi3d_output', 'bi3d_mask'), 134 | ('left_image_bi3d', 'front_stereo_camera/left/image_resize'), 135 | ('left_camera_info_bi3d', 136 | 'front_stereo_camera/left/camera_info_resize'), 137 | ('right_image_bi3d', 'front_stereo_camera/right/image_resize'), 138 | ('right_camera_info_bi3d', 'front_stereo_camera/right/camera_info_resize')] 139 | ) 140 | 141 | freespace_segmentation_node = ComposableNode( 142 | name='freespace_segmentation_node', 143 | package='isaac_ros_bi3d_freespace', 144 | plugin='nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode', 145 | parameters=[{ 146 | 'base_link_frame': base_link_frame, 147 | 'camera_frame': camera_frame, 148 | 'f_x': f_x_, 149 | 'f_y': f_y_, 150 | 'grid_height': grid_height, 151 | 'grid_width': grid_width, 152 | 'grid_resolution': grid_resolution, 153 | 'use_sim_time': True 154 | }]) 155 | 156 | container = ComposableNodeContainer( 157 | name='bi3d_freespace_container', 158 | namespace='bi3d_freespace', 159 | package='rclcpp_components', 160 | executable='component_container_mt', 161 | composable_node_descriptions=[ 162 | bi3d_node, 163 | freespace_segmentation_node, 164 | image_resize_node_right, 165 | image_resize_node_left 166 | ], 167 | output='screen', 168 | arguments=['--ros-args', '--log-level', 'info', 169 | '--log-level', 'color_format_convert:=info', 170 | '--log-level', 'NitrosImage:=info', 171 | '--log-level', 'NitrosNode:=info' 172 | ], 173 | ) 174 | 175 | rviz_config_path = os.path.join(get_package_share_directory( 176 | 'isaac_ros_bi3d_freespace'), 'config', 'isaac_ros_bi3d_freespace_isaac_sim.rviz') 177 | 178 | rviz_node = Node( 179 | package='rviz2', 180 | executable='rviz2', 181 | arguments=['-d', rviz_config_path], 182 | output='screen') 183 | 184 | final_launch_description = launch_args + [container, rviz_node] 185 | return (launch.LaunchDescription(final_launch_description)) 186 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/launch/isaac_ros_bi3d_freespace_realsense.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import os 19 | 20 | from ament_index_python.packages import get_package_share_directory 21 | 22 | import launch 23 | from launch.actions import DeclareLaunchArgument 24 | from launch.substitutions import LaunchConfiguration 25 | from launch_ros.actions import ComposableNodeContainer 26 | from launch_ros.descriptions import ComposableNode 27 | 28 | 29 | def generate_launch_description(): 30 | launch_args = [ 31 | DeclareLaunchArgument( 32 | 'featnet_engine_file_path', 33 | default_value='', 34 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 35 | DeclareLaunchArgument( 36 | 'segnet_engine_file_path', 37 | default_value='', 38 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 39 | DeclareLaunchArgument( 40 | 'max_disparity_values', 41 | default_value='64', 42 | description='The maximum number of disparity values given for Bi3D inference'), 43 | DeclareLaunchArgument( 44 | 'base_link_frame', 45 | default_value='base_link', 46 | description='The name of the tf2 frame corresponding to the origin of the robot base'), 47 | DeclareLaunchArgument( 48 | 'camera_frame', 49 | default_value='camera_infra1_optical_frame', 50 | description='The name of the tf2 frame corresponding to the camera optical center'), 51 | DeclareLaunchArgument( 52 | 'f_x', 53 | default_value='386.16015625', 54 | description='The number of pixels per distance unit in the x dimension'), 55 | DeclareLaunchArgument( 56 | 'f_y', 57 | default_value='386.16015625', 58 | description='The number of pixels per distance unit in the y dimension'), 59 | DeclareLaunchArgument( 60 | 'grid_height', 61 | default_value='1000', 62 | description='The desired height of the occupancy grid, in cells'), 63 | DeclareLaunchArgument( 64 | 'grid_width', 65 | default_value='1000', 66 | description='The desired width of the occupancy grid, in cells'), 67 | DeclareLaunchArgument( 68 | 'grid_resolution', 69 | default_value='0.01', 70 | description='The desired resolution of the occupancy grid, in m/cell'), 71 | ] 72 | 73 | # Bi3DNode parameters 74 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 75 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 76 | max_disparity_values = LaunchConfiguration('max_disparity_values') 77 | 78 | # FreespaceSegmentationNode parameters 79 | base_link_frame = LaunchConfiguration('base_link_frame') 80 | camera_frame = LaunchConfiguration('camera_frame') 81 | f_x = LaunchConfiguration('f_x') 82 | f_y = LaunchConfiguration('f_y') 83 | grid_height = LaunchConfiguration('grid_height') 84 | grid_width = LaunchConfiguration('grid_width') 85 | grid_resolution = LaunchConfiguration('grid_resolution') 86 | 87 | bi3d_node = ComposableNode( 88 | name='bi3d_node', 89 | package='isaac_ros_bi3d', 90 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 91 | parameters=[{ 92 | 'featnet_engine_file_path': featnet_engine_file_path, 93 | 'segnet_engine_file_path': segnet_engine_file_path, 94 | 'max_disparity_values': max_disparity_values, 95 | 'image_width': 640, 96 | 'image_height': 480}], 97 | remappings=[ 98 | ('left_image_bi3d', 'infra1/image_rect_raw'), 99 | ('right_image_bi3d', 'infra2/image_rect_raw'), 100 | ('left_camera_info_bi3d', 'infra1/camera_info'), 101 | ('right_camera_info_bi3d', 'infra2/camera_info'), 102 | ('bi3d_node/bi3d_output', 'bi3d_mask')]) 103 | 104 | image_format_converter_node_left = ComposableNode( 105 | package='isaac_ros_image_proc', 106 | plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', 107 | name='image_format_node_left', 108 | parameters=[{ 109 | 'encoding_desired': 'rgb8', 110 | 'image_width': 640, 111 | 'image_height': 480 112 | }], 113 | remappings=[ 114 | ('image_raw', 'infra1/image_rect_raw_mono'), 115 | ('image', 'infra1/image_rect_raw')] 116 | ) 117 | 118 | image_format_converter_node_right = ComposableNode( 119 | package='isaac_ros_image_proc', 120 | plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', 121 | name='image_format_node_right', 122 | parameters=[{ 123 | 'encoding_desired': 'rgb8', 124 | 'image_width': 640, 125 | 'image_height': 480 126 | }], 127 | remappings=[ 128 | ('image_raw', 'infra2/image_rect_raw_mono'), 129 | ('image', 'infra2/image_rect_raw')] 130 | ) 131 | 132 | freespace_segmentation_node = ComposableNode( 133 | name='freespace_segmentation_node', 134 | package='isaac_ros_bi3d_freespace', 135 | plugin='nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode', 136 | parameters=[{ 137 | 'base_link_frame': base_link_frame, 138 | 'camera_frame': camera_frame, 139 | 'f_x': f_x, 140 | 'f_y': f_y, 141 | 'grid_height': grid_height, 142 | 'grid_width': grid_width, 143 | 'grid_resolution': grid_resolution, 144 | }]) 145 | 146 | tf_publisher = ComposableNode( 147 | name='static_transform_publisher', 148 | package='tf2_ros', 149 | plugin='tf2_ros::StaticTransformBroadcasterNode', 150 | parameters=[{ 151 | 'frame_id': base_link_frame, 152 | 'child_frame_id': 'camera_link', 153 | 'translation.x': 0.0, 154 | 'translation.y': 0.0, 155 | 'translation.z': 0.1, 156 | 'rotation.x': 0.0, 157 | 'rotation.y': 0.0, 158 | 'rotation.z': 0.0, 159 | 'rotation.w': 1.0 160 | }]) 161 | 162 | # RealSense 163 | realsense_config_file_path = os.path.join( 164 | get_package_share_directory('isaac_ros_bi3d'), 165 | 'config', 'realsense.yaml' 166 | ) 167 | 168 | realsense_node = ComposableNode( 169 | package='realsense2_camera', 170 | plugin='realsense2_camera::RealSenseNodeFactory', 171 | parameters=[realsense_config_file_path], 172 | remappings=[ 173 | ('infra1/image_rect_raw', 'infra1/image_rect_raw_mono'), 174 | ('infra2/image_rect_raw', 'infra2/image_rect_raw_mono') 175 | ] 176 | ) 177 | 178 | container = ComposableNodeContainer( 179 | name='bi3d_container', 180 | namespace='bi3d', 181 | package='rclcpp_components', 182 | executable='component_container_mt', 183 | composable_node_descriptions=[bi3d_node, 184 | image_format_converter_node_left, 185 | image_format_converter_node_right, 186 | freespace_segmentation_node, 187 | tf_publisher, 188 | realsense_node], 189 | output='screen', 190 | remappings=[ 191 | ('left_image_bi3d', 'infra1/image_rect_raw'), 192 | ('right_image_bi3d', 'infra2/image_rect_raw')] 193 | ) 194 | 195 | final_launch_description = launch_args + [container] 196 | return (launch.LaunchDescription(final_launch_description)) 197 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 20 | 21 | 22 | 23 | isaac_ros_bi3d_freespace 24 | 3.2.5 25 | Freespace segmentation using the Bi3D inference network for ROS 26 | 27 | Isaac ROS Maintainers 28 | Apache-2.0 29 | https://developer.nvidia.com/isaac-ros-gems/ 30 | Jaiveer Singh 31 | 32 | ament_cmake_auto 33 | 34 | rclcpp 35 | rclcpp_components 36 | tf2 37 | tf2_ros 38 | isaac_ros_nitros 39 | isaac_ros_nitros_disparity_image_type 40 | isaac_ros_nitros_occupancy_grid_type 41 | 42 | isaac_ros_common 43 | 44 | gxf_isaac_occupancy_grid_projector 45 | 46 | ament_lint_auto 47 | ament_lint_common 48 | isaac_ros_test 49 | ament_cmake_gtest 50 | 51 | 52 | ament_cmake 53 | 54 | 55 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/src/freespace_segmentation_node.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include "isaac_ros_bi3d_freespace/freespace_segmentation_node.hpp" 19 | 20 | 21 | #include "isaac_ros_nitros_occupancy_grid_type/nitros_occupancy_grid.hpp" 22 | #include "isaac_ros_nitros_disparity_image_type/nitros_disparity_image.hpp" 23 | 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "rclcpp_components/register_node_macro.hpp" 26 | 27 | namespace nvidia 28 | { 29 | namespace isaac_ros 30 | { 31 | namespace bi3d_freespace 32 | { 33 | 34 | using nvidia::gxf::optimizer::GraphIOGroupSupportedDataTypesInfoList; 35 | 36 | constexpr char INPUT_COMPONENT_KEY[] = "freespace_segmentation/mask_in"; 37 | constexpr char INPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_disparity_image_32FC1"; 38 | constexpr char INPUT_TOPIC_NAME[] = "bi3d_mask"; 39 | 40 | constexpr char OUTPUT_COMPONENT_KEY[] = "sink/sink"; 41 | constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_occupancy_grid"; 42 | constexpr char OUTPUT_TOPIC_NAME[] = "freespace_segmentation/occupancy_grid"; 43 | 44 | constexpr char APP_YAML_FILENAME[] = "config/freespace_segmentation_node.yaml"; 45 | constexpr char PACKAGE_NAME[] = "isaac_ros_bi3d_freespace"; 46 | 47 | const std::vector> EXTENSIONS = { 48 | {"isaac_ros_gxf", "gxf/lib/std/libgxf_std.so"}, 49 | {"isaac_ros_gxf", "gxf/lib/cuda/libgxf_cuda.so"}, 50 | {"isaac_ros_gxf", "gxf/lib/multimedia/libgxf_multimedia.so"}, 51 | {"gxf_isaac_occupancy_grid_projector", "gxf/lib/libgxf_isaac_occupancy_grid_projector.so"}, 52 | }; 53 | const std::vector PRESET_EXTENSION_SPEC_NAMES = { 54 | "isaac_ros_bi3d_freespace" 55 | }; 56 | const std::vector EXTENSION_SPEC_FILENAMES = {}; 57 | const std::vector GENERATOR_RULE_FILENAMES = {}; 58 | #pragma GCC diagnostic push 59 | #pragma GCC diagnostic ignored "-Wpedantic" 60 | const nitros::NitrosPublisherSubscriberConfigMap CONFIG_MAP = { 61 | {INPUT_COMPONENT_KEY, 62 | { 63 | .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, 64 | .qos = rclcpp::QoS(10), 65 | .compatible_data_format = INPUT_DEFAULT_TENSOR_FORMAT, 66 | .topic_name = INPUT_TOPIC_NAME, 67 | } 68 | }, 69 | {OUTPUT_COMPONENT_KEY, 70 | { 71 | .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, 72 | .qos = rclcpp::QoS(10), 73 | .compatible_data_format = OUTPUT_DEFAULT_TENSOR_FORMAT, 74 | .topic_name = OUTPUT_TOPIC_NAME, 75 | .frame_id_source_key = INPUT_COMPONENT_KEY, 76 | } 77 | }, 78 | }; 79 | #pragma GCC diagnostic pop 80 | 81 | FreespaceSegmentationNode::FreespaceSegmentationNode(const rclcpp::NodeOptions & options) 82 | : nitros::NitrosNode( 83 | options, 84 | APP_YAML_FILENAME, 85 | CONFIG_MAP, 86 | PRESET_EXTENSION_SPEC_NAMES, 87 | EXTENSION_SPEC_FILENAMES, 88 | GENERATOR_RULE_FILENAMES, 89 | EXTENSIONS, 90 | PACKAGE_NAME), 91 | tf_buffer_{std::make_unique(this->get_clock())}, 92 | transform_listener_{std::make_shared(*tf_buffer_)}, 93 | base_link_frame_(declare_parameter("base_link_frame", "base_link")), 94 | camera_frame_(declare_parameter("camera_frame", "camera")), 95 | f_x_(declare_parameter("f_x", 0.0)), 96 | f_y_(declare_parameter("f_y", 0.0)), 97 | grid_height_(declare_parameter("grid_height", 100)), 98 | grid_width_(declare_parameter("grid_width", 100)), 99 | grid_resolution_(declare_parameter("grid_resolution", 0.01)) 100 | { 101 | RCLCPP_DEBUG(get_logger(), "[FreespaceSegmentationNode] Initializing FreespaceSegmentationNode"); 102 | 103 | config_map_[OUTPUT_COMPONENT_KEY].callback = 104 | [this]([[maybe_unused]] const gxf_context_t context, nitros::NitrosTypeBase & msg) { 105 | msg.frame_id = base_link_frame_; 106 | }; 107 | 108 | registerSupportedType(); 109 | registerSupportedType(); 110 | 111 | RCLCPP_DEBUG(get_logger(), "[FreespaceSegmentationNode] Constructor"); 112 | if (f_x_ <= 0 || f_y_ <= 0) { 113 | RCLCPP_ERROR( 114 | get_logger(), "[FreespaceSegmentationNode] Set the focal length before running."); 115 | throw std::invalid_argument( 116 | "[FreespaceSegmentationNode] Invalid focal length " 117 | "fx and fy should be valid focal lengths in pixels."); 118 | } 119 | 120 | startNitrosNode(); 121 | } 122 | 123 | void FreespaceSegmentationNode::postLoadGraphCallback() 124 | { 125 | RCLCPP_DEBUG(get_logger(), "[FreespaceSegmentationNode] postLoadGraphCallback()."); 126 | 127 | // Collect tf2 transform at initialization time 128 | RCLCPP_INFO(this->get_logger(), "Waiting for tf2 transform..."); 129 | 130 | rclcpp::Rate rate{1}; 131 | 132 | bool transform_exists = false; 133 | while (!transform_exists) { 134 | if (!rclcpp::ok()) { 135 | RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); 136 | return; 137 | } 138 | try { 139 | geometry_msgs::msg::TransformStamped t = tf_buffer_->lookupTransform( 140 | base_link_frame_, camera_frame_, this->now()); 141 | 142 | projection_transform_param_ = { 143 | t.transform.translation.x, 144 | t.transform.translation.y, 145 | t.transform.translation.z, 146 | 147 | t.transform.rotation.x, 148 | t.transform.rotation.y, 149 | t.transform.rotation.z, 150 | t.transform.rotation.w, 151 | }; 152 | 153 | transform_exists = true; 154 | } catch (const tf2::TransformException & ex) { 155 | RCLCPP_ERROR( 156 | this->get_logger(), "Could not transform %s to %s: %s", 157 | base_link_frame_.c_str(), camera_frame_.c_str(), ex.what()); 158 | } 159 | rate.sleep(); 160 | } 161 | RCLCPP_INFO(this->get_logger(), "Transform secured"); 162 | 163 | getNitrosContext().setParameter1DFloat64Vector( 164 | "freespace_segmentation", "nvidia::isaac_ros::freespace_segmentation::OccupancyGridProjector", 165 | "projection_transform", 166 | projection_transform_param_); 167 | 168 | intrinsics_param_ = {f_x_, f_y_}; 169 | getNitrosContext().setParameter1DFloat64Vector( 170 | "freespace_segmentation", "nvidia::isaac_ros::freespace_segmentation::OccupancyGridProjector", 171 | "intrinsics", 172 | intrinsics_param_); 173 | 174 | getNitrosContext().setParameterInt32( 175 | "freespace_segmentation", "nvidia::isaac_ros::freespace_segmentation::OccupancyGridProjector", 176 | "grid_height", grid_height_); 177 | getNitrosContext().setParameterInt32( 178 | "freespace_segmentation", "nvidia::isaac_ros::freespace_segmentation::OccupancyGridProjector", 179 | "grid_width", grid_width_); 180 | getNitrosContext().setParameterFloat64( 181 | "freespace_segmentation", "nvidia::isaac_ros::freespace_segmentation::OccupancyGridProjector", 182 | "grid_resolution", grid_resolution_); 183 | } 184 | 185 | FreespaceSegmentationNode::~FreespaceSegmentationNode() {} 186 | 187 | } // namespace bi3d_freespace 188 | } // namespace isaac_ros 189 | } // namespace nvidia 190 | 191 | RCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode) 192 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/test/freespace_segmentation_node_test.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include 19 | #include "freespace_segmentation_node.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | // Objective: to cover code lines where exceptions are thrown 23 | // Approach: send Invalid Arguments for node parameters to trigger the exception 24 | 25 | 26 | TEST(freespace_segmentation_node_test, test_invalid_focal_length) 27 | { 28 | rclcpp::init(0, nullptr); 29 | rclcpp::NodeOptions options; 30 | options.append_parameter_override("f_x", 0.0); 31 | EXPECT_THROW( 32 | { 33 | try { 34 | nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode freespace_segmentation_node( 35 | options); 36 | } catch (const std::invalid_argument & e) { 37 | EXPECT_THAT(e.what(), testing::HasSubstr("Invalid focal length")); 38 | throw; 39 | } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { 40 | EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set")); 41 | throw; 42 | } 43 | }, std::invalid_argument); 44 | rclcpp::shutdown(); 45 | } 46 | 47 | 48 | int main(int argc, char ** argv) 49 | { 50 | testing::InitGoogleTest(&argc, argv); 51 | return RUN_ALL_TESTS(); 52 | } 53 | -------------------------------------------------------------------------------- /isaac_ros_bi3d_freespace/test/isaac_ros_freespace_segmentation_pol.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import time 19 | 20 | from isaac_ros_test import IsaacROSBaseTest 21 | 22 | from launch_ros.actions import ComposableNodeContainer 23 | from launch_ros.descriptions import ComposableNode 24 | 25 | from nav_msgs.msg import OccupancyGrid 26 | import pytest 27 | import rclpy 28 | from stereo_msgs.msg import DisparityImage 29 | 30 | 31 | @pytest.mark.rostest 32 | def generate_test_description(): 33 | freespace_segmentation_node = ComposableNode( 34 | name='freespace_segmentation', 35 | package='isaac_ros_bi3d_freespace', 36 | plugin='nvidia::isaac_ros::bi3d_freespace::FreespaceSegmentationNode', 37 | namespace=IsaacROSFreespaceSegmentationTest.generate_namespace(), 38 | parameters=[{ 39 | 'base_link_frame': 'base_link', 40 | 'camera_frame': 'camera', 41 | 'f_x': 2000.0, 42 | 'f_y': 2000.0, 43 | 'grid_width': 200, 44 | 'grid_height': 100, 45 | 'grid_resolution': 0.01 46 | }] 47 | ) 48 | 49 | tf_publisher = ComposableNode( 50 | name='static_transform_publisher', 51 | package='tf2_ros', 52 | plugin='tf2_ros::StaticTransformBroadcasterNode', 53 | parameters=[{ 54 | 'frame_id': 'base_link', 55 | 'child_frame_id': 'camera', 56 | 'translation.x': -0.3, 57 | 'translation.y': 0.2, 58 | 'translation.z': 0.5, 59 | 'rotation.x': -0.12, 60 | 'rotation.y': 0.98, 61 | 'rotation.z': -0.17, 62 | 'rotation.w': 0.02, 63 | }] 64 | ) 65 | 66 | container = ComposableNodeContainer( 67 | name='freespace_segmentation_container', 68 | namespace='', 69 | package='rclcpp_components', 70 | executable='component_container_mt', 71 | composable_node_descriptions=[freespace_segmentation_node, tf_publisher], 72 | output='screen', 73 | arguments=['--ros-args', '--log-level', 'info'] 74 | ) 75 | return IsaacROSFreespaceSegmentationTest.generate_test_description([container]) 76 | 77 | 78 | class IsaacROSFreespaceSegmentationTest(IsaacROSBaseTest): 79 | IMAGE_HEIGHT = 576 80 | IMAGE_WIDTH = 960 81 | TIMEOUT = 1000 82 | GXF_WAIT_SEC = 3 83 | 84 | def _create_disparity_image(self, name): 85 | 86 | disp_image = DisparityImage() 87 | disp_image.image.height = self.IMAGE_HEIGHT 88 | disp_image.image.width = self.IMAGE_WIDTH 89 | disp_image.image.encoding = '32FC1' 90 | disp_image.image.is_bigendian = False 91 | disp_image.image.step = self.IMAGE_WIDTH * 4 92 | 93 | """ 94 | Creates the following test pattern: 95 | 96 | ### ### 97 | 98 | 99 | # 100 | # 101 | ################### 102 | 103 | """ 104 | 105 | data = [] 106 | for row in range(self.IMAGE_HEIGHT): 107 | if row == 0: 108 | data += [0] * self.IMAGE_WIDTH + [1] * self.IMAGE_WIDTH + \ 109 | [1] * self.IMAGE_WIDTH + [0] * self.IMAGE_WIDTH 110 | elif row == 575: 111 | data += [0] * self.IMAGE_WIDTH + [0] * self.IMAGE_WIDTH + \ 112 | [0] * self.IMAGE_WIDTH + [0] * self.IMAGE_WIDTH 113 | elif row > 400: 114 | data += [0] * 4 + [1] * (self.IMAGE_WIDTH - 1) * 4 115 | else: 116 | data += [1] * self.IMAGE_WIDTH * 4 117 | 118 | disp_image.image.data = data 119 | 120 | disp_image.header.frame_id = name 121 | return disp_image 122 | 123 | def test_pol(self): 124 | time.sleep(self.GXF_WAIT_SEC) 125 | 126 | received_messages = {} 127 | 128 | self.generate_namespace_lookup(['bi3d_mask', 'freespace_segmentation/occupancy_grid']) 129 | 130 | disparity_image_pub = self.node.create_publisher( 131 | DisparityImage, self.namespaces['bi3d_mask'], self.DEFAULT_QOS 132 | ) 133 | 134 | subs = self.create_logging_subscribers( 135 | [('freespace_segmentation/occupancy_grid', OccupancyGrid)], received_messages) 136 | 137 | try: 138 | disparity_image = self._create_disparity_image('camera') 139 | 140 | end_time = time.time() + self.TIMEOUT 141 | done = False 142 | 143 | while time.time() < end_time: 144 | disparity_image_pub.publish(disparity_image) 145 | 146 | rclpy.spin_once(self.node, timeout_sec=0.1) 147 | 148 | if 'freespace_segmentation/occupancy_grid' in received_messages: 149 | done = True 150 | break 151 | self.assertTrue( 152 | done, "Didn't recieve output on freespace_segmentation/occupancy_grid topic") 153 | 154 | occupancy_grid = received_messages['freespace_segmentation/occupancy_grid'] 155 | self.assertTrue(0 in occupancy_grid.data, 'No cells were marked as free!') 156 | 157 | finally: 158 | self.node.destroy_subscription(subs) 159 | self.node.destroy_publisher(disparity_image_pub) 160 | -------------------------------------------------------------------------------- /resources/rosbags/bi3dnode_rosbag/bi3dnode_rosbag_0.db3: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:d3f42dcc4e18fa7074c022ca41a4a184b7226206bc133326928415e8eb6e1902 3 | size 135094272 4 | -------------------------------------------------------------------------------- /resources/rosbags/bi3dnode_rosbag/metadata.yaml: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:4cbfd9f41f34454317b9597a7b87f8866ca85c1e1b839dee83f959d2996a8ce2 3 | size 3981 4 | --------------------------------------------------------------------------------