Skip to content

Commit 320c75d

Browse files
Isaac ROS 3.2
1 parent e4cf201 commit 320c75d

File tree

63 files changed

+2850
-552
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

63 files changed

+2850
-552
lines changed

README.md

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ the 3D pose of an object and distance for navigation or manipulation.
2424

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

8282
## Performance
8383

84-
| 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/> |
85-
|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
86-
| [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/> |
87-
| [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/> |
88-
| [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/> |
84+
| 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/> |
85+
|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
86+
| [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/> |
87+
| [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/> |
88+
| [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/> |
8989

9090
---
9191

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

116116
## Latest
117117

118-
Update 2024-09-26: Update for ZED compatibility
118+
Update 2024-12-10: Added pose estimate post-processing utilities

isaac_ros_centerpose/CMakeLists.txt

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,13 @@ if(BUILD_TESTING)
4949
find_package(ament_lint_auto REQUIRED)
5050
ament_lint_auto_find_test_dependencies()
5151

52+
# Gtest for decoder_node
53+
ament_add_gtest(centerpose_decoder_node_test test/centerpose_decoder_node_test.cpp)
54+
target_link_libraries(centerpose_decoder_node_test centerpose_decoder_node)
55+
target_include_directories(centerpose_decoder_node_test PUBLIC include/isaac_ros_centerpose/)
56+
target_include_directories(centerpose_decoder_node_test PUBLIC /usr/src/googletest/googlemock/include/)
57+
ament_target_dependencies(centerpose_decoder_node_test rclcpp)
58+
ament_target_dependencies(centerpose_decoder_node_test isaac_ros_nitros)
5259
# The FindPythonInterp and FindPythonLibs modules are removed
5360
if(POLICY CMP0148)
5461
cmake_policy(SET CMP0148 OLD)
@@ -59,4 +66,10 @@ if(BUILD_TESTING)
5966
add_launch_test(test/test_centerpose_pol_triton.py TIMEOUT "600")
6067
endif()
6168

69+
70+
# Embed versioning information into installed files
71+
ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)
72+
include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake")
73+
generate_version_info(${PROJECT_NAME})
74+
6275
ament_auto_package(INSTALL_TO_SHARE config launch)

isaac_ros_centerpose/package.xml

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
2222
<package format="3">
2323
<name>isaac_ros_centerpose</name>
24-
<version>3.1.0</version>
24+
<version>3.2.0</version>
2525
<description>CenterPose: Pose Estimation using Deep Learning</description>
2626

2727
<maintainer email="[email protected]">Isaac ROS Maintainers</maintainer>
@@ -63,6 +63,7 @@
6363
<test_depend>ament_lint_common</test_depend>
6464
<test_depend>isaac_ros_test</test_depend>
6565
<test_depend>sensor_msgs_py</test_depend>
66+
<test_depend>ament_cmake_gtest</test_depend>
6667

6768
<export>
6869
<build_type>ament_cmake</build_type>
Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
2+
// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
//
16+
// SPDX-License-Identifier: Apache-2.0
17+
18+
#include <gmock/gmock.h>
19+
#include "centerpose_decoder_node.hpp"
20+
#include "rclcpp/rclcpp.hpp"
21+
22+
// Objective: to cover code lines where exceptions are thrown
23+
// Approach: send Invalid Arguments for node parameters to trigger the exception
24+
25+
26+
TEST(centerpose_decoder_node_test, test_invalid_output_field_size)
27+
{
28+
rclcpp::init(0, nullptr);
29+
rclcpp::NodeOptions options;
30+
EXPECT_THROW(
31+
{
32+
try {
33+
nvidia::isaac_ros::centerpose::CenterPoseDecoderNode centerpose_decoder_node(options);
34+
} catch (const std::invalid_argument & e) {
35+
EXPECT_THAT(e.what(), testing::HasSubstr("Error: received invalid output field size"));
36+
throw;
37+
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
38+
EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set"));
39+
throw;
40+
}
41+
}, std::invalid_argument);
42+
rclcpp::shutdown();
43+
}
44+
45+
TEST(centerpose_decoder_node_test, test_cuboid_scaling_factor)
46+
{
47+
rclcpp::init(0, nullptr);
48+
rclcpp::NodeOptions options;
49+
options.append_parameter_override("output_field_size", std::vector<int64_t>{128, 128});
50+
options.append_parameter_override("cuboid_scaling_factor", 0.0);
51+
EXPECT_THROW(
52+
{
53+
try {
54+
nvidia::isaac_ros::centerpose::CenterPoseDecoderNode centerpose_decoder_node(options);
55+
} catch (const std::invalid_argument & e) {
56+
EXPECT_THAT(
57+
e.what(),
58+
testing::HasSubstr("Error: received a less than or equal to zero cuboid scaling factor"));
59+
throw;
60+
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
61+
EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set"));
62+
throw;
63+
}
64+
}, std::invalid_argument);
65+
rclcpp::shutdown();
66+
}
67+
68+
TEST(centerpose_decoder_node_test, test_score_threshold)
69+
{
70+
rclcpp::init(0, nullptr);
71+
rclcpp::NodeOptions options;
72+
options.append_parameter_override("output_field_size", std::vector<int64_t>{128, 128});
73+
options.append_parameter_override("cuboid_scaling_factor", 1.0);
74+
options.append_parameter_override("score_threshold", 1.0);
75+
EXPECT_THROW(
76+
{
77+
try {
78+
nvidia::isaac_ros::centerpose::CenterPoseDecoderNode centerpose_decoder_node(options);
79+
} catch (const std::invalid_argument & e) {
80+
EXPECT_THAT(
81+
e.what(),
82+
testing::HasSubstr("Error: received score threshold greater or equal to 1.0"));
83+
throw;
84+
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
85+
EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set"));
86+
throw;
87+
}
88+
}, std::invalid_argument);
89+
rclcpp::shutdown();
90+
}
91+
92+
93+
int main(int argc, char ** argv)
94+
{
95+
testing::InitGoogleTest(&argc, argv);
96+
return RUN_ALL_TESTS();
97+
}

isaac_ros_dope/CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,4 +52,10 @@ if(BUILD_TESTING)
5252
add_launch_test(test/isaac_ros_dope_pol.py TIMEOUT "600")
5353
endif()
5454

55+
56+
# Embed versioning information into installed files
57+
ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)
58+
include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake")
59+
generate_version_info(${PROJECT_NAME})
60+
5561
ament_auto_package(INSTALL_TO_SHARE config launch)

isaac_ros_dope/config/dope_config.yaml

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -58,12 +58,4 @@ dope:
5858
"Peaches" : [ 5.7781000137329102, 7.0961999893188477, 6.5925998687744141 ],
5959
"GreenBeans" : [ 5.758699893951416, 7.0608000755310059, 6.5732002258300781 ],
6060
"PeasAndCarrots" : [ 5.8512001037597656, 7.0636000633239746, 6.5918002128601074 ]
61-
}
62-
63-
# 9 element camera matrix (using default from Ketchup demo)
64-
# Taken from: https://github.com/andrewyguo/dope_training/blob/master/inference/config/camera_info.yaml
65-
camera_matrix: [
66-
364.16501736, 0.0, 121.36296296,
67-
0.0, 364.16501736, 121.36296296,
68-
0.0, 0.0, 1.0
69-
]
61+
}

isaac_ros_dope/config/dope_node.yaml

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -22,28 +22,36 @@ components:
2222
type: nvidia::gxf::DoubleBufferReceiver
2323
parameters:
2424
capacity: 12
25-
- name: posearray_out
25+
- name: detection3darray_out
2626
type: nvidia::gxf::DoubleBufferTransmitter
2727
parameters:
2828
capacity: 12
29+
- name: camera_model_input
30+
type: nvidia::gxf::DoubleBufferReceiver
31+
parameters:
32+
capacity: 12
2933
- name: allocator
3034
type: nvidia::gxf::UnboundedAllocator
3135
- name: dope_decoder
3236
type: nvidia::isaac_ros::dope::DopeDecoder
3337
parameters:
3438
tensorlist_receiver: tensorlist_in
35-
posearray_transmitter: posearray_out
39+
camera_model_input: camera_model_input
40+
detection3darray_transmitter: detection3darray_out
3641
allocator: allocator
3742
object_dimensions: []
38-
camera_matrix: []
3943
object_name: ""
4044
- type: nvidia::gxf::MessageAvailableSchedulingTerm
4145
parameters:
4246
receiver: tensorlist_in
4347
min_size: 1
48+
- type: nvidia::gxf::MessageAvailableSchedulingTerm
49+
parameters:
50+
receiver: camera_model_input
51+
min_size: 1
4452
- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm
4553
parameters:
46-
transmitter: posearray_out
54+
transmitter: detection3darray_out
4755
min_size: 1
4856
---
4957
name: sink
@@ -65,7 +73,7 @@ components:
6573
- name: edge0
6674
type: nvidia::gxf::Connection
6775
parameters:
68-
source: dope_decoder/posearray_out
76+
source: dope_decoder/detection3darray_out
6977
target: sink/signal
7078
---
7179
components:
Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
%YAML 1.2
2+
# Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved.
3+
#
4+
# NVIDIA CORPORATION and its licensors retain all intellectual property
5+
# and proprietary rights in and to this software, related documentation
6+
# and any modifications thereto. Any use, reproduction, disclosure or
7+
# distribution of this software and related documentation without an express
8+
# license agreement from NVIDIA CORPORATION is strictly prohibited.
9+
---
10+
id: [0xb9a27f9a9c9b3c40, 0xf7620fa5914c9644]
11+
name: DopeDecoder
12+
version: 1.0.0
13+
components:
14+
- id: [0x03022c23f899e397, 0xaf817e602e657ba6]
15+
type: nvidia::isaac_ros::dope::DopeDecoder
16+
input_output_groups:
17+
- input_keys: [tensorlist_receiver, camera_model_input]
18+
output_keys: [detection3darray_transmitter]
19+
input_format_keys: []
20+
output_format_keys: []
21+
supported_formats:
22+
- platforms: [any]
23+
details:
24+
- input_formats: [nitros_tensor_list_nchw_rgb_f32, nitros_camera_info]
25+
output_formats: [nitros_detection3_d_array]
26+
costs:
27+
throughput: 10bytes/s
28+
latency: 10ms
29+
power: 100J
30+
accuracy: 100%

0 commit comments

Comments
 (0)