Skip to content

Commit

Permalink
Merge pull request #54 from NVIDIA-ISAAC-ROS/release-3.2
Browse files Browse the repository at this point in the history
Isaac ROS 3.2
  • Loading branch information
jaiveersinghNV authored Dec 11, 2024
2 parents e4cf201 + 320c75d commit 3cc6704
Show file tree
Hide file tree
Showing 63 changed files with 2,850 additions and 552 deletions.
14 changes: 7 additions & 7 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ the 3D pose of an object and distance for navigation or manipulation.

`isaac_ros_foundationpose` is used in a graph of nodes to estimate the pose of
a novel object using 3D bounding cuboid dimensions. It’s developed on top of
[FoundationPose](https://github.com/NVlabs/FoundationPose) model, which is
[FoundationPose](https://catalog.ngc.nvidia.com/orgs/nvidia/teams/isaac/models/foundationpose) model, which is
a pre-trained deep learning model developed by NVLabs. FoundationPose
is capable for both pose estimation and tracking on unseen objects without requiring fine-tuning,
and its accuracy outperforms existing state-of-art methods.
Expand Down Expand Up @@ -81,11 +81,11 @@ which can also be found at [Isaac ROS DNN Inference](https://github.com/NVIDIA-I

## Performance

| Sample Graph<br/><br/> | Input Size<br/><br/> | AGX Orin<br/><br/> | Orin NX<br/><br/> | Orin Nano 8GB<br/><br/> | x86_64 w/ RTX 4060 Ti<br/><br/> | x86_64 w/ RTX 4090<br/><br/> |
|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
| [FoundationPose Pose Estimation Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_foundationpose_benchmark/scripts/isaac_ros_foundationpose_node.py)<br/><br/><br/><br/> | 720p<br/><br/><br/><br/> | [1.72 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_foundationpose_node-agx_orin.json)<br/><br/><br/>690 ms @ 30Hz<br/><br/> | –<br/><br/><br/><br/> | –<br/><br/><br/><br/> | –<br/><br/><br/><br/> | [7.02 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_foundationpose_node-x86_4090.json)<br/><br/><br/>170 ms @ 30Hz<br/><br/> |
| [DOPE Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_dope_benchmark/scripts/isaac_ros_dope_graph.py)<br/><br/><br/><br/> | VGA<br/><br/><br/><br/> | [41.3 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-agx_orin.json)<br/><br/><br/>42 ms @ 30Hz<br/><br/> | [17.5 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-orin_nx.json)<br/><br/><br/>76 ms @ 30Hz<br/><br/> | –<br/><br/><br/><br/> | [85.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-nuc_4060ti.json)<br/><br/><br/>24 ms @ 30Hz<br/><br/> | [199 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-x86_4090.json)<br/><br/><br/>14 ms @ 30Hz<br/><br/> |
| [Centerpose Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_centerpose_benchmark/scripts/isaac_ros_centerpose_graph.py)<br/><br/><br/><br/> | VGA<br/><br/><br/><br/> | [36.3 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-agx_orin.json)<br/><br/><br/>4.8 ms @ 30Hz<br/><br/> | [19.7 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-orin_nx.json)<br/><br/><br/>4.9 ms @ 30Hz<br/><br/> | [13.8 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-orin_nano.json)<br/><br/><br/>7.4 ms @ 30Hz<br/><br/> | [50.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-nuc_4060ti.json)<br/><br/><br/>23 ms @ 30Hz<br/><br/> | [50.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-x86_4090.json)<br/><br/><br/>20 ms @ 30Hz<br/><br/> |
| Sample Graph<br/><br/> | Input Size<br/><br/> | AGX Orin<br/><br/> | Orin NX<br/><br/> | Orin Nano 8GB<br/><br/> | x86_64 w/ RTX 4090<br/><br/> |
|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
| [FoundationPose Pose Estimation Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_foundationpose_benchmark/scripts/isaac_ros_foundationpose_node.py)<br/><br/><br/><br/> | 720p<br/><br/><br/><br/> | [1.72 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_foundationpose_node-agx_orin.json)<br/><br/><br/>690 ms @ 30Hz<br/><br/> | –<br/><br/><br/><br/> | –<br/><br/><br/><br/> | [9.61 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_foundationpose_node-x86-4090.json)<br/><br/><br/>110 ms @ 30Hz<br/><br/> |
| [DOPE Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_dope_benchmark/scripts/isaac_ros_dope_graph.py)<br/><br/><br/><br/> | VGA<br/><br/><br/><br/> | [27.7 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-agx_orin.json)<br/><br/><br/>16 ms @ 30Hz<br/><br/> | [17.8 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-orin_nx.json)<br/><br/><br/>14 ms @ 30Hz<br/><br/> | –<br/><br/><br/><br/> | [187 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-x86-4090.json)<br/><br/><br/>7.8 ms @ 30Hz<br/><br/> |
| [Centerpose Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_centerpose_benchmark/scripts/isaac_ros_centerpose_graph.py)<br/><br/><br/><br/> | VGA<br/><br/><br/><br/> | [47.1 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-agx_orin.json)<br/><br/><br/>6.5 ms @ 30Hz<br/><br/> | [30.0 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-orin_nx.json)<br/><br/><br/>50 ms @ 30Hz<br/><br/> | [19.9 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-orin_nano.json)<br/><br/><br/>28 ms @ 30Hz<br/><br/> | [50.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-x86-4090.json)<br/><br/><br/>12 ms @ 30Hz<br/><br/> |

---

Expand Down Expand Up @@ -115,4 +115,4 @@ Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/re

## Latest

Update 2024-09-26: Update for ZED compatibility
Update 2024-12-10: Added pose estimate post-processing utilities
13 changes: 13 additions & 0 deletions isaac_ros_centerpose/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,13 @@ if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()

# Gtest for decoder_node
ament_add_gtest(centerpose_decoder_node_test test/centerpose_decoder_node_test.cpp)
target_link_libraries(centerpose_decoder_node_test centerpose_decoder_node)
target_include_directories(centerpose_decoder_node_test PUBLIC include/isaac_ros_centerpose/)
target_include_directories(centerpose_decoder_node_test PUBLIC /usr/src/googletest/googlemock/include/)
ament_target_dependencies(centerpose_decoder_node_test rclcpp)
ament_target_dependencies(centerpose_decoder_node_test isaac_ros_nitros)
# The FindPythonInterp and FindPythonLibs modules are removed
if(POLICY CMP0148)
cmake_policy(SET CMP0148 OLD)
Expand All @@ -59,4 +66,10 @@ if(BUILD_TESTING)
add_launch_test(test/test_centerpose_pol_triton.py TIMEOUT "600")
endif()


# Embed versioning information into installed files
ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)
include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake")
generate_version_info(${PROJECT_NAME})

ament_auto_package(INSTALL_TO_SHARE config launch)
3 changes: 2 additions & 1 deletion isaac_ros_centerpose/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>isaac_ros_centerpose</name>
<version>3.1.0</version>
<version>3.2.0</version>
<description>CenterPose: Pose Estimation using Deep Learning</description>

<maintainer email="[email protected]">Isaac ROS Maintainers</maintainer>
Expand Down Expand Up @@ -63,6 +63,7 @@
<test_depend>ament_lint_common</test_depend>
<test_depend>isaac_ros_test</test_depend>
<test_depend>sensor_msgs_py</test_depend>
<test_depend>ament_cmake_gtest</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
97 changes: 97 additions & 0 deletions isaac_ros_centerpose/test/centerpose_decoder_node_test.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,97 @@
// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// SPDX-License-Identifier: Apache-2.0

#include <gmock/gmock.h>
#include "centerpose_decoder_node.hpp"
#include "rclcpp/rclcpp.hpp"

// Objective: to cover code lines where exceptions are thrown
// Approach: send Invalid Arguments for node parameters to trigger the exception


TEST(centerpose_decoder_node_test, test_invalid_output_field_size)
{
rclcpp::init(0, nullptr);
rclcpp::NodeOptions options;
EXPECT_THROW(
{
try {
nvidia::isaac_ros::centerpose::CenterPoseDecoderNode centerpose_decoder_node(options);
} catch (const std::invalid_argument & e) {
EXPECT_THAT(e.what(), testing::HasSubstr("Error: received invalid output field size"));
throw;
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set"));
throw;
}
}, std::invalid_argument);
rclcpp::shutdown();
}

TEST(centerpose_decoder_node_test, test_cuboid_scaling_factor)
{
rclcpp::init(0, nullptr);
rclcpp::NodeOptions options;
options.append_parameter_override("output_field_size", std::vector<int64_t>{128, 128});
options.append_parameter_override("cuboid_scaling_factor", 0.0);
EXPECT_THROW(
{
try {
nvidia::isaac_ros::centerpose::CenterPoseDecoderNode centerpose_decoder_node(options);
} catch (const std::invalid_argument & e) {
EXPECT_THAT(
e.what(),
testing::HasSubstr("Error: received a less than or equal to zero cuboid scaling factor"));
throw;
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set"));
throw;
}
}, std::invalid_argument);
rclcpp::shutdown();
}

TEST(centerpose_decoder_node_test, test_score_threshold)
{
rclcpp::init(0, nullptr);
rclcpp::NodeOptions options;
options.append_parameter_override("output_field_size", std::vector<int64_t>{128, 128});
options.append_parameter_override("cuboid_scaling_factor", 1.0);
options.append_parameter_override("score_threshold", 1.0);
EXPECT_THROW(
{
try {
nvidia::isaac_ros::centerpose::CenterPoseDecoderNode centerpose_decoder_node(options);
} catch (const std::invalid_argument & e) {
EXPECT_THAT(
e.what(),
testing::HasSubstr("Error: received score threshold greater or equal to 1.0"));
throw;
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set"));
throw;
}
}, std::invalid_argument);
rclcpp::shutdown();
}


int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 6 additions & 0 deletions isaac_ros_dope/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -52,4 +52,10 @@ if(BUILD_TESTING)
add_launch_test(test/isaac_ros_dope_pol.py TIMEOUT "600")
endif()


# Embed versioning information into installed files
ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)
include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake")
generate_version_info(${PROJECT_NAME})

ament_auto_package(INSTALL_TO_SHARE config launch)
10 changes: 1 addition & 9 deletions isaac_ros_dope/config/dope_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -58,12 +58,4 @@ dope:
"Peaches" : [ 5.7781000137329102, 7.0961999893188477, 6.5925998687744141 ],
"GreenBeans" : [ 5.758699893951416, 7.0608000755310059, 6.5732002258300781 ],
"PeasAndCarrots" : [ 5.8512001037597656, 7.0636000633239746, 6.5918002128601074 ]
}

# 9 element camera matrix (using default from Ketchup demo)
# Taken from: https://github.com/andrewyguo/dope_training/blob/master/inference/config/camera_info.yaml
camera_matrix: [
364.16501736, 0.0, 121.36296296,
0.0, 364.16501736, 121.36296296,
0.0, 0.0, 1.0
]
}
18 changes: 13 additions & 5 deletions isaac_ros_dope/config/dope_node.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -22,28 +22,36 @@ components:
type: nvidia::gxf::DoubleBufferReceiver
parameters:
capacity: 12
- name: posearray_out
- name: detection3darray_out
type: nvidia::gxf::DoubleBufferTransmitter
parameters:
capacity: 12
- name: camera_model_input
type: nvidia::gxf::DoubleBufferReceiver
parameters:
capacity: 12
- name: allocator
type: nvidia::gxf::UnboundedAllocator
- name: dope_decoder
type: nvidia::isaac_ros::dope::DopeDecoder
parameters:
tensorlist_receiver: tensorlist_in
posearray_transmitter: posearray_out
camera_model_input: camera_model_input
detection3darray_transmitter: detection3darray_out
allocator: allocator
object_dimensions: []
camera_matrix: []
object_name: ""
- type: nvidia::gxf::MessageAvailableSchedulingTerm
parameters:
receiver: tensorlist_in
min_size: 1
- type: nvidia::gxf::MessageAvailableSchedulingTerm
parameters:
receiver: camera_model_input
min_size: 1
- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm
parameters:
transmitter: posearray_out
transmitter: detection3darray_out
min_size: 1
---
name: sink
Expand All @@ -65,7 +73,7 @@ components:
- name: edge0
type: nvidia::gxf::Connection
parameters:
source: dope_decoder/posearray_out
source: dope_decoder/detection3darray_out
target: sink/signal
---
components:
Expand Down
30 changes: 30 additions & 0 deletions isaac_ros_dope/config/isaac_ros_dope.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
%YAML 1.2
# Copyright (c) 2024, 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.
---
id: [0xb9a27f9a9c9b3c40, 0xf7620fa5914c9644]
name: DopeDecoder
version: 1.0.0
components:
- id: [0x03022c23f899e397, 0xaf817e602e657ba6]
type: nvidia::isaac_ros::dope::DopeDecoder
input_output_groups:
- input_keys: [tensorlist_receiver, camera_model_input]
output_keys: [detection3darray_transmitter]
input_format_keys: []
output_format_keys: []
supported_formats:
- platforms: [any]
details:
- input_formats: [nitros_tensor_list_nchw_rgb_f32, nitros_camera_info]
output_formats: [nitros_detection3_d_array]
costs:
throughput: 10bytes/s
latency: 10ms
power: 100J
accuracy: 100%
Loading

0 comments on commit 3cc6704

Please sign in to comment.