diff --git a/README.md b/README.md index 560dac5..984a82e 100644 --- a/README.md +++ b/README.md @@ -51,17 +51,19 @@ The following are the benchmark performance results of the prepared pipelines in - [Updates](#updates) ## Latest Update -Update 2022-06-30: Initial release +Update 2022-08-31: Update to use latest model and to be compatible with JetPack 5.0.2 ## 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. +> **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. + | 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.1 DP](https://developer.nvidia.com/embedded/jetpack) | For best performance, ensure that [power settings](https://docs.nvidia.com/jetson/archives/r34.1/DeveloperGuide/text/SD/PlatformPowerAndPerformance.html) are configured appropriately. | +| 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) | @@ -140,16 +142,17 @@ To simplify development, we strongly recommend leveraging the Isaac ROS Dev Dock ```bash ros2 launch isaac_ros_bi3d isaac_ros_bi3d.launch.py featnet_engine_file_path:=/tmp/models/bi3d/bi3dnet_featnet.plan \ segnet_engine_file_path:=/tmp/models/bi3d/bi3dnet_segnet.plan \ - disparity_values:=[18] + max_disparity_values:=10 ``` 10. Visualize and validate the output of the package: ```bash - ros2 run isaac_ros_bi3d isaac_ros_bi3d_visualizer.py --max_disparity_value 18 \ - --rosbag_path /workspaces/isaac_ros-dev/src/isaac_ros_proximity_segmentation/resources/rosbags/bi3dnode_rosbag/ + ros2 run isaac_ros_bi3d isaac_ros_bi3d_visualizer.py \ + --rosbag_path /workspaces/isaac_ros-dev/src/isaac_ros_proximity_segmentation/resources/rosbags/bi3dnode_rosbag/ \ + --max_disparity_value 50 ``` -
Visualizer Output
+
Visualizer Output
## Next Steps ### Try More Examples @@ -211,7 +214,7 @@ Bi3D predicts if an obstacle is within a given proximity field via a series of b ```bash ros2 launch isaac_ros_bi3d isaac_ros_bi3d.launch.py featnet_engine_file_path:= \ segnet_engine_file_path:= + max_disparity_values:= ``` #### Interpreting the Output @@ -224,23 +227,24 @@ The prediction of freespace eliminates the need for ground plane removal in the #### ROS Parameters -| ROS Parameter | Type | Default | Description | -| -------------------------- | -------------- | -------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `featnet_engine_file_path` | `string` | `'path_to_featnet_engine'` | The path to the Bi3D Featnet engine plan | -| `segnet_engine_file_path` | `string` | `'path_to_segnet_engine'` | The path to the Bi3D Segnet engine plan | -| `disparity_values` | `integer list` | `[18]` | A list of disparity values for Bi3D inference. Disparity values represent the distance a point moves from one image to the other in a stereo pair. Thus, they are inversely proportional to the distance from the camera. See [Example Usage of Isaac ROS Proximity Segmentation](./docs/bi3d-example.md) for an example of how to convert from the depth to the disparity value. Isaac ROS Proximity Segmentation supports up to a theoretical maximum of 64 disparity values during inference. However, the maximum length of disparities that a user may run in practice is dependent on the user's hardware and availability of memory. | +| ROS Parameter | Type | Default | Description | +| -------------------------- | --------- | -------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `featnet_engine_file_path` | `string` | `'path_to_featnet_engine'` | The path to the Bi3D Featnet engine plan | +| `segnet_engine_file_path` | `string` | `'path_to_segnet_engine'` | The path to the Bi3D Segnet engine plan | +| `max_disparity_values` | `integer` | `64` | The maximum number of disparity values used for Bi3D inference. Isaac ROS Proximity Segmentation supports up to a theoretical maximum of 64 disparity values during inference. However, the maximum length of disparities that a user may run in practice is dependent on the user's hardware and availability of memory. | #### ROS Topics Subscribed -| ROS Topic | Interface | Description | -| ------------------ | ---------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `left_image_bi3d` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | 1. The message must be a ROS `sensor_msgs/Image` of size **W=960, H=576** with `rgb8` image encoding.

2. The frame ID must be `left_image`.

3. There should only be a single publisher node publishing to `left_image_bi3d`. Timing behaviour with multiple publishers is not guaranteed by Bi3DNode
and inference may not be performed on correct image pairs. Bi3D will process input pairs on a first available basis. Use a separate instance of Bi3DNode for
each unique scene (publisher) that you wish to process. | -| `right_image_bi3d` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | 1. The message must be a ROS `sensor_msgs/Image` of size **W=960, H=576** with `rgb8` image encoding.

2. The frame ID must be `right_image`.

3. There should only be a single publisher node publishing to `right_image_bi3d`. Timing behaviour with multiple publishers is not guaranteed by Bi3DNode
and inference may not be performed on correct image pairs. Bi3D will process inputs pairs on a first available basis. Use a separate instance of Bi3DNode for
each unique scene (publisher) that you wish to process. | +| ROS Topic | Interface | Description | +| ----------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `left_image_bi3d` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | 1. The message must be a ROS `sensor_msgs/Image` of size **W=960, H=576** with `rgb8` image encoding.

2. There should only be a single publisher node publishing to `left_image_bi3d`. Timing behaviour with multiple publishers is not guaranteed by Bi3DNode and inference may not be performed on correct image pairs. Bi3D will process input pairs on a first available basis. Use a separate instance of Bi3DNode for each unique scene (publisher) that you wish to process. | +| `right_image_bi3d` | [sensor_msgs/Image](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Image.msg) | 1. The message must be a ROS `sensor_msgs/Image` of size **W=960, H=576** with `rgb8` image encoding.

2. There should only be a single publisher node publishing to `right_image_bi3d`. Timing behaviour with multiple publishers is not guaranteed by Bi3DNode and inference may not be performed on correct image pairs. Bi3D will process inputs pairs on a first available basis. Use a separate instance of Bi3DNode for each unique scene (publisher) that you wish to process. | +| `bi3d_disparity_values` | [isaac_ros_bi3d_interfaces/Bi3DInferenceParametersArray](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_bi3d_interfaces/msg/Bi3DInferenceParametersArray.msg) | The message containing the disparity values used for Bi3D inference. The disparity values may be dynamic and change between subsequent messages. The number of disparity values must not exceed the value set in the `max_disparity_values` ROS parameter. | #### ROS Topics Published -| ROS Topic | Interface | Description | -| ----------------------- | ---------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `bi3d_node/bi3d_output` | [stereo_msgs/DisparityImage](https://github.com/ros2/common_interfaces/blob/humble/stereo_msgs/msg/DisparityImage.msg) | The proximity segmentation of Bi3D given as a disparity image. For pixels not deemed freespace, their value is set to the closest (largest) disparity plane that is breached. A pixel value is set to 0 if it doesn't breach any disparity plane or if it is freespace.

Output Resolution: 960x576 (WxH) | - +| ROS Topic | Interface | Description | +| --------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `bi3d_node/bi3d_output` | [stereo_msgs/DisparityImage](https://github.com/ros2/common_interfaces/blob/humble/stereo_msgs/msg/DisparityImage.msg) | The proximity segmentation of Bi3D given as a disparity image. For pixels not deemed freespace, their value is set to the closest (largest) disparity plane that is breached. A pixel value is set to 0 if it doesn't breach any disparity plane or if it is freespace.

Output Resolution: 960x576 (WxH) | +| `bi3d_node/bi3d_disparity_values` | [isaac_ros_bi3d_interfaces/Bi3DInferenceParametersArray](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_bi3d_interfaces/msg/Bi3DInferenceParametersArray.msg) | The disparity values used for Bi3D inference. The timestamp is matched to the timestamp in the correpsonding output image from `bi3d_node/bi3d_output` | ## Troubleshooting ### Isaac ROS Troubleshooting @@ -251,6 +255,7 @@ For solutions to problems with using DNN models and Triton, please check [here]( ## Updates -| Date | Changes | -| ---------- | --------------- | -| 2022-06-30 | Initial release | +| Date | Changes | +| ---------- | ------------------------------------------------------------------ | +| 2022-08-31 | Update to use latest model and to be compatible with JetPack 5.0.2 | +| 2022-06-30 | Initial release | diff --git a/isaac_ros_bi3d/CMakeLists.txt b/isaac_ros_bi3d/CMakeLists.txt index 22e5ccd..fee4445 100644 --- a/isaac_ros_bi3d/CMakeLists.txt +++ b/isaac_ros_bi3d/CMakeLists.txt @@ -50,11 +50,11 @@ add_library(bi3d_postprocess SHARED IMPORTED) if(${ARCHITECTURE} STREQUAL "x86_64") set_property( TARGET bi3d_postprocess PROPERTY IMPORTED_LOCATION - ${CMAKE_CURRENT_SOURCE_DIR}/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocess.so) + ${CMAKE_CURRENT_SOURCE_DIR}/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocessor.so) elseif(${ARCHITECTURE} STREQUAL "aarch64") set_property( TARGET bi3d_postprocess PROPERTY IMPORTED_LOCATION - ${CMAKE_CURRENT_SOURCE_DIR}/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocess.so) + ${CMAKE_CURRENT_SOURCE_DIR}/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocessor.so) endif() # bi3d_node diff --git a/isaac_ros_bi3d/config/bi3d_node.yaml b/isaac_ros_bi3d/config/bi3d_node.yaml index 4b6db56..1d38b1d 100644 --- a/isaac_ros_bi3d/config/bi3d_node.yaml +++ b/isaac_ros_bi3d/config/bi3d_node.yaml @@ -6,13 +6,14 @@ # and any modifications thereto. Any use, reproduction, disclosure or # distribution of this software and related documentation without an express # license agreement from NVIDIA CORPORATION is strictly prohibited. +################################################## --- name: sync components: - name: data_receiver_left type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 12 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: @@ -21,7 +22,7 @@ components: - name: data_receiver_right type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 12 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: @@ -100,17 +101,49 @@ components: parameters: capacity: 1 --- +name: disparity_roundrobin +components: +- name: data_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: data_receiver + min_size: 1 +- type: nvidia::gxf::Broadcast + parameters: + source: data_receiver + mode: RoundRobin +- name: data_transmitter_dla0 + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: data_transmitter_dla0 + min_size: 1 +- name: data_transmitter_dla1 + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: data_transmitter_dla1 + min_size: 1 +--- name: gather components: - name: input_dla0 type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 12 + capacity: 6 policy: 0 - name: input_dla1 type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 12 + capacity: 6 policy: 0 - type: nvidia::gxf::MultiMessageAvailableSchedulingTerm parameters: @@ -148,6 +181,15 @@ components: parameters: receiver: data_receiver_right min_size: 1 +- name: data_receiver_disparity + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 6 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: data_receiver_disparity + min_size: 1 - name: data_transmitter type: nvidia::gxf::DoubleBufferTransmitter parameters: @@ -161,7 +203,13 @@ components: parameters: storage_type: 1 block_size: 2211840 - num_blocks: 50 + num_blocks: 12 +- name: forward_pool + type: nvidia::gxf::BlockMemoryPool + parameters: + storage_type: 0 + block_size: 256 + num_blocks: 12 - name: stream type: nvidia::gxf::CudaStreamPool parameters: @@ -176,10 +224,12 @@ components: right_image_name: "right_image" output_name: "disparity" pool: pool + forward_pool: forward_pool stream_pool: stream left_image_receiver: data_receiver_left right_image_receiver: data_receiver_right output_transmitter: data_transmitter + disparity_receiver: data_receiver_disparity image_type: "RGB_U8" pixel_mean: [0.0078431372549, 0.0078431372549, 0.0078431372549] @@ -206,7 +256,6 @@ components: threshold_value_low: 0.0 threshold_value_high: 1.0 max_disparity_levels: 64 - disparity_values: [18] --- name: bi3d_dla1 components: @@ -228,6 +277,15 @@ components: parameters: receiver: data_receiver_right min_size: 1 +- name: data_receiver_disparity + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 6 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: data_receiver_disparity + min_size: 1 - name: data_transmitter type: nvidia::gxf::DoubleBufferTransmitter parameters: @@ -241,7 +299,13 @@ components: parameters: storage_type: 1 block_size: 2211840 - num_blocks: 50 + num_blocks: 12 +- name: forward_pool + type: nvidia::gxf::BlockMemoryPool + parameters: + storage_type: 0 + block_size: 256 + num_blocks: 12 - name: stream type: nvidia::gxf::CudaStreamPool parameters: @@ -256,10 +320,12 @@ components: right_image_name: "right_image" output_name: "disparity" pool: pool + forward_pool: forward_pool stream_pool: stream left_image_receiver: data_receiver_left right_image_receiver: data_receiver_right output_transmitter: data_transmitter + disparity_receiver: data_receiver_disparity image_type: "RGB_U8" pixel_mean: [0.0078431372549, 0.0078431372549, 0.0078431372549] @@ -286,7 +352,6 @@ components: threshold_value_low: 0.0 threshold_value_high: 1.0 max_disparity_levels: 64 - disparity_values: [18] --- name: sync_bi3d_dla0 components: @@ -343,7 +408,7 @@ components: parameters: storage_type: 1 block_size: 2211840 - num_blocks: 50 + num_blocks: 24 - name: data_receiver type: nvidia::gxf::DoubleBufferReceiver parameters: @@ -361,12 +426,46 @@ components: parameters: transmitter: data_transmitter min_size: 1 -- type: nvidia::isaac_ros::Bi3DPostprocess +- type: nvidia::isaac_ros::Bi3DPostprocessor parameters: bi3d_receiver: data_receiver output_transmitter: data_transmitter pool: pool - disparity_values: [18] + disparity_tensor_name: "disparity" + disparity_values_tensor_name: "bi3d_inference_disparities" +--- +name: bi3d_output_message_splitter +components: +- name: data_receiver + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: data_receiver + min_size: 1 +- name: disparity_image_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 12 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: disparity_image_transmitter + min_size: 1 +- name: disparity_values_transmitter + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 12 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: disparity_values_transmitter + min_size: 1 +- type: nvidia::isaac_ros::Bi3DMessageSplitter + parameters: + receiver: data_receiver + disparity_image_transmitter: disparity_image_transmitter + disparity_values_transmitter: disparity_values_transmitter --- name: bi3d_output_vault components: @@ -386,6 +485,24 @@ components: max_waiting_count: 1 drop_waiting: false --- +name: disparity_values_output_vault +components: +- name: input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: input + min_size: 1 +- name: vault + type: nvidia::gxf::Vault + parameters: + source: input + max_waiting_count: 1 + drop_waiting: false +--- components: - name: connection type: nvidia::gxf::Connection @@ -415,6 +532,20 @@ components: target: bi3d_dla0/data_receiver_right --- components: +- name: connection + type: nvidia::gxf::Connection + parameters: + source: disparity_roundrobin/data_transmitter_dla0 + target: bi3d_dla0/data_receiver_disparity +--- +components: +- name: connection + type: nvidia::gxf::Connection + parameters: + source: disparity_roundrobin/data_transmitter_dla1 + target: bi3d_dla1/data_receiver_disparity +--- +components: - name: connection type: nvidia::gxf::Connection parameters: @@ -468,8 +599,22 @@ components: type: nvidia::gxf::Connection parameters: source: bi3d_postprocess/data_transmitter + target: bi3d_output_message_splitter/data_receiver +--- +components: +- name: connection + type: nvidia::gxf::Connection + parameters: + source: bi3d_output_message_splitter/disparity_image_transmitter target: bi3d_output_vault/input --- +components: +- name: connection + type: nvidia::gxf::Connection + parameters: + source: bi3d_output_message_splitter/disparity_values_transmitter + target: disparity_values_output_vault/input +--- name: scheduler components: - name: clock @@ -480,5 +625,3 @@ components: worker_thread_number: 8 stop_on_deadlock: false - - diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocess.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocess.so deleted file mode 100755 index 8f6b901..0000000 --- a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocess.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3b0b7f74e03bcbbb930083cbc7142be9b056aed2826d745584191ce322ae31e9 -size 190272 diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocessor.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocessor.so new file mode 100755 index 0000000..cfbc443 --- /dev/null +++ b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_bi3d_postprocessor.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c1c0238952b11a230afd491b9dfef8882522ab2623e490fa2cb57a66a643b849 +size 220688 diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_cvcore_bi3d.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_cvcore_bi3d.so index c024095..1bdf6c4 100755 --- a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_cvcore_bi3d.so +++ b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libgxf_cvcore_bi3d.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:5f0e347f150ce2d50ea66e3a87f2f509c65f321b23e83a0527c9719ffce5aa03 -size 2141032 +oid sha256:066f2da3f369df89e1a4954a549de96d2bce9ef5649cd34ed06e773582c2dd56 +size 2150416 diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libnvcv_tensorops.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libnvcv_tensorops.so index 3cb9317..e86c2bb 100755 --- a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libnvcv_tensorops.so +++ b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/jetpack50/libnvcv_tensorops.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c26b35a91f9ce9b8aa416d33ce57227c59dfa876d89f85248b19006d9cc4303e -size 295824 +oid sha256:a94e2f10048dfefdfec3c084b42fb42c17e7c6eaa7dd5078d5904fa6c1511888 +size 295992 diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocess.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocess.so deleted file mode 100755 index a5db14a..0000000 --- a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocess.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4be3d5a571ed6fd727e4c76b1fbbb20845121e835a76d7bd5320bf0a493720ef -size 206312 diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocessor.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocessor.so new file mode 100755 index 0000000..7fb27cf --- /dev/null +++ b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_bi3d_postprocessor.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c035c6b4e4f90c3883d3fb897828d3a13a64db71b61b10d5e08b0864c50796a2 +size 234576 diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_cvcore_bi3d.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_cvcore_bi3d.so index e0b524c..723edf3 100755 --- a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_cvcore_bi3d.so +++ b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libgxf_cvcore_bi3d.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3ae494685b9c6bf8bcf09258426f65ff65c628eee20a618d7c000dd699d80719 -size 2288168 +oid sha256:da4da8b56d785e4d18ebfb7535186263a21d340a8716aca8681c7f4d1e914af4 +size 2297368 diff --git a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libnvcv_tensorops.so b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libnvcv_tensorops.so index ac24091..cb5cf1c 100755 --- a/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libnvcv_tensorops.so +++ b/isaac_ros_bi3d/cvcore/cvcore_bi3d/lib/x86_64/libnvcv_tensorops.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:68ba19823b3c14453f901a3532cafe577897f80b5434dbda85fc7f26f426b0b4 -size 313608 +oid sha256:d3ab1d035950c7592564efcf5e8669da48c24e9f00ce929bc121d87ed35a7df9 +size 313776 diff --git a/isaac_ros_bi3d/include/isaac_ros_bi3d/bi3d_node.hpp b/isaac_ros_bi3d/include/isaac_ros_bi3d/bi3d_node.hpp index 6d1f86d..5d0e33f 100644 --- a/isaac_ros_bi3d/include/isaac_ros_bi3d/bi3d_node.hpp +++ b/isaac_ros_bi3d/include/isaac_ros_bi3d/bi3d_node.hpp @@ -54,7 +54,7 @@ class Bi3DNode : public nitros::NitrosNode const std::vector segnet_output_layers_name_; // Bi3D extra parameters - std::vector disparity_values_; + int64_t max_disparity_values_; }; } // namespace bi3d diff --git a/isaac_ros_bi3d/launch/isaac_ros_bi3d.launch.py b/isaac_ros_bi3d/launch/isaac_ros_bi3d.launch.py index c46583b..2d06857 100644 --- a/isaac_ros_bi3d/launch/isaac_ros_bi3d.launch.py +++ b/isaac_ros_bi3d/launch/isaac_ros_bi3d.launch.py @@ -24,15 +24,15 @@ def generate_launch_description(): default_value='', description='The absolute path to the Bi3D Segnet TensorRT engine plan'), DeclareLaunchArgument( - 'disparity_values', - default_value='[18]', - description='The disparity values (in px) used for Bi3D inference'), + 'max_disparity_values', + default_value='64', + description='The maximum number of disparity values given for Bi3D inference'), ] # Bi3DNode parameters featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') - disparity_values = LaunchConfiguration('disparity_values') + max_disparity_values = LaunchConfiguration('max_disparity_values') bi3d_node = ComposableNode( name='bi3d_node', @@ -41,7 +41,7 @@ def generate_launch_description(): parameters=[{ 'featnet_engine_file_path': featnet_engine_file_path, 'segnet_engine_file_path': segnet_engine_file_path, - 'disparity_values': disparity_values}]) + 'max_disparity_values': max_disparity_values}]) container = ComposableNodeContainer( name='bi3d_container', diff --git a/isaac_ros_bi3d/package.xml b/isaac_ros_bi3d/package.xml index b520e2f..be45f4c 100644 --- a/isaac_ros_bi3d/package.xml +++ b/isaac_ros_bi3d/package.xml @@ -13,7 +13,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. isaac_ros_bi3d - 0.10.0 + 0.11.0 Bi3D inference network for ROS Hemal Shah @@ -26,11 +26,15 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. rclcpp rclcpp_components isaac_ros_nitros + isaac_ros_nitros_bi3d_inference_param_array_type + isaac_ros_nitros_disparity_image_type + isaac_ros_nitros_image_type isaac_ros_stereo_image_proc ament_lint_auto ament_lint_common isaac_ros_test + isaac_ros_bi3d_interfaces ament_cmake diff --git a/isaac_ros_bi3d/src/bi3d_node.cpp b/isaac_ros_bi3d/src/bi3d_node.cpp index d075ba1..77665ee 100644 --- a/isaac_ros_bi3d/src/bi3d_node.cpp +++ b/isaac_ros_bi3d/src/bi3d_node.cpp @@ -17,6 +17,11 @@ #include #include +#include \ + "isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp" +#include "isaac_ros_nitros_disparity_image_type/nitros_disparity_image.hpp" +#include "isaac_ros_nitros_image_type/nitros_image.hpp" + #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" @@ -36,10 +41,18 @@ constexpr char INPUT_LEFT_IMAGE_TOPIC_NAME[] = "left_image_bi3d"; constexpr char INPUT_RIGHT_IMAGE_COMPONENT_KEY[] = "sync/data_receiver_right"; constexpr char INPUT_RIGHT_IMAGE_TOPIC_NAME[] = "right_image_bi3d"; +constexpr char INPUT_DISPARITY_COMPONENT_KEY[] = "disparity_roundrobin/data_receiver"; +constexpr char INPUT_DISPARITY_DEFAULT_TENSOR_FORMAT[] = "nitros_bi3d_inference_param_array"; +constexpr char INPUT_DISPARITY_TOPIC_NAME[] = "bi3d_disparity_values"; + constexpr char OUTPUT_BI3D_KEY[] = "bi3d_output_vault/vault"; constexpr char OUTPUT_BI3D_DEFAULT_TENSOR_FORMAT[] = "nitros_disparity_image_32FC1"; constexpr char OUTPUT_BI3D_TOPIC_NAME[] = "bi3d_node/bi3d_output"; +constexpr char OUTPUT_DISPARITY_KEY[] = "disparity_values_output_vault/vault"; +constexpr char OUTPUT_DISPARITY_DEFAULT_TENSOR_FORMAT[] = "nitros_bi3d_inference_param_array"; +constexpr char OUTPUT_DISPARITY_TOPIC_NAME[] = "bi3d_node/bi3d_disparity_values"; + constexpr char APP_YAML_FILENAME[] = "config/bi3d_node.yaml"; constexpr char PACKAGE_NAME[] = "isaac_ros_bi3d"; @@ -51,7 +64,7 @@ const std::vector> EXTENSIONS = { {"isaac_ros_nitros", "gxf/multimedia/libgxf_multimedia.so"}, {"isaac_ros_stereo_image_proc", "lib/libgxf_synchronization.so"}, {"isaac_ros_bi3d", "cvcore_bi3d/libgxf_cvcore_bi3d.so"}, - {"isaac_ros_bi3d", "cvcore_bi3d/libgxf_bi3d_postprocess.so"} + {"isaac_ros_bi3d", "cvcore_bi3d/libgxf_bi3d_postprocessor.so"} }; const std::vector PRESET_EXTENSION_SPEC_NAMES = { "isaac_ros_bi3d", @@ -64,7 +77,7 @@ const nitros::NitrosPublisherSubscriberConfigMap CONFIG_MAP = { {INPUT_LEFT_IMAGE_COMPONENT_KEY, { .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, - .qos = rclcpp::QoS(1), + .qos = rclcpp::QoS(10), .compatible_data_format = INPUT_IMAGE_DEFAULT_TENSOR_FORMAT, .topic_name = INPUT_LEFT_IMAGE_TOPIC_NAME, } @@ -72,18 +85,35 @@ const nitros::NitrosPublisherSubscriberConfigMap CONFIG_MAP = { {INPUT_RIGHT_IMAGE_COMPONENT_KEY, { .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, - .qos = rclcpp::QoS(1), + .qos = rclcpp::QoS(10), .compatible_data_format = INPUT_IMAGE_DEFAULT_TENSOR_FORMAT, .topic_name = INPUT_RIGHT_IMAGE_TOPIC_NAME, } }, + {INPUT_DISPARITY_COMPONENT_KEY, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(10), + .compatible_data_format = INPUT_DISPARITY_DEFAULT_TENSOR_FORMAT, + .topic_name = INPUT_DISPARITY_TOPIC_NAME, + } + }, {OUTPUT_BI3D_KEY, { .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, - .qos = rclcpp::QoS(1), + .qos = rclcpp::QoS(10), .compatible_data_format = OUTPUT_BI3D_DEFAULT_TENSOR_FORMAT, .topic_name = OUTPUT_BI3D_TOPIC_NAME, - .frame_id_source_key = INPUT_LEFT_IMAGE_COMPONENT_KEY + .frame_id_source_key = INPUT_LEFT_IMAGE_COMPONENT_KEY, + } + }, + {OUTPUT_DISPARITY_KEY, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(10), + .compatible_data_format = OUTPUT_DISPARITY_DEFAULT_TENSOR_FORMAT, + .topic_name = OUTPUT_DISPARITY_TOPIC_NAME, + .frame_id_source_key = INPUT_DISPARITY_COMPONENT_KEY, } }, }; @@ -150,7 +180,7 @@ Bi3DNode::Bi3DNode(const rclcpp::NodeOptions & options) {"278"})), // Bi3D extra parameters - disparity_values_(declare_parameter>("disparity_values", {18})) + max_disparity_values_(declare_parameter("max_disparity_values", 64)) { RCLCPP_DEBUG(get_logger(), "[Bi3DNode] Initializing Bi3DNode"); @@ -168,6 +198,11 @@ Bi3DNode::Bi3DNode(const rclcpp::NodeOptions & options) std::bind( &Bi3DNode::Bi3DVideoBufferNameCallback, this, std::placeholders::_1, std::placeholders::_2, "right_image"); + + registerSupportedType(); + registerSupportedType(); + registerSupportedType(); + startNitrosNode(); } @@ -203,24 +238,16 @@ void Bi3DNode::postLoadGraphCallback() getNitrosContext().setParameter1DStrVector( "bi3d_dla1", "nvidia::cvcore::Bi3D", "segnet_output_layers_name", segnet_output_layers_name_); - // Bi3D extra parameters - std::vector dispVals(begin(disparity_values_), end(disparity_values_)); - sort(dispVals.begin(), dispVals.end()); - getNitrosContext().setParameter1DInt32Vector( - "bi3d_dla0", "nvidia::cvcore::Bi3D", "disparity_values", dispVals); - getNitrosContext().setParameter1DInt32Vector( - "bi3d_dla1", "nvidia::cvcore::Bi3D", "disparity_values", dispVals); - getNitrosContext().setParameter1DInt32Vector( - "bi3d_postprocess", "nvidia::isaac_ros::Bi3DPostprocess", "disparity_values", dispVals); - - // Set Bi3D block memory size depending on number of output disparities + // Set Bi3D block memory size depending on maximum number of output disparities getNitrosContext().setParameterUInt64( - "bi3d_dla0", "nvidia::gxf::BlockMemoryPool", "block_size", BI3D_BLOCK_SIZE * dispVals.size()); + "bi3d_dla0", "nvidia::gxf::BlockMemoryPool", "block_size", + BI3D_BLOCK_SIZE * max_disparity_values_); getNitrosContext().setParameterUInt64( - "bi3d_dla1", "nvidia::gxf::BlockMemoryPool", "block_size", BI3D_BLOCK_SIZE * dispVals.size()); + "bi3d_dla1", "nvidia::gxf::BlockMemoryPool", "block_size", + BI3D_BLOCK_SIZE * max_disparity_values_); getNitrosContext().setParameterUInt64( "bi3d_postprocess", "nvidia::gxf::BlockMemoryPool", "block_size", - BI3D_BLOCK_SIZE * dispVals.size()); + BI3D_BLOCK_SIZE * max_disparity_values_); RCLCPP_INFO( get_logger(), "[Bi3DNode] Setting featnet_engine_file_path: %s.", diff --git a/isaac_ros_bi3d/test/isaac_ros_bi3d_test.py b/isaac_ros_bi3d/test/isaac_ros_bi3d_test.py index 2828082..edba713 100644 --- a/isaac_ros_bi3d/test/isaac_ros_bi3d_test.py +++ b/isaac_ros_bi3d/test/isaac_ros_bi3d_test.py @@ -10,6 +10,7 @@ import subprocess import time +from isaac_ros_bi3d_interfaces.msg import Bi3DInferenceParametersArray from isaac_ros_test import IsaacROSBaseTest from launch_ros.actions import ComposableNodeContainer @@ -74,7 +75,7 @@ def generate_test_description(): 'segnet_engine_file_path': segnet_engine_file_path, 'featnet_output_layers_name': ['97'], 'segnet_output_layers_name': ['294'], - 'disparity_values': [18]}] + 'max_disparity_values': 1}] ) container = ComposableNodeContainer( @@ -108,6 +109,11 @@ def _create_image(self, name): image.header.frame_id = name return image + def _create_disparity_value_array(self): + disp_vals = Bi3DInferenceParametersArray() + disp_vals.disparity_values = [18] + return disp_vals + def test_image_bi3d(self): end_time = time.time() + self.TIMEOUT while time.time() < end_time: @@ -124,7 +130,7 @@ def test_image_bi3d(self): received_messages = {} self.generate_namespace_lookup(['left_image_bi3d', 'right_image_bi3d', - 'bi3d_node/bi3d_output']) + 'bi3d_disparity_values', 'bi3d_node/bi3d_output']) subs = self.create_logging_subscribers( [('bi3d_node/bi3d_output', DisparityImage)], received_messages) @@ -135,10 +141,15 @@ def test_image_bi3d(self): image_right_pub = self.node.create_publisher( Image, self.namespaces['right_image_bi3d'], self.DEFAULT_QOS ) + disparity_values_pub = self.node.create_publisher( + Bi3DInferenceParametersArray, self.namespaces['bi3d_disparity_values'], + self.DEFAULT_QOS + ) try: left_image = self._create_image('left_image') right_image = self._create_image('right_image') + disparity_values = self._create_disparity_value_array() end_time = time.time() + self.TIMEOUT done = False @@ -146,6 +157,7 @@ def test_image_bi3d(self): while time.time() < end_time: image_left_pub.publish(left_image) image_right_pub.publish(right_image) + disparity_values_pub.publish(disparity_values) rclpy.spin_once(self.node, timeout_sec=0.1) diff --git a/isaac_ros_nitros_bi3d_inference_param_array_type/CMakeLists.txt b/isaac_ros_nitros_bi3d_inference_param_array_type/CMakeLists.txt new file mode 100644 index 0000000..f62acd5 --- /dev/null +++ b/isaac_ros_nitros_bi3d_inference_param_array_type/CMakeLists.txt @@ -0,0 +1,90 @@ +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +cmake_minimum_required(VERSION 3.8) +project(isaac_ros_nitros_bi3d_inference_param_array_type LANGUAGES C CXX) + + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +# Default to Release build +if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) +endif() +message( STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}" ) + +execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) +message( STATUS "Architecture: ${ARCHITECTURE}" ) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +# for #include +find_package(CUDA REQUIRED) +include_directories("${CUDA_INCLUDE_DIRS}") + +option(USE_NVTX "Enable NVTX markers for improved profiling (if available)" ON) +if(USE_NVTX) + add_definitions(-DUSE_NVTX) + link_directories("${CUDA_TOOLKIT_ROOT_DIR}/lib64") + link_libraries("nvToolsExt") +endif() + + +# NitrosBi3DInferenceParamArray +ament_auto_add_library(${PROJECT_NAME} SHARED + src/nitros_bi3d_inference_param_array.cpp) +target_compile_definitions(${PROJECT_NAME} + PRIVATE "COMPOSITION_BUILDING_DLL" +) +target_link_libraries(${PROJECT_NAME}) +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) + +if(BUILD_TESTING) + # NitrosBi3DInferenceParamArrayForwardNode + ament_auto_add_library(nitros_bi3d_inference_param_array_forward_node + SHARED test/src/nitros_bi3d_inference_param_array_forward_node.cpp) + target_compile_definitions(nitros_bi3d_inference_param_array_forward_node + PRIVATE "COMPOSITION_BUILDING_DLL" + ) + target_link_libraries(nitros_bi3d_inference_param_array_forward_node ${PROJECT_NAME}) + rclcpp_components_register_nodes(nitros_bi3d_inference_param_array_forward_node + "nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode") + set(node_plugins "${node_plugins}nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode;\ + $\n") + # Install test/config directory + install( + DIRECTORY test/config + DESTINATION share/${PROJECT_NAME}/test + ) + + find_package(ament_lint_auto REQUIRED) + + # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License + set(ament_cmake_copyright_FOUND TRUE) + + ament_lint_auto_find_test_dependencies() + + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/isaac_ros_nitros_bi3d_inference_param_array_type_test_pol.py TIMEOUT "15") +endif() + +ament_auto_package() diff --git a/isaac_ros_nitros_bi3d_inference_param_array_type/include/isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp b/isaac_ros_nitros_bi3d_inference_param_array_type/include/isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp new file mode 100644 index 0000000..faa1d05 --- /dev/null +++ b/isaac_ros_nitros_bi3d_inference_param_array_type/include/isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp @@ -0,0 +1,86 @@ +/** + * Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#ifndef ISAAC_ROS_NITROS_BI3D_INFERENCE_PARAM_ARRAY_TYPE__NITROS_BI3D_INFERENCE_PARAM_ARRAY_HPP_ +#define ISAAC_ROS_NITROS_BI3D_INFERENCE_PARAM_ARRAY_TYPE__NITROS_BI3D_INFERENCE_PARAM_ARRAY_HPP_ +/* + * Type adaptation for: + * Nitros type: NitrosBi3DInferenceParamArray + * ROS type: isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray + */ + +#include + +#include "isaac_ros_nitros/types/nitros_format_agent.hpp" +#include "isaac_ros_nitros/types/nitros_type_base.hpp" +#include "isaac_ros_bi3d_interfaces/msg/bi3_d_inference_parameters_array.hpp" + + +#include "rclcpp/type_adapter.hpp" + + +namespace nvidia +{ +namespace isaac_ros +{ +namespace nitros +{ + +// Type +struct NitrosBi3DInferenceParamArray; + +// Formats +struct nitros_bi3d_inference_param_array_t +{ + using MsgT = NitrosBi3DInferenceParamArray; + static const inline std::string supported_type_name = "nitros_bi3d_inference_param_array"; +}; + +// NITROS data type registration factory +NITROS_TYPE_FACTORY_BEGIN(NitrosBi3DInferenceParamArray) +// Supported data formats +NITROS_FORMAT_FACTORY_BEGIN() +NITROS_FORMAT_ADD(nitros_bi3d_inference_param_array_t) +NITROS_FORMAT_FACTORY_END() +// Required extensions +NITROS_TYPE_EXTENSION_FACTORY_BEGIN() +NITROS_TYPE_EXTENSION_ADD("isaac_ros_nitros", "gxf/std/libgxf_std.so") +NITROS_TYPE_EXTENSION_FACTORY_END() +NITROS_TYPE_FACTORY_END() + + +} // namespace nitros +} // namespace isaac_ros +} // namespace nvidia + + +template<> +struct rclcpp::TypeAdapter< + nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, + isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray> +{ + using is_specialized = std::true_type; + using custom_type = nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray; + using ros_message_type = isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray; + + static void convert_to_ros_message( + const custom_type & source, + ros_message_type & destination); + + static void convert_to_custom( + const ros_message_type & source, + custom_type & destination); +}; + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, + isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray); + +#endif // ISAAC_ROS_NITROS_BI3D_INFERENCE_PARAM_ARRAY_TYPE__NITROS_BI3D_INFERENCE_PARAM_ARRAY_HPP_ diff --git a/isaac_ros_nitros_bi3d_inference_param_array_type/package.xml b/isaac_ros_nitros_bi3d_inference_param_array_type/package.xml new file mode 100644 index 0000000..75c522f --- /dev/null +++ b/isaac_ros_nitros_bi3d_inference_param_array_type/package.xml @@ -0,0 +1,38 @@ + + + + + + + isaac_ros_nitros_bi3d_inference_param_array_type + 0.11.0 + Isaac ROS Nitros Bi3D Inferece Param Array Type + + Hemal Shah + NVIDIA Isaac ROS Software License + https://developer.nvidia.com/isaac-ros-gems/ + Peter Du + + ament_cmake + + rclcpp + rclcpp_components + isaac_ros_bi3d_interfaces + isaac_ros_nitros + + ament_lint_auto + ament_lint_common + isaac_ros_test + + + ament_cmake + + diff --git a/isaac_ros_nitros_bi3d_inference_param_array_type/src/nitros_bi3d_inference_param_array.cpp b/isaac_ros_nitros_bi3d_inference_param_array_type/src/nitros_bi3d_inference_param_array.cpp new file mode 100644 index 0000000..f33acb1 --- /dev/null +++ b/isaac_ros_nitros_bi3d_inference_param_array_type/src/nitros_bi3d_inference_param_array.cpp @@ -0,0 +1,245 @@ +/** + * Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#include + +#include +#include + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" +#pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#pragma GCC diagnostic ignored "-Wpedantic" +#include "gxf/core/entity.hpp" +#include "gxf/core/gxf.h" +#include "gxf/std/tensor.hpp" +#include "gxf/std/timestamp.hpp" +#pragma GCC diagnostic pop + +#include \ + "isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp" +#include "isaac_ros_nitros/types/type_adapter_nitros_context.hpp" + +#include "rclcpp/rclcpp.hpp" + + +constexpr char kEntityName[] = "memory_pool"; +constexpr char kComponentName[] = "unbounded_allocator"; +constexpr char kComponentTypeName[] = "nvidia::gxf::UnboundedAllocator"; + +void rclcpp::TypeAdapter< + nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, + isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray>::convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) +{ + nvidia::isaac_ros::nitros::nvtxRangePushWrapper( + "NitrosTensorList::convert_to_ros_message", + nvidia::isaac_ros::nitros::CLR_PURPLE); + + RCLCPP_DEBUG( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), + "[convert_to_ros_message] Conversion started for handle = %ld", source.handle); + + auto msg_entity = nvidia::gxf::Entity::Shared( + nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), source.handle); + auto gxf_tensor = msg_entity->get(); + + if (!gxf_tensor) { + std::string error_msg = + "[convert_to_ros_message] No tensor found: conversion from " + "gxf::Tensor to ROS Bi3D Inference Parameters Array failed!"; + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.c_str()); + throw std::runtime_error(error_msg.c_str()); + } + + // Move data from GXF tensor to ROS message + destination.disparity_values.resize(gxf_tensor.value()->size()); + switch (gxf_tensor.value()->storage_type()) { + case nvidia::gxf::MemoryStorageType::kHost: + { + std::memcpy( + destination.disparity_values.data(), gxf_tensor.value()->pointer(), + gxf_tensor.value()->size()); + } + break; + case nvidia::gxf::MemoryStorageType::kDevice: + { + const cudaError_t cuda_error = cudaMemcpy( + destination.disparity_values.data(), gxf_tensor.value()->pointer(), + gxf_tensor.value()->size(), cudaMemcpyDeviceToHost); + if (cuda_error != cudaSuccess) { + std::stringstream error_msg; + error_msg << + "[convert_to_ros_message] cudaMemcpy failed for conversion from " + "gxf::Tensor to ROS Bi3D Inference Parameters Array: " << + cudaGetErrorName(cuda_error) << + " (" << cudaGetErrorString(cuda_error) << ")"; + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + } + break; + default: + std::string error_msg = + "[convert_to_ros_message] MemoryStorageType not supported: conversion from " + "gxf::Tensor to ROS Bi3D Inference Parameters Array failed!"; + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.c_str()); + throw std::runtime_error(error_msg.c_str()); + } + + // Populate timestamp information back into ROS header + auto input_timestamp = msg_entity->get("timestamp"); + if (!input_timestamp) { // Fallback to label 'timestamp' + input_timestamp = msg_entity->get(); + } + if (input_timestamp) { + destination.header.stamp.sec = static_cast( + input_timestamp.value()->acqtime / static_cast(1e9)); + destination.header.stamp.nanosec = static_cast( + input_timestamp.value()->acqtime % static_cast(1e9)); + } + + // Set frame ID + destination.header.frame_id = source.frame_id; + + RCLCPP_DEBUG( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), + "[convert_to_ros_message] Conversion completed"); + + nvidia::isaac_ros::nitros::nvtxRangePopWrapper(); +} + + +void rclcpp::TypeAdapter< + nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, + isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray>::convert_to_custom( + const ros_message_type & source, custom_type & destination) +{ + nvidia::isaac_ros::nitros::nvtxRangePushWrapper( + "NitrosBi3DInferenceParamArray::convert_to_custom", + nvidia::isaac_ros::nitros::CLR_PURPLE); + + RCLCPP_DEBUG( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), + "[convert_to_custom] Conversion started"); + + // Get pointer to allocator component + gxf_uid_t cid; + nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid( + kEntityName, kComponentName, kComponentTypeName, cid); + + auto maybe_allocator_handle = + nvidia::gxf::Handle::Create( + nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), cid); + if (!maybe_allocator_handle) { + std::stringstream error_msg; + error_msg << + "[convert_to_custom] Failed to get allocator's handle: " << + GxfResultStr(maybe_allocator_handle.error()); + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + auto allocator_handle = maybe_allocator_handle.value(); + + auto message = nvidia::gxf::Entity::New( + nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext()); + if (!message) { + std::stringstream error_msg; + error_msg << + "[convert_to_custom] Error initializing new message entity: " << + GxfResultStr(message.error()); + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + + auto gxf_tensor = message->add("bi3d_inference_disparities"); + std::array dims = {static_cast(source.disparity_values.size())}; + + nvidia::gxf::Expected result; + + // Bi3D disparity value tensor must be on host + nvidia::gxf::MemoryStorageType storage_type = nvidia::gxf::MemoryStorageType::kHost; + + // Initializing GXF tensor + nvidia::gxf::PrimitiveType type = + static_cast(nvidia::gxf::PrimitiveType::kInt32); + result = gxf_tensor.value()->reshape( + nvidia::gxf::Shape(dims, 1), storage_type, allocator_handle); + + if (!result) { + std::stringstream error_msg; + error_msg << + "[convert_to_custom] Error initializing GXF tensor of type " << + static_cast(type) << ": " << + GxfResultStr(result.error()); + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + + // Sort disparity values data + std::vector sorted_data(begin(source.disparity_values), end(source.disparity_values)); + sort(sorted_data.begin(), sorted_data.end()); + + // Copy to GXF tensor + const cudaMemcpyKind operation = cudaMemcpyHostToHost; + const cudaError_t cuda_error = cudaMemcpy( + gxf_tensor.value()->pointer(), + sorted_data.data(), + gxf_tensor.value()->size(), + operation); + if (cuda_error != cudaSuccess) { + std::stringstream error_msg; + error_msg << + "[convert_to_custom] cudaMemcpy failed for copying data from " + "Bi3D ROS Float Array to GXF Tensor: " << + cudaGetErrorName(cuda_error) << + " (" << cudaGetErrorString(cuda_error) << ")"; + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + + // Add timestamp to the message + uint64_t input_timestamp = + source.header.stamp.sec * static_cast(1e9) + + source.header.stamp.nanosec; + auto output_timestamp = message->add("timestamp"); + if (!output_timestamp) { + std::stringstream error_msg; + error_msg << "[convert_to_custom] Failed to add a timestamp component to message: " << + GxfResultStr(output_timestamp.error()); + RCLCPP_ERROR( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + output_timestamp.value()->acqtime = input_timestamp; + + // Set frame ID + destination.frame_id = source.header.frame_id; + + // Set message entity + destination.handle = message->eid(); + GxfEntityRefCountInc( + nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid()); + + RCLCPP_DEBUG( + rclcpp::get_logger("NitrosBi3DInferenceParamArray"), + "[convert_to_custom] Conversion completed"); + + nvidia::isaac_ros::nitros::nvtxRangePopWrapper(); +} diff --git a/isaac_ros_nitros_bi3d_inference_param_array_type/test/config/test_forward_node.yaml b/isaac_ros_nitros_bi3d_inference_param_array_type/test/config/test_forward_node.yaml new file mode 100644 index 0000000..3f8e1f8 --- /dev/null +++ b/isaac_ros_nitros_bi3d_inference_param_array_type/test/config/test_forward_node.yaml @@ -0,0 +1,56 @@ +%YAML 1.2 +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. +--- +name: forward +components: +- name: input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 +- name: output + type: nvidia::gxf::DoubleBufferTransmitter +- type: nvidia::isaac_ros::MessageForward + parameters: + in: input + out: output +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: input + min_size: 1 +--- +name: vault +components: +- name: input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: input + min_size: 1 +- name: vault + type: nvidia::gxf::Vault + parameters: + source: input + max_waiting_count: 1 + drop_waiting: false +--- +components: +- type: nvidia::gxf::Connection + parameters: + source: forward/output + target: vault/input +--- +components: +- type: nvidia::gxf::GreedyScheduler + parameters: + clock: clock + stop_on_deadlock: false +- name: clock + type: nvidia::gxf::RealtimeClock \ No newline at end of file diff --git a/isaac_ros_nitros_bi3d_inference_param_array_type/test/isaac_ros_nitros_bi3d_inference_param_array_type_test_pol.py b/isaac_ros_nitros_bi3d_inference_param_array_type/test/isaac_ros_nitros_bi3d_inference_param_array_type_test_pol.py new file mode 100644 index 0000000..0a7850d --- /dev/null +++ b/isaac_ros_nitros_bi3d_inference_param_array_type/test/isaac_ros_nitros_bi3d_inference_param_array_type_test_pol.py @@ -0,0 +1,123 @@ +# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import time + +from isaac_ros_bi3d_interfaces.msg import Bi3DInferenceParametersArray +from isaac_ros_test import IsaacROSBaseTest + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +import launch_testing +import pytest +import rclpy + + +@pytest.mark.rostest +def generate_test_description(): + """Generate launch description with NitrosNode test node.""" + test_ns = IsaacROSNitrosBi3DInferenceParamArrayTest.generate_namespace() + container = ComposableNodeContainer( + name='image_container', + namespace='isaac_ros_nitros_container', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='isaac_ros_nitros_bi3d_inference_param_array_type', + plugin='nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode', + name='NitrosBi3DInferenceParamArrayForwardNode', + namespace=test_ns, + parameters=[{ + 'compatible_format': 'nitros_bi3d_inference_param_array' + }] + ), + ], + output='both', + arguments=['--ros-args', '--log-level', 'info'], + ) + + return IsaacROSNitrosBi3DInferenceParamArrayTest.generate_test_description([ + container, + launch.actions.TimerAction( + period=2.5, actions=[launch_testing.actions.ReadyToTest()]) + ]) + + +class IsaacROSNitrosBi3DInferenceParamArrayTest(IsaacROSBaseTest): + """ + Proof-of-Life Test for Isaac ROS Nitros Node. + + 1. Sets up ROS publisher to send Bi3DInferenceParametersArray values + 2. Sets up ROS subscriber to listen to output channel of NitrosNode + 3. Verify received messages + """ + + def test_forward_node(self) -> None: + self.node._logger.info('Starting Isaac ROS NitrosNode POL Test') + + # Subscriber + received_messages = {} + + subscriber_topic_namespace = self.generate_namespace('topic_forward_output') + test_subscribers = [ + (subscriber_topic_namespace, Bi3DInferenceParametersArray) + ] + + subs = self.create_logging_subscribers( + subscription_requests=test_subscribers, + received_messages=received_messages, + use_namespace_lookup=False, + accept_multiple_messages=True, + add_received_message_timestamps=True + ) + + # Publisher + publisher_topic_namespace = self.generate_namespace('topic_forward_input') + pub = self.node.create_publisher( + Bi3DInferenceParametersArray, + publisher_topic_namespace, + self.DEFAULT_QOS) + + try: + # Construct Bi3D inference parameters array message + INPUT_DATA = [1, 2, 3] + bi3d_inf_params = Bi3DInferenceParametersArray() + bi3d_inf_params.disparity_values = INPUT_DATA + + # Start sending messages + self.node.get_logger().info('Start publishing messages') + sent_count = 0 + end_time = time.time() + 2.0 + while time.time() < end_time: + sent_count += 1 + pub.publish(bi3d_inf_params) + rclpy.spin_once(self.node, timeout_sec=0.2) + + # Conclude the test + received_count = len(received_messages[subscriber_topic_namespace]) + self.node._logger.info( + f'Test Results:\n' + f'# of Messages Sent: {sent_count}\n' + f'# of Messages Received: {received_count}\n' + f'# of Messages Dropped: {sent_count - received_count}\n' + f'Message Drop Rate: {((sent_count-received_count)/sent_count)*100}%' + ) + + self.assertGreater(len(received_messages[subscriber_topic_namespace]), 0) + for i in range(len(INPUT_DATA)): + self.assertEqual( + received_messages[subscriber_topic_namespace][-1][0].disparity_values[i], + bi3d_inf_params.disparity_values[i]) + + self.node._logger.info('Source and received messages are matched.') + + finally: + [self.node.destroy_subscription(sub) for sub in subs] + self.assertTrue(self.node.destroy_publisher(pub)) diff --git a/isaac_ros_nitros_bi3d_inference_param_array_type/test/src/nitros_bi3d_inference_param_array_forward_node.cpp b/isaac_ros_nitros_bi3d_inference_param_array_type/test/src/nitros_bi3d_inference_param_array_forward_node.cpp new file mode 100644 index 0000000..d201843 --- /dev/null +++ b/isaac_ros_nitros_bi3d_inference_param_array_type/test/src/nitros_bi3d_inference_param_array_forward_node.cpp @@ -0,0 +1,90 @@ +/** + * Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#include \ + "isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp" +#include "isaac_ros_nitros/nitros_node.hpp" + +#include "rclcpp_components/register_node_macro.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace nitros +{ + +constexpr char PACKAE_NAME[] = "isaac_ros_nitros_bi3d_inference_param_array_type"; +constexpr char FORWARD_FORMAT[] = "nitros_bi3d_inference_param_array"; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +class NitrosBi3DInferenceParamArrayForwardNode : public NitrosNode +{ +public: + explicit NitrosBi3DInferenceParamArrayForwardNode(const rclcpp::NodeOptions & options) + : NitrosNode( + options, + // Application graph filename + "test/config/test_forward_node.yaml", + // I/O configuration map + { + {"forward/input", + { + .type = NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(1), + .compatible_data_format = FORWARD_FORMAT, + .topic_name = "topic_forward_input", + .use_compatible_format_only = true, + } + }, + {"vault/vault", + { + .type = NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(1), + .compatible_data_format = FORWARD_FORMAT, + .topic_name = "topic_forward_output", + .use_compatible_format_only = true, + } + } + }, + // Extension specs + {}, + // Optimizer's rule filenames + {}, + // Extension so file list + { + {"isaac_ros_nitros", "gxf/std/libgxf_std.so"}, + {"isaac_ros_nitros", "gxf/cuda/libgxf_cuda.so"}, + {"isaac_ros_nitros", "gxf/serialization/libgxf_serialization.so"}, + {"isaac_ros_nitros", "gxf/libgxf_message_compositor.so"} + }, + // Test node package name + PACKAE_NAME) + { + std::string compatible_format = declare_parameter("compatible_format", ""); + if (!compatible_format.empty()) { + config_map_["forward/input"].compatible_data_format = compatible_format; + config_map_["vault/vault"].compatible_data_format = compatible_format; + } + + registerSupportedType(); + + startNitrosNode(); + } +}; +#pragma GCC diagnostic pop + +} // namespace nitros +} // namespace isaac_ros +} // namespace nvidia + +RCLCPP_COMPONENTS_REGISTER_NODE( + nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode) diff --git a/resources/bi3d_visualizer_output.gif b/resources/bi3d_visualizer_output.gif new file mode 100644 index 0000000..9c89f40 --- /dev/null +++ b/resources/bi3d_visualizer_output.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6d206dc5d51e40cb9537dce5d23c5cb1a47b98ae414fccf72af78d43024bb777 +size 661652 diff --git a/resources/rosbags/bi3dnode_rosbag/bi3dnode_rosbag_0.db3 b/resources/rosbags/bi3dnode_rosbag/bi3dnode_rosbag_0.db3 index d1a0074..4f72ae9 100644 --- a/resources/rosbags/bi3dnode_rosbag/bi3dnode_rosbag_0.db3 +++ b/resources/rosbags/bi3dnode_rosbag/bi3dnode_rosbag_0.db3 @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c58d5996ac8f721c58e72094f1e14988cc63c25abc34732f256e31ded83639ea -size 77869056 +oid sha256:301c23d12ca21db82fcf7a78341e12a1cc2d4f12719f47d599ffe2f71b380249 +size 52957184 diff --git a/resources/rosbags/bi3dnode_rosbag/metadata.yaml b/resources/rosbags/bi3dnode_rosbag/metadata.yaml index 356108a..426ea32 100644 --- a/resources/rosbags/bi3dnode_rosbag/metadata.yaml +++ b/resources/rosbags/bi3dnode_rosbag/metadata.yaml @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:54f8808e001e16b5db72ccfda08b73e426635fead3448507236e8f220dd0b2fd -size 1477 +oid sha256:aca1c0ebd7f7aca5616fbdce023bcd45039935fb3dcb4b2ae448a13d3cc7e7e8 +size 2001