diff --git a/.gitattributes b/.gitattributes
index e2f3e0ca..8d1daa40 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -1,3 +1,6 @@
+# Ignore Python files in linguist
+*.py linguist-detectable=false
+
# Images
*.gif filter=lfs diff=lfs merge=lfs -text
*.jpg filter=lfs diff=lfs merge=lfs -text
diff --git a/README.md b/README.md
index 7e97b44e..169afbc0 100644
--- a/README.md
+++ b/README.md
@@ -1,22 +1,31 @@
-# Isaac ROS Nvblox (Preview)
+# Isaac ROS Nvblox
-
+
## Overview
-Nvblox is a package for building a 3D reconstruction of the environment around your robot from sensor observations in real-time. The reconstruction is intended to be used by path planners to generate collision-free paths. Under the hood, nvblox uses NVIDIA CUDA to accelerate this task to allow operation at real-time rates. This repository contains ROS2 integration for the [nvblox core library](https://github.com/nvidia-isaac/nvblox).
+Isaac ROS Nvblox contains ROS 2 packages for 3D reconstruction and cost maps for navigation. `nvblox_nav2` processes depth and pose to reconstruct a 3D scene in real-time and a 2D costmap for [Nav2](https://github.com/ros-planning/navigation2). The costmap is used in planning during navigation as a vision-based solution to avoid obstacles.
-Given a stream of depth images and the corresponding pose of the depth sensor, Isaac ROS Nvblox produces both a 2D distance map slice, representing the distance from each point to the nearest reconstructed object, and also a 3D mesh for visualization in RVIZ. An optional RGB stream can also be used to color the reconstruction for visualization.
+`nvblox_nav2` is designed to work with stereo cameras, which provide a depth image, and the corresponding pose uses GPU acceleration to compute 3D reconstruction and 2D costmaps using [nvblox](https://github.com/nvidia-isaac/nvblox).
-Read more about Nvblox's technical details [here](docs/technical-details.md).
+
-The figure below shows a simple system utilizing nvblox for path planning.
+Above is a typical graph that uses `nvblox_nav2`. The input color image corresponding to the depth image is processed with `unet`, using the PeopleSemSegNet DNN model to estimate a segmentation mask for persons in the color image. Pose corresponding to the depth image is computed using `visual_slam`. The resulting persons mask and pose is used with the color image and depth image to perform 3D scene reconstruction. The output costmap is provided through a cost map plugin into [Nav2](https://github.com/ros-planning/navigation2), with an optional colorized 3D reconstruction into a Rviz using the mesh visualization plugin.
+
+
+
+`nvblox_nav2` builds the reconstructed map in the form of a TSDF (Truncated Signed Distance Function) stored in a 3D voxel grid. This approach is similar to 3D occupancy grid mapping approaches in which occupancy probabilities are stored at each voxel. However, TSDF-based approaches like nvblox store the (signed) distance to the closest surface at each voxel. The surface of the environment can then be extracted as the zero-level set of this voxelized function. Typically, TSDF-based reconstructions provide higher quality surface reconstructions.
+
+In addition to their use in reconstruction, distance fields are also useful for path planning because they provide an immediate means of checking whether potential future robot positions are in collision.
+
+People are common obstacles for mobile robots, and while part of a costmap, people should not be part of the 3D reconstruction. Planners that provide behavioral awareness by navigating differently depending on their proximity to people, benefit from a costmap for people. Person segmentation is computed using the color image, with the resulting mask applied to the depth image separating depth into scene depth and person depth images. The scene depth image is forwarded to TSDF mapping as explained above, the depth image for people is processed to an occupancy grid map.
+
+To relax the assumption that occupancy grid maps only capture static objects, Nvblox applies an occupancy decay step. At a fixed frequency, all voxel occupancy probabilities are decayed towards 0.5 over time. This means that the state of the map (occupied or free) becomes less certain after it has fallen out of the field of view, until it becomes unknown (0.5 occupancy probability).
-
## Table of Contents
-- [Isaac ROS Nvblox (Preview)](#isaac-ros-nvblox-preview)
+- [Isaac ROS Nvblox](#isaac-ros-nvblox)
- [Overview](#overview)
- [Table of Contents](#table-of-contents)
- [Latest Update](#latest-update)
@@ -26,46 +35,42 @@ The figure below shows a simple system utilizing nvblox for path planning.
- [Next Steps](#next-steps)
- [Try More Examples](#try-more-examples)
- [Customize your Dev Environment](#customize-your-dev-environment)
- - [Package Reference](#package-reference)
- - [`nvblox_nav2`](#nvblox_nav2)
- - [Usage](#usage)
- - [ROS Parameters](#ros-parameters)
- - [ROS Topics Subscribed](#ros-topics-subscribed)
- - [ROS Topics Published](#ros-topics-published)
- - [ROS Services Advertised](#ros-services-advertised)
+ - [Packages Overview](#packages-overview)
+ - [ROS 2 Parameters](#ros-2-parameters)
+ - [ROS 2 Topics and Services](#ros-2-topics-and-services)
- [Troubleshooting](#troubleshooting)
- [Isaac ROS Troubleshooting](#isaac-ros-troubleshooting)
- - [`realsense-ros` packages don't build with ROS2 Humble](#realsense-ros-packages-dont-build-with-ros2-humble)
+ - [*realsense-ros* packages don't build with ROS 2 Humble](#realsense-ros-packages-dont-build-with-ros-2-humble)
+ - [Troubleshooting the Nvblox Realsense Example](#troubleshooting-the-nvblox-realsense-example)
+ - [Troubleshooting ROS 2 communication issues](#troubleshooting-ros-2-communication-issues)
- [Updates](#updates)
## Latest Update
-Update 2022-10-19: Updated OSS licensing
+Update 2023-04-05: Human reconstruction and new weighting functions.
## 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.
+This package is designed and tested to be compatible with ROS 2 Humble running on [Jetson](https://developer.nvidia.com/embedded-computing) or an x86_64 system with an NVIDIA GPU.
-> **Note**: Versions of ROS2 earlier than Humble are **not** supported. This package depends on specific ROS2 implementation features that were only introduced beginning with the Humble release.
+> **Note**: Versions of ROS 2 earlier than Humble are **not** supported. This package depends on specific ROS 2 implementation features that were only introduced beginning with the Humble release.
-| Platform | Hardware | Software | Notes |
-| -------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| Jetson | [Jetson Orin](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
[Jetson Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/) | [JetPack 5.0.2](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) |
+| Platform | Hardware | Software | Notes |
+| -------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| Jetson | [Jetson Orin](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
[Jetson Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/) | [JetPack 5.1.1](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.8+](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.
+> **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. Complete the RealSense setup guide [here](https://github.com/NVIDIA-ISAAC-ROS/.github/blob/main/profile/realsense-setup.md).
-
-3. Clone this repository and its dependencies under `~/workspaces/isaac_ros-dev/src`.
+2. Clone this repository and its dependencies under `~/workspaces/isaac_ros-dev/src`.
```bash
cd ~/workspaces/isaac_ros-dev/src
@@ -80,36 +85,28 @@ To simplify development, we strongly recommend leveraging the Isaac ROS Dev Dock
cd isaac_ros_nvblox && git lfs pull
```
-4. Configure the container created by `isaac_ros_common/scripts/run_dev.sh` to include `librealsense`. Create the `.isaac_ros_common-config` file in the `isaac_ros_common/scripts` directory:
-
- ```bash
- cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common/scripts && \
- touch .isaac_ros_common-config && \
- echo CONFIG_IMAGE_KEY=humble.nav2.realsense > .isaac_ros_common-config
- ```
-
-5. Pull down a ROS Bag of sample data:
+3. Pull down a ROS Bag of sample data:
```bash
cd ~/workspaces/isaac_ros-dev/src/isaac_ros_nvblox && \
git lfs pull -X "" -I "nvblox_ros/test/test_cases/rosbags/nvblox_pol"
```
-6. Launch the Docker container using the `run_dev.sh` script:
+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
```
-7. Inside the container, install package-specific dependencies via `rosdep`:
+5. Inside the container, install package-specific dependencies via `rosdep`:
```bash
cd /workspaces/isaac_ros-dev/ && \
rosdep install -i -r --from-paths src --rosdistro humble -y --skip-keys "libopencv-dev libopencv-contrib-dev libopencv-imgproc-dev python-opencv python3-opencv nvblox"
```
-8. Build and source the workspace:
+6. Build and source the workspace:
```bash
cd /workspaces/isaac_ros-dev && \
@@ -117,106 +114,78 @@ To simplify development, we strongly recommend leveraging the Isaac ROS Dev Dock
source install/setup.bash
```
-9. (Optional) Run tests to verify complete and correct installation:
+8. (Optional) Run tests to verify complete and correct installation:
```bash
colcon test --executor sequential
```
-10. In a **current terminal** inside the Docker container, run the launch file for Nvblox with `nav2`:
+9. In a **current terminal** inside the Docker container, run the launch file for Nvblox with `nav2`:
```bash
source /workspaces/isaac_ros-dev/install/setup.bash && \
- ros2 launch nvblox_nav2 carter_sim.launch.py
+ ros2 launch nvblox_examples_bringup isaac_sim_example.launch.py
```
-11. Open a **second terminal** inside the docker container:
+10. Open a **second terminal** inside the docker container:
```bash
cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \
./scripts/run_dev.sh
```
-12. In the **second terminal**, play the ROS Bag:
+11. In the **second terminal**, play the ROS Bag:
```bash
ros2 bag play src/isaac_ros_nvblox/nvblox_ros/test/test_cases/rosbags/nvblox_pol
```
-You should see the robot reconstructing a mesh, with a costmap overlaid on top.
+You should see the robot reconstructing a mesh, with the 2d esdf slice overlaid on top.
+
+
## Next Steps
### Try More Examples
-To continue your exploration, check out the following suggested examples:
+The launch files for all examples are available in the `nvblox_examples_bringup` package:
+| Launch file | Arguments | Description |
+| ------------------------------------ | ----------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------- |
+| `isaac_sim_example.launch.py` | `run_nav2`, `run_rviz` | Example to run with Isaac Sim ([tutorial](./docs/tutorial-isaac-sim.md)) |
+| `isaac_sim_humans_example.launch.py` | `run_nav2`, `run_rviz` | Example to run with Isaac Sim including human reconstruction ([tutorial](./docs/tutorial-human-reconstruction-isaac-sim.md)) |
+| `realsense_example.launch.py` | `from_bag`, `bag_path`, `run_rviz` | Example to run with a realsense camera ([tutorial](./docs/tutorial-realsense.md)) |
+| `realsense_humans_example.launch.py` | `from_bag`, `bag_path`, `run_rviz` | Example to run with a realsense camera including human reconstruction ([tutorial](./docs/tutorial-human-reconstruction-realsense.md)) |
+| `record_realsense.launch.py` | `launch_realsense`, `run_rqt` | Record realsense data to replay with the above examples ([tutorial](./docs/tutorial-realsense-record.md)) |
-- [Tutorial with Isaac Sim](./docs/tutorial-isaac-sim.md)
-- [Tutorial with realsense, VSLAM and nvblox](./docs/tutorial-nvblox-vslam-realsense.md)
### 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
-
-### `nvblox_nav2`
-
-#### Usage
-
-```bash
-ros2 launch nvblox_nav2 carter_sim.launch.py
-```
-
-#### ROS Parameters
-
-| ROS Parameter | Type | Default | Description |
-| --------------------------------------------- | -------- | ------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `voxel_size` | `float` | `0.05` | Voxel size (in meters) to use for the map. |
-| `esdf` | `bool` | `true` | Whether to compute the ESDF (map of distances to the nearest obstacle). |
-| `esdf_2d` | `bool` | `true` | Whether to compute the ESDF in 2D (true) or 3D (false). |
-| `distance_slice` | `bool` | `true` | Whether to output a distance slice of the ESDF to be used for path planning. |
-| `mesh` | `bool` | `true` | Whether to output a mesh for visualization in rviz, to be used with `nvblox_rviz_plugin`. |
-| `global_frame` | `string` | `map` | The name of the TF frame to be used as the global frame. |
-| `slice_height` | `float` | `1.0` | The *output* slice height for the distance slice and ESDF pointcloud. Does not need to be within min and max height below. In units of meters. |
-| `min_height` | `float` | `0.0` | The minimum height, in meters, to consider obstacles part of the 2D ESDF slice. |
-| `max_height` | `float` | `1.0` | The maximum height, in meters, to consider obstacles part of the 2D ESDF slice. |
-| `max_tsdf_update_hz` | `float` | `10.0` | The maximum rate (in Hz) at which to integrate depth images into the TSDF. A value of 0.0 means there is no cap. |
-| `max_color_update_hz` | `float` | `5.0` | The maximum rate (in Hz) at which to integrate color images into the color layer. A value of 0.0 means there is no cap. |
-| `max_mesh_update_hz` | `float` | `5.0` | The maximum rate (in Hz) at which to update and color the mesh. A value of 0.0 means there is no cap. |
-| `max_esdf_update_hz` | `float` | `2.0` | The maximum rate (in Hz) at which to update the ESDF and output the distance slice. A value of 0.0 means there is no cap. |
-| `tsdf_integrator_max_integration_distance_m` | `float` | `10.0` | The maximum distance, in meters, to integrate the TSDF up to. |
-| `tsdf_integrator_truncation_distance_vox` | `float` | `4.0` | The truncation distance, in units of voxels, for the TSDF. |
-| `tsdf_integrator_max_weight` | `float` | `100.0` | Maximum weight for the TSDF. Setting this number higher will lead to higher-quality reconstructions but worse performance in dynamic scenes. |
-| `mesh_integrator_min_weight` | `float` | `1e-4` | Minimum weight of the TSDF to consider for inclusion in the mesh. |
-| `mesh_integrator_weld_vertices` | `bool` | `false` | Whether to weld identical vertices together in the mesh. Currently reduces the number of vertices by a factor of 5x, but is quite slow so we do not recommend you use this setting. |
-| `color_integrator_max_integration_distance_m` | `float` | `10.0` | Maximum distance, in meters, to integrate the color up to. |
-| `esdf_integrator_min_weight` | `float` | `1e-4` | Minimum weight of the TSDF to consider for inclusion in the ESDF. |
-| `esdf_integrator_min_site_distance_vox` | `float` | `1.0` | Minimum distance to consider a voxel within a surface for the ESDF calculation. |
-| `esdf_integrator_max_distance_m` | `float` | `10.0` | Maximum distance to compute the ESDF up to, in meters. |
-
-#### ROS Topics Subscribed
-
-| ROS Topic | Interface | Description |
-| ------------------- | -------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `depth/image` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | The input depth image to be integrated. Must be paired with a `camera_info` message below. Supports both floating-point (depth in meters) and `uint16` (depth in millimeters, OpenNI format). |
-| `depth/camera_info` | [sensor_msgs/CameraInfo](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/CameraInfo.msg) | Required topic along with the depth image; contains intrinsic calibration parameters of the depth camera. |
-| `color/image` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Image.msg) | Optional input color image to be integrated. Must be paired with a `camera_info` message below. Only used to color the mesh. |
-| `color/camera_info` | [sensor_msgs/CameraInfo](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/CameraInfo.msg) | Optional topic along with the color image above, contains intrinsics of the color camera. |
-
-#### ROS Topics Published
-
-| ROS Topic | Interface | Description |
-| ------------ | ----------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `mesh` | [nvblox_msgs/Mesh](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/msg/Mesh.msg) | A visualization topic showing the mesh produced from the TSDF in a form that can be seen in RViz using `nvblox_rviz_plugin`. Set ``max_mesh_update_hz`` to control its update rate. |
-| `pointcloud` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | A pointcloud of the 2D ESDF (Euclidean Signed Distance Field), with intensity as the metric distance to the nearest obstacle. Set ``max_esdf_update_hz`` to control its update rate. |
-| `map_slice` | [nvblox_msgs/DistanceMapSlice](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/msg/DistanceMapSlice.msg) | A 2D slice of the ESDF, to be consumed by `nvblox_nav2` package for interfacing with Nav2. Set ``max_esdf_update_hz`` to control its update rate. |
-
-#### ROS Services Advertised
-
-| ROS Service | Interface | Description |
-| ----------- | ---------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `save_ply` | [std_srvs/Empty](https://github.com/ros2/common_interfaces/blob/master/std_srvs/srv/Empty.srv) | This service has an empty request and response. Calling this service will write a mesh to disk at the path specified by the `output_dir` parameter. |
+## Packages Overview
+
+* **isaac_ros_nvblox**: A meta-package.
+* **nvblox_examples_bringup**: Launch files and configurations for launching the examples.
+* **nvblox_image_padding**: Node to pad and crop images for adjusting the image size to the fixed input resolution that is required by the [image segmentation network](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_segmentation).
+* **nvblox_isaac_sim**: Contains scripts for launching Isaac Sim configured for use with nvblox.
+* **realsense_splitter**: Node for using the realsense camera with inbuilt projector. See why this is needed [here](./docs/troubleshooting-nvblox-realsense.md).
+* **semantic_label_conversion**: Package for converting semantic labels coming from Isaac Sim to mask images used by nvblox ([readme](./nvblox_examples/semantic_label_conversion/README.md)).
+* **nvblox_msgs**: Custom messages for transmitting the output distance map slice and mesh over ROS 2.
+* **nvblox_nav2**: Contains a custom plugin that allows ROS 2 Nav2 to consume nvblox distance map outputs.
+* **nvblox_performance_measurement**: Multiple packages containing tools for measuring nvblox performance ([readme](./nvblox_performance_measurement/README.md)).
+* **nvblox_ros**: The ROS 2 wrapper for the core reconstruction library and the nvblox node.
+* **nvblox_ros_common**: Package providing repository wide utility functions.
+* **nvblox_rviz_plugin**: A plugin for displaying nvblox's (custom) mesh type in RVIZ.
+* **\[submodule\] nvblox**: The core (ROS independent) reconstruction library.
+
+
+## ROS 2 Parameters
+
+Find all available ROS 2 parameters [here](./docs/parameters.md).
+
+## ROS 2 Topics and Services
+
+Find all ROS 2 subscribers, publishers and services [here](./docs/topics-and-services.md).
## Troubleshooting
@@ -224,15 +193,25 @@ ros2 launch nvblox_nav2 carter_sim.launch.py
For solutions to problems with Isaac ROS, please check [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/troubleshooting.md).
-### `realsense-ros` packages don't build with ROS2 Humble
+### *realsense-ros* packages don't build with ROS 2 Humble
Please follow the workaround [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/troubleshooting.md#realsense-driver-doesnt-work-with-ros2-humble).
+### Troubleshooting the Nvblox Realsense Example
+
+See our troubleshooting page [here](./docs/troubleshooting-nvblox-realsense.md).
+
+### Troubleshooting ROS 2 communication issues
+
+If it looks like you are dropping messages or you are not receiving any messages, please consult our troubleshooting page [here](./docs/troubleshooting-nvblox-ros2.md).
+
## Updates
-| Date | Changes |
-| ---------- | ----------------------------------------------------------------------------------------------------------------------------------------- |
-| 2022-10-19 | Updated OSS licensing |
-| 2022-08-31 | Update to be compatible with JetPack 5.0.2. Serialization of nvblox maps to file. Support for 3D LIDAR input and performace improvements. |
-| 2022-06-30 | Support for ROS2 Humble and miscellaneous bug fixes. |
-| 2022-03-21 | Initial version. |
+| Date | Changes |
+| ---------- | ------------------------------------------------------------------------------------------------------------------------------------------ |
+| 2023-04-05 | Human reconstruction and new weighting functions. |
+| 2022-12-10 | Updated documentation. |
+| 2022-10-19 | Updated OSS licensing |
+| 2022-08-31 | Update to be compatible with JetPack 5.0.2. Serialization of nvblox maps to file. Support for 3D LIDAR input and performance improvements. |
+| 2022-06-30 | Support for ROS 2 Humble and miscellaneous bug fixes. |
+| 2022-03-21 | Initial version. |
diff --git a/ci/Jenkinsfile b/ci/Jenkinsfile
deleted file mode 100644
index 8c5e356d..00000000
--- a/ci/Jenkinsfile
+++ /dev/null
@@ -1,143 +0,0 @@
-// 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.
-@Library('ci-groovkins-lib@9729a1fa623eedf0de3f13be8b3bd484adfdec54')
-
-import com.nvidia.isaac.ci.utils.WorkflowScriptUtil
-import com.nvidia.isaac.ci.isaac_ros.dto.CredentialsConfig
-import com.nvidia.isaac.ci.isaac_ros.VarsDef
-
-import com.nvidia.isaac.ci.isaac_ros.IsaacRosBuildPipeline
-
-CredentialsConfig credentials_config = CredentialsConfig.from_default()
-WorkflowScriptUtil utils = new WorkflowScriptUtil(this, credentials_config)
-IsaacRosBuildPipeline pipeline_helper = new IsaacRosBuildPipeline(this, credentials_config, utils)
-
-pipeline {
-
- agent {
- docker {
- label 'isaac-gpu'
- image 'nvcr.io/nvstaging/isaac-amr/nvblox_ros_base:latest'
- args '-u root -e NVIDIA_VISIBLE_DEVICES=all -e NVIDIA_DRIVER_CAPABILITIES=all'
- reuseNode true
- }
- }
-
- options {
- gitLabConnection('gitlab-master')
- ansiColor('xterm')
- timestamps()
- timeout(time: 2, unit: 'HOURS')
- buildDiscarder(logRotator(numToKeepStr: '60', daysToKeepStr: '30'))
- }
-
- triggers {
- cron(env.BRANCH_NAME == 'envoy-dev' ? 'H 4 * * *' : '')
- gitlab(
- triggerOnPush: true,
- triggerOnMergeRequest: true,
- triggerOpenMergeRequestOnPush: 'never',
- triggerOnNoteRequest: true,
- noteRegex: 'Jenkins please retry a build',
- skipWorkInProgressMergeRequest: true,
- ciSkip: false,
- setBuildDescription: true,
- addNoteOnMergeRequest: true,
- addCiMessage: true,
- addVoteOnMergeRequest: true,
- acceptMergeRequestOnSuccess: false,
- branchFilterType: 'All',
- pendingBuildName: 'Jenkins',
- cancelPendingBuildsOnUpdate: true,
- )
- }
-
- stages {
-
- stage('build') {
- steps {
- updateGitlabCommitStatus name: 'build', state: 'running'
- script {
- utils.run_with_ssh_key("secrets/account/ssh-key",
- "/root/.ssh/id_rsa") {
- utils.prepare_ssh(VarsDef.SSH_FOLDER)
-
- sh'''
- cd ${WORKSPACE}
- git config --global --add safe.directory ${WORKSPACE}
- git submodule deinit --all -f
- git submodule update --init --recursive
- '''
-
- sh '''
- mkdir -p /ros/src
- ln -sT ${WORKSPACE} /ros/src/isaac_ros_nvblox
- '''
-
- sh '''#!/bin/bash
- source /opt/ros/foxy/setup.bash
- cd /ros/
- colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
- '''
-
- sh '''#!/bin/bash
- source /opt/ros/foxy/setup.bash
- cd /ros/
- colcon test --event-handlers console_direct+
- '''
-
- sh '''#!/bin/bash
- source /opt/ros/foxy/setup.bash && \
- cd /ros/ && \
- ./src/isaac_ros_nvblox/ci/copy_results.sh
- '''
- }
- }
- }
- }
-
- }
-
- post {
- always {
-
- archiveArtifacts (
- artifacts: 'test_results/*.xml',
- fingerprint: true
- )
- xunit (
- testTimeMargin: '3000',
- thresholdMode: 1,
- thresholds: [
- skipped(failureThreshold: '0'),
- failed(failureThreshold: '16')
- ],
- tools: [JUnit(
- pattern: 'test_results/*.xunit.xml',
- deleteOutputFiles: true,
- failIfNotNew: false,
- skipNoTestFiles: true,
- stopProcessingIfError: true
- )]
- )
- junit(
- allowEmptyResults: true,
- testResults: '**/test-reports/*.xml'
- )
- utils.prepare_cleanup()
- cleanWs disableDeferredWipeout: true, deleteDirs: true
- }
- failure {
- updateGitlabCommitStatus name: 'build', state: 'failed'
- }
- success {
- updateGitlabCommitStatus name: 'build', state: 'success'
- }
- }
-
-}
diff --git a/ci/copy_results.sh b/ci/copy_results.sh
deleted file mode 100755
index 64469e10..00000000
--- a/ci/copy_results.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-#!/bin/bash
-mkdir -p ${WORKSPACE}/test_results
-colcon test-result --all --result-files-only | xargs -I '{}' cp '{}' -t ${WORKSPACE}/test_results
diff --git a/ci/nvblox_nightly.jenkinsfile b/ci/nvblox_nightly.jenkinsfile
deleted file mode 100644
index b55295ea..00000000
--- a/ci/nvblox_nightly.jenkinsfile
+++ /dev/null
@@ -1,147 +0,0 @@
-// ISAAC ROS NVBlox nightly pipeline
-//
-// 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.
-
-// See: https://gitlab-master.nvidia.com/robotics/isaac-pipeline-library
-@Library('ci-groovkins-lib@d3b3733a3ad1de47cfa77ad5d04970ab403da38a')
-
-import com.nvidia.isaac.ci.utils.WorkflowScriptUtil
-import com.nvidia.isaac.ci.isaac_ros.dto.CredentialsConfig
-
-import com.nvidia.isaac.ci.isaac_ros.IsaacRosBuildPipeline
-import com.nvidia.isaac.ci.isaac_ros.VarsDef
-import com.nvidia.isaac.ci.utils.Docker
-import com.nvidia.isaac.ci.utils.dto.BaseCredentialsConfig
-
-Docker docker_utils = new Docker(this,
- BaseCredentialsConfig.from_default())
-
-
-CredentialsConfig credentials_config = CredentialsConfig.from_default()
-WorkflowScriptUtil utils = new WorkflowScriptUtil(this, credentials_config)
-
-pipeline {
- agent {
- label 'isaac && builder'
- }
- triggers {
- // Run at around 1am UTC.
- cron('0 1 * * *')
- }
- environment {
- NGC_REGISTRY_CREDENTIALS = 'vault-ngc'
- }
- options {
- ansiColor('xterm')
- timestamps()
- timeout(time: 2, unit: 'HOURS')
- buildDiscarder(logRotator(numToKeepStr: '60', daysToKeepStr: '30'))
- skipDefaultCheckout(true)
- }
- parameters {
- string(
- name: 'BRANCH_TO_BUILD',
- description: 'The branch name or commit in ISAAC ROS NVBLOX repository to build.',
- defaultValue: 'envoy-dev',
- trim: true
- )
- }
-
- stages {
- stage('Clone') {
- steps {
- cleanWs disableDeferredWipeout: true, deleteDirs: true
- script {
- if (!params.BRANCH_TO_BUILD) {
- error('BRANCH_TO_BUILD pipeline parameter is required, none provided.')
- }
-
- sh '''
- mkdir -p src
- '''
- utils.run_with_ssh_key("secrets/account/ssh-key",
- "${HOME}/.ssh/id_rsa") {
- utils.prepare_ssh("${HOME}/.ssh")
- def scmUrl = scm.getUserRemoteConfigs()[0].getUrl()
- sh """
- echo Building branch ${params.BRANCH_TO_BUILD}
- """
- dir('src/isaac_ros_nvblox') {
- checkout([$class: 'GitSCM', branches: [[name: params.BRANCH_TO_BUILD]], extensions: [[$class: 'GitLFSPull'], [$class: 'SubmoduleOption', disableSubmodules: false, parentCredentials: true, recursiveSubmodules: true, reference: '', trackingSubmodules: false]], userRemoteConfigs: [[url: 'ssh://git@gitlab-master.nvidia.com:12051/isaac_ros/isaac_ros_nvblox.git']]])
- }
-
- dir ('src/nvblox_tools') {
- checkout([$class: 'GitSCM', branches: [[name: 'main']], extensions: [[$class: 'GitLFSPull'], [$class: 'SubmoduleOption', disableSubmodules: false, parentCredentials: true, recursiveSubmodules: true, reference: '', trackingSubmodules: false]], userRemoteConfigs: [[url: 'ssh://git@gitlab-master.nvidia.com:12051/nvblox/nvblox_tools.git']]])
- }
-
- sh """
- cp src/nvblox_tools/docker_utils/.dockerignore src/
- """
- }
- }
- }
- }
- stage ('Build base docker image') {
- steps {
- dir('src') {
- script {
- docker.withRegistry('https://nvcr.io', NGC_REGISTRY_CREDENTIALS) {
- def baseImage = docker.build("nvcr.io/nvstaging/isaac-amr/nvblox_ros_base:latest",
- "-f ./nvblox_tools/docker_utils/Dockerfile.ros_base .")
- baseImage.push()
- sh "docker rmi -f ${baseImage.imageName()}"
- }
- }
- }
- }
- }
- stage ('Build isaacsim image') {
- steps {
- dir ('src') {
- script {
- docker.withRegistry('https://nvcr.io', NGC_REGISTRY_CREDENTIALS) {
- def isaacsimImage = docker.build("nvcr.io/nvstaging/isaac-amr/nvblox_isaacsim:latest",
- "-f ./nvblox_tools/docker_utils/Dockerfile.nvblox_isaacsim .")
- isaacsimImage.push()
- sh "docker rmi -f ${isaacsimImage.imageName()}"
- }
- }
- }
- }
- }
- stage ('Build nvblox image') {
- steps {
- dir ('src') {
- script {
- docker.withRegistry('https://nvcr.io', NGC_REGISTRY_CREDENTIALS) {
- def nvbloxImage = docker.build("nvcr.io/nvstaging/isaac-amr/nvblox_isaac_ros:latest",
- "-f ./nvblox_tools/docker_utils/Dockerfile.nvblox_ros .")
- nvbloxImage.push()
- sh "docker rmi -f ${nvbloxImage.imageName()}"
- }
- }
- }
- }
- }
-
-
- }
- post {
- always {
- script {
- utils.prepare_cleanup()
- }
- }
- cleanup {
- script {
- cleanWs disableDeferredWipeout: true, deleteDirs: true
- }
- }
- }
-}
diff --git a/docs/linux-people-isaac-sim-workaround.md b/docs/linux-people-isaac-sim-workaround.md
new file mode 100644
index 00000000..817ae54b
--- /dev/null
+++ b/docs/linux-people-isaac-sim-workaround.md
@@ -0,0 +1,11 @@
+# Workaround for running People Animation on Linux
+
+Currently, python scripts for animation can only run if the usd is on the same filesystem as the script. This means that it is required to upload the scripts next to where the usd is stored:
+
+1. To retrieve where the scripts are stored, click Window -> Extensions and search for `omni.anim.people`. Then in the extension details click on the folder icon. This will open a window in your browser showing where the extension is stored.
+
+
+
+2. You now need to upload the `scripts` folder from the `omni/anim/people` folder into a `people_anim_scripts` folder next to the scene usd. To do so, you can right-click on the *Content* tab on the bottom then select *Upload files and folders* to upload the scripts folder.
+
+3. This step will need to be done each time a new version of the extension is released.
diff --git a/docs/parameters.md b/docs/parameters.md
new file mode 100644
index 00000000..3086ab1c
--- /dev/null
+++ b/docs/parameters.md
@@ -0,0 +1,110 @@
+# Parameters
+
+This page contains all parameters exposed to ROS 2.
+For the provided examples in the `nvblox_example_bringup` package, parameters
+are set using YAML configuration files under `nvblox_examples/nvblox_examples_bringup/config`.
+
+The base parameters are set in `nvblox_base.yaml` and there exist multiple specialization files that are overwriting parameters from the base file depending on which example is launched.
+
+File tree:
+```bash
+config
+├── nvblox
+ ├── nvblox_base.yaml
+ └── specializations
+ ├── nvblox_humans.yaml
+ ├── nvblox_isaac_sim.yaml
+ └── nvblox_realsense.yaml
+```
+
+Loaded specializations for each example:
+| Launch file | Specializations |
+| ---------------------------------- | --------------------------------------------- |
+| isaac_sim_example.launch.py | `nvblox_isaac_sim.yaml` |
+| isaac_sim_humans_example.launch.py | `nvblox_isaac_sim.yaml`, `nvblox_humans.yaml` |
+| realsense_example.launch.py | `nvblox_realsense.yaml` |
+| realsense_humans_example.launch.py | `nvblox_realsense.yaml`,`nvblox_humans.yaml` |
+
+## General Parameters
+
+| ROS Parameter | Type | Default | Description |
+| ----------------------------------------- | -------- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| `global_frame` | `string` | `odom` | The name of the TF frame to be used as the global frame. For the realsense examples, this parameter is exposed as a launch argument. |
+| `voxel_size` | `float` | `0.05` | Voxel size (in meters) to use for the map. |
+| `use_tf_transforms` | `bool` | `true` | Whether to use TF transforms at all. If set to false, `use_topic_transforms` must be true and `pose_frame` *needs* to be set. |
+| `use_topic_transforms` | `bool` | `false` | Whether to listen to topics for transforms. If set to true, will try to get `global_frame` to `pose_frame` transform from the topics. If set to false, everything will be resolved through TF. |
+| `compute_esdf` | `bool` | `true` | Whether to compute the ESDF (map of distances to the nearest obstacle). |
+| `esdf_update_rate_hz` | `float` | `2.0` | The rate (in Hz) at which to update the ESDF and output the distance slice. |
+| `esdf_2d` | `bool` | `true` | Whether to compute the ESDF in 2D (true) or 3D (false). |
+| `esdf_distance_slice` | `bool` | `true` | Whether to output a distance slice of the ESDF to be used for path planning. |
+| `esdf_slice_height` | `float` | `1.0` | The *output* slice height for the distance slice and ESDF pointcloud. Does not need to be within min and max height below. In units of meters. |
+| `esdf_2d_min_height` | `float` | `0.0` | The minimum height, in meters, to consider obstacles part of the 2D ESDF slice. |
+| `esdf_2d_max_height` | `float` | `1.0` | The maximum height, in meters, to consider obstacles part of the 2D ESDF slice. |
+| `compute_mesh` | `bool` | `true` | Whether to output a mesh for visualization in rviz, to be used with `nvblox_rviz_plugin`. |
+| `mesh_update_rate_hz` | `float` | `5.0` | The rate (in Hz) at which to update and publish the mesh. |
+| `use_color` | `bool` | `true` | Whether to integrate color images to color the mesh. |
+| `max_color_update_hz` | `float` | `5.0` | The maximum rate (in Hz) at which to integrate color images into the color layer. A value of 0.0 means there is no cap. |
+| `use_depth` | `bool` | `true` | Whether to integrate depth images. |
+| `max_depth_update_hz` | `float` | `10.0` | The maximum rate (in Hz) at which to integrate depth images. A value of 0.0 means there is no cap. |
+| `use_lidar` | `bool` | `true` | Whether to integrate LiDAR scans. |
+| `max_lidar_update_hz` | `float` | `10.0` | The maximum rate (in Hz) at which to integrate LiDAR scans. A value of 0.0 means there is no cap. |
+| `lidar_width` | `int` | `1800` | Width of the LIDAR scan, in number of beams. Defaults are for the Velodyne VLP16. |
+| `lidar_height` | `int` | `16` | Height of the LIDAR scan, in number of beams. Defaults are for the VLP16. |
+| `lidar_vertical_fov_rad` | `float` | `30 degrees (in radians)` | The vertical field of view of the LIDAR scan, in radians. Horizontal FoV is assumed to be 360 degrees. This is used to calculate the individual beam angle offsets. |
+| `use_static_occupancy_layer` | `float` | `false` | Whether to use the static occupancy layer for projective integration. If this flag is set to false (default), TSDF integration is used. |
+| `occupancy_publication_rate_hz` | `float` | `2.0` | The rate (in Hz) at which to publish the static occupancy pointcloud. |
+| `max_poll_rate_hz` | `float` | `100.0` | Specifies what rate to poll the color & depth updates at. Will exit as no-op if no new images. Set this higher than you expect images to come in at. |
+| `maximum_sensor_message_queue_length` | `int` | `30` | How many messages to store in the sensor messages queues (depth, color, lidar) before deleting oldest messages. |
+| `map_clearing_radius_m` | `float` | `-1.0` | Radius around the `map_clearing_frame_id` outside which we clear the map. Note that values <= 0.0 indicate that no clearing is performed. |
+| `map_clearing_frame_id` | `string` | `base_link` | The name of the TF frame around which we clear the map. |
+| `clear_outside_radius_rate_hz` | `float` | `1.0` | The rate (in Hz) at wich we clear the map outside of the `map_clearing_radius_m`. |
+| `depth_qos` | `string` | `SYSTEM_DEFAULT` | ROS 2 QoS string for the depth subscription. |
+| `color_qos` | `string` | `SYSTEM_DEFAULT` | ROS 2 QoS string for the color subscription. |
+| `pose_frame` | `float` | `base_link` | Only used if `use_topic_transforms` is set to true. Pose and transform messages will be interpreted as being in this pose frame, and the remaining transform to the sensor frame will be looked up on the TF tree. |
+| `slice_visualization_attachment_frame_id` | `string` | `base_link` | Frame to which the map slice bounds visualization is centered on the xy-plane. |
+| `slice_visualization_side_length` | `float` | `10.0` | Side length of the map slice bounds visualization plane. |
+
+# Mapper Parameters
+
+The nvblox node holds a mapper object that takes care of updating all the layers.
+Depending on the `use_static_occupancy_layer` parameter above, it builds either a tsdf or occupancy probability grid.
+Below is a list of all the parameters to set up this mapper.
+
+| ROS Parameter | Type | Default | Description |
+| --------------------------------------------------------------- | -------- | ---------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `mapper.projective_integrator_max_integration_distance_m` | `float` | `7.0` | The maximum distance, in meters, to integrate the TSDF or occupancy map up to for depth images. |
+| `mapper.projective_integrator_truncation_distance_vox` | `float` | `4.0` | The truncation distance, in units of voxels, for the TSDF or occupancy map. |
+| `mapper.lidar_projective_integrator_max_integration_distance_m` | `float` | `10.0` | The maximum distance, in meters, to integrate the TSDF or occupancy map up to for LiDAR scans. |
+| `mapper.weighting_mode` | `string` | `inverse_square` | The weighting mode, applied to TSDF and color integrations. Options: [`constant`, `constant_dropoff`, `inverse_square`, `inverse_square_dropoff`]. |
+| `mapper.tsdf_integrator_max_weight` | `float` | `100.0` | Maximum weight for the TSDF. Setting this number higher will lead to higher-quality reconstructions but worse performance in dynamic scenes. |
+| `mapper.free_region_occupancy_probability` | `float` | `0.3` | The inverse sensor model occupancy probability for voxels observed as free space. |
+| `mapper.occupied_region_occupancy_probability` | `float` | `0.7` | The inverse sensor model occupancy probability for voxels observed as occupied. |
+| `mapper.unobserved_region_occupancy_probability` | `float` | `0.5` | The inverse sensor model occupancy probability for unobserved voxels. |
+| `mapper.occupied_region_half_width_m` | `float` | `0.1` | Half the width of the region which is considered as occupied. |
+| `mapper.esdf_integrator_min_weight` | `float` | `1e-4` | Minimum weight of the TSDF to consider for inclusion in the ESDF. |
+| `mapper.esdf_integrator_max_distance_m` | `float` | `2.0` | Maximum distance to compute the ESDF up to, in meters. |
+| `mapper.esdf_integrator_max_site_distance_vox` | `float` | `1.0` | Maximum distance to consider a voxel within a surface for the ESDF calculation. |
+| `mapper.mesh_integrator_min_weight` | `float` | `1e-4` | Minimum weight of the TSDF to consider for inclusion in the mesh. |
+| `mapper.mesh_integrator_weld_vertices` | `bool` | `true` | Whether to weld identical vertices together in the mesh. |
+| `mapper.color_integrator_max_integration_distance_m` | `float` | `7.0` | Maximum distance, in meters, to integrate the color up to. |
+
+# Human Reconstruction Parameters
+
+In case of the `nvblox_human_node`, there are two individual mappers (contained in a multi-mapper object) taking care of updating the layers.
+The first mapper is equivalent to the mapper of the `nvblox_node` explained above and is also initialized with the same parameters.
+
+The second mapper handles dynamic reconstruction of humans in an occupancy layer.
+Using the human occupancy layer an esdf layer and costmap are generated.
+The parameters of the human mapper for occupancy and esdf integration are duplicates of the parameters for the static mapper.
+Only the parent parameter name needs to be changed to `human_mapper` (i.e. `mapper.esdf_integrator_min_weight` -> `human_mapper.esdf_integrator_min_weight`).
+
+Below you find additional parameters that only exist for the 'nvblox_human_node'.
+
+| ROS Parameter | Type | Default | Description |
+| ------------------------------------------------ | ------- | ------- | ------------------------------------------------------------------------------------------------------------------ |
+| `human_esdf_update_rate_hz` | `float` | `10.0` | The rate (in Hz) at which to update the human ESDF, output the distance slice and additional human visualizations. |
+| `human_occupancy_decay_rate_hz` | `float` | `10.0` | The rate (in Hz) at which to decay the human occupancy layer. |
+| `human_mapper.free_region_decay_probability` | `float` | `0.55` | The decay probability that is applied to the free region on decay. Must be in `[0.5, 1.0]`. |
+| `human_mapper.occupied_region_decay_probability` | `float` | `0.4` | The decay probability that is applied to the occupied region on decay. Must be in `[0.0, 0.5]`. |
+
+> **Note**: Decay of the occupancy layer is needed to handle dynamic objects like humans. Without the decay observed dynamic objects stay in the map even if not seen for a long time period. In that time the object might have moved and the map is therefore invalid. Using the decay we simulate the loss of knowledge about parts of the dynamic map currently not observed over time.
diff --git a/docs/technical-details.md b/docs/technical-details.md
index 942bbc55..09c6dc97 100644
--- a/docs/technical-details.md
+++ b/docs/technical-details.md
@@ -3,3 +3,17 @@
Nvblox builds the reconstructed map in the form of a Truncated Signed Distance Function (TSDF) stored in a 3D voxel grid. This approach is similar to 3D occupancy grid mapping approaches in which occupancy probabilities are stored at each voxel. However, TSDF-based approaches like nvblox store the (signed) distance to the closest surface at each voxel. The surface of the environment can then be extracted as the zero-level set of this voxelized function. Typically, TSDF-based reconstructions provide higher quality surface reconstructions.
In addition to their use in reconstruction, distance fields are also useful for path planning, because they provide an immediate means of checking whether potential future robot positions are in collision. The dual utility of distance functions for both reconstruction and planning motivates their use in nvblox (a reconstruction library for path planning).
+
+
+
+The diagram above indicates data and processes in nvblox. In the default configuration nvblox builds TSDF, color, mesh, and ESDF *layers*. Each *layer* is a independent, but aligned and co-located, voxel grid containing data of the appropriate type. For example, voxels on the TSDF layer store distance and weight data, while the color layer voxels store color values.
+
+# Human Reconstruction
+
+Additionally to the static TSDF mapping we also provide a human reconstruction and dynamic mapping utility using occupancy grid mapping.
+
+Human segmentation is applied to each processed color frame with [*Isaac ROS Image Segmentation*](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_segmentation). The depth masker module uses the segmentation mask from the color image to separate the depth-map into human and non-human parts. While the non-human labelled part of the depth frame is still forwarded to TSDF mapping as explained above, the human labelled part is processed to an occupancy grid map.
+
+To relax the assumption that occupancy grid maps only capture static objects we apply an occupancy decay step. At a fixed frequency, all voxel occupancy probabilities are decayed towards `0.5` over time. This means that the state of the map (occupied or free) becomes less certain after it has fallen out of the field of view, until it becomes unknown (`0.5` occupancy probability).
+
+
diff --git a/docs/topics-and-services.md b/docs/topics-and-services.md
new file mode 100644
index 00000000..0d2fe1b5
--- /dev/null
+++ b/docs/topics-and-services.md
@@ -0,0 +1,65 @@
+
+# ROS Topics and Services
+
+Find all topics and services of the `nvblox_node` and the `nvblox_human_node` below.
+
+> **Note**: The examples in the [Isaac Sim](./tutorial-human-reconstruction-isaac-sim.md) and [realsense](./tutorial-human-reconstruction-realsense.md) human reconstruction tutorials launch the `nvblox_human_node`, while the standard [Isaac Sim](tutorial-isaac-sim.md) and [realsense](tutorial-realsense.md) tutorials launch the `nvblox_node`. The `nvblox_human_node` inherits from the `nvblox_node` and extends its capabilities.
+
+## ROS Topics Subscribed
+
+| ROS Topic | Interface | Description |
+|---------------------|-----------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `pointcloud` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | Input 3D LIDAR pointcloud. Make sure to set the `lidar_height`, `lidar_width`, and `lidar_vertical_fov_rad` parameters if using this input, as it uses those to convert the pointcloud into a depth image. |
+| `color/image` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Image.msg) | Optional input color image to be integrated. Must be paired with a `camera_info` message below. Only used to color the mesh. |
+| `color/camera_info` | [sensor_msgs/CameraInfo](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/CameraInfo.msg) | Optional topic along with the color image above. Contains intrinsics of the color camera. |
+| `depth/camera_info` | [sensor_msgs/CameraInfo](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/CameraInfo.msg) | Required topic along with the depth image. Contains intrinsic calibration parameters of the depth camera. |
+| `depth/image` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | The input depth image to be integrated. Must be paired with a `camera_info` message below. Supports both floating-point (depth in meters) and `uint16` (depth in millimeters, OpenNI format). |
+| `transform` | [geometry_message/TransformStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/TransformStamped.msg) | Odometry as stamped transform messages. Not required if `use_tf_transforms` is set to true. |
+| `pose` | [geometry_message/PoseStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/PoseStamped.msg) | Odometry as stamped pose messages. Not required if `use_tf_transforms` is set to true. |
+
+Additionally subscribed topics by the `nvblox_human_node`:
+
+| ROS Topic | Interface | Description |
+|--------------------|----------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `mask/image` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Image.msg) | The mask image with the human segmentation (non-human pixels = 0 and human pixels = 1). In the [nvblox human examples](./tutorial-human-reconstruction.md) this is published by [*Isaac ROS Image Segmentation*](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_segmentation). |
+| `mask/camera_info` | [sensor_msgs/CameraInfo](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/CameraInfo.msg) | Required topic along with the mask image. Contains intrinsic calibration parameters of the mask camera. |
+
+## ROS Topics Published
+
+| ROS Topic | Interface | Description |
+|----------------------|-------------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `~/mesh` | [nvblox_msgs/Mesh](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/msg/Mesh.msg) | A visualization topic showing the mesh produced from the TSDF in a form that can be seen in RViz using `nvblox_rviz_plugin`. Set ``mesh_update_rate_hz`` to control its update rate. |
+| `~/esdf_pointcloud` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | A pointcloud of the static 2D ESDF (Euclidean Signed Distance Field), with intensity as the metric distance to the nearest obstacle. Set ``esdf_update_rate_hz`` to control its update rate. |
+| `~/occupancy` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | A pointcloud of the occupancy map (only voxels with occupation ``probability > 0.5``). Set ``occupancy_publication_rate_hz`` to control its publication rate. |
+| `~/map_slice` | [nvblox_msgs/DistanceMapSlice](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/msg/DistanceMapSlice.msg) | A 2D slice of the static ESDF, to be consumed by `nvblox_nav2` package for interfacing with Nav2. Set ``esdf_update_rate_hz`` to control its update rate. |
+| `~/map_slice_bounds` | [visualization_msgs/Marker](https://github.com/ros2/common_interfaces/blob/humble/visualization_msgs/msg/Marker.msg) | A visualization topic showing the mesh slice bounds that can be set with the parameters `esdf_2d_min_height` and `esdf_2d_min_height`. |
+| `~/mesh_marker` | [visualization_msgs/Marker](https://github.com/ros2/common_interfaces/blob/humble/visualization_msgs/msg/Marker.msg) | A visualization topic showing the mesh using a marker message. |
+
+Additionally published topics by the `nvblox_human_node`:
+| ROS Topic | Interface | Description |
+|------------------------------|-------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `~/human_pointcloud` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | Pointcloud visualizing the back-projected pixels of the latest human masked depth frame (without temporal fusion). |
+| `~/human_voxels` | [visualization_msgs/Marker](https://github.com/ros2/common_interfaces/blob/humble/visualization_msgs/msg/Marker.msg) | The pointcloud published at `~/human_pointcloud` mapped to the corresponding voxels. |
+| `~/human_occupancy` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | A pointcloud of the human occupancy map (only voxels with occupation ``probability > 0.5``). |
+| `~/human_esdf_pointcloud` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | A pointcloud of the human 2D ESDF (Euclidean Signed Distance Field), with intensity as the metric distance to the nearest human. Set ``human_esdf_update_rate_hz`` to control its update rate. |
+| `~/combined_esdf_pointcloud` | [sensor_msgs/PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | A pointcloud of the combined static and human 2D ESDF (minimal distance of both), with intensity as the metric distance to the nearest obstacle or human. Set ``human_esdf_update_rate_hz`` to control its update rate. |
+| `~/human_map_slice` | [nvblox_msgs/DistanceMapSlice](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/msg/DistanceMapSlice.msg) | A 2D slice of the human ESDF, to be consumed by `nvblox_nav2` package for interfacing with Nav2. Set ``human_esdf_update_rate_hz`` to control its update rate. |
+| `~/combined_map_slice` | [nvblox_msgs/DistanceMapSlice](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/msg/DistanceMapSlice.msg) | A 2D slice of the combined static and human ESDF (minimal distance of both), to be consumed by `nvblox_nav2` package for interfacing with Nav2. Set ``human_esdf_update_rate_hz`` to control its update rate. |
+| `~/depth_frame_overlay` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Image.msg) | Debug image showing the mask overlaid on the depth image. |
+| `~/color_frame_overlay` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Image.msg) | Debug image showing the mask overlaid on the color image. |
+
+
+## ROS Services Advertised
+
+| ROS Service | Interface | Description |
+|--------------|---------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------|
+| `~/save_ply` | [nvblox_msgs/FilePath](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/srv/FilePath.srv) | Will save the mesh as the PLY (standard polygon file format, which can be viewed with MeshLab or CloudCompare) at the specified location. |
+| `~/save_map` | [nvblox_msgs/FilePath](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/srv/FilePath.srv) | Will serialize the entire map, including TSDF, ESDF, etc., at the given location. |
+| `~/load_map` | [nvblox_msgs/FilePath](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox/blob/main/nvblox_msgs/srv/FilePath.srv) | Will overwrite the current map in the node with a map loaded from the given path. |
+
+Example service calls from the command line:
+```bash
+ros2 service call /nvblox_node/save_ply nvblox_msgs/srv/FilePath "{file_path: '/home/USERNAME/super_cool_map.ply'}"
+ros2 service call /nvblox_node/save_map nvblox_msgs/srv/FilePath "{file_path: '/home/USERNAME/super_cool_map.nvblx'}"
+ros2 service call /nvblox_node/load_map nvblox_msgs/srv/FilePath "{file_path: '/home/USERNAME/super_cool_map.nvblx'}"
+```
diff --git a/docs/troubleshooting-nvblox-vslam-realsense.md b/docs/troubleshooting-nvblox-realsense.md
similarity index 87%
rename from docs/troubleshooting-nvblox-vslam-realsense.md
rename to docs/troubleshooting-nvblox-realsense.md
index 32b859c0..2398b69a 100644
--- a/docs/troubleshooting-nvblox-vslam-realsense.md
+++ b/docs/troubleshooting-nvblox-realsense.md
@@ -1,12 +1,12 @@
# Troubleshoot for RealSense-based Reconstruction Example
-This page contains troubleshooting for the nvblox-VSLAM-realsense example found [here](tutorial-nvblox-vslam-realsense.md). The example combines nvblox-based reconstruction with VSLAM. Both of these processes, nvblox and VSLAM, are driven by the realsense depth camera.
+This page contains troubleshooting for the nvblox-VSLAM-realsense example found [here](tutorial-realsense.md). The example combines nvblox-based reconstruction with VSLAM. Both of these processes, nvblox and VSLAM, are driven by the realsense depth camera.
The setup for the example is shown below.
-Images (2 x IR images and 1 x Depth image) are published at 60Hz from the realsense ROS2 node. At the time of publishing we are using the `ros2-development` branch of [realsense-ros](https://github.com/IntelRealSense/realsense-ros/tree/ros2-development). This is the branch used by the ROS2 apt packages (viewable on the [ros package index](https://index.ros.org/p/realsense2_camera/github-IntelRealSense-realsense-ros/#humble)).
+Images (2 x IR images and 1 x Depth image) are published at 60Hz from the realsense ROS 2 node. At the time of publishing we are using the `ros2-development` branch of [realsense-ros](https://github.com/IntelRealSense/realsense-ros/tree/ros2-development). This is the branch used by the ROS 2 apt packages (viewable on the [ros package index](https://index.ros.org/p/realsense2_camera/github-IntelRealSense-realsense-ros/#humble)).
> **Why do we include the realsense splitter?**: The depth output from the realsense is significantly improved when using its inbuilt projector which increases the amount of texture in the scene. However, VSLAM performance is disrupted by the projector. We, therefore, use the realsense's toggling feature such that the projector is switched on and off in interleaved frames (on, off, on, off, on, off,...etc.). The splitter splits this interleaved image stream into two channels, one with the projector on and one with the projector off. IR images with the projector off are passed to VSLAM, while depth images with the projector on are passed to nvblox. This splitting occurs based on the metadata associated with each frame, published by the realsense, which contains a field which states if the projector was on at the time of frame capture.
@@ -21,7 +21,7 @@ No reconstruction appears in RVIZ, and the terminal reports:
[nvblox_node-3] at line 93 in /opt/ros/humble/src/geometry2/tf2/src/buffer_core.cpp
```
-**Explanation:** This message is repeating because nvblox can't get the camera pose in the odom frame. It can't get the pose because VSLAM is not publishing it. VSLAM is not publishing anything because (likely) it is not receiving input data.
+**Explanation**: This message is repeating because nvblox can't get the camera pose in the odom frame. It can't get the pose because VSLAM is not publishing it. VSLAM is not publishing anything because (likely) it is not receiving input data.
So the issue here is that VSLAM is not getting data. This is caused either by one of:
1) Images not coming off the camera into Linux.
@@ -66,7 +66,7 @@ You're here because your realsense-viewer is operating as expected, i.e. is show
Start the example:
```bash
-ros2 launch nvblox_examples_bringup nvblox_vslam_realsense.launch.py
+ros2 launch nvblox_examples_bringup realsense_example.launch.py
```
In rviz check that the following images can be viewed.
@@ -110,16 +110,16 @@ On a supported kernel version, the apt package patches the kernel when you insta
If everything is as expected on a supported kernel and you run
```bash
-helen@holeynikova-dt:~/workspaces/isaac_ros-dev/src/isaac_ros_common (dev)$ uname -r
+USERNAME@HOSTNAME:~/workspaces/isaac_ros-dev/src/isaac_ros_common (dev)$ uname -r
5.8.0-43-generic
-helen@holeynikova-dt:~/workspaces/isaac_ros-dev/src/isaac_ros_common (dev)$ dkms status
+USERNAME@HOSTNAME:~/workspaces/isaac_ros-dev/src/isaac_ros_common (dev)$ dkms status
librealsense2-dkms, 1.3.18, 5.8.0-43-generic, x86_64: installed
librealsense2-dkms, 1.3.18, 5.8.0-67-generic, x86_64: installed
nvidia, 510.85.02, 5.13.0-40-generic, x86_64: installed
nvidia, 510.85.02, 5.8.0-43-generic, x86_64: installed
-helen@holeynikova-dt:/lib/udev/rules.d$ modinfo uvcvideo | grep "version:"
+USERNAME@HOSTNAME:/lib/udev/rules.d$ modinfo uvcvideo | grep "version:"
version: 1.1.2.realsense-1.3.18
srcversion: 964E8D3335D17053B8EEDD2
```
@@ -131,11 +131,11 @@ The important thing to see here is that the word "realsense" appears:
Likely, if you're here, you have a recent kernel and the words realsense do not appear, or appear in the second command next to an old kernel version. Something like:
```bash
-shubham@shubham-linuxbox:~$ uname -r
+USERNAME@HOSTNAME:~$ uname -r
5.15.0-48-generic
-shubham@shubham-linuxbox:~$ dkms status
+USERNAME@HOSTNAME:~$ dkms status
nvidia, 510.85.02, 5.15.0-48-generic, x86_64: installed
-shubham@shubham-linuxbox:~$ modinfo uvcvideo | grep "version:"
+USERNAME@HOSTNAME:~$ modinfo uvcvideo | grep "version:"
version: 1.1.1
srcversion: 18D809600E7D1E107042647
```
@@ -149,10 +149,10 @@ In the directory where you downloaded the file run: `sudo apt install ./libreals
Now the commands above, on my machine, return:
```bash
-alex@shallowinsight:~$ uname -r
+USERNAME@HOSTNAME:~$ uname -r
5.15.0-48-generic
-alex@shallowinsight:~$ dkms status
+USERNAME@HOSTNAME:~$ dkms status
librealsense2-dkms, 1.3.14, 5.15.0-48-generic, x86_64: installed
librealsense2-dkms, 1.3.14, 5.15.0-50-generic, x86_64: installed
nvidia, 510.85.02, 5.15.0-48-generic, x86_64: installed
@@ -160,7 +160,7 @@ nvidia, 510.85.02, 5.15.0-50-generic, x86_64: installed
nvidia, 510.85.02, 5.4.0-126-generic, x86_64: installed
nvidia, 510.85.02, 5.4.0-128-generic, x86_64: installed
-alex@shallowinsight:~$ modinfo uvcvideo | grep "version:"
+USERNAME@HOSTNAME:~$ modinfo uvcvideo | grep "version:"
version: 1.1.2.realsense-1.3.14
srcversion: 26234508927E0F6886C9A48
```
@@ -177,7 +177,7 @@ Manually build librealsense within the container without CUDA. Run the following
/opt/realsense/build-librealsense.sh -n
```
-now rebuild the ROS2 workspace
+now rebuild the ROS 2 workspace
```bash
rm -rf build/ install/ log/
diff --git a/docs/troubleshooting-nvblox-ros2.md b/docs/troubleshooting-nvblox-ros2.md
new file mode 100644
index 00000000..0cc2c7fe
--- /dev/null
+++ b/docs/troubleshooting-nvblox-ros2.md
@@ -0,0 +1,35 @@
+# Troubleshooting ROS 2 communication issues
+
+If it looks like you are dropping messages or you are not receiving any messages, you might be at the right place here.
+
+## DDS tuning
+
+In case you encounter issues with reliability of ROS 2 message delivery,
+you could try to increase the maximum receive buffer size:
+
+```bash
+sudo sysctl -w net.core.rmem_max=2147483647
+```
+
+For more information please consult the [DDS tuning information of ROS 2](https://docs.ros.org/en/humble/How-To-Guides/DDS-tuning.html).
+
+## RTPS profile
+
+You can find the rtps profile of Isaac ROS [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docker/middleware_profiles/rtps_udp_profile.xml).
+If you encounter communication issues when launching many ROS 2 nodes simultaneously you might want to increase the `maxInitialPeersRange` parameter like shown below:
+
+```xml
+NVIDIA Isaac ROS Software License
+
+
+
+ UdpTransport
+ UDPv4
+ 100
+
+
+
+...
+```
+
+After saving the modification, make sure to restart you docker container.
diff --git a/docs/tutorial-human-reconstruction-isaac-sim.md b/docs/tutorial-human-reconstruction-isaac-sim.md
new file mode 100644
index 00000000..752262da
--- /dev/null
+++ b/docs/tutorial-human-reconstruction-isaac-sim.md
@@ -0,0 +1,81 @@
+# Tutorial For Human Reconstruction in Isaac Sim
+
+
+ Launch isaac sim example with no humans in scene
+- Mode 2
+ - `with_people` : **True**
+ - `random_command_generation` : **False**
+ - `use_generated_command_file` : **False/True**
+ Launch isaac sim example with humans in scene. Either use default command file for humans with argument use_generated_command_file as **FALSE** or a custom command file with the argument as **TRUE**. If custom command file is to be used the argument for anim_people_waypoint_dir must be set.
+- Mode 3
+ - `with_people` : **True**
+ - `random_command_generation` : **True**
+ - `use_generated_command_file` : **False**
+ Launch isaac sim headless and generate custom command file for human animation. Argument anim_people_waypoint_dir has to be set. The script generated this command file in the specified directory and then closes the simulation. This command file can be used the next time the script is used to run the example with humans in scene.
+
+#### **Parameters Explained**
+
+| Parameter | Default | Description |
+|---------------------|------------------------------------------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `scenario_path` | */Isaac/Samples/NvBlox/
carter_warehouse_navigation.usd*| Path of the scenario to launch relative to the nucleus server base path. Scenario must contain a carter robot. If the scene contains animated humans, the script expects to find them under /World/Humans.|
+| `environment_prim_path` | */World/WareHouse* | Path to the world to create a navigation mesh.|
+| `tick_rate_hz` | *20* | The rate (in hz) that we step the simulation at.|
+| `anim_people_waypoint_dir` | | Directory location to save the people animation commands as a text file|
+| `with_people` | *False* | To choose whether to have scene with people or not |
+| `random_command_generation` | *False* | Choose whether we generate random waypoint or run sim |
+| `num_waypoints` | *5* | Number of waypoints to generate for each human in the scene |
+| `use_generated_command_file` | *False* | Choose whether to use generated/custom command file or to use the default one to run the people animation |
diff --git a/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/__init__.py b/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/__init__.py
new file mode 100644
index 00000000..6bbcb131
--- /dev/null
+++ b/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/__init__.py
@@ -0,0 +1,16 @@
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
diff --git a/nvblox_isaac_sim/omniverse_scripts/common/scenario.py b/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/common/scenario.py
similarity index 96%
rename from nvblox_isaac_sim/omniverse_scripts/common/scenario.py
rename to nvblox_examples/nvblox_isaac_sim/omniverse_scripts/common/scenario.py
index 337f3fed..59cb7fc2 100644
--- a/nvblox_isaac_sim/omniverse_scripts/common/scenario.py
+++ b/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/common/scenario.py
@@ -1,5 +1,5 @@
# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
diff --git a/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py b/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py
new file mode 100644
index 00000000..2afd9160
--- /dev/null
+++ b/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py
@@ -0,0 +1,363 @@
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+import argparse
+import os
+import random
+import time
+
+import omni
+
+ADDITIONAL_EXTENSIONS_BASE = ['omni.isaac.ros2_bridge-humble']
+
+ADDITIONAL_EXTENSIONS_PEOPLE = [
+ 'omni.anim.people', 'omni.anim.navigation.bundle', 'omni.anim.timeline',
+ 'omni.anim.graph.bundle', 'omni.anim.graph.core', 'omni.anim.graph.ui',
+ 'omni.anim.retarget.bundle', 'omni.anim.retarget.core',
+ 'omni.anim.retarget.ui', 'omni.kit.scripting']
+
+
+def enable_extensions_for_sim(with_people: bool = False):
+ """
+ Enable the required extensions for the simulation.
+
+ Args:
+ with_people (bool, optional): Loads the human animation extensions if the scene with
+ humans is requested. Defaults to False.
+
+ """
+ from omni.isaac.core.utils.extensions import enable_extension
+ add_exten = ADDITIONAL_EXTENSIONS_BASE
+ if with_people:
+ add_exten = ADDITIONAL_EXTENSIONS_BASE + ADDITIONAL_EXTENSIONS_PEOPLE
+ for idx in range(len(add_exten)):
+ enable_extension(add_exten[idx])
+
+
+def rebuild_nav_mesh():
+ """
+ Rebuild the navmesh with the correct settings. Used for the people to move around.
+
+ Called only when the sim with people is requested.
+ """
+ # Navigation mesh rebake settings
+ import omni.kit.commands
+ print('Rebuilding navigation mesh...')
+
+ omni.kit.commands.execute(
+ 'ChangeSetting',
+ path='/exts/omni.anim.navigation.core/navMesh/config/height',
+ value=1.5)
+ omni.kit.commands.execute(
+ 'ChangeSetting',
+ path='/exts/omni.anim.navigation.core/navMesh/config/radius',
+ value=0.5)
+ omni.kit.commands.execute(
+ 'ChangeSetting',
+ path='/exts/omni.anim.navigation.core/navMesh/config/maxSlope',
+ value=60.0)
+ omni.kit.commands.execute(
+ 'ChangeSetting',
+ path='/exts/omni.anim.navigation.core/navMesh/config/maxClimb',
+ value=0.2)
+ omni.kit.commands.execute(
+ 'ChangeSetting',
+ path='/persistent/exts/omni.anim.navigation.core/navMesh/autoRebakeDelaySeconds',
+ value=4)
+ omni.kit.commands.execute(
+ 'ChangeSetting',
+ path='/persistent/exts/omni.anim.navigation.core/navMesh/viewNavMesh',
+ value=False)
+ print('Navigation mesh rebuilt.')
+
+
+def create_people_commands(environment_prim_path: str,
+ anim_people_command_dir: str,
+ max_number_tries: int,
+ num_waypoints_per_human: int):
+ """
+ Create the occupancy grid which returns the free points for the humans to move.
+
+ These points are then randomly sampled and assigned to each person and is saved as a txt file
+ which can be used as the command file for people animation. It directly gets the context of
+ the scene once the scene is opened and played in IsaacSim.
+
+ Args:
+ environment_prim_path (str): The warehouse enviroment prim path
+ anim_people_command_dir (str): The local directory to be used for storing the command file
+ max_number_tries (int): The number of times the script attempts to select a new waypoint
+ for a human.
+ num_waypoints_per_human (int): The number of waypoints to create per human.
+
+ """
+ from omni.isaac.occupancy_map import _occupancy_map
+ import omni.anim.navigation.core as navcore
+ from omni.isaac.core.prims import XFormPrim
+ import carb
+ navigation_interface = navcore.acquire_interface()
+ print('Creating randomized paths for people...')
+ bounding_box = omni.usd.get_context().compute_path_world_bounding_box(
+ environment_prim_path)
+
+ physx = omni.physx.acquire_physx_interface()
+ stage_id = omni.usd.get_context().get_stage_id()
+
+ generator = _occupancy_map.Generator(physx, stage_id)
+ # 0.05m cell size, output buffer will have 0 for occupied cells, 1 for
+ # unoccupied, and 6 for cells that cannot be seen
+ # this assumes your usd stage units are in m, and not cm
+ generator.update_settings(.05, 0, 1, 6)
+ # Setting the transform for the generator
+ generator.set_transform(
+ # The starting location to map from
+ (0, 0, 0.025),
+ # The min bound
+ (bounding_box[0][0], bounding_box[0][1], 0.025),
+ # The max bound
+ (bounding_box[1][0], bounding_box[1][1], 0.025))
+ generator.generate2d()
+ # Get locations of free points
+ free_points = generator.get_free_positions()
+ # Find out number of humans in scene and create a list with their names
+ human_names = []
+ human_prims = []
+ for human in omni.usd.get_context().get_stage().GetPrimAtPath(
+ '/World/Humans').GetAllChildren():
+ human_name = human.GetChildren()[0].GetName()
+ # To remove biped setup prim from the list
+ if 'CharacterAnimation' in human_name or 'Biped_Setup' in human_name:
+ continue
+ human_names.append(human_name)
+ human_prims.append(XFormPrim(human.GetPath().pathString))
+
+ random_waypoints = {}
+ for human_name, human_prim in zip(human_names, human_prims):
+ # Get the human initial position.
+ start_point, _ = human_prim.get_world_pose()
+ # Initialize target waypoints.
+ random_waypoints[human_name] = []
+ for _ in range(num_waypoints_per_human):
+ current_tries = 0
+ path = None
+ # Sample for a maximum number of tries then give up.
+ while path is None and current_tries < max_number_tries:
+ new_waypoint = random.sample(free_points, 1)
+ # Check that a path exists between the starting position and the destination
+ path = navigation_interface.query_navmesh_path(
+ carb.Float3(start_point), new_waypoint[0])
+ current_tries += 1
+ if path is not None:
+ new_waypoint[0].z = 0.0
+ random_waypoints[human_name].append(new_waypoint[0])
+ print(
+ f'Found path for {human_name} from {start_point} to {new_waypoint[0]} after \
+ {current_tries} tries')
+ start_point = new_waypoint[0]
+ else:
+ print(
+ f'Could not find path for {human_name} after {max_number_tries}, skipping')
+
+ # Save as command file
+ command_file_path = os.path.join(
+ anim_people_command_dir, 'human_cmd_file.txt')
+ print(f'Saving randomized commands to {command_file_path}')
+ with open(command_file_path, 'w') as file:
+ for human_name, waypoints in random_waypoints.items():
+ human_command_line = f'{human_name} GoTo '
+ for next_waypoint in waypoints:
+ human_command_line += f'{next_waypoint[0]} {next_waypoint[1]} 0 '
+ human_command_line += '_\n'
+ file.write(human_command_line)
+
+
+def update_people_command_file_path(anim_people_command_dir: str):
+ """
+ Update the command file path settings in the simulation scene to the custom one.
+
+ Args:
+ anim_people_command_dir (str): The directory where the generated/custom command file is
+ stored.
+
+ """
+ import omni.kit.commands
+ omni.kit.commands.execute(
+ 'ChangeSetting',
+ path='/exts/omni.anim.people/command_settings/command_file_path',
+ value=anim_people_command_dir + '/human_cmd_file.txt')
+
+
+def main(scenario_path: str,
+ anim_people_command_dir: str,
+ environment_prim_path: str,
+ random_command_generation: bool,
+ num_waypoints: int,
+ headless: bool = False,
+ with_people: bool = True,
+ use_generated_command_file: bool = False,
+ tick_rate_hz: float = 20.0):
+
+ # Start up the simulator
+ from omni.isaac.kit import SimulationApp
+ simulation_app = SimulationApp({
+ 'renderer': 'RayTracedLighting',
+ 'headless': headless
+ })
+
+ import omni.kit.commands
+ from omni.isaac.core import SimulationContext
+ from omni.isaac.core.utils.nucleus import get_assets_root_path
+
+ # Enables the simulation extensions
+ enable_extensions_for_sim(with_people)
+
+ assets_root_path = get_assets_root_path()
+ if assets_root_path is None:
+ print(
+ 'Could not find Isaac Sim assets folder. Make sure you have an up to date local \
+ Nucleus server or that you have a proper connection to the internet.'
+ )
+ simulation_app.close()
+ exit()
+
+ usd_path = assets_root_path + scenario_path
+ print(f'Opening stage {usd_path}...')
+
+ # Load the stage
+ omni.usd.get_context().open_stage(usd_path, None)
+
+ # Wait two frames so that stage starts loading
+ simulation_app.update()
+ simulation_app.update()
+
+ print('Loading stage...')
+ from omni.isaac.core.utils.stage import is_stage_loading
+
+ while is_stage_loading():
+ simulation_app.update()
+ print('Loading Complete')
+
+ # If the scene with people is requested we build/rebuild the navmesh
+ # We also point to the generated/custom command file for people animation if set by user
+ if with_people:
+ rebuild_nav_mesh()
+ if not random_command_generation and use_generated_command_file:
+ update_people_command_file_path(anim_people_command_dir)
+
+ time_dt = 1.0 / tick_rate_hz
+ print(f'Running sim at {tick_rate_hz} Hz, with dt of {time_dt}')
+ # Run physics at 60 Hz and render time at the set frequency to see the sim as real time
+ simulation_context = SimulationContext(stage_units_in_meters=1.0,
+ physics_dt=1.0 / 60,
+ rendering_dt=time_dt)
+
+ simulation_context.play()
+ simulation_context.step()
+
+ # Physics can only be seen when the scene is played
+ # If we want to generate the command file for the people simulation we call the
+ # create_people_commands method, which stores the command file and then we close the
+ # simulation and point the user on how to use the generated file
+ if with_people and random_command_generation:
+ print('Creating human animation file...')
+ create_people_commands(environment_prim_path,
+ anim_people_command_dir, 10, num_waypoints)
+ print(
+ 'Human command file has been created at {}/human_human_cmd_file.txt'
+ .format(anim_people_command_dir))
+ print(
+ 'Please restart the simulation with --with_people and \n \
+ --use_generated_command_file to use the generated command file in human simulation'
+ )
+ simulation_context.stop()
+ simulation_app.close()
+
+ # Simulate for a few seconds to warm up sim and let everything settle
+ for _ in range(2*round(tick_rate_hz)):
+ simulation_context.step()
+
+ # Run the sim
+ last_frame_time = time.monotonic()
+ while simulation_app.is_running():
+ simulation_context.step()
+ current_frame_time = time.monotonic()
+ if current_frame_time - last_frame_time < time_dt:
+ time.sleep(time_dt - (current_frame_time - last_frame_time))
+ last_frame_time = time.monotonic()
+
+ simulation_context.stop()
+ simulation_app.close()
+
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(
+ description='Sample app for running Carter in a Warehouse for NVblox.')
+ parser.add_argument(
+ '--scenario_path',
+ help='Path of the scenario to launch relative to the nucleus server '
+ 'base path. Scenario must contain a carter robot. If the scene '
+ 'contains animated humans, the script expects to find them under /World/Humans. '
+ 'Example scenarios are /Isaac/Samples/NvBlox/carter_warehouse_navigation_with_people.usd '
+ 'or /Isaac/Samples/NvBlox/carter_warehouse_navigation.usd',
+ default='/Isaac/Samples/NvBlox/carter_warehouse_navigation.usd')
+ parser.add_argument('--environment_prim_path',
+ default='/World/WareHouse',
+ help='Path to the world to create a navigation mesh.')
+ parser.add_argument(
+ '--tick_rate_hz',
+ type=int,
+ help='The rate (in hz) that we step the simulation at.',
+ default=20)
+ parser.add_argument(
+ '--anim_people_waypoint_dir',
+ help='Directory location to save the waypoints in a yaml file')
+ parser.add_argument('--headless', action='store_true',
+ help='Run the simulation headless.')
+ parser.add_argument(
+ '--with_people',
+ help='If used, animation extensions for humans will be enabled.',
+ action='store_true')
+ parser.add_argument(
+ '--random_command_generation',
+ help='Choose whether we generate random waypoint or run sim',
+ action='store_true')
+ parser.add_argument('--num_waypoints', type=int,
+ help='Number of waypoints to generate for each human in the scene.',
+ default=5)
+ parser.add_argument(
+ '--use_generated_command_file',
+ help='Choose whether to use generated/custom command file or to use the default one to run\
+ the people animation',
+ action='store_true')
+ # This allows for IsaacSim options to be passed on the SimulationApp.
+ args, unknown = parser.parse_known_args()
+
+ # If we want to generate the command file then we run the simulation headless
+ if args.random_command_generation:
+ args.headless = True
+
+ # Check if the command file directory is given if it has to be generated or used for the
+ # simulation
+ if args.random_command_generation or args.use_generated_command_file:
+ if not args.anim_people_waypoint_dir:
+ raise ValueError(
+ 'Input to command file directory required if custom command file has to be \
+ generated/used!!'
+ )
+
+ main(args.scenario_path, args.anim_people_waypoint_dir, args.environment_prim_path,
+ args.random_command_generation, args.num_waypoints, args.headless, args.with_people,
+ args.use_generated_command_file, args.tick_rate_hz)
diff --git a/nvblox_isaac_sim/package.xml b/nvblox_examples/nvblox_isaac_sim/package.xml
similarity index 98%
rename from nvblox_isaac_sim/package.xml
rename to nvblox_examples/nvblox_isaac_sim/package.xml
index 0f7bb453..a1a47a05 100644
--- a/nvblox_isaac_sim/package.xml
+++ b/nvblox_examples/nvblox_isaac_sim/package.xml
@@ -21,7 +21,7 @@
nvblox_isaac_sim
- 0.20.0
+ 0.30.0
Launch files for working with nvblox_ros and Isaac Sim
Helen Oleynikova
diff --git a/nvblox_isaac_sim/resource/nvblox_isaac_sim b/nvblox_examples/nvblox_isaac_sim/resource/nvblox_isaac_sim
similarity index 100%
rename from nvblox_isaac_sim/resource/nvblox_isaac_sim
rename to nvblox_examples/nvblox_isaac_sim/resource/nvblox_isaac_sim
diff --git a/nvblox_isaac_sim/setup.cfg b/nvblox_examples/nvblox_isaac_sim/setup.cfg
similarity index 100%
rename from nvblox_isaac_sim/setup.cfg
rename to nvblox_examples/nvblox_isaac_sim/setup.cfg
diff --git a/nvblox_isaac_sim/setup.py b/nvblox_examples/nvblox_isaac_sim/setup.py
similarity index 92%
rename from nvblox_isaac_sim/setup.py
rename to nvblox_examples/nvblox_isaac_sim/setup.py
index f046d903..67c84803 100644
--- a/nvblox_isaac_sim/setup.py
+++ b/nvblox_examples/nvblox_isaac_sim/setup.py
@@ -1,5 +1,5 @@
# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -21,7 +21,7 @@
setup(
name=package_name,
- version='0.20.0',
+ version='0.30.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
diff --git a/nvblox_isaac_sim/test/test_flake8.py b/nvblox_examples/nvblox_isaac_sim/test/test_flake8.py
similarity index 100%
rename from nvblox_isaac_sim/test/test_flake8.py
rename to nvblox_examples/nvblox_isaac_sim/test/test_flake8.py
diff --git a/nvblox_isaac_sim/test/test_pep257.py b/nvblox_examples/nvblox_isaac_sim/test/test_pep257.py
similarity index 92%
rename from nvblox_isaac_sim/test/test_pep257.py
rename to nvblox_examples/nvblox_isaac_sim/test/test_pep257.py
index b234a384..2ff83693 100644
--- a/nvblox_isaac_sim/test/test_pep257.py
+++ b/nvblox_examples/nvblox_isaac_sim/test/test_pep257.py
@@ -19,5 +19,5 @@
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
+ rc = main(argv=['.', 'test', '--add-ignore', 'D407'])
assert rc == 0, 'Found code style errors / warnings'
diff --git a/nvblox_examples/realsense_splitter/CMakeLists.txt b/nvblox_examples/realsense_splitter/CMakeLists.txt
index f4af67ea..96bb3d34 100644
--- a/nvblox_examples/realsense_splitter/CMakeLists.txt
+++ b/nvblox_examples/realsense_splitter/CMakeLists.txt
@@ -1,5 +1,5 @@
# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -18,7 +18,7 @@
cmake_minimum_required(VERSION 3.5)
project(realsense_splitter)
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
add_compile_options(-Wall -Wextra -Wpedantic)
# find dependencies
@@ -29,6 +29,7 @@ find_package(rclcpp_components REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(message_filters REQUIRED)
find_package(realsense2_camera_msgs REQUIRED)
+find_package(nvblox_ros_common REQUIRED)
#############
@@ -36,7 +37,6 @@ find_package(realsense2_camera_msgs REQUIRED)
#############
add_library(realsense_splitter_component
src/realsense_splitter_node.cpp
- src/qos.cpp
)
target_compile_definitions(realsense_splitter_component
PRIVATE "COMPOSITION_BUILDING_DLL")
@@ -46,6 +46,7 @@ ament_target_dependencies(realsense_splitter_component
sensor_msgs
message_filters
realsense2_camera_msgs
+ nvblox_ros_common
)
target_include_directories(realsense_splitter_component PUBLIC
$
diff --git a/nvblox_isaac_sim/nvblox_isaac_sim/__init__.py b/nvblox_examples/realsense_splitter/COLCON_IGNORE
similarity index 100%
rename from nvblox_isaac_sim/nvblox_isaac_sim/__init__.py
rename to nvblox_examples/realsense_splitter/COLCON_IGNORE
diff --git a/nvblox_examples/realsense_splitter/include/realsense_splitter/qos.hpp b/nvblox_examples/realsense_splitter/include/realsense_splitter/qos.hpp
index f5ebc737..6c41aa1d 100644
--- a/nvblox_examples/realsense_splitter/include/realsense_splitter/qos.hpp
+++ b/nvblox_examples/realsense_splitter/include/realsense_splitter/qos.hpp
@@ -1,11 +1,11 @@
// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
-// http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
diff --git a/nvblox_examples/realsense_splitter/include/realsense_splitter/realsense_splitter_node.hpp b/nvblox_examples/realsense_splitter/include/realsense_splitter/realsense_splitter_node.hpp
index a7d5945a..d6448296 100644
--- a/nvblox_examples/realsense_splitter/include/realsense_splitter/realsense_splitter_node.hpp
+++ b/nvblox_examples/realsense_splitter/include/realsense_splitter/realsense_splitter_node.hpp
@@ -1,11 +1,11 @@
// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
-// http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
@@ -41,7 +41,8 @@ class RealsenseSplitterNode : public rclcpp::Node
enum class EmitterMode : int
{
kOn = 1,
- kOff = 0
+ kOff = 0,
+ kUnknown = 2
};
// Input callbacks
diff --git a/nvblox_examples/realsense_splitter/package.xml b/nvblox_examples/realsense_splitter/package.xml
index 65843a24..9e0b41e1 100644
--- a/nvblox_examples/realsense_splitter/package.xml
+++ b/nvblox_examples/realsense_splitter/package.xml
@@ -21,7 +21,7 @@
realsense_splitter
- 0.20.0
+ 0.30.0
Splits realsense outputs based on emitter status.
alex
Apache-2.0
@@ -37,6 +37,7 @@
sensor_msgs
message_filters
realsense2_camera_msgs
+ nvblox_ros_common
ament_lint_auto
ament_lint_common
diff --git a/nvblox_examples/realsense_splitter/src/realsense_splitter_node.cpp b/nvblox_examples/realsense_splitter/src/realsense_splitter_node.cpp
index 1645e0e9..6174b26f 100644
--- a/nvblox_examples/realsense_splitter/src/realsense_splitter_node.cpp
+++ b/nvblox_examples/realsense_splitter/src/realsense_splitter_node.cpp
@@ -1,11 +1,11 @@
// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
-// http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
@@ -17,13 +17,13 @@
#include "realsense_splitter/realsense_splitter_node.hpp"
-#include "realsense_splitter/qos.hpp"
+#include
namespace nvblox
{
RealsenseSplitterNode::RealsenseSplitterNode(const rclcpp::NodeOptions & options)
-: Node("odometry_flattener_node", options)
+: Node("realsense_splitter_node", options)
{
RCLCPP_INFO(get_logger(), "Creating a RealsenseSplitterNode().");
@@ -102,8 +102,19 @@ int RealsenseSplitterNode::getEmitterModeFromMetadataMsg(
constexpr size_t field_name_length =
sizeof(frame_emitter_mode_str) / sizeof(frame_emitter_mode_str[0]);
// Find the field
- size_t field_location =
- metadata->json_data.find(frame_emitter_mode_str) + field_name_length - 1;
+ const size_t frame_emitter_mode_start_location =
+ metadata->json_data.find(frame_emitter_mode_str);
+ // If the emitter mode is not found, return unknown and warn the user.
+ if (frame_emitter_mode_start_location == metadata->json_data.npos) {
+ constexpr int kPublishPeriodMs = 1000;
+ auto & clk = *get_clock();
+ RCLCPP_WARN_THROTTLE(
+ get_logger(), clk, kPublishPeriodMs,
+ "Realsense frame metadata did not contain \"frame_emitter_mode\". Splitter will not work.");
+ return static_cast(EmitterMode::kUnknown);
+ }
+ // If it is found, parse the field.
+ const size_t field_location = frame_emitter_mode_start_location + field_name_length - 1;
const int emitter_mode =
static_cast(metadata->json_data[field_location]) -
static_cast('0');
diff --git a/nvblox_examples/realsense_splitter/src/realsense_splitter_node_main.cpp b/nvblox_examples/realsense_splitter/src/realsense_splitter_node_main.cpp
index c5d33a08..a7238d25 100644
--- a/nvblox_examples/realsense_splitter/src/realsense_splitter_node_main.cpp
+++ b/nvblox_examples/realsense_splitter/src/realsense_splitter_node_main.cpp
@@ -1,11 +1,11 @@
// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
-// http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
diff --git a/nvblox_examples/semantic_label_conversion/CMakeLists.txt b/nvblox_examples/semantic_label_conversion/CMakeLists.txt
new file mode 100644
index 00000000..22207646
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/CMakeLists.txt
@@ -0,0 +1,36 @@
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.5)
+project(semantic_label_conversion)
+
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+# Installing script so their runable
+install(PROGRAMS
+ semantic_label_conversion/semantic_label_converter.py
+ semantic_label_conversion/semantic_label_stamper.py
+ DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY params launch
+ DESTINATION share/${PROJECT_NAME})
+
+ament_package()
diff --git a/nvblox_examples/semantic_label_conversion/README.md b/nvblox_examples/semantic_label_conversion/README.md
new file mode 100644
index 00000000..b63c2906
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/README.md
@@ -0,0 +1,50 @@
+# How to get semantic GT from Isaac Sim
+
+This package helps to convert semantic labels coming from Isaac Sim to consistent label images in mono8 and rgb8 used in nvblox.
+
+# Nodes
+
+`semantic_label_conversion.launch.py` launches the `semantic_label_stamper` and `semantic_label_converter` nodes.
+The parameters in `params/semantic_label_conversion.yaml` are loaded.
+
+## **semantic_label_stamper** node
+
+Adds timestamps to the semantic labels. This is required to synchronize the semantic image and labels in the `semantic_label_converter` node with a `TimeSynchronizer`.
+
+| Subscriber Topics | Type | Description |
+|----------------------------------|-----------------------|---------------------------------------------------|
+| `//semantic_labels` | `std_msgs/msg/String` | Semantic labels definition published by Isaac Sim |
+
+| Publisher Topics | Type | Description |
+|-----------------------------------------------------|---------------------------------------------|---------------------------------------|
+| `/semantic_conversion//labels_stamped` | `semantic_labels/msg/SemanticLabelsStamped` | Resulting timestamped semantic labels |
+
+
+## **semantic_label_converter** node
+
+Outputs the resulting mono8 and rgb8 GT images.
+
+| Subscriber Topics | Type | Description |
+|-----------------------------------------------------|---------------------------------------------|------------------------------------------------------------------------------|
+| `//semantic` | `sensor_msgs/msg/Image` | GT image published by Isaac Sim |
+| `/semantic_conversion//labels_stamped` | `semantic_labels/msg/SemanticLabelsStamped` | Timestamped semantic labels published by the `semantic_label_converter` node |
+
+| Publisher Topics | Type | Description |
+|---------------------------------------------------------|-------------------------|-----------------------------------------|
+| `/semantic_conversion//semantic_mono8` | `sensor_msgs/msg/Image` | Resulting semantic GT one channel image |
+| `/semantic_conversion//semantic_colorized` | `sensor_msgs/msg/Image` | Resulting semantic GT colorized image |
+
+
+## Configuration
+
+The configuration file can be found in `params/semantic_label_conversion.yaml`.
+
+| Parameter | Type | Description |
+|-----------------------------------|----------|-----------------------------------------------------------------------------------------------------------------------------------------|
+| `camera_1_enabled` | `bool` | Enable the conversion for camera 1 images |
+| `camera_1_name` | `string` | Name of camera 1 (topic names are dependent on this parameter) |
+| `camera_2_enabled` | `bool` | Enable the conversion for camera 2 images |
+| `camera_2_name` | `string` | Name of camera 2 (topic names are dependent on this parameter) |
+| `labels.names` | `list` | List of strings containing label names defined in Isaac Sim that you wish to convert |
+| `label..output_id` | `int` | Mono8 value that each label name gets converted to in the `/semantic_conversion//semantic_mono8` image |
+| `label..output_color` | `list` | Rgb8 value stored as a list that each label name gets converted to in the `/semantic_conversion//semantic_colorized` image |
diff --git a/nvblox_examples/semantic_label_conversion/launch/semantic_label_conversion.launch.py b/nvblox_examples/semantic_label_conversion/launch/semantic_label_conversion.launch.py
new file mode 100644
index 00000000..74e7734f
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/launch/semantic_label_conversion.launch.py
@@ -0,0 +1,48 @@
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+ # Config file
+ config_file = os.path.join(
+ get_package_share_directory('semantic_label_conversion'),
+ 'params', 'semantic_label_conversion.yaml')
+
+ semantic_label_stamper = Node(package='semantic_label_conversion',
+ executable='semantic_label_stamper.py',
+ name='semantic_label_stamper',
+ output='screen',
+ emulate_tty=True,
+ parameters=[config_file])
+
+ semantic_label_converter = Node(package='semantic_label_conversion',
+ executable='semantic_label_converter.py',
+ name='semantic_label_converter',
+ output='screen',
+ emulate_tty=True,
+ parameters=[config_file])
+
+ return LaunchDescription([
+ semantic_label_stamper,
+ semantic_label_converter
+ ])
diff --git a/nvblox_examples/semantic_label_conversion/package.xml b/nvblox_examples/semantic_label_conversion/package.xml
new file mode 100644
index 00000000..2eaf20eb
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/package.xml
@@ -0,0 +1,44 @@
+
+
+
+
+
+
+ semantic_label_conversion
+ 0.30.0
+ Package to convert semantic labels coming from Isaac Sim to consistent label images
+ Emilie Wirbel
+ NVIDIA Isaac ROS Software License
+ https://developer.nvidia.com/isaac-ros-gems/
+ Emilie Wirbel
+ Remo Steiner
+
+ ament_cmake
+
+ nvblox_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+ ros2launch
+
+
+ ament_cmake
+
+
diff --git a/nvblox_examples/semantic_label_conversion/params/semantic_label_conversion.yaml b/nvblox_examples/semantic_label_conversion/params/semantic_label_conversion.yaml
new file mode 100644
index 00000000..63089f68
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/params/semantic_label_conversion.yaml
@@ -0,0 +1,19 @@
+/**:
+ ros__parameters:
+ camera_1_enabled: true
+ camera_1_name: front/stereo_camera/left
+ camera_2_enabled: false
+ camera_2_name: front/stereo_camera/right
+
+ labels:
+ # Semantic label names defined in Isaac Sim
+ names: ["unlabelled", "person"]
+
+ # Definition of how the label names are converted
+ # to IDs on the mask image and color on the colorized mask
+ unlabelled:
+ output_id: 0
+ output_color: [0, 0, 0]
+ person:
+ output_id: 1
+ output_color: [255, 0, 0]
diff --git a/nvblox_examples/semantic_label_conversion/semantic_label_conversion/__init__.py b/nvblox_examples/semantic_label_conversion/semantic_label_conversion/__init__.py
new file mode 100644
index 00000000..6bbcb131
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/semantic_label_conversion/__init__.py
@@ -0,0 +1,16 @@
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
diff --git a/nvblox_examples/semantic_label_conversion/semantic_label_conversion/semantic_label_converter.py b/nvblox_examples/semantic_label_conversion/semantic_label_conversion/semantic_label_converter.py
new file mode 100755
index 00000000..5b6d311b
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/semantic_label_conversion/semantic_label_converter.py
@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+import json
+import numpy as np
+from typing import Dict, Tuple
+
+import message_filters
+import rclpy
+from cv_bridge import CvBridge
+from rclpy.node import Node
+from nvblox_msgs.msg import SemanticLabelsStamped
+from sensor_msgs.msg import Image
+
+
+class SemanticConverter(Node):
+ def __init__(self) -> None:
+ '''
+ Helper node to convert IsaacSim Semantic Labels to a consistent semantic segmentation image
+ '''
+ super().__init__('semantic_label_converter')
+
+ # Declare params
+ self.declare_parameter('camera_1_enabled', True)
+ self.declare_parameter('camera_2_enabled', True)
+ self.declare_parameter('camera_1_name', 'left')
+ self.declare_parameter('camera_2_name', 'right')
+ self.declare_parameter('labels.names', ['unlabelled', 'person'])
+
+ # Get params
+ camera_1_enabled = self.get_parameter(
+ 'camera_1_enabled').get_parameter_value().bool_value
+ camera_2_enabled = self.get_parameter(
+ 'camera_2_enabled').get_parameter_value().bool_value
+ camera_1_name = self.get_parameter(
+ 'camera_1_name').get_parameter_value().string_value
+ camera_2_name = self.get_parameter(
+ 'camera_2_name').get_parameter_value().string_value
+ label_names = self.get_parameter(
+ 'labels.names').get_parameter_value().string_array_value
+
+ # Get dictionary that defines
+ # how label names are converted to ids and color
+ self.label_conversion_dict = {}
+ for label_name in label_names:
+ output_id_param_name = f"labels.{label_name}.output_id"
+ output_color_param_name = f"labels.{label_name}.output_color"
+
+ self.declare_parameter(output_id_param_name, 0)
+ self.declare_parameter(output_color_param_name, [0, 0, 0])
+
+ output_id = self.get_parameter(
+ output_id_param_name).get_parameter_value().integer_value
+ output_color = list(self.get_parameter(
+ output_color_param_name).get_parameter_value().integer_array_value)
+
+ self.label_conversion_dict[label_name] = {
+ 'output_id': output_id,
+ 'output_color': output_color
+ }
+
+ if camera_1_enabled:
+ self.init_camera(camera_1_name)
+ if camera_2_enabled:
+ self.init_camera(camera_2_name)
+
+ self.bridge = CvBridge()
+
+ def init_camera(self, camera_name: str) -> None:
+ '''
+ Initialize publishers and subscribers for a camera
+ Args:
+ camera_name (str): The name of the camera
+ '''
+ # Subscriber
+ image_subscriber = message_filters.Subscriber(
+ self, Image, f"/{camera_name}/semantic")
+ labels_subscriber = message_filters.Subscriber(
+ self, SemanticLabelsStamped,
+ f"/semantic_conversion/{camera_name}/labels_stamped")
+
+ # Publisher
+ publisher_mono8 = self.create_publisher(
+ Image, f"/semantic_conversion/{camera_name}/semantic_mono8", 1)
+ publisher_colorized = self.create_publisher(
+ Image, f"/semantic_conversion/{camera_name}/semantic_colorized", 1)
+
+ # Synchronized callback
+ def on_camera_image_received(image_msg, label_msg): return \
+ self.on_image_received(
+ publisher_mono8, publisher_colorized, image_msg, label_msg)
+ ts = message_filters.TimeSynchronizer(
+ [image_subscriber, labels_subscriber], 10)
+ ts.registerCallback(on_camera_image_received)
+
+ def on_image_received(self, publisher_mono8, publisher_colorized, image_msg: Image,
+ labels_msg: SemanticLabelsStamped) -> None:
+ '''
+ Callback to convert semantic image from IsaacSim to a consistent label image in mono8
+
+ Args:
+ image_msg (Image): Input image from Isaacsim, is in CV16SC1 format and labels id vary with scene
+ labels_msg (SemanticLabelsStamped): Stamped input labels message for the current image.
+ '''
+ # Load the labels as a json
+ labels_dict = json.loads(labels_msg.labels)
+ # Build LUT for color conversions
+ lut_labels, lut_colors = self.build_labels_lut(labels_dict)
+ shape = (image_msg.height, image_msg.width, 1)
+ # Convert from signed int 32 to unsigned char (will cause issues if more than 255 classes)
+ data = np.frombuffer(image_msg.data, dtype=np.int32).reshape(shape)
+ data_mono8 = data.astype(np.uint8)
+
+ # Generate labels arrays with the lookups (for id and coloring)
+ try:
+ labels = np.take(lut_labels, data_mono8)
+ colors = np.take(lut_colors, data_mono8, 0).squeeze()
+ except Exception as e:
+ print("Exception raised in semantic label converter:\n", e)
+ print("WARNING: We will skip this semantic image and label pair.")
+ return
+
+ # Convert back to image messages and publish
+ labels_msg = self.bridge.cv2_to_imgmsg(labels, 'mono8')
+ labels_msg.header = image_msg.header
+ colors_msg = self.bridge.cv2_to_imgmsg(colors, "rgb8")
+ colors_msg.header = image_msg.header
+ publisher_mono8.publish(labels_msg)
+ publisher_colorized.publish(colors_msg)
+
+ def build_labels_lut(
+ self, current_labels: Dict[str,
+ Dict[str,
+ str]]) -> Tuple[np.array, np.array]:
+ '''
+ Build labels lookup table (LUT) from current labels dictionary. The dictionary is formatted as
+ {: {"class": }} where class_id_x is the id of the class in the
+ image and class_name_x is the one that was entered in the semantics schema. This lookup
+ maps all classes that are not in the reference as unlabelled, and remaps ids / colors of the
+ reference classes to the desired reference one
+
+ Args:
+ current_labels (Dict[str, Dict[str, str]]): Labels string coming from IsaacSim
+
+ Returns:
+ Tuple[np.array, np.array]: LUT for ids (Nx1) and colors (Nx3) where N is the number
+ of classes
+ '''
+ # First, get the maximum label that appears in the image
+ max_label = -1
+ for label_id, _ in current_labels.items():
+ label_id_int = int(label_id)
+ if label_id_int > max_label:
+ max_label = label_id_int
+ # Initialize all remappings to zero
+ lut_labels = np.zeros(max_label + 1, dtype=np.uint8)
+ lut_colors = np.zeros((max_label + 1, 3), dtype=np.uint8)
+ # Go over all labels that are present, and get their remap
+ for label_id, label_name_dict in current_labels.items():
+ label_id_int = int(label_id)
+ label_name = label_name_dict.get("class", None)
+ if label_name is None:
+ continue
+ label_name = label_name.lower()
+ # Look for the output id / color if if exists
+ target_label = self.label_conversion_dict.get(
+ label_name, {}).get("output_id", 0)
+ target_color = self.label_conversion_dict.get(label_name, {}).get(
+ "output_color", [0, 0, 0])
+ # Update the remap
+ lut_labels[label_id_int] = target_label
+ lut_colors[label_id_int] = target_color
+ return lut_labels, lut_colors
+
+
+def main():
+ rclpy.init()
+ converter = SemanticConverter()
+ rclpy.spin(converter)
+ converter.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/nvblox_examples/semantic_label_conversion/semantic_label_conversion/semantic_label_stamper.py b/nvblox_examples/semantic_label_conversion/semantic_label_conversion/semantic_label_stamper.py
new file mode 100755
index 00000000..ef8a6b1b
--- /dev/null
+++ b/nvblox_examples/semantic_label_conversion/semantic_label_conversion/semantic_label_stamper.py
@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+import json
+
+import rclpy
+from rclpy.node import Node
+from nvblox_msgs.msg import SemanticLabelsStamped
+from std_msgs.msg import String
+
+
+class LabelsConverter(Node):
+ def __init__(self) -> None:
+ super().__init__('semantic_label_stamper')
+
+ # Declare params
+ self.declare_parameter('camera_1_enabled', True)
+ self.declare_parameter('camera_2_enabled', True)
+ self.declare_parameter('camera_1_name', 'left')
+ self.declare_parameter('camera_2_name', 'right')
+
+ # Get params
+ camera_1_enabled = self.get_parameter(
+ 'camera_1_enabled').get_parameter_value().bool_value
+ camera_2_enabled = self.get_parameter(
+ 'camera_2_enabled').get_parameter_value().bool_value
+ camera_1_name = self.get_parameter(
+ 'camera_1_name').get_parameter_value().string_value
+ camera_2_name = self.get_parameter(
+ 'camera_2_name').get_parameter_value().string_value
+
+ if camera_1_enabled:
+ self.init_camera(camera_1_name)
+ if camera_2_enabled:
+ self.init_camera(camera_2_name)
+
+ def init_camera(self, camera_name: str) -> None:
+ '''
+ Initialize publishers and subscribers for a camera
+ Args:
+ camera_name (str): The name of the camera
+ '''
+ # Publisher
+ labels_publisher = self.create_publisher(
+ SemanticLabelsStamped, f"/semantic_conversion/{camera_name}/labels_stamped", 10)
+
+ # Subscriber
+ def on_camera_labels(msg): return self.on_labels(labels_publisher, msg)
+ self.create_subscription(
+ String, f"/{camera_name}/semantic_labels", on_camera_labels, 10)
+
+ def on_labels(self, publisher, labels_string: String) -> None:
+ '''
+ Parse the string message for the labels and turn it into a custom message with Header
+
+ Args:
+ labels_string (String): String message coming out of IsaacSim
+ '''
+ # Load the string with json
+ labels_data = json.loads(labels_string.data)
+ # Get the timestamp issue
+ time_data = labels_data['time_stamp']
+ # Get the data without timestamp
+ labels_string = labels_data.copy()
+ labels_string.pop('time_stamp', None)
+ # Build and publish message
+ out_msg = SemanticLabelsStamped()
+ out_msg.labels = json.dumps(labels_string)
+ out_msg.header.stamp.sec = int(time_data['sec'])
+ out_msg.header.stamp.nanosec = int(time_data['nanosec'])
+ publisher.publish(out_msg)
+
+
+def main():
+ rclpy.init()
+ semantic_label_stamper = LabelsConverter()
+ rclpy.spin(semantic_label_stamper)
+ semantic_label_stamper.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/nvblox_isaac_sim/omniverse_scripts/__init__.py b/nvblox_isaac_sim/omniverse_scripts/__init__.py
deleted file mode 100644
index e69de29b..00000000
diff --git a/nvblox_isaac_sim/omniverse_scripts/carter_warehouse.py b/nvblox_isaac_sim/omniverse_scripts/carter_warehouse.py
deleted file mode 100644
index 82f31be9..00000000
--- a/nvblox_isaac_sim/omniverse_scripts/carter_warehouse.py
+++ /dev/null
@@ -1,359 +0,0 @@
-# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-#
-# SPDX-License-Identifier: Apache-2.0
-
-import argparse
-import time
-
-from omni.isaac.kit import SimulationApp
-
-
-def meters_to_stage_units(length_m: float) -> float:
- from omni.isaac.core.utils.stage import get_stage_units
- stage_units_in_meters = get_stage_units()
- return length_m / stage_units_in_meters
-
-
-def stage_units_to_camera_units(length_in_stage_units: float) -> float:
- camera_lengths_in_stage_units = 1.0 / 10.0
- return length_in_stage_units / camera_lengths_in_stage_units
-
-
-def setup_carter_sensors(carter_prim_path: str,
- camera_focal_length_m: float = 0.009,
- carter_version: int = 1):
-
- # Set up variables based on carter version.
- left_cam_path = 'chassis_link/camera_mount/carter_camera_stereo_left'
- right_cam_path = 'chassis_link/camera_mount/carter_camera_stereo_right'
-
- if carter_version == 2:
- left_cam_path = (
- 'chassis_link/stereo_cam_left/stereo_cam_left_sensor_frame/'
- 'camera_sensor_left')
- right_cam_path = (
- 'chassis_link/stereo_cam_right/stereo_cam_right_sensor_frame/'
- 'camera_sensor_right')
-
- import omni
- from pxr import Sdf
-
- # Enable
- # Change the focal length of the camera (default in the carter model quite narrow).
- camera_focal_length_in_camera_units = stage_units_to_camera_units(
- meters_to_stage_units(camera_focal_length_m))
-
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(f'{carter_prim_path}/{left_cam_path}.focalLength'),
- value=camera_focal_length_in_camera_units,
- prev=None)
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(f'{carter_prim_path}/{right_cam_path}.focalLength'),
- value=camera_focal_length_in_camera_units,
- prev=None)
-
- # Lidar setup
- carter_lidar_path = 'chassis_link/carter_lidar'
-
- # Set up Lidar for a 32 layer like configuration
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(f'{carter_prim_path}/{carter_lidar_path}.highLod'),
- value=True,
- prev=None)
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(
- f'{carter_prim_path}/{carter_lidar_path}.horizontalResolution'),
- value=0.2,
- prev=None,
- )
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(f'{carter_prim_path}/{carter_lidar_path}.minRange'),
- value=0.7,
- prev=None,
- )
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(
- f'{carter_prim_path}/{carter_lidar_path}.verticalFov'),
- value=30.0,
- prev=None,
- )
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(
- f'{carter_prim_path}/{carter_lidar_path}.verticalResolution'),
- value=1.0,
- prev=None,
- )
-
- # Modify the omnigraph to get lidar point cloud published
-
- import omni.graph.core as og
-
- controller = og.Controller()
-
- # Create pcl reader node
- read_pcl_node_path = f'{carter_prim_path}/ActionGraph/isaac_read_lidar_point_cloud_node'
- read_pcl_node = controller.create_node(
- read_pcl_node_path,
- 'omni.isaac.range_sensor.IsaacReadLidarPointCloud',
- True,
- )
- # Add relationship to lidar prim
- import omni.usd
- stage = omni.usd.get_context().get_stage()
- read_pcl_prim = stage.GetPrimAtPath(read_pcl_node_path)
- input_rel = read_pcl_prim.GetRelationship('inputs:lidarPrim')
- input_rel.SetTargets([f'{carter_prim_path}/{carter_lidar_path}'])
- # Retrieve on playback node
- playback_node = controller.node('on_playback_tick',
- f'{carter_prim_path}/ActionGraph')
- # Connect the tick to the lidar pcl read
- controller.connect(playback_node.get_attribute('outputs:tick'),
- read_pcl_node.get_attribute('inputs:execIn'))
-
- # Create ros2 publisher node
- publish_pcl_node = controller.create_node(
- f'{carter_prim_path}/ActionGraph/ros2_publish_point_cloud',
- 'omni.isaac.ros2_bridge.ROS2PublishPointCloud', True)
- # Set frame id
- publish_pcl_node.get_attribute('inputs:frameId').set('carter_lidar')
- # Connect pcl read to pcl publish
- controller.connect(read_pcl_node.get_attribute('outputs:execOut'),
- publish_pcl_node.get_attribute('inputs:execIn'))
- controller.connect(read_pcl_node.get_attribute('outputs:pointCloudData'),
- publish_pcl_node.get_attribute('inputs:pointCloudData'))
- # Get timestamp node and connect it
- timestamp_node = controller.node('isaac_read_simulation_time',
- f'{carter_prim_path}/ActionGraph')
- controller.connect(timestamp_node.get_attribute('outputs:simulationTime'),
- publish_pcl_node.get_attribute('inputs:timeStamp'))
- # Get context node and connect it
- context_node = controller.node('ros2_context',
- f'{carter_prim_path}/ActionGraph')
- controller.connect(context_node.get_attribute('outputs:context'),
- publish_pcl_node.get_attribute('inputs:context'))
- # Get namespace node and connect it
- namespace_node = controller.node('node_namespace',
- f'{carter_prim_path}/ActionGraph')
- controller.connect(namespace_node.get_attribute('inputs:value'),
- publish_pcl_node.get_attribute('inputs:nodeNamespace'))
-
- # Configure left camera resolution
- # Create camera node and set target resolution
- resolution_node = controller.create_node(
- f'{carter_prim_path}/ROS_Cameras/isaac_set_viewport_resolution',
- 'omni.isaac.core_nodes.IsaacSetViewportResolution',
- True,
- )
- resolution_node.get_attribute('inputs:width').set(640)
- resolution_node.get_attribute('inputs:height').set(480)
- # Get viewport creation node
- viewport_node = controller.node('isaac_create_viewport_left',
- f'{carter_prim_path}/ROS_Cameras')
- # Connect it
- controller.connect(viewport_node.get_attribute('outputs:execOut'),
- resolution_node.get_attribute('inputs:execIn'))
- controller.connect(viewport_node.get_attribute('outputs:viewport'),
- resolution_node.get_attribute('inputs:viewport'))
-
- # Change publication topics
- left_rgb = controller.node('ros2_create_camera_left_rgb',
- f'{carter_prim_path}/ROS_Cameras')
- left_rgb.get_attribute('inputs:topicName').set('/left/rgb')
- left_info = controller.node('ros2_create_camera_left_info',
- f'{carter_prim_path}/ROS_Cameras')
- left_info.get_attribute('inputs:topicName').set('/left/camera_info')
- left_depth = controller.node('ros2_create_camera_left_depth',
- f'{carter_prim_path}/ROS_Cameras')
- left_depth.get_attribute('inputs:topicName').set('/left/depth')
-
- # Finally, enable left rgb and depth
- for enable_name in [
- 'enable_camera_left', 'enable_camera_left_rgb',
- 'enable_camera_left_depth'
- ]:
- left_enable = controller.node(enable_name,
- f'{carter_prim_path}/ROS_Cameras')
- left_enable.get_attribute('inputs:condition').set(True)
-
-
-def setup_forklifts_collision():
- # Finally forklifts
-
- import omni.kit.commands
- from omni.isaac.core.utils.prims import is_prim_path_valid
- from pxr import Sdf
-
- for forklift_name in ['Forklift', 'Forklift_01']:
- forklift_path = f'/World/warehouse_with_forklifts/{forklift_name}'
- # Ignore if they do not exist
- if not is_prim_path_valid(forklift_path):
- continue
- # Enable collision on main body
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(
- f'{forklift_path}/S_ForkliftBody.physics:collisionEnabled'),
- value=True,
- prev=None,
- )
- # Disable collision on invible main body box
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(
- f'{forklift_path}/chassis_collision.physics:collisionEnabled'),
- value=False,
- prev=None,
- )
- # Enable collision on tine
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(
- f'{forklift_path}/S_ForkliftFork/collision_box.physics:collisionEnabled'
- ),
- value=False,
- prev=None,
- )
- # Disable collision on tine box
- omni.kit.commands.execute(
- 'ChangeProperty',
- prop_path=Sdf.Path(
- f'{forklift_path}/S_ForkliftFork/S_ForkliftFork.physics:collisionEnabled'
- ),
- value=True,
- prev=None,
- )
-
-
-def main(scenario_path: str,
- tick_rate_hz: float = 20.0,
- headless: bool = False,
- carter_prim_path: str = '/World/Carter_ROS',
- carter_version: int = 1):
- # Start up the simulator
- simulation_app = SimulationApp({
- 'renderer': 'RayTracedLighting',
- 'headless': headless
- })
- import omni
- from omni.isaac.core import SimulationContext
- # enable ROS2 bridge extension
- from omni.isaac.core.utils.extensions import enable_extension
- enable_extension('omni.isaac.ros2_bridge')
-
- from omni.isaac.core.utils.nucleus import get_assets_root_path
-
- assets_root_path = get_assets_root_path()
- if assets_root_path is None:
- import carb
- carb.log_error('Could not find Isaac Sim assets folder')
- simulation_app.close()
- exit()
-
- usd_path = assets_root_path + scenario_path
- print('------{0} n scenario_path:{1}', usd_path, scenario_path)
-
- # Load the stage
-
- omni.usd.get_context().open_stage(usd_path, None)
-
- # Wait two frames so that stage starts loading
- simulation_app.update()
- simulation_app.update()
-
- print('Loading stage...')
- from omni.isaac.core.utils.stage import is_stage_loading
-
- while is_stage_loading():
- simulation_app.update()
- print('Loading Complete')
-
- time_dt = 1.0 / tick_rate_hz
- print(f'Running sim at {tick_rate_hz} Hz, with dt of {time_dt}')
- simulation_context = SimulationContext(stage_units_in_meters=1.0)
-
- # Configure sensors
- print(
- f'Configuring sensors for Carter {carter_version} at: {carter_prim_path}'
- )
- setup_carter_sensors(carter_prim_path, carter_version=carter_version)
- setup_forklifts_collision()
-
- simulation_context.play()
- simulation_context.step()
-
- # Simulate for one second to warm up sim and let everything settle
- # Otherwise the resizes below sometimes don't stick.
- for frame in range(round(tick_rate_hz)):
- simulation_context.step()
-
- # Dock the second camera window
- right_viewport = omni.ui.Workspace.get_window('Viewport')
- left_viewport = omni.ui.Workspace.get_window('Viewport 2')
- if right_viewport is not None and left_viewport is not None:
- left_viewport.dock_in(right_viewport, omni.ui.DockPosition.LEFT)
- right_viewport = None
- left_viewport = None
-
- # Run the sim
- last_frame_time = time.monotonic()
- while simulation_app.is_running():
- simulation_context.step()
- current_frame_time = time.monotonic()
- if current_frame_time - last_frame_time < time_dt:
- time.sleep(time_dt - (current_frame_time - last_frame_time))
- last_frame_time = time.monotonic()
-
- simulation_context.stop()
- simulation_app.close()
-
-
-if __name__ == '__main__':
- parser = argparse.ArgumentParser(
- description='Sample app for running Carter in a Warehouse for NVblox.')
- parser.add_argument(
- '--scenario_path',
- metavar='scenario_path',
- type=str,
- help='Path of the scenario to launch relative to the nucleus server '
- 'base path. Scenario must contain a carter robot.',
- default='/Isaac/Samples/ROS2/Scenario/carter_warehouse_navigation.usd')
- parser.add_argument(
- '--tick_rate_hz',
- metavar='tick_rate_hz',
- type=int,
- help='The rate (in hz) that we step the simulation at.',
- default=20)
- parser.add_argument('--carter_prim_path',
- metavar='carter_prim_path',
- type=str,
- default='/World/Carter_ROS',
- help='Path to Carter.')
- parser.add_argument('--headless', action='store_true')
- parser.add_argument('--carter_version',
- type=int,
- default=1,
- help='Version of the Carter robot (1 or 2)')
- args, unknown = parser.parse_known_args()
-
- main(args.scenario_path, args.tick_rate_hz, args.headless,
- args.carter_prim_path, args.carter_version)
diff --git a/nvblox_msgs/CMakeLists.txt b/nvblox_msgs/CMakeLists.txt
index d06b9aea..65d9f06e 100644
--- a/nvblox_msgs/CMakeLists.txt
+++ b/nvblox_msgs/CMakeLists.txt
@@ -40,11 +40,12 @@ find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
- "msg/Index3D.msg"
+ "srv/FilePath.srv"
+ "msg/SemanticLabelsStamped.msg"
"msg/MeshBlock.msg"
"msg/Mesh.msg"
+ "msg/Index3D.msg"
"msg/DistanceMapSlice.msg"
- "srv/FilePath.srv"
DEPENDENCIES std_msgs geometry_msgs
)
diff --git a/nvblox_msgs/msg/SemanticLabelsStamped.msg b/nvblox_msgs/msg/SemanticLabelsStamped.msg
new file mode 100644
index 00000000..810b2071
--- /dev/null
+++ b/nvblox_msgs/msg/SemanticLabelsStamped.msg
@@ -0,0 +1,2 @@
+std_msgs/Header header
+string labels
diff --git a/nvblox_msgs/package.xml b/nvblox_msgs/package.xml
index 1a960ff6..ffefb211 100644
--- a/nvblox_msgs/package.xml
+++ b/nvblox_msgs/package.xml
@@ -21,8 +21,8 @@
nvblox_msgs
- 0.20.0
- ROS2 messages for Nvblox types
+ 0.30.0
+ ROS 2 messages for Nvblox types
Helen Oleynikova
Hemal Shah
diff --git a/nvblox_nav2/CMakeLists.txt b/nvblox_nav2/CMakeLists.txt
index 5fb575b3..16564d35 100644
--- a/nvblox_nav2/CMakeLists.txt
+++ b/nvblox_nav2/CMakeLists.txt
@@ -62,19 +62,6 @@ include_directories(include)
install(TARGETS ${lib_name}
DESTINATION lib)
-install(DIRECTORY
- launch
- DESTINATION share/${PROJECT_NAME}
-)
-install(DIRECTORY
- params
- DESTINATION share/${PROJECT_NAME}
-)
-install(DIRECTORY
- config
- DESTINATION share/${PROJECT_NAME}
-)
-
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
@@ -85,4 +72,5 @@ endif()
# Install the plugin
pluginlib_export_plugin_description_file(nav2_costmap_2d nvblox_costmap_layer.xml)
ament_target_dependencies(${lib_name} ${dep_pkgs})
-ament_auto_package(INSTALL_TO_SHARE launch)
+
+ament_package()
diff --git a/nvblox_nav2/config/carter_nvblox_nav2.rviz b/nvblox_nav2/config/carter_nvblox_nav2.rviz
deleted file mode 100644
index daf79709..00000000
--- a/nvblox_nav2/config/carter_nvblox_nav2.rviz
+++ /dev/null
@@ -1,529 +0,0 @@
-Panels:
- - Class: rviz_common/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /TF1
- - /TF1/Frames1
- - /TF1/Tree1
- - /NvbloxMesh1
- - /Robot & Sensors1
- - /Robot & Sensors1/rgb_left1
- - /Robot & Sensors1/depth_left1
- - /Global Navigation1
- - /Global Navigation1/Global Costmap1
- - /Global Navigation1/Global Path1
- - /Local Navigation1
- - /Local Navigation1/Local Costmap1
- - /Elbrus1/Camera1
- - /Elbrus1/Camera1/Visibility1
- - /Elbrus1/PointCloud21/Autocompute Value Bounds1
- - /Marker1
- - /PointCloud21
- - /PointCloud21/Autocompute Value Bounds1
- - /MarkerArray1
- - /MarkerArray1/Topic1
- Splitter Ratio: 0.447075217962265
- Tree Height: 940
- - Class: rviz_common/Selection
- Name: Selection
- - Class: rviz_common/Tool Properties
- Expanded:
- - /2D Goal Pose1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz_common/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
-Visualization Manager:
- Class: ""
- Displays:
- - Class: rviz_default_plugins/TF
- Enabled: true
- Frame Timeout: 15
- Frames:
- All Enabled: true
- base_link:
- Value: true
- camera_left_ROS_frame:
- Value: true
- camera_right_ROS_frame:
- Value: true
- camera_sensor_left:
- Value: true
- camera_sensor_right:
- Value: true
- carter_lidar:
- Value: true
- caster_frame_base:
- Value: true
- caster_swivel_left:
- Value: true
- caster_swivel_right:
- Value: true
- caster_wheel_left:
- Value: true
- caster_wheel_right:
- Value: true
- chassis_link:
- Value: true
- map:
- Value: true
- odom:
- Value: true
- wheel_left:
- Value: true
- wheel_right:
- Value: true
- Marker Scale: 1
- Name: TF
- Show Arrows: true
- Show Axes: true
- Show Names: false
- Tree:
- map:
- odom:
- base_link:
- camera_left_ROS_frame:
- {}
- camera_right_ROS_frame:
- {}
- camera_sensor_left:
- {}
- camera_sensor_right:
- {}
- carter_lidar:
- {}
- chassis_link:
- caster_frame_base:
- caster_swivel_left:
- caster_wheel_left:
- {}
- caster_swivel_right:
- caster_wheel_right:
- {}
- wheel_left:
- {}
- wheel_right:
- {}
- Update Interval: 0
- Value: true
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz_default_plugins/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 100
- Reference Frame:
- Value: true
- - Ceiling Height: 1.5
- Class: nvblox_rviz_plugin/NvbloxMesh
- Cut Ceiling: true
- Enabled: true
- Mesh Color: Color + Shading
- Name: NvbloxMesh
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /nvblox_node/mesh
- Value: true
- - Class: rviz_common/Group
- Displays:
- - Class: rviz_default_plugins/Image
- Enabled: false
- Max Value: 1
- Median window: 5
- Min Value: 0
- Name: rgb_left
- Normalize Range: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /left/rgb
- Value: false
- - Class: rviz_default_plugins/Image
- Enabled: false
- Max Value: 15
- Median window: 5
- Min Value: 0
- Name: depth_left
- Normalize Range: false
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /left/depth
- Value: false
- - Alpha: 1
- Class: rviz_default_plugins/RobotModel
- Collision Enabled: false
- Description File: /home/helen/ros_ws/src/ov_navigation/carter_description/urdf/carter.urdf
- Description Source: File
- Description Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: ""
- Enabled: false
- Links:
- All Links Enabled: true
- Expand Joint Details: false
- Expand Link Details: false
- Expand Tree: false
- Link Tree Style: Links in Alphabetic Order
- Mass Properties:
- Inertia: false
- Mass: false
- Name: RobotModel
- TF Prefix: ""
- Update Interval: 0
- Value: false
- Visual Enabled: true
- Enabled: true
- Name: Robot & Sensors
- - Class: rviz_common/Group
- Displays:
- - Alpha: 0.699999988079071
- Class: rviz_default_plugins/Map
- Color Scheme: costmap
- Draw Behind: false
- Enabled: true
- Name: Global Costmap
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /global_costmap/costmap
- Update Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /global_costmap/costmap_updates
- Use Timestamp: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz_default_plugins/Path
- Color: 25; 255; 0
- Enabled: true
- Head Diameter: 0.30000001192092896
- Head Length: 0.20000000298023224
- Length: 0.30000001192092896
- Line Style: Billboards
- Line Width: 0.10000000149011612
- Name: Global Path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.029999999329447746
- Shaft Diameter: 0.10000000149011612
- Shaft Length: 0.10000000149011612
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /plan
- Value: true
- Enabled: true
- Name: Global Navigation
- - Class: rviz_common/Group
- Displays:
- - Alpha: 1
- Buffer Length: 1
- Class: rviz_default_plugins/Path
- Color: 239; 41; 41
- Enabled: true
- Head Diameter: 0.30000001192092896
- Head Length: 0.20000000298023224
- Length: 0.30000001192092896
- Line Style: Billboards
- Line Width: 0.029999999329447746
- Name: Local Path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.029999999329447746
- Shaft Diameter: 0.10000000149011612
- Shaft Length: 0.10000000149011612
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /local_plan
- Value: true
- - Alpha: 0.699999988079071
- Class: rviz_default_plugins/Map
- Color Scheme: costmap
- Draw Behind: false
- Enabled: true
- Name: Local Costmap
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /local_costmap/costmap
- Update Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /local_costmap/costmap_updates
- Use Timestamp: false
- Value: true
- Enabled: true
- Name: Local Navigation
- - Class: rviz_common/Group
- Displays:
- - Class: rviz_default_plugins/Camera
- Enabled: false
- Far Plane Distance: 100
- Image Rendering: background
- Name: Camera
- Overlay Alpha: 1
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /left/rgb
- Value: false
- Visibility:
- Elbrus:
- PointCloud2: true
- Value: true
- Global Navigation:
- "": true
- Value: false
- Grid: false
- Local Navigation:
- "": true
- Value: false
- Marker: true
- MarkerArray: true
- NvbloxMesh: false
- PointCloud2: true
- Robot & Sensors:
- "": true
- Value: false
- TF: false
- Value: true
- Zoom Factor: 1
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: 0
- Value: false
- Axis: Z
- Channel Name: intensity
- Class: rviz_default_plugins/PointCloud2
- Color: 255; 25; 0
- Color Transformer: AxisColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Max Intensity: 4096
- Min Color: 0; 0; 0
- Min Intensity: 0
- Name: PointCloud2
- Position Transformer: XYZ
- Selectable: true
- Size (Pixels): 3
- Size (m): 0.05000000074505806
- Style: Spheres
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /visual_slam/vis/loop_closure_cloud
- Use Fixed Frame: true
- Use rainbow: true
- Value: true
- Enabled: false
- Name: Elbrus
- - Class: rviz_default_plugins/Marker
- Enabled: true
- Name: Marker
- Namespaces:
- {}
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /nvblox_node/map_slice_bounds
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 0
- Min Value: -0.5
- Value: false
- Axis: Z
- Channel Name: intensity
- Class: rviz_default_plugins/PointCloud2
- Color: 255; 255; 255
- Color Transformer: AxisColor
- Decay Time: 0
- Enabled: false
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Max Intensity: 4096
- Min Color: 0; 0; 0
- Min Intensity: 0
- Name: PointCloud2
- Position Transformer: XYZ
- Selectable: true
- Size (Pixels): 3
- Size (m): 0.02500000037252903
- Style: Flat Squares
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /point_cloud
- Use Fixed Frame: true
- Use rainbow: true
- Value: false
- - Class: rviz_default_plugins/MarkerArray
- Enabled: false
- Name: MarkerArray
- Namespaces:
- {}
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /nvblox_node/mesh_marker
- Value: false
- Enabled: true
- Global Options:
- Background Color: 255; 255; 255
- Fixed Frame: odom
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz_default_plugins/Interact
- Hide Inactive Objects: true
- - Class: rviz_default_plugins/MoveCamera
- - Class: rviz_default_plugins/Select
- - Class: rviz_default_plugins/FocusCamera
- - Class: rviz_default_plugins/Measure
- Line color: 128; 128; 0
- - Class: rviz_default_plugins/SetInitialPose
- Covariance x: 0.25
- Covariance y: 0.25
- Covariance yaw: 0.06853891909122467
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /initialpose
- - Class: rviz_default_plugins/SetGoal
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /goal_pose
- - Class: rviz_default_plugins/PublishPoint
- Single click: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /clicked_point
- Transformation:
- Current:
- Class: rviz_default_plugins/TF
- Value: true
- Views:
- Current:
- Class: rviz_default_plugins/Orbit
- Distance: 23.513273239135742
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: 2.2487809658050537
- Y: -0.7407564520835876
- Z: -1.808356523513794
- Focal Shape Fixed Size: false
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.6247962117195129
- Target Frame: base_link
- Value: Orbit (rviz)
- Yaw: 3.115401029586792
- Saved: ~
-Window Geometry:
- Camera:
- collapsed: false
- Displays:
- collapsed: false
- Height: 1163
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000017b00000435fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000435000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000010007200670062005f006c00650066007402000008f4000000fe000003740000026efb0000001400640065007000740068005f006c0065006600740200000c8c000001e70000014600000125fb0000000a0049006d0061006700650200000c900000023f0000025e000001d3fb0000000c00430061006d0065007200610200000c2f00000104000001e600000190000000010000010f00000435fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000435000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ea0000043500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1920
- X: 1920
- Y: 0
- depth_left:
- collapsed: false
- rgb_left:
- collapsed: false
diff --git a/nvblox_nav2/launch/carter_sim.launch.py b/nvblox_nav2/launch/carter_sim.launch.py
deleted file mode 100644
index 40bb1d2a..00000000
--- a/nvblox_nav2/launch/carter_sim.launch.py
+++ /dev/null
@@ -1,118 +0,0 @@
-# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-#
-# SPDX-License-Identifier: Apache-2.0
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-
- params_file_arg = DeclareLaunchArgument(
- 'params_file', default_value=os.path.join(
- get_package_share_directory(
- 'nvblox_nav2'), 'params', 'carter_nav2.yaml'),
- description='Full path to param file to load')
- use_sim_time_arg = DeclareLaunchArgument(
- 'use_sim_time', default_value='True',
- description='Use simulation (Omniverse Isaac Sim) clock if true')
-
- nvblox_param_arg = DeclareLaunchArgument(
- 'nvblox_params_file',
- default_value=os.path.join(
- get_package_share_directory('nvblox_nav2'), 'params', 'nvblox.yaml'
- )
- )
- use_depth_arg = DeclareLaunchArgument(
- 'use_depth', default_value='True',
- description='Use depth as an input for nvblox reconstruction'
- )
- use_lidar_arg = DeclareLaunchArgument(
- 'use_lidar', default_value='True',
- description='Use lidar as an input for nvblox reconstruction'
- )
- run_rviz_arg = DeclareLaunchArgument(
- 'run_rviz', default_value='True',
- description='Whether to start RVIZ')
- run_nav2_arg = DeclareLaunchArgument(
- 'run_nav2', default_value='True',
- description='Whether to run nav2')
-
- nav2_bringup_launch_dir = os.path.join(
- get_package_share_directory('nav2_bringup'), 'launch')
- rviz_config_dir = os.path.join(get_package_share_directory(
- 'nvblox_nav2'), 'config', 'carter_nvblox_nav2.rviz')
-
- rviz_launch = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(nav2_bringup_launch_dir, 'rviz_launch.py')),
- launch_arguments={'namespace': '', 'use_namespace': 'False',
- 'autostart': 'True',
- 'rviz_config': rviz_config_dir}.items(),
- condition=IfCondition(LaunchConfiguration('run_rviz')))
-
- nav2_launch = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(nav2_bringup_launch_dir, 'navigation_launch.py')),
- launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'params_file': LaunchConfiguration('params_file'),
- 'autostart': 'True'}.items(),
- condition=IfCondition(LaunchConfiguration('run_nav2')))
-
- nvblox_node = Node(
- package='nvblox_ros', executable='nvblox_node',
- parameters=[LaunchConfiguration('nvblox_params_file'),
- {'use_sim_time': LaunchConfiguration('use_sim_time'),
- 'use_depth': LaunchConfiguration('use_depth'),
- 'use_lidar': LaunchConfiguration('use_lidar')}],
- output='screen',
- remappings=[('depth/image', '/left/depth'),
- ('depth/camera_info', '/left/camera_info'),
- ('color/image', '/left/rgb'),
- ('color/camera_info', '/left/camera_info'),
- ('pointcloud', '/point_cloud')
- ])
-
- map_to_odom_publisher = Node(
- package='tf2_ros', executable='static_transform_publisher',
- parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
- output='screen',
- arguments=['0.0', '0.0', '-0.3', '0.0', '0.0', '0.0', 'map',
- 'odom'],
- condition=IfCondition(LaunchConfiguration('run_nav2')))
-
- return LaunchDescription([
- params_file_arg,
- use_sim_time_arg,
- nvblox_param_arg,
- use_depth_arg,
- use_lidar_arg,
- run_rviz_arg,
- run_nav2_arg,
- rviz_launch,
- nav2_launch,
- nvblox_node,
- map_to_odom_publisher])
diff --git a/nvblox_nav2/launch/carter_sim_elbrus.launch.py b/nvblox_nav2/launch/carter_sim_elbrus.launch.py
deleted file mode 100644
index 903a4040..00000000
--- a/nvblox_nav2/launch/carter_sim_elbrus.launch.py
+++ /dev/null
@@ -1,149 +0,0 @@
-# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-#
-# SPDX-License-Identifier: Apache-2.0
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.actions import IncludeLaunchDescription
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
-from launch_ros.actions import Node
-from launch_ros.descriptions import ComposableNode
-
-
-def generate_launch_description():
-
- use_sim_time = LaunchConfiguration('use_sim_time', default='True')
-
- param_dir = LaunchConfiguration(
- 'params_file',
- default=os.path.join(
- get_package_share_directory(
- 'nvblox_nav2'), 'params', 'carter_nav2.yaml'
- ),
- )
-
- nvblox_param_dir = LaunchConfiguration(
- 'nvblox_params_file', default=os.path.join(
- get_package_share_directory('nvblox_nav2'),
- 'params', 'nvblox.yaml'),)
-
- nav2_bringup_launch_dir = os.path.join(
- get_package_share_directory('nav2_bringup'), 'launch')
-
- rviz_config_dir = os.path.join(get_package_share_directory(
- 'nvblox_nav2'), 'config', 'carter_nvblox_nav2.rviz')
-
- lifecycle_nodes = ['map_server']
-
- visual_slam_node = ComposableNode(
- name='visual_slam_node',
- package='isaac_ros_visual_slam',
- plugin='isaac_ros::visual_slam::VisualSlamNode',
- remappings=[('stereo_camera/left/camera_info', '/left/camera_info'),
- ('stereo_camera/right/camera_info', '/right/camera_info'),
- ('stereo_camera/left/image', '/left/rgb'),
- ('stereo_camera/right/image', '/right/rgb')],
- parameters=[{
- 'enable_rectified_pose': True,
- 'denoise_input_images': True,
- 'rectified_images': True,
- 'enable_debug_mode': False,
- 'debug_dump_path': '/tmp/elbrus',
- 'left_camera_frame': 'carter_camera_stereo_left',
- 'right_camera_frame': 'carter_camera_stereo_right',
- 'map_frame': 'map',
- 'fixed_frame': 'odom',
- 'odom_frame': 'odom',
- 'base_frame': 'base_link',
- 'current_smooth_frame': 'base_link_smooth',
- 'current_rectified_frame': 'base_link_rectified',
- 'enable_observations_view': True,
- 'enable_landmarks_view': True,
- 'enable_reading_slam_internals': True,
- 'enable_slam_visualization': True,
- 'enable_localization_n_mapping': True,
- 'use_sim_time': use_sim_time
- }]
- )
- visual_slam_launch_container = ComposableNodeContainer(
- name='visual_slam_launch_container',
- namespace='',
- package='rclcpp_components',
- executable='component_container',
- composable_node_descriptions=[
- visual_slam_node
- ],
- output='screen'
- )
-
- return LaunchDescription(
- [
- DeclareLaunchArgument(
- 'params_file', default_value=param_dir,
- description='Full path to param file to load'),
- DeclareLaunchArgument(
- 'use_sim_time', default_value='True',
- description='Use simulation (Omniverse Isaac Sim) clock if true'),
- DeclareLaunchArgument(
- 'run_rviz', default_value='true',
- description='Whether to start RVIZ'),
- DeclareLaunchArgument(
- 'run_nav2', default_value='true',
- description='Whether to run nav2'),
- Node(
- package='nav2_lifecycle_manager', executable='lifecycle_manager',
- name='lifecycle_manager_map', output='screen',
- parameters=[{'use_sim_time': use_sim_time},
- {'autostart': True},
- {'node_names': lifecycle_nodes}],
- condition=IfCondition(
- LaunchConfiguration('run_nav2'))
- ),
- IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(nav2_bringup_launch_dir, 'rviz_launch.py')),
- launch_arguments={'namespace': '', 'use_namespace': 'False',
- 'autostart': 'True',
- 'rviz_config': rviz_config_dir}.items(),
- condition=IfCondition(LaunchConfiguration('run_rviz')),),
- IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(
- nav2_bringup_launch_dir, 'navigation_launch.py')),
- launch_arguments={'use_sim_time': use_sim_time,
- 'params_file': param_dir,
- 'autostart': 'True'}.items(),
- condition=IfCondition(
- LaunchConfiguration('run_nav2'))
- ),
- Node(
- package='nvblox_ros', executable='nvblox_node',
- parameters=[nvblox_param_dir, {'use_sim_time': use_sim_time,
- 'global_frame': 'odom'}],
- output='screen',
- remappings=[('depth/image', '/left/depth'),
- ('depth/camera_info', '/left/camera_info'),
- ('color/image', '/left/rgb'),
- ('color/camera_info', '/left/camera_info'), ]),
- visual_slam_launch_container
- ])
diff --git a/nvblox_nav2/package.xml b/nvblox_nav2/package.xml
index f6eb9ac6..dc0b6777 100644
--- a/nvblox_nav2/package.xml
+++ b/nvblox_nav2/package.xml
@@ -21,8 +21,8 @@
nvblox_nav2
- 0.20.0
- NVBlox ROS2 Nav2 interface
+ 0.30.0
+ NVBlox ROS 2 Nav2 interface
Helen Oleynikova
Hemal Shah
@@ -34,7 +34,6 @@
ament_cmake
nav_msgs
- nav2_bringup
nav2_costmap_2d
nvblox_msgs
pluginlib
diff --git a/nvblox_nav2/params/nvblox.yaml b/nvblox_nav2/params/nvblox.yaml
deleted file mode 100644
index 269ebcd8..00000000
--- a/nvblox_nav2/params/nvblox.yaml
+++ /dev/null
@@ -1,47 +0,0 @@
-%YAML 1.2
-# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-#
-# SPDX-License-Identifier: Apache-2.0
----
-nvblox_node:
- ros__parameters:
- global_frame: odom
- voxel_size: 0.05
- slice_height: 0.75
- #min_height: -0.25
- #min_height: -0.1
- min_height: 0.1
- max_height: 1.0
- esdf: true
- esdf_2d: true
- mesh: true
- distance_slice: true
- use_color: true
- tsdf_integrator_max_integration_distance_m: 4.0
- lidar_tsdf_integrator_max_integration_distance_m: 10.0
- tsdf_integrator_truncation_distance_vox: 4.0
- tsdf_integrator_max_weight: 20.0
- esdf_integrator_min_weight: 2.0
- esdf_integrator_max_distance_m: 2.0
- esdf_integrator_max_site_distance_vox: 5.0
- max_tsdf_update_hz: 0.0
- max_pointcloud_update_hz: 0.0
- max_color_update_hz: 5.0
- max_mesh_update_hz: 1.0
- max_esdf_update_hz: 10.0
- lidar_width: 1800
- lidar_height: 31
- output_dir: /tmp/
diff --git a/nvblox_nav2/src/nvblox_costmap_layer.cpp b/nvblox_nav2/src/nvblox_costmap_layer.cpp
index 721816ed..55199716 100644
--- a/nvblox_nav2/src/nvblox_costmap_layer.cpp
+++ b/nvblox_nav2/src/nvblox_costmap_layer.cpp
@@ -74,11 +74,26 @@ void NvbloxCostmapLayer::updateBounds(
{
// We give the full AABB of the map that we have available to the
// upstream controller.
+ // According to nav2 the bounds can only grow bigger. So we only update the
+ // bounds if it grows bigger or we keep the old values
if (slice_ != nullptr) {
- *min_x = slice_->origin.x;
- *max_x = *min_x + slice_->width * slice_->resolution;
- *min_y = slice_->origin.y;
- *max_y = *min_y + slice_->height * slice_->resolution;
+ const double current_plugin_min_x = slice_->origin.x;
+ const double current_plugin_min_y = slice_->origin.y;
+ const double current_plugin_max_x = current_plugin_min_x + slice_->width * slice_->resolution;
+ const double current_plugin_max_y = current_plugin_min_y + slice_->height * slice_->resolution;
+
+ if (current_plugin_min_x < *min_x) {
+ *min_x = current_plugin_min_x;
+ }
+ if (current_plugin_max_x > *max_x) {
+ *max_x = current_plugin_max_x;
+ }
+ if (current_plugin_min_y < *min_y) {
+ *min_y = current_plugin_min_y;
+ }
+ if (current_plugin_max_y > *max_y) {
+ *max_y = current_plugin_max_y;
+ }
}
auto node = node_.lock();
diff --git a/nvblox_performance_measurement/README.md b/nvblox_performance_measurement/README.md
new file mode 100644
index 00000000..ffb42a8c
--- /dev/null
+++ b/nvblox_performance_measurement/README.md
@@ -0,0 +1,63 @@
+# nvblox_ros Performance Measurement
+
+The nvblox core library comes equipped with timers and nvtx ranges which provide a means of measuring performance. However, to get an accurate picture of nvblox performance on a robot, we would like to measure its performance inside a ROS 2 system.
+
+For this purpose, we have tools for measuring performance in this subfolder.
+
+The setup is indicated below:
+
+ - We wrap the `nvblox_ros` node in a layer which exposes further information, specifically about which input messages are added to the reconstruction.
+ - The `nvblox_performance` node subscribes to input and output messages and records their timestamps.
+ - We also monitor the CPU percentage taken by the `nvblox_ros` node.
+
+
