diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..f53b7af --- /dev/null +++ b/.gitattributes @@ -0,0 +1,24 @@ +# Images +*.gif filter=lfs diff=lfs merge=lfs -text +*.jpg filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.psd filter=lfs diff=lfs merge=lfs -text + +# Archives +*.gz filter=lfs diff=lfs merge=lfs -text +*.tar filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text + +# Documents +*.pdf filter=lfs diff=lfs merge=lfs -text + +# Shared libraries +*.so filter=lfs diff=lfs merge=lfs -text +*.so.* filter=lfs diff=lfs merge=lfs -text + +# ROS Bags +**/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text + +# DNN Model files +*.onnx filter=lfs diff=lfs merge=lfs -text \ No newline at end of file diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8ef8ee6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +# Ignore all pycache files +**/__pycache__/** + +# Ignore TensorRT plan files +*.plan +*.engine diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..88bc900 --- /dev/null +++ b/LICENSE @@ -0,0 +1,66 @@ +NVIDIA ISAAC ROS SOFTWARE LICENSE + +This license is a legal agreement between you and NVIDIA Corporation ("NVIDIA") and governs the use of the NVIDIA Isaac ROS software and materials provided hereunder (“SOFTWARE”). + +This license can be accepted only by an adult of legal age of majority in the country in which the SOFTWARE is used. + +If you are entering into this license on behalf of a company or other legal entity, you represent that you have the legal authority to bind the entity to this license, in which case “you” will mean the entity you represent. + +If you don’t have the required age or authority to accept this license, or if you don’t accept all the terms and conditions of this license, do not download, install or use the SOFTWARE. + +You agree to use the SOFTWARE only for purposes that are permitted by (a) this license, and (b) any applicable law, regulation or generally accepted practices or guidelines in the relevant jurisdictions. + +1. LICENSE. Subject to the terms of this license, NVIDIA hereby grants you a non-exclusive, non-transferable license, without the right to sublicense (except as expressly provided in this license) to: +a. Install and use the SOFTWARE, +b. Modify and create derivative works of sample or reference source code delivered in the SOFTWARE, and +c. Distribute any part of the SOFTWARE (i) as incorporated into a software application that has material additional functionality beyond the included portions of the SOFTWARE, or (ii) unmodified in binary format, in each case subject to the distribution requirements indicated in this license. + +2. DISTRIBUTION REQUIREMENTS. These are the distribution requirements for you to exercise the distribution grant above: + a. The following notice shall be included in modifications and derivative works of source code distributed: “This software contains source code provided by NVIDIA Corporation.” + b. You agree to distribute the SOFTWARE subject to the terms at least as protective as the terms of this license, including (without limitation) terms relating to the license grant, license restrictions and protection of NVIDIA’s intellectual property rights. Additionally, you agree that you will protect the privacy, security and legal rights of your application users. + c. You agree to notify NVIDIA in writing of any known or suspected distribution or use of the SOFTWARE not in compliance with the requirements of this license, and to enforce the terms of your agreements with respect to the distributed portions of the SOFTWARE. +3. AUTHORIZED USERS. You may allow employees and contractors of your entity or of your subsidiary(ies) to access and use the SOFTWARE from your secure network to perform work on your behalf. If you are an academic institution you may allow users enrolled or employed by the academic institution to access and use the SOFTWARE from your secure network. You are responsible for the compliance with the terms of this license by your authorized users. + +4. LIMITATIONS. Your license to use the SOFTWARE is restricted as follows: + a. The SOFTWARE is licensed for you to develop applications only for their use in systems with NVIDIA GPUs. + b. You may not reverse engineer, decompile or disassemble, or remove copyright or other proprietary notices from any portion of the SOFTWARE or copies of the SOFTWARE. + c. Except as expressly stated above in this license, you may not sell, rent, sublicense, transfer, distribute, modify, or create derivative works of any portion of the SOFTWARE. + d. Unless you have an agreement with NVIDIA for this purpose, you may not indicate that an application created with the SOFTWARE is sponsored or endorsed by NVIDIA. + e. You may not bypass, disable, or circumvent any technical limitation, encryption, security, digital rights management or authentication mechanism in the SOFTWARE. + f. You may not use the SOFTWARE in any manner that would cause it to become subject to an open source software license. As examples, licenses that require as a condition of use, modification, and/or distribution that the SOFTWARE be: (i) disclosed or distributed in source code form; (ii) licensed for the purpose of making derivative works; or (iii) redistributable at no charge. + g. You acknowledge that the SOFTWARE as delivered is not tested or certified by NVIDIA for use in connection with the design, construction, maintenance, and/or operation of any system where the use or failure of such system could result in a situation that threatens the safety of human life or results in catastrophic damages (each, a "Critical Application"). Examples of Critical Applications include use in avionics, navigation, autonomous vehicle applications, ai solutions for automotive products, military, medical, life support or other life critical applications. NVIDIA shall not be liable to you or any third party, in whole or in part, for any claims or damages arising from such uses. You are solely responsible for ensuring that any product or service developed with the SOFTWARE as a whole includes sufficient features to comply with all applicable legal and regulatory standards and requirements. + h. You agree to defend, indemnify and hold harmless NVIDIA and its affiliates, and their respective employees, contractors, agents, officers and directors, from and against any and all claims, damages, obligations, losses, liabilities, costs or debt, fines, restitutions and expenses (including but not limited to attorney’s fees and costs incident to establishing the right of indemnification) arising out of or related to your use of goods and/or services that include or utilize the SOFTWARE, or for use of the SOFTWARE outside of the scope of this license or not in compliance with its terms. + +5. UPDATES. NVIDIA may, at its option, make available patches, workarounds or other updates to this SOFTWARE. Unless the updates are provided with their separate governing terms, they are deemed part of the SOFTWARE licensed to you as provided in this license. + +6. PRE-RELEASE VERSIONS. SOFTWARE versions identified as alpha, beta, preview, early access or otherwise as pre-release may not be fully functional, may contain errors or design flaws, and may have reduced or different security, privacy, availability, and reliability standards relative to commercial versions of NVIDIA software and materials. You may use a pre-release SOFTWARE version at your own risk, understanding that these versions are not intended for use in production or business-critical systems. + +7. COMPONENTS UNDER OTHER LICENSES. The SOFTWARE may include NVIDIA or third-party components with separate legal notices or terms as may be described in proprietary notices accompanying the SOFTWARE, such as components governed by open source software licenses. If and to the extent there is a conflict between the terms in this license and the license terms associated with a component, the license terms associated with the component controls only to the extent necessary to resolve the conflict. + +8. OWNERSHIP. + +8.1 NVIDIA reserves all rights, title and interest in and to the SOFTWARE not expressly granted to you under this license. NVIDIA and its suppliers hold all rights, title and interest in and to the SOFTWARE, including their respective intellectual property rights. The SOFTWARE is copyrighted and protected by the laws of the United States and other countries, and international treaty provisions. + +8.2 Subject to the rights of NVIDIA and its suppliers in the SOFTWARE, you hold all rights, title and interest in and to your applications and your derivative works of the sample or reference source code delivered in the SOFTWARE including their respective intellectual property rights. With respect to source code samples or reference source code licensed to you, NVIDIA and its affiliates are free to continue independently developing source code samples and you covenant not to sue NVIDIA, its affiliates or their licensees with respect to later versions of NVIDIA released source code. + +9. FEEDBACK. You may, but are not obligated to, provide to NVIDIA Feedback. “Feedback” means suggestions, fixes, modifications, feature requests or other feedback regarding the SOFTWARE. Feedback, even if designated as confidential by you, shall not create any confidentiality obligation for NVIDIA. NVIDIA and its designees have a perpetual, non-exclusive, worldwide, irrevocable license to use, reproduce, publicly display, modify, create derivative works of, license, sublicense, and otherwise distribute and exploit Feedback as NVIDIA sees fit without payment and without obligation or restriction of any kind on account of intellectual property rights or otherwise. + +10. NO WARRANTIES. THE SOFTWARE IS PROVIDED AS-IS. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES EXPRESSLY DISCLAIM ALL WARRANTIES OF ANY KIND OR NATURE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR A PARTICULAR PURPOSE. NVIDIA DOES NOT WARRANT THAT THE SOFTWARE WILL MEET YOUR REQUIREMENTS OR THAT THE OPERATION THEREOF WILL BE UNINTERRUPTED OR ERROR-FREE, OR THAT ALL ERRORS WILL BE CORRECTED. + +11. LIMITATIONS OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES SHALL NOT BE LIABLE FOR ANY SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL DAMAGES, OR FOR ANY LOST PROFITS, PROJECT DELAYS, LOSS OF USE, LOSS OF DATA OR LOSS OF GOODWILL, OR THE COSTS OF PROCURING SUBSTITUTE PRODUCTS, ARISING OUT OF OR IN CONNECTION WITH THIS LICENSE OR THE USE OR PERFORMANCE OF THE SOFTWARE, WHETHER SUCH LIABILITY ARISES FROM ANY CLAIM BASED UPON BREACH OF CONTRACT, BREACH OF WARRANTY, TORT (INCLUDING NEGLIGENCE), PRODUCT LIABILITY OR ANY OTHER CAUSE OF ACTION OR THEORY OF LIABILITY, EVEN IF NVIDIA HAS PREVIOUSLY BEEN ADVISED OF, OR COULD REASONABLY HAVE FORESEEN, THE POSSIBILITY OF SUCH DAMAGES. IN NO EVENT WILL NVIDIA’S AND ITS AFFILIATES TOTAL CUMULATIVE LIABILITY UNDER OR ARISING OUT OF THIS LICENSE EXCEED US$10.00. THE NATURE OF THE LIABILITY OR THE NUMBER OF CLAIMS OR SUITS SHALL NOT ENLARGE OR EXTEND THIS LIMIT. + +12. TERMINATION. Your rights under this license will terminate automatically without notice from NVIDIA if you fail to comply with any term and condition of this license or if you commence or participate in any legal proceeding against NVIDIA with respect to the SOFTWARE. NVIDIA may terminate this license with advance written notice to you, if NVIDIA decides to no longer provide the SOFTWARE in a country or, in NVIDIA’s sole discretion, the continued use of it is no longer commercially viable. Upon any termination of this license, you agree to promptly discontinue use of the SOFTWARE and destroy all copies in your possession or control. Your prior distributions in accordance with this license are not affected by the termination of this license. All provisions of this license will survive termination, except for the license granted to you. + +13. APPLICABLE LAW. This license will be governed in all respects by the laws of the United States and of the State of Delaware, without regard to the conflicts of laws principles. The United Nations Convention on Contracts for the International Sale of Goods is specifically disclaimed. You agree to all terms of this license in the English language. The state or federal courts residing in Santa Clara County, California shall have exclusive jurisdiction over any dispute or claim arising out of this license. Notwithstanding this, you agree that NVIDIA shall still be allowed to apply for injunctive remedies or urgent legal relief in any jurisdiction. + +14. NO ASSIGNMENT. This license and your rights and obligations thereunder may not be assigned by you by any means or operation of law without NVIDIA’s permission. Any attempted assignment not approved by NVIDIA in writing shall be void and of no effect. NVIDIA may assign, delegate or transfer this license and its rights and obligations, and if to a non-affiliate you will be notified. + +15. EXPORT. The SOFTWARE is subject to United States export laws and regulations. You agree to comply with all applicable U.S. and international export laws, including the Export Administration Regulations (EAR) administered by the U.S. Department of Commerce and economic sanctions administered by the U.S. Department of Treasury’s Office of Foreign Assets Control (OFAC). These laws include restrictions on destinations, end-users and end-use. By accepting this license, you confirm that you are not currently residing in a country or region currently embargoed by the U.S. and that you are not otherwise prohibited from receiving the SOFTWARE. + +16. GOVERNMENT USE. The SOFTWARE is, and shall be treated as being, “Commercial Items” as that term is defined at 48 CFR § 2.101, consisting of “commercial computer software” and “commercial computer software documentation”, respectively, as such terms are used in, respectively, 48 CFR § 12.212 and 48 CFR §§ 227.7202 & 252.227-7014(a)(1). Use, duplication or disclosure by the U.S. Government or a U.S. Government subcontractor is subject to the restrictions in this license pursuant to 48 CFR § 12.212 or 48 CFR § 227.7202. In no event shall the US Government user acquire rights in the SOFTWARE beyond those specified in 48 C.F.R. 52.227-19(b)(1)-(2). + +17. NOTICES. Please direct your legal notices or other correspondence to NVIDIA Corporation, 2788 San Tomas Expressway, Santa Clara, California 95051, United States of America, Attention: Legal Department. + +18. ENTIRE AGREEMENT. This license is the final, complete and exclusive agreement between the parties relating to the subject matter of this license and supersedes all prior or contemporaneous understandings and agreements relating to this subject matter, whether oral or written. If any court of competent jurisdiction determines that any provision of this license is illegal, invalid or unenforceable, the remaining provisions will remain in full force and effect. Any amendment or waiver under this license shall be in writing and signed by representatives of both parties. + +(v. November 17, 2021) diff --git a/README.md b/README.md index 38dedc1..34141ef 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,227 @@ -# isaac_ros_dnn_stereo_disparity -Deep learned stereo disparity estimation +# Isaac ROS DNN Stereo Disparity +DNN Stereo Disparity includes packages for predicting disparity of stereo input. + +
+ +## Overview +This repository provides an NVIDIA hardware-accelerated package for DNN-based stereo disparity. Stereo disparity (with additional processing) can produce a depth image or point cloud of a scene for robot navigation. The `isaac_ros_ess` package uses the [ESS](https://catalog.ngc.nvidia.com/orgs/nvidia/teams/isaac/models/dnn_stereo_disparity) DNN to perform stereo depth estimation via continuous disparity prediction. Given a pair of stereo input images, the package generates a disparity map of the left input image. + +### ESS DNN +[ESS](https://arxiv.org/pdf/1803.09719.pdf) stands for Enhanced Semi-Supervised stereo disparity DNN, which was developed by NVIDIA. The ESS DNN is used to predict the disparity for each pixel from stereo camera image pairs. This network has improvements over classic CV approaches that use epi-polar geometry to compute disparity, as the DNN can learn to predict disparity in cases where epi-polar geometry feature matching fails. The semi-supervised learning and stereo disparity matching makes the DNN robust to environment that is unseen in the training datasets and occluded objects. This DNN is optimized for and evaluated with RGB global shutter stereo camera images, and accuracy may vary with monochrome images. + +The predicted [disparity](https://en.wikipedia.org/wiki/Binocular_disparity) values represent the distance a point moves from one image to the other in a stereo image pair (a.k.a. the binocular image pair). The disparity is inversely proportional to the depth (i.e. `disparity = focalLength x baseline / depth`). Given the [focal length](https://en.wikipedia.org/wiki/Focal_length) and [baseline](https://en.wikipedia.org/wiki/Stereo_camera) of the camera that generates a stereo image pair, the predicted disparity map from the `isaac_ros_ess` package can be used to compute depth and generate a [point cloud](https://en.wikipedia.org/wiki/Point_cloud). + +### Isaac ROS NITROS Acceleration +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. + +## Performance +The following are the benchmark performance results of the prepared pipelines in this package, by supported platform: + +| Pipeline | AGX Orin | AGX Xavier | x86_64 w/ RTX 3060 Ti | +| -------------------------- | -------- | ---------- | --------------------- | +| ESS Disparity Node (1080p) | 51 fps | 24 fps | 118 fps | + +## Table of Contents +- [Isaac ROS DNN Stereo Disparity](#isaac-ros-dnn-stereo-disparity) + - [Overview](#overview) + - [ESS DNN](#ess-dnn) + - [Isaac ROS NITROS Acceleration](#isaac-ros-nitros-acceleration) + - [Performance](#performance) + - [Table of Contents](#table-of-contents) + - [Latest Update](#latest-update) + - [Supported Platforms](#supported-platforms) + - [Docker](#docker) + - [Quickstart](#quickstart) + - [Next Steps](#next-steps) + - [Try Advanced Examples](#try-advanced-examples) + - [Try NITROS-Accelerated Graph with Argus](#try-nitros-accelerated-graph-with-argus) + - [Use Different Models](#use-different-models) + - [Customize Your Dev Environment](#customize-your-dev-environment) + - [Package Reference](#package-reference) + - [`isaac_ros_ess`](#isaac_ros_ess) + - [Overview](#overview-1) + - [Usage](#usage) + - [ROS Parameters](#ros-parameters) + - [ROS Topics Subscribed](#ros-topics-subscribed) + - [ROS Topics Published](#ros-topics-published) + - [Input Restrictions](#input-restrictions) + - [Output Interpretations](#output-interpretations) + - [Troubleshooting](#troubleshooting) + - [Isaac ROS Troubleshooting](#isaac-ros-troubleshooting) + - [Deep Learning Troubleshooting](#deep-learning-troubleshooting) + - [Package not found while launching the visualizer script](#package-not-found-while-launching-the-visualizer-script) + - [Symptom](#symptom) + - [Solution](#solution) + - [Problem reserving CacheChange in reader](#problem-reserving-cachechange-in-reader) + - [Symptom](#symptom-1) + - [Solution](#solution-1) + - [Updates](#updates) + +## Latest Update +Update 2022-06-30: Initial release. + +## Supported Platforms +This package is designed and tested to be compatible with ROS2 Humble running on [Jetson](https://developer.nvidia.com/embedded-computing) or an x86_64 system with an NVIDIA GPU. + + +| Platform | Hardware | Software | Notes | +| -------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Jetson | [AGX Orin](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
[AGX Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/) | [JetPack 5.0.1 DP](https://developer.nvidia.com/embedded/jetpack) | For best performance, ensure that [power settings](https://docs.nvidia.com/jetson/archives/r34.1/DeveloperGuide/text/SD/PlatformPowerAndPerformance.html) are configured appropriately. | +| x86_64 | NVIDIA GPU | [Ubuntu 20.04+](https://releases.ubuntu.com/20.04/)
[CUDA 11.6.1+](https://developer.nvidia.com/cuda-downloads) | + +### Docker +To simplify development, we strongly recommend leveraging the Isaac ROS Dev Docker images by following [these steps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md). This will streamline your development environment setup with the correct versions of dependencies on both Jetson and x86_64 platforms. + +> **Note:** All Isaac ROS Quickstarts, tutorials, and examples have been designed with the Isaac ROS Docker images as a prerequisite. + +## Quickstart +1. Set up your development environment by following the instructions [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md). + +2. Clone this repository and its dependencies under `~/workspaces/isaac_ros-dev/src`: + ```bash + mkdir -p ~/workspaces/isaac_ros-dev/src && cd ~/workspaces/isaac_ros-dev/src + ``` + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common + ``` + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nitros + ``` + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_pipeline + ``` + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_stereo_disparity + ``` + +3. Pull down a ROS Bag of sample data: + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity && \ + git lfs pull -X "" -I "resources/rosbags/ess_rosbag" + ``` + +4. Launch the Docker container using the `run_dev.sh` script: + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && ./scripts/run_dev.sh + ``` + +5. Download the pre-trained ESS model from the [ESS model page](https://catalog.ngc.nvidia.com/orgs/nvidia/teams/isaac/models/dnn_stereo_disparity): + ```bash + cd /workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/resources && \ + wget 'https://api.ngc.nvidia.com/v2/models/nvidia/isaac/dnn_stereo_disparity/versions/1.0.0/files/ess.etlt' + ``` + +6. Convert the encrypted model (`.etlt`) to a TensorRT engine plan: + ```bash + /opt/nvidia/tao/tao-converter -k ess -t fp16 -e /workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/resources/ess.engine -o output_left /workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/resources/ess.etlt + ``` + +7. Build and source the workspace: + ```bash + cd /workspaces/isaac_ros-dev && \ + colcon build && \ + source install/setup.bash + ``` + **Note**: We recommend rebuilding the package each time the source files are edited. Before rebuilding, first clean the workspace by running `rm -r build install log`. + +8. (Optional) Run tests to verify complete and correct installation: + ```bash + colcon test --executor sequential + ``` + +9. Launch the ESS Disparity Node: + ```bash + ros2 launch isaac_ros_ess isaac_ros_ess.launch.py engine_file_path:=/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/resources/ess.engine + ``` + +10. Visualize and validate the output of the package: + ```bash + ros2 run isaac_ros_ess isaac_ros_ess_visualizer.py + ``` +
+ +## Next Steps +### Try Advanced Examples +To continue exploring the DNN Stereo Disparity package, check out the following suggested examples: + +- [Generating disparity maps from a stereo pair of image files](./docs/visualize-image.md) + +### Try NITROS-Accelerated Graph with Argus +If you have an Argus-compatible camera, you can launch the NITROS-accelerated graph by following the [tutorial](docs/tutorial-nitros-graph.md). + +### Use Different Models +Click [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference/blob/main/docs/model-preparation.md) for more information about how to use NGC models. + +### Customize Your Dev Environment +To customize your development environment, reference [this guide](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/modify-dockerfile.md). + +## Package Reference +### `isaac_ros_ess` +#### Overview +The `isaac_ros_ess` package offers functionality to generate a stereo disparity map from stereo images using a trained ESS model. Given a pair of stereo input images, the package generates a continuous disparity image for the left input image. + +#### Usage +```bash +ros2 launch isaac_ros_ess isaac_ros_ess.launch.py engine_file_path:= +``` +#### ROS Parameters +| ROS Parameter | Type | Default | Description | +| ------------------ | -------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `engine_file_path` | `string` | N/A - Required | The absolute path to the ESS engine file. | +| `image_type` | `string` | `"RGB_U8"`. | The input image encoding type. Supports `"RGB_U8"` and `"BGR_U8"`.
Note that if users do not specify this parameter and there is an upstreaming Isaac ROS NITROS node, the type will be decided automatically. | + +#### ROS Topics Subscribed +| ROS Topic | Interface | Description | +| ------------------- | -------------------------------------------------------------------------------------------------------------- | --------------------------------- | +| `left/image_rect` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | The left image of a stereo pair. | +| `right/image_rect` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | The right image of a stereo pair. | +| `left/camera_info` | [sensor_msgs/CameraInfo](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/CameraInfo.msg) | The left camera model. | +| `right/camera_info` | [sensor_msgs/CameraInfo](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/CameraInfo.msg) | The right camera model. | + +#### ROS Topics Published +| ROS Topic | Interface | Description | +| ----------- | ---------------------------------------------------------------------------------------------------------------------- | ------------------------------------------- | +| `disparity` | [stereo_msgs/DisparityImage](https://github.com/ros2/common_interfaces/blob/humble/stereo_msgs/msg/DisparityImage.msg) | The continuous stereo disparity estimation. | + +#### Input Restrictions +1. The input left and right images must have the **same dimension and resolution**, and the resolution must be **no larger than `1920x1200`**. + +2. Each input pair (`left/image_rect`, `right/image_rect`, `left/camera_info` and `right/camera_info`) should have the **same timestamp**; otherwise, the synchronizing module inside the ESS Disparity Node will drop the input with smaller timestamps. + +#### Output Interpretations +1. The `isaas_ros_ess` package outputs a disparity image with the same resolution as the input stereo pairs. The input images are rescaled to the ESS model input size before inferencing and the output prediction is rescaled back before publishing. To alter this behavior, use input images with the model-native resolution: `W=960, H=576`. + +2. The left and right `CameraInfo` are used to composite a `stereo_msgs/DisparityImage`. If you only care about the disparity image, and don't need the baseline and focal length information, you can pass dummy camera messages. + +## Troubleshooting +### Isaac ROS Troubleshooting +For solutions to problems with Isaac ROS, check [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/troubleshooting.md). + +### Deep Learning Troubleshooting +For solutions to problems using DNN models, check [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference/blob/main/docs/troubleshooting.md). + +### Package not found while launching the visualizer script +#### Symptom +```bash +$ ros2 run isaac_ros_ess isaac_ros_ess_visualizer.py +Package 'isaac_ros_ess' not found +``` + +#### Solution +Use the `colcon build --packages-up-to isaac_ros_ess` command to build `isaac_ros_ess`; do not use the `--symlink-install` option. Run `source install/setup.bash` after the build. + +### Problem reserving CacheChange in reader +#### Symptom +When using a ROS bag as input, `isaac_ros_ess` throws an error if the input topics are published too fast: +```bash +[component_container-1] 2022-06-24 09:04:43.584 [RTPS_MSG_IN Error] (ID:281473268431152) Problem reserving CacheChange in reader: 01.0f.cd.10.ab.f2.65.b6.01.00.00.00|0.0.20.4 -> Function processDataMsg +``` + +#### Solution +Make sure that the ROS bag has a reasonable size and publish rate. + +## Updates + +| Date | Changes | +| ---------- | --------------- | +| 2022-06-30 | Initial release | diff --git a/docs/tutorial-nitros-graph.md b/docs/tutorial-nitros-graph.md new file mode 100644 index 0000000..85ff4ba --- /dev/null +++ b/docs/tutorial-nitros-graph.md @@ -0,0 +1,41 @@ +# Tutorial to Run NITROS-Accelerated Graph with Argus Camera + +```mermaid +graph LR; + argus_node("ArgusStereoNode (Raw Image)") --> left_rectify_node("RectifyNode (Rectified Image)"); + argus_node --> right_rectify_node("RectifyNode (Rectified Image)"); + left_rectify_node --> ess_node("ESSDisparityNode (DNN Inference)"); + right_rectify_node --> ess_node; + ess_node --> point_cloud_point("PointCloudNode (Point Cloud Output)"); +``` + +If you have an [Argus-compatible camera](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_argus_camera), you can also use the launch file provided in this package to start a fully NITROS-accelerated stereo disparity graph. + +To start the graph, follow the steps below: + +1. Follow the [Quickstart section](../README.md#quickstart) up to step 6 in the main README. + +2. Outside the container, clone an additional repository required to run Argus-compatible camera under `~/workspaces/isaac_ros-dev/src`. + + ```bash + cd ~/workspaces/isaac_ros-dev/src + ``` + + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_argus_camera + ``` + +3. Inside the container, build and source the workspace: + ```bash + cd /workspaces/isaac_ros-dev && \ + colcon build && \ + source install/setup.bash + ``` +4. (Optional) Run tests to verify complete and correct installation: + ```bash + colcon test --executor sequential + ``` + +5. Launch ESS Disparity Node: + ```bash + ros2 launch isaac_ros_ess isaac_ros_argus_ess.launch.py engine_file_path:=/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/resources/ess.engine diff --git a/docs/visualize-image.md b/docs/visualize-image.md new file mode 100644 index 0000000..5d9ac77 --- /dev/null +++ b/docs/visualize-image.md @@ -0,0 +1,32 @@ +# Instructions to Generate Disparity Maps for Stereo Images + +These are instructions for generating a disparity map for a given stereo image pair. + +In addition to supporting the ROS Bag input type, the `isaac_ros_ess_visualizer.py` script also supports raw input images and camera info files. Follow the steps to generate a disparity estimation from raw inputs: + +1. Complete the [Quickstart](../README.md#quickstart) guide first. + +2. Pull the example data: + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity && \ + git lfs pull -X "" -I "resources/examples" + ``` + +3. Launch the ESS Disparity Node: + ```bash + ros2 launch isaac_ros_ess isaac_ros_ess.launch.py engine_file_path:=/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/resources/ess.engine + ``` + +4. Visualize and validate the output of the package: + ```bash + ros2 run isaac_ros_ess isaac_ros_ess_visualizer.py --raw_inputs + ``` +
+ +5. Try your own examples: + ```bash + ros2 run isaac_ros_ess isaac_ros_ess_visualizer.py --raw_inputs \ + --left_image_path '' \ + --right_image_path '' \ + --camera_info_path '' + ``` diff --git a/isaac_ros_ess/CMakeLists.txt b/isaac_ros_ess/CMakeLists.txt new file mode 100644 index 0000000..bf044e4 --- /dev/null +++ b/isaac_ros_ess/CMakeLists.txt @@ -0,0 +1,91 @@ +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +cmake_minimum_required(VERSION 3.5) +project(isaac_ros_ess LANGUAGES C CXX) + + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Default to Release build +if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) +endif() +message( STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}" ) + +execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) +message( STATUS "Architecture: ${ARCHITECTURE}" ) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +# isaac_ros_ess_node +ament_auto_add_library(isaac_ros_ess_node SHARED src/ess_disparity_node.cpp) +target_compile_definitions(isaac_ros_ess_node + PRIVATE "COMPOSITION_BUILDING_DLL" +) +target_link_libraries(isaac_ros_ess_node) +rclcpp_components_register_node(isaac_ros_ess_node + PLUGIN "nvidia::isaac_ros::dnn_stereo_disparity::ESSDisparityNode" + EXECUTABLE isaac_ros_ess) + +# Install config directory +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +# Install package executable +install(TARGETS isaac_ros_ess_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install extensions directory +if( ${ARCHITECTURE} STREQUAL "x86_64" ) + install(DIRECTORY gxf/lib/gxf_x86_64/ DESTINATION share/${PROJECT_NAME}/gxf) +elseif( ${ARCHITECTURE} STREQUAL "aarch64" ) + install(DIRECTORY gxf/lib/gxf_jetpack50/ DESTINATION share/${PROJECT_NAME}/gxf) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License + set(ament_cmake_copyright_FOUND TRUE) + + ament_lint_auto_find_test_dependencies() + + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/isaac_ros_ess_test.py TIMEOUT "300") +endif() + +# Launch/Visualizer python scripts +ament_python_install_package(${PROJECT_NAME}) +# Install Python executables +install(PROGRAMS + launch/isaac_ros_ess.launch.py + launch/isaac_ros_argus_ess.launch.py + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/isaac_ros_ess_visualizer.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_auto_package() diff --git a/isaac_ros_ess/config/ess_inference.yaml b/isaac_ros_ess/config/ess_inference.yaml new file mode 100644 index 0000000..69bda97 --- /dev/null +++ b/isaac_ros_ess/config/ess_inference.yaml @@ -0,0 +1,266 @@ +%YAML 1.2 +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. +--- +name: sync +components: +- name: left_image_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: left_image_receiver + min_size: 1 +- name: right_image_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: right_image_receiver + min_size: 1 +- name: data_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 + policy: 0 +- name: left_cam_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: left_cam_receiver + min_size: 1 +- name: right_cam_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: right_cam_receiver + min_size: 1 +- type: nvidia::isaac_ros::Synchronization + parameters: + left_image_input: left_image_receiver + right_image_input: right_image_receiver + left_cam_input: left_cam_receiver + right_cam_input: right_cam_receiver + left_image_output: left_image_transmitter + right_image_output: right_image_transmitter + left_cam_output: left_cam_transmitter + right_cam_output: right_cam_transmitter + inputs: [left_image_receiver, right_image_receiver, left_cam_receiver, right_cam_receiver] + outputs: [left_image_transmitter, right_image_transmitter, left_cam_transmitter, right_cam_transmitter] +- name: left_image_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: left_image_transmitter + min_size: 1 +- name: right_image_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: right_image_transmitter + min_size: 1 +- name: left_cam_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: left_cam_transmitter + min_size: 1 +- name: right_cam_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: right_cam_transmitter + min_size: 1 +--- +name: ess +components: +- name: data_receiver_left + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: data_receiver_left + min_size: 1 +- name: data_receiver_right + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: data_receiver_right + min_size: 1 +- name: adapter + type: nvidia::cvcore::tensor_ops::ImageAdapter + parameters: + message_type: VideoBuffer +- name: data_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 12 + policy: 0 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: data_transmitter + min_size: 1 +- name: pool + type: nvidia::gxf::BlockMemoryPool + parameters: + storage_type: 1 + block_size: 18432000 + num_blocks: 40 +- type: nvidia::cvcore::ESS + parameters: + output_name: disparity + pool: pool + left_image_receiver: data_receiver_left + right_image_receiver: data_receiver_right + output_transmitter: data_transmitter + output_adapter: adapter + image_type: RGB_U8 + pixel_mean: [-128, -128, -128] + normalization: [0.00392156862, 0.00392156862, 0.00392156862] + standard_deviation: [0.5, 0.5, 0.5] + max_batch_size: 1 + input_layer_width: 960 + input_layer_height: 576 + model_input_type: RGB_U8 + engine_file_path: engine_file_path_placeholder + input_layers_name: [input_left, input_right] + output_layers_name: [output_left] + preprocess_type: RESIZE +--- +name: disparity_compositor +components: +- name: disparity_in + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: disparity_in + min_size: 1 +- name: left_cam_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: left_cam_receiver + min_size: 1 +- name: right_cam_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: right_cam_receiver + min_size: 1 +- name: data_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: data_transmitter + min_size: 1 +- name: pool + type: nvidia::gxf::BlockMemoryPool + parameters: + storage_type: 1 + block_size: 18432000 + num_blocks: 40 +- type: nvidia::isaac_ros::DisparityCompositor + parameters: + left_camera_model_receiver: left_cam_receiver + right_camera_model_receiver: right_cam_receiver + disparity_receiver: disparity_in + output_transmitter: data_transmitter + min_disparity: 0 + max_disparity: -1 +--- +name: vault +components: +- name: signal + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: signal + min_size: 1 +- name: vault + type: nvidia::gxf::Vault + parameters: + source: signal + max_waiting_count: 1 + drop_waiting: false +--- +name: connections +components: +- type: nvidia::gxf::Connection + parameters: + source: sync/left_image_transmitter + target: ess/data_receiver_left +- type: nvidia::gxf::Connection + parameters: + source: sync/right_image_transmitter + target: ess/data_receiver_right +- type: nvidia::gxf::Connection + parameters: + source: sync/left_cam_transmitter + target: disparity_compositor/left_cam_receiver +- type: nvidia::gxf::Connection + parameters: + source: sync/right_cam_transmitter + target: disparity_compositor/right_cam_receiver +- type: nvidia::gxf::Connection + parameters: + source: ess/data_transmitter + target: disparity_compositor/disparity_in +- type: nvidia::gxf::Connection + parameters: + source: disparity_compositor/data_transmitter + target: vault/signal +--- +components: +- name: clock + type: nvidia::gxf::RealtimeClock +- type: nvidia::gxf::GreedyScheduler + parameters: + clock: clock + stop_on_deadlock: false diff --git a/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libgxf_cvcore_ess.so b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libgxf_cvcore_ess.so new file mode 100755 index 0000000..5fd2e78 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libgxf_cvcore_ess.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8174d9db76b0fbcf92de894363abc28cfe21fd95f5c0a19bb14271047aa2694c +size 2143104 diff --git a/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_core.so b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_core.so new file mode 100755 index 0000000..517db51 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_core.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aefbb7610d7dafcad34a5ea582b9fcb5a5ce34a87980b8734dad55ecd6cd021e +size 182712 diff --git a/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_ess.so b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_ess.so new file mode 100755 index 0000000..21b6944 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_ess.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fd4d111ee47514909b4ab2bd47ed944b7d6ef7236377ec43b9b8233b99d89231 +size 58000 diff --git a/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_inferencer.so b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_inferencer.so new file mode 100755 index 0000000..777d011 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_inferencer.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a48ce0cc3986bc286697b34bdd51e77f53ec77a7466380704f400327be614751 +size 61488 diff --git a/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_tensorops.so b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_tensorops.so new file mode 100755 index 0000000..3cb9317 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_tensorops.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c26b35a91f9ce9b8aa416d33ce57227c59dfa876d89f85248b19006d9cc4303e +size 295824 diff --git a/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_trtbackend.so b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_trtbackend.so new file mode 100755 index 0000000..00c50a7 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_jetpack50/ess/libnvcv_trtbackend.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:289bef727caa168a3f52d3d6a278307c2b874fb0b00d6526962905063e25fe84 +size 47264 diff --git a/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libgxf_cvcore_ess.so b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libgxf_cvcore_ess.so new file mode 100755 index 0000000..af915f2 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libgxf_cvcore_ess.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b43f66ef8da78db25b561a97c553992bfeaa2bad30c10e22e100a4435c031d63 +size 2289664 diff --git a/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_core.so b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_core.so new file mode 100755 index 0000000..d9140e7 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_core.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5eb944fde46c64608e22330d729ea78cfce7744b49aa1f7e00d5cc26d54c8689 +size 198208 diff --git a/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_ess.so b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_ess.so new file mode 100755 index 0000000..677b778 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_ess.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:703f4099cda931a8a2d2790976c696fd3d39929675df109ceb9efe9c9ed59cf7 +size 65016 diff --git a/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_inferencer.so b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_inferencer.so new file mode 100755 index 0000000..a2148b0 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_inferencer.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ecfb7720bea6d07a4bbba9d2fcc6c0e642850fcc26e013e0d5a43bd206cb291 +size 67976 diff --git a/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_tensorops.so b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_tensorops.so new file mode 100755 index 0000000..ac24091 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_tensorops.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:68ba19823b3c14453f901a3532cafe577897f80b5434dbda85fc7f26f426b0b4 +size 313608 diff --git a/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_trtbackend.so b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_trtbackend.so new file mode 100755 index 0000000..c03c1c3 --- /dev/null +++ b/isaac_ros_ess/gxf/lib/gxf_x86_64/ess/libnvcv_trtbackend.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:52f200683d97749900df6fea3860d9a5a013d326c0aee9883ad13d8ac2099607 +size 50568 diff --git a/isaac_ros_ess/include/isaac_ros_ess/ess_disparity_node.hpp b/isaac_ros_ess/include/isaac_ros_ess/ess_disparity_node.hpp new file mode 100644 index 0000000..bf0b89b --- /dev/null +++ b/isaac_ros_ess/include/isaac_ros_ess/ess_disparity_node.hpp @@ -0,0 +1,58 @@ +/** + * Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#ifndef ISAAC_ROS_ESS__ESS_DISPARITY_NODE_HPP_ +#define ISAAC_ROS_ESS__ESS_DISPARITY_NODE_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "isaac_ros_nitros/nitros_node.hpp" + +using StringList = std::vector; + +namespace nvidia +{ +namespace isaac_ros +{ +namespace dnn_stereo_disparity +{ + +class ESSDisparityNode : public nitros::NitrosNode +{ +public: + explicit ESSDisparityNode(const rclcpp::NodeOptions &); + + ~ESSDisparityNode(); + + ESSDisparityNode(const ESSDisparityNode &) = delete; + + ESSDisparityNode & operator=(const ESSDisparityNode &) = delete; + + // The callback for submitting parameters to the node's graph + void postLoadGraphCallback() override; + +private: + const std::string image_type_; + const int input_layer_width_; + const int input_layer_height_; + const std::string model_input_type_; + const std::string engine_file_path_; + const std::vector input_layers_name_; + const std::vector output_layers_name_; +}; + +} // namespace dnn_stereo_disparity +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_ESS__ESS_DISPARITY_NODE_HPP_ diff --git a/isaac_ros_ess/isaac_ros_ess/__init__.py b/isaac_ros_ess/isaac_ros_ess/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_ess/launch/isaac_ros_argus_ess.launch.py b/isaac_ros_ess/launch/isaac_ros_argus_ess.launch.py new file mode 100644 index 0000000..4d08ec9 --- /dev/null +++ b/isaac_ros_ess/launch/isaac_ros_argus_ess.launch.py @@ -0,0 +1,106 @@ +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + launch_args = [ + DeclareLaunchArgument( + 'engine_file_path', + default_value='', + description='The absolute path to the ESS engine plan.'), + ] + engine_file_path = LaunchConfiguration('engine_file_path') + + argus_stereo_node = ComposableNode( + name='argus_stereo', + package='isaac_ros_argus_camera', + plugin='nvidia::isaac_ros::argus::ArgusStereoNode', + parameters=[{ + 'left_optical_frame_name': 'left/image_rect', + 'right_optical_frame_name': 'right/image_rect', + }], + ) + + left_rectify_node = ComposableNode( + name='left_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + parameters=[{ + 'output_width': 1920, + 'output_height': 1200, + }], + remappings=[ + ('image_raw', 'left/image_raw'), + ('camera_info', 'left/camerainfo'), + ('image_rect', 'left/image_rect'), + ('camera_info_rect', 'left/camera_info_rect') + ] + ) + + right_rectify_node = ComposableNode( + name='right_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + parameters=[{ + 'output_width': 1920, + 'output_height': 1200, + }], + remappings=[ + ('image_raw', 'right/image_raw'), + ('camera_info', 'right/camerainfo'), + ('image_rect', 'right/image_rect'), + ('camera_info_rect', 'right/camera_info_rect') + ] + ) + + disparity_node = ComposableNode( + name='disparity', + package='isaac_ros_ess', + plugin='nvidia::isaac_ros::dnn_stereo_disparity::ESSDisparityNode', + parameters=[{'engine_file_path': engine_file_path}], + remappings=[ + ('left/camera_info', 'left/camera_info_rect'), + ('right/camera_info', 'right/camera_info_rect') + ] + ) + + point_cloud_node = ComposableNode( + name='point_cloud_node', + package='isaac_ros_stereo_image_proc', + plugin='nvidia::isaac_ros::stereo_image_proc::PointCloudNode', + parameters=[{ + 'approximate_sync': False, + 'use_color': False, + 'use_system_default_qos': True, + }], + remappings=[ + ('left/image_rect_color', 'left/image_rect'), + ('left/camera_info', 'left/camera_info_rect'), + ('right/camera_info', 'right/camera_info_rect') + ] + ) + + container = ComposableNodeContainer( + name='disparity_container', + namespace='disparity', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + argus_stereo_node, left_rectify_node, right_rectify_node, + disparity_node, point_cloud_node + ], + output='screen' + ) + + return (launch.LaunchDescription(launch_args + [container])) diff --git a/isaac_ros_ess/launch/isaac_ros_ess.launch.py b/isaac_ros_ess/launch/isaac_ros_ess.launch.py new file mode 100644 index 0000000..c2a749e --- /dev/null +++ b/isaac_ros_ess/launch/isaac_ros_ess.launch.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + launch_args = [ + DeclareLaunchArgument( + 'engine_file_path', + default_value='', + description='The absolute path to the ESS engine plan.'), + ] + engine_file_path = LaunchConfiguration('engine_file_path') + + disparity_node = ComposableNode( + name='disparity', + package='isaac_ros_ess', + plugin='nvidia::isaac_ros::dnn_stereo_disparity::ESSDisparityNode', + parameters=[{'engine_file_path': engine_file_path}]) + + container = ComposableNodeContainer( + name='disparity_container', + namespace='disparity', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[disparity_node], + output='screen' + ) + + return (launch.LaunchDescription(launch_args + [container])) diff --git a/isaac_ros_ess/package.xml b/isaac_ros_ess/package.xml new file mode 100644 index 0000000..e396675 --- /dev/null +++ b/isaac_ros_ess/package.xml @@ -0,0 +1,38 @@ + + + + + + + isaac_ros_ess + 0.9.0 + DNN Stereo Disparity Network for Isaac ROS + + Xutong Ren + NVIDIA Isaac ROS Software License + https://developer.nvidia.com/isaac-ros-gems/ + Xutong Ren + + ament_cmake_auto + + rclcpp + rclcpp_components + isaac_ros_nitros + isaac_ros_stereo_image_proc + + ament_lint_auto + ament_lint_common + isaac_ros_test + + + ament_cmake + + diff --git a/isaac_ros_ess/scripts/isaac_ros_ess_visualizer.py b/isaac_ros_ess/scripts/isaac_ros_ess_visualizer.py new file mode 100644 index 0000000..0ea1f14 --- /dev/null +++ b/isaac_ros_ess/scripts/isaac_ros_ess_visualizer.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +# This script loads images from a folder and sends them to the ESSDisparityNode for inference, +# then saves the output prediction to spcified location as an image. + +import argparse +import subprocess + +import cv2 +import cv_bridge +from isaac_ros_test import JSONConversion +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image +from stereo_msgs.msg import DisparityImage + + +def get_args(): + parser = argparse.ArgumentParser(description='ESS Disparity Node Visualizer') + parser.add_argument('--save_image', action='store_true', help='Save output or display it.') + parser.add_argument('--result_path', default='/workspaces/isaac_ros-dev/src/output.png', + help='Absolute path to save your result.') + parser.add_argument('--raw_inputs', action='store_true', + help='Use rosbag as inputs or raw image and camera info files as inputs.') + parser.add_argument('--rosbag_path', + default='/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/' + 'resources/rosbags/ess_rosbag', + help='Absolute path to your rosbag.') + parser.add_argument('--left_image_path', + default='/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/' + 'resources/examples/left.png', + help='Absolute path your left image.') + parser.add_argument('--right_image_path', + default='/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/' + 'resources/examples/right.png', + help='Absolute path your right image.') + parser.add_argument('--camera_info_path', + default='/workspaces/isaac_ros-dev/src/isaac_ros_dnn_stereo_disparity/' + 'resources/examples/camera.json', + help='Absolute path your camera info json file.') + args = parser.parse_args() + return args + + +class ESSVisualizer(Node): + + def __init__(self, args): + super().__init__('ess_visualizer') + self.args = args + self.encoding = 'rgb8' + self._bridge = cv_bridge.CvBridge() + + self._disp_sub = self.create_subscription( + DisparityImage, 'disparity', self.ess_callback, 10) + + if self.args.raw_inputs: + self._prepare_raw_inputs() + else: + self._prepare_rosbag_inputs() + + def _prepare_rosbag_inputs(self): + subprocess.Popen('ros2 bag play -l ' + self.args.rosbag_path, shell=True) + + def _prepare_raw_inputs(self): + self._img_left_pub = self.create_publisher( + Image, 'left/image_rect', 10) + self._img_right_pub = self.create_publisher( + Image, 'right/image_rect', 10) + self._camera_left_pub = self.create_publisher( + CameraInfo, 'left/camera_info', 10) + self._camera_right_pub = self.create_publisher( + CameraInfo, 'right/camera_info', 10) + + self.create_timer(5, self.timer_callback) + + left_img = cv2.imread(self.args.left_image_path) + right_img = cv2.imread(self.args.right_image_path) + self.left_msg = self._bridge.cv2_to_imgmsg(np.array(left_img), self.encoding) + self.right_msg = self._bridge.cv2_to_imgmsg(np.array(right_img), self.encoding) + + self.camera_info = JSONConversion.load_camera_info_from_json(self.args.camera_info_path) + + def timer_callback(self): + self._img_left_pub.publish(self.left_msg) + self._img_right_pub.publish(self.right_msg) + self._camera_left_pub.publish(self.camera_info) + self._camera_right_pub.publish(self.camera_info) + self.get_logger().info('Inputs were published.') + + def ess_callback(self, disp_msg): + self.get_logger().info('Result was received.') + disp_img = self._bridge.imgmsg_to_cv2(disp_msg.image) + # Normalize and convert to colormap for visalization + disp_img = (disp_img - disp_img.min()) / disp_img.max() * 255 + color_map = cv2.applyColorMap(disp_img.astype(np.uint8), cv2.COLORMAP_VIRIDIS) + if self.args.save_image: + cv2.imwrite(self.args.result_path, color_map) + else: + cv2.imshow('ess_output', color_map) + cv2.waitKey(1) + + +def main(): + args = get_args() + rclpy.init() + rclpy.spin(ESSVisualizer(args)) + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_ess/src/ess_disparity_node.cpp b/isaac_ros_ess/src/ess_disparity_node.cpp new file mode 100644 index 0000000..e714b27 --- /dev/null +++ b/isaac_ros_ess/src/ess_disparity_node.cpp @@ -0,0 +1,181 @@ +/** + * Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#include "isaac_ros_ess/ess_disparity_node.hpp" + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace dnn_stereo_disparity +{ + +using nvidia::gxf::optimizer::GraphIOGroupSupportedDataTypesInfoList; + +constexpr char INPUT_LEFT_COMPONENT_KEY[] = "sync/left_image_receiver"; +constexpr char INPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_rgb8"; +constexpr char INPUT_LEFT_TOPIC_NAME[] = "left/image_rect"; + +constexpr char INPUT_RIGHT_COMPONENT_KEY[] = "sync/right_image_receiver"; +constexpr char INPUT_RIGHT_TOPIC_NAME[] = "right/image_rect"; + +constexpr char OUTPUT_COMPONENT_KEY[] = "vault/vault"; +constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_disparity_image_32FC1"; +constexpr char OUTPUT_TOPIC_NAME[] = "disparity"; + +constexpr char INPUT_LEFT_CAM_COMPONENT_KEY[] = "sync/left_cam_receiver"; +constexpr char INPUT_CAMERA_INFO_FORMAT[] = "nitros_camera_info"; +constexpr char INPUT_LEFT_CAMERA_TOPIC_NAME[] = "left/camera_info"; + +constexpr char INPUT_RIGHT_CAM_COMPONENT_KEY[] = "sync/right_cam_receiver"; +constexpr char INPUT_RIGHT_CAMERA_TOPIC_NAME[] = "right/camera_info"; + +constexpr char APP_YAML_FILENAME[] = "config/ess_inference.yaml"; +constexpr char PACKAGE_NAME[] = "isaac_ros_ess"; + +const std::vector> EXTENSIONS = { + {"isaac_ros_nitros", "gxf/std/libgxf_std.so"}, + {"isaac_ros_nitros", "gxf/multimedia/libgxf_multimedia.so"}, + {"isaac_ros_nitros", "gxf/cuda/libgxf_cuda.so"}, + {"isaac_ros_nitros", "gxf/serialization/libgxf_serialization.so"}, + {"isaac_ros_nitros", "gxf/tensorops/libgxf_tensorops.so"}, + {"isaac_ros_stereo_image_proc", "lib/libgxf_disparity_extension.so"}, + {"isaac_ros_stereo_image_proc", "lib/libgxf_synchronization.so"}, + {"isaac_ros_ess", "gxf/ess/libgxf_cvcore_ess.so"} +}; +const std::vector PRESET_EXTENSION_SPEC_NAMES = { + "isaac_ros_ess", +}; +const std::vector EXTENSION_SPEC_FILENAMES = {}; +const std::vector GENERATOR_RULE_FILENAMES = {}; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +const nitros::NitrosPublisherSubscriberConfigMap CONFIG_MAP = { + {INPUT_LEFT_COMPONENT_KEY, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(1), + .compatible_data_format = INPUT_DEFAULT_TENSOR_FORMAT, + .topic_name = INPUT_LEFT_TOPIC_NAME, + } + }, + {INPUT_RIGHT_COMPONENT_KEY, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(1), + .compatible_data_format = INPUT_DEFAULT_TENSOR_FORMAT, + .topic_name = INPUT_RIGHT_TOPIC_NAME, + } + }, + {OUTPUT_COMPONENT_KEY, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(1), + .compatible_data_format = OUTPUT_DEFAULT_TENSOR_FORMAT, + .topic_name = OUTPUT_TOPIC_NAME, + .frame_id_source_key = INPUT_LEFT_COMPONENT_KEY, + } + }, + {INPUT_LEFT_CAM_COMPONENT_KEY, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(1), + .compatible_data_format = INPUT_CAMERA_INFO_FORMAT, + .topic_name = INPUT_LEFT_CAMERA_TOPIC_NAME, + } + }, + {INPUT_RIGHT_CAM_COMPONENT_KEY, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(1), + .compatible_data_format = INPUT_CAMERA_INFO_FORMAT, + .topic_name = INPUT_RIGHT_CAMERA_TOPIC_NAME, + } + } +}; +#pragma GCC diagnostic pop + +ESSDisparityNode::ESSDisparityNode(const rclcpp::NodeOptions & options) +: nitros::NitrosNode(options, + APP_YAML_FILENAME, + CONFIG_MAP, + PRESET_EXTENSION_SPEC_NAMES, + EXTENSION_SPEC_FILENAMES, + GENERATOR_RULE_FILENAMES, + EXTENSIONS, + PACKAGE_NAME), + image_type_(declare_parameter("image_type", "")), + input_layer_width_(declare_parameter("input_layer_width", 960)), + input_layer_height_(declare_parameter("input_layer_height", 576)), + model_input_type_(declare_parameter("model_input_type", "RGB_U8")), + engine_file_path_(declare_parameter("engine_file_path", "")), + input_layers_name_(declare_parameter>( + "input_layers_name", {"input_left", "input_right"})), + output_layers_name_(declare_parameter>( + "output_layers_name", {"output_left"})) +{ + RCLCPP_DEBUG(get_logger(), "[ESSDisparityNode] Initializing ESSDisparityNode."); + + if (engine_file_path_.empty()) { + throw std::invalid_argument("[ESSDisparityNode] Empty engine_file_path"); + } + + if (!image_type_.empty()) { + if (image_type_ != "RGB_U8" && image_type_ != "BGR_U8") { + RCLCPP_INFO( + get_logger(), "[ESSDisparityNode] Unspported image type: %s.", image_type_.c_str()); + throw std::invalid_argument("[ESSDisparityNode] Only support image_type RGB_U8 and BGR_U8."); + } + auto nitros_format = image_type_ == "RGB_U8" ? "nitros_image_rgb8" : "nitros_image_bgr8"; + config_map_[INPUT_LEFT_COMPONENT_KEY].compatible_data_format = nitros_format; + config_map_[INPUT_LEFT_COMPONENT_KEY].use_compatible_format_only = true; + config_map_[INPUT_RIGHT_COMPONENT_KEY].compatible_data_format = nitros_format; + config_map_[INPUT_RIGHT_COMPONENT_KEY].use_compatible_format_only = true; + } + + startNitrosNode(); +} + +void ESSDisparityNode::postLoadGraphCallback() +{ + // Forward ESSDisparityNode parameters + getNitrosContext().setParameterInt32( + "ess", "nvidia::cvcore::ESS", "input_layer_width", input_layer_width_); + getNitrosContext().setParameterInt32( + "ess", "nvidia::cvcore::ESS", "input_layer_height", input_layer_height_); + getNitrosContext().setParameterStr( + "ess", "nvidia::cvcore::ESS", "model_input_type", model_input_type_); + getNitrosContext().setParameterStr( + "ess", "nvidia::cvcore::ESS", "engine_file_path", engine_file_path_); + getNitrosContext().setParameter1DStrVector( + "ess", "nvidia::cvcore::ESS", "input_layers_name", input_layers_name_); + getNitrosContext().setParameter1DStrVector( + "ess", "nvidia::cvcore::ESS", "output_layers_name", output_layers_name_); + + RCLCPP_INFO( + get_logger(), "[ESSDisparityNode] Setting engine_file_path: %s.", engine_file_path_.c_str()); +} + +ESSDisparityNode::~ESSDisparityNode() {} + +} // namespace dnn_stereo_disparity +} // namespace isaac_ros +} // namespace nvidia + +// Register as a component +RCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::dnn_stereo_disparity::ESSDisparityNode) diff --git a/isaac_ros_ess/test/camera_info.json b/isaac_ros_ess/test/camera_info.json new file mode 100644 index 0000000..a2dbb3b --- /dev/null +++ b/isaac_ros_ess/test/camera_info.json @@ -0,0 +1,51 @@ +{ + "header": { + "frame_id": "tf_camera" + }, + "width": 1920, + "height": 1080, + "distortion_model": "plumb_bob", + "D": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "K": [ + 434.943999, + 0.000000, + 651.073921, + 0.0, + 431.741273, + 441.878037, + 0.0, + 0.0, + 1.0 + ], + "R": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + "P": [ + 434.943999, + 0.0, + 651.073921, + 160.0, + 0.0, + 431.741273, + 441.878037, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ] +} diff --git a/isaac_ros_ess/test/dummy_model.onnx b/isaac_ros_ess/test/dummy_model.onnx new file mode 100644 index 0000000..a3e6d8c --- /dev/null +++ b/isaac_ros_ess/test/dummy_model.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c8bfb3a36567863f53a67010cef0b5d6c31d159515e926d3dfe7b1f8497b2bbe +size 426 diff --git a/isaac_ros_ess/test/isaac_ros_ess_test.py b/isaac_ros_ess/test/isaac_ros_ess_test.py new file mode 100644 index 0000000..9778d00 --- /dev/null +++ b/isaac_ros_ess/test/isaac_ros_ess_test.py @@ -0,0 +1,150 @@ +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import os +import subprocess +import time + +from isaac_ros_test import IsaacROSBaseTest, JSONConversion + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import pytest +import rclpy + +from sensor_msgs.msg import CameraInfo, Image +from stereo_msgs.msg import DisparityImage + + +@pytest.mark.rostest +def generate_test_description(): + dir_path = os.path.dirname(os.path.realpath(__file__)) + engine_file_path = '/tmp/dummy_model.engine' + if not os.path.isfile(engine_file_path): + args = [ + '/usr/src/tensorrt/bin/trtexec', + f'--saveEngine={engine_file_path}', + f'--onnx={dir_path}/dummy_model.onnx' + ] + print('Generating model engine file by command: ', ' '.join(args)) + result = subprocess.run( + args, + env=os.environ, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE + ) + if result.returncode != 0: + raise Exception( + f'Failed to convert with status: {result.returncode}.\n' + f'stderr:\n' + result.stderr.decode('utf-8') + ) + + disparity_node = ComposableNode( + name='disparity', + package='isaac_ros_ess', + plugin='nvidia::isaac_ros::dnn_stereo_disparity::ESSDisparityNode', + namespace=IsaacROSDisparityTest.generate_namespace(), + parameters=[{'engine_file_path': engine_file_path}], + ) + + container = ComposableNodeContainer( + name='disparity_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[disparity_node], + output='screen', + arguments=['--ros-args', '--log-level', 'info'] + ) + return IsaacROSDisparityTest.generate_test_description([container]) + + +class IsaacROSDisparityTest(IsaacROSBaseTest): + IMAGE_HEIGHT = 1080 + IMAGE_WIDTH = 1920 + TIMEOUT = 10 + ENGINE_FILE_PATH = '/tmp/dummy_model.engine' + CAMERA_INFO_PATH = os.path.dirname(os.path.realpath(__file__)) + '/camera_info.json' + + def _create_image(self): + image = Image() + image.height = self.IMAGE_HEIGHT + image.width = self.IMAGE_WIDTH + image.encoding = 'rgb8' + image.is_bigendian = False + image.step = self.IMAGE_WIDTH * 3 + image.data = [0] * self.IMAGE_HEIGHT * self.IMAGE_WIDTH * 3 + return image + + def test_image_disparity(self): + end_time = time.time() + self.TIMEOUT + while time.time() < end_time: + if os.path.isfile(self.ENGINE_FILE_PATH): + break + self.assertTrue(os.path.isfile(self.ENGINE_FILE_PATH), + 'Model engine file was not generated in time.') + + received_messages = {} + self.generate_namespace_lookup(['left/image_rect', 'right/image_rect', + 'left/camera_info', 'right/camera_info', + 'disparity']) + + subs = self.create_logging_subscribers( + [('disparity', DisparityImage)], received_messages) + + image_left_pub = self.node.create_publisher( + Image, self.namespaces['left/image_rect'], self.DEFAULT_QOS + ) + image_right_pub = self.node.create_publisher( + Image, self.namespaces['right/image_rect'], self.DEFAULT_QOS + ) + camera_info_left = self.node.create_publisher( + CameraInfo, self.namespaces['left/camera_info'], self.DEFAULT_QOS + ) + camera_info_right = self.node.create_publisher( + CameraInfo, self.namespaces['right/camera_info'], self.DEFAULT_QOS + ) + + try: + left_image = self._create_image() + right_image = self._create_image() + camera_info = JSONConversion.load_camera_info_from_json(self.CAMERA_INFO_PATH) + + end_time = time.time() + self.TIMEOUT + done = False + + while time.time() < end_time: + image_left_pub.publish(left_image) + image_right_pub.publish(right_image) + camera_info_left.publish(camera_info) + camera_info_right.publish(camera_info) + + rclpy.spin_once(self.node, timeout_sec=0.1) + + if 'disparity' in received_messages: + done = True + break + self.assertTrue(done, 'Didnt recieve output on disparity topic') + + disparity = received_messages['disparity'] + self.assertEqual(disparity.image.encoding, '32FC1') + self.assertEqual(disparity.image.height, self.IMAGE_HEIGHT) + self.assertEqual(disparity.image.width, self.IMAGE_WIDTH) + self.assertEqual(disparity.image.step, self.IMAGE_WIDTH * 4) + self.assertAlmostEqual(disparity.f, -0.3678634) + self.assertAlmostEqual(disparity.t, 434.9440002) + self.assertAlmostEqual(disparity.min_disparity, 0.0) + self.assertAlmostEqual(disparity.max_disparity, 2147483648.0) + + finally: + [self.node.destroy_subscription(sub) for sub in subs] + self.node.destroy_publisher(image_left_pub) + self.node.destroy_publisher(image_right_pub) + self.node.destroy_publisher(camera_info_right) + self.node.destroy_publisher(camera_info_left) diff --git a/resources/examples/camera.json b/resources/examples/camera.json new file mode 100644 index 0000000..4295762 --- /dev/null +++ b/resources/examples/camera.json @@ -0,0 +1,51 @@ +{ + "header": { + "frame_id": "tf_camera" + }, + "width": 1920, + "height": 1200, + "distortion_model": "plumb_bob", + "D": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "K": [ + 434.943999, + 0.000000, + 651.073921, + 0.0, + 431.741273, + 441.878037, + 0.0, + 0.0, + 1.0 + ], + "R": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + "P": [ + 434.943999, + 0.0, + 651.073921, + 160.0, + 0.0, + 431.741273, + 441.878037, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ] +} diff --git a/resources/examples/left.png b/resources/examples/left.png new file mode 100644 index 0000000..523be90 --- /dev/null +++ b/resources/examples/left.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cab9ef3d16401c8f8b83b1a7376b0b7246a3539cc5e185905adb3155efec7fde +size 227859 diff --git a/resources/examples/right.png b/resources/examples/right.png new file mode 100644 index 0000000..9073a61 --- /dev/null +++ b/resources/examples/right.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:58fa4e9371501f0a76d5adf64dfaaa42913753847644e9f51530907af9d92649 +size 229503 diff --git a/resources/output_raw.png b/resources/output_raw.png new file mode 100644 index 0000000..a436f2a --- /dev/null +++ b/resources/output_raw.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6c2707be1384db98a27fd826eee21d170cb1849a13cc7e315c7cbc5a65e85be0 +size 438171 diff --git a/resources/output_rosbag.png b/resources/output_rosbag.png new file mode 100644 index 0000000..1ab5eab --- /dev/null +++ b/resources/output_rosbag.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6a144de04d5f8962819ff70e983fce67150e94191ddd3a28d164a6a3ff19dfee +size 386195 diff --git a/resources/rosbags/ess_rosbag/ess_rosbag_0.db3 b/resources/rosbags/ess_rosbag/ess_rosbag_0.db3 new file mode 100644 index 0000000..d551de7 --- /dev/null +++ b/resources/rosbags/ess_rosbag/ess_rosbag_0.db3 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6f929b197e6e471d716170d7555ec59622f4539185cb027f9d8a8067dda765f5 +size 12468224 diff --git a/resources/rosbags/ess_rosbag/metadata.yaml b/resources/rosbags/ess_rosbag/metadata.yaml new file mode 100644 index 0000000..b378b2a --- /dev/null +++ b/resources/rosbags/ess_rosbag/metadata.yaml @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:998c273b00c01779f359e371e367e7d4f4f874b3db5221945bbc65a736958b14 +size 2428 diff --git a/resources/warehouse.gif b/resources/warehouse.gif new file mode 100644 index 0000000..3af646a --- /dev/null +++ b/resources/warehouse.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8aded79f169309471f7a71785111aa4c8747c0b21912d802ed099db3bf3857d1 +size 2050117