Skip to content

Commit f157b69

Browse files
MyzharMyzhar
authored
Merge pull request #51 from stereolabs/add_nitros_examples
Add nitros examples
2 parents b847a16 + 3edbd4a commit f157b69

File tree

13 files changed

+1282
-0
lines changed

13 files changed

+1282
-0
lines changed

‎isaac_ros/README.md‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
# Isaac ROS Examples
2+
3+
This repository contains example applications that demonstrate how to use the [Isaac ROS](https://developer.nvidia.com/isaac-ros) SDK with the [ZED ROS2 Wrapper](https://github.com/stereolabs/zed-ros2-wrapper).
4+
5+
## Prerequisites
6+
7+
- [NVIDIA Isaac ROS with NITROS](https://docs.stereolabs.com/isaac-ros)
8+
- [ZED ROS2 Wrapper](https://docs.stereolabs.com/ros2)
9+
10+
## Available Examples
11+
12+
- **WIP** - [zed_isaac_ros_nitros_sub](https://github.com/stereolabs/zed-ros2-examples/tree/main/isaac_ros/zed_isaac_ros_nitros_sub): A simple example that subscribes to ZED camera topics using NITROS and display communication benchmarks.
13+
- [zed_isaac_ros_april_tag](https://github.com/stereolabs/zed-ros2-examples/tree/main/isaac_ros/zed_isaac_ros_april_tag): An example that creates an Isaac ROS graphs to detects AprilTags in the ZED camera feed using Isaac ROS nodes.
14+

‎isaac_ros/zed_isaac_ros_april_tag/CMakeLists.txt‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(zed_isaac_ros_april_tag)
3+
4+
## Generate symbols for IDE indexer
5+
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
6+
7+
################################################
8+
# Check the ROS2 version
9+
10+
set(ROS2_FOUND FALSE)
11+
if(DEFINED ENV{ROS_DISTRO})
12+
set(FOUND_ROS2_DISTRO $ENV{ROS_DISTRO})
13+
set(ROS2_FOUND TRUE)
14+
#message("* Found ROS2 ${FOUND_ROS2_DISTRO}")
15+
else()
16+
message("* ROS2 distro variable not set. Trying to figure it out...")
17+
set(ROS2_DISTROS "ardent;crystal;dashing;eloquent;foxy;galactic;humble;jazzy;rolling")
18+
set(ROS2_FOUND FALSE)
19+
foreach(distro ${ROS2_DISTROS})
20+
if(NOT ROS2_FOUND)
21+
find_path(RCLCPP_H rclcpp.hpp PATHS /opt/ros/${distro}/include/rclcpp)
22+
if(RCLCPP_H)
23+
message("* Found ROS2 ${distro}")
24+
set(FOUND_ROS2_DISTRO ${distro})
25+
set(ROS2_FOUND TRUE)
26+
endif()
27+
endif()
28+
endforeach()
29+
endif()
30+
31+
if(ROS2_FOUND)
32+
if(${FOUND_ROS2_DISTRO} STREQUAL "humble")
33+
#message("* ROS2 ${FOUND_ROS2_DISTRO} is officially supported by this package.")
34+
add_definitions(-DFOUND_HUMBLE)
35+
elseif(${FOUND_ROS2_DISTRO} STREQUAL "iron")
36+
#message("* ROS2 ${FOUND_ROS2_DISTRO} is officially supported by this package.")
37+
add_definitions(-DFOUND_IRON)
38+
elseif(${FOUND_ROS2_DISTRO} STREQUAL "jazzy")
39+
#message("* ROS2 ${FOUND_ROS2_DISTRO} is officially supported by this package.")
40+
add_definitions(-DFOUND_JAZZY)
41+
else()
42+
message("*** WARNING *** Unsupported ROS2 ${FOUND_ROS2_DISTRO}. '${PROJECT_NAME}' may not work correctly.")
43+
endif()
44+
else()
45+
message("*** WARNING *** ROS2 distro is unknown. This package could not work correctly.")
46+
endif()
47+
################################################
48+
49+
if(ROS2_FOUND)
50+
#############################################
51+
# Dependencies
52+
53+
find_package(ament_cmake_auto REQUIRED)
54+
ament_auto_find_build_dependencies()
55+
56+
find_package(zed_components REQUIRED)
57+
find_package(zed_wrapper REQUIRED)
58+
find_package(isaac_ros_apriltag REQUIRED)
59+
find_package(isaac_ros_apriltag_interfaces REQUIRED)
60+
61+
if(BUILD_TESTING)
62+
find_package(ament_lint_auto REQUIRED)
63+
ament_lint_auto_find_test_dependencies()
64+
endif()
65+
66+
###############################################################################
67+
#Add all files in subdirectories of the project in
68+
# a dummy_target so qtcreator have access to all files
69+
file(GLOB_RECURSE all_files ${CMAKE_SOURCE_DIR}/*)
70+
add_custom_target(all_${PROJECT_NAME}_files SOURCES ${all_files})
71+
72+
# create ament index resource which references the libraries in the binary dir
73+
set(node_plugins "")
74+
75+
###############################################################################
76+
# Install
77+
78+
# Install LAUNCH files
79+
install(DIRECTORY
80+
launch config rviz2
81+
DESTINATION share/${PROJECT_NAME}
82+
)
83+
84+
ament_package()
85+
endif(ROS2_FOUND)

‎isaac_ros/zed_isaac_ros_april_tag/README.md‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# ZED + Isaac ROS AprilTag Example
2+
3+
This example creates an Isaac ROS graph to detect AprilTags in the ZED camera feed using Isaac ROS nodes.
4+
5+
Details about the examples are available in the [Isaac ROS with ZED documentation](https://docs.stereolabs.com/isaac-ros/tutorial_apriltag).

‎isaac_ros/zed_isaac_ros_april_tag/config/foo.txt‎

Whitespace-only changes.

‎isaac_ros/zed_isaac_ros_april_tag/config/zed_isaac_ros_april_tag.yaml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
# config/zed_isaac_ros_april_tag.yaml
2+
# Parameters for AprilTag detection
3+
---
4+
/**:
5+
ros__parameters:
6+
size: 0.155 # The tag edge size in meters, assuming square markers. For example, 0.22
7+
max_tags: 64 # The maximum number of tags to be detected. For example, 64
8+
tile_size: 4 # Tile/window size used for adaptive thresholding in pixels. For example, 4
9+
tag_family: 'tag36h11' # Tag family to detect. CUDA backend only supports tag36h11. CPU and PVA backends support tag36h11, tag16h5, tag25h9, tag36h10, tag36h11, circle21h7, circle49h12, custom48h12, standard41h12, standard52h13
10+
backends: 'CUDA' # Backend to perform detection with. Options include CPU, CUDA, PVA

‎isaac_ros/zed_isaac_ros_april_tag/config/zed_params.yaml‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
# config/zed_params.yaml
2+
# Stereolabs ZED Stereo camera parameters override file
3+
# This file is used to override the default parameters of the ZED camera.
4+
5+
/**:
6+
ros__parameters:
7+
general:
8+
grab_resolution: 'HD1080' # The native camera grab resolution. 'HD2K', 'HD1200', 'HD1080', 'HD720', 'SVGA', 'AUTO'
9+
grab_frame_rate: 30 # ZED SDK internal grabbing rate (HD1200/HD1080: 60, 30, 15 - SVGA: 120, 60, 30, 15)
10+
pub_resolution: 'CUSTOM' # The resolution used for image and depth map publishing. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission
11+
pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM'
12+
pub_frame_rate: 30.0

‎isaac_ros/zed_isaac_ros_april_tag/launch/foo.txt‎

Whitespace-only changes.

‎isaac_ros/zed_isaac_ros_april_tag/launch/zed_isaac_ros_april_tag.launch.py‎

Lines changed: 200 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,200 @@
1+
# Copyright 2024 Stereolabs
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the 'License');
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an 'AS IS' BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import os
16+
import yaml
17+
18+
from ament_index_python.packages import get_package_share_directory
19+
from launch.launch_description_sources import PythonLaunchDescriptionSource
20+
21+
from launch import LaunchDescription
22+
from launch.actions import (
23+
DeclareLaunchArgument,
24+
OpaqueFunction,
25+
IncludeLaunchDescription,
26+
LogInfo
27+
)
28+
from launch.substitutions import (
29+
LaunchConfiguration,
30+
TextSubstitution
31+
)
32+
from launch_ros.actions import (
33+
ComposableNodeContainer,
34+
LoadComposableNodes
35+
)
36+
37+
from launch_ros.descriptions import (
38+
ComposableNode
39+
)
40+
# Enable colored output
41+
os.environ["RCUTILS_COLORIZED_OUTPUT"] = "1"
42+
43+
44+
def launch_setup(context, *args, **kwargs):
45+
46+
# Get the path to the camera configuration file
47+
camera_config_override_path = os.path.join(
48+
get_package_share_directory('zed_isaac_ros_april_tag'),
49+
'config',
50+
'zed_params.yaml'
51+
)
52+
53+
# Get the path to the AprilTag configuration file
54+
apriltag_config_path = os.path.join(
55+
get_package_share_directory('zed_isaac_ros_april_tag'),
56+
'config',
57+
'zed_isaac_ros_april_tag.yaml'
58+
)
59+
60+
# List of actions to be launched
61+
actions = []
62+
63+
namespace_val = 'zed_isaac'
64+
disable_tf = LaunchConfiguration('disable_tf')
65+
camera_model = LaunchConfiguration('camera_model')
66+
67+
disable_tf_val = disable_tf.perform(context)
68+
69+
# ROS 2 Component Container
70+
container_name = 'zed_container'
71+
info = '* Starting Composable node container: ' + namespace_val + '/' + container_name
72+
actions.append(LogInfo(msg=TextSubstitution(text=info)))
73+
74+
# Note: It is crucial that the 'executable' field is set to be 'component_container_mt'
75+
# so that the created nodes can be started and communicated correctly within the same process.
76+
77+
zed_container = ComposableNodeContainer(
78+
name=container_name,
79+
namespace=namespace_val,
80+
package='rclcpp_components',
81+
executable='component_container_mt',
82+
arguments=['--ros-args', '--log-level', 'info'],
83+
output='screen',
84+
)
85+
actions.append(zed_container)
86+
87+
88+
# ZED Wrapper launch file
89+
zed_wrapper_launch = IncludeLaunchDescription(
90+
launch_description_source=PythonLaunchDescriptionSource([
91+
get_package_share_directory('zed_wrapper'),
92+
'/launch/zed_camera.launch.py'
93+
]),
94+
launch_arguments={
95+
'camera_model': camera_model,
96+
'container_name': container_name,
97+
'namespace': namespace_val,
98+
'enable_ipc': 'false',
99+
'ros_params_override_path': camera_config_override_path
100+
}.items()
101+
)
102+
actions.append(zed_wrapper_launch)
103+
104+
# Read the resolution from the ZED parameters file
105+
with open(camera_config_override_path, 'r') as f:
106+
configuration = yaml.safe_load(f)
107+
print(f'Loaded configuration: {configuration}')
108+
resolution = configuration["/**"]["ros__parameters"]["general"]["grab_resolution"]
109+
pub_resolution = configuration["/**"]["ros__parameters"]["general"]["pub_resolution"]
110+
pub_downscale_factor = configuration["/**"]["ros__parameters"]["general"]["pub_downscale_factor"]
111+
if pub_resolution == 'CUSTOM':
112+
rescale = pub_downscale_factor
113+
else:
114+
rescale = 1.0
115+
116+
if resolution == 'HD2K':
117+
image_width = 2208
118+
image_height = 1242
119+
elif resolution == 'HD1200':
120+
image_width = 1280
121+
image_height = 720
122+
elif resolution == 'HD1080':
123+
image_width = 1920
124+
image_height = 1080
125+
elif resolution == 'HD720':
126+
image_width = 1280
127+
image_height = 720
128+
elif resolution == 'SVGA':
129+
image_width = 960
130+
image_height = 600
131+
elif resolution == 'VGA':
132+
image_width = 672
133+
image_height = 376
134+
135+
# Isaac ROS Node to convert from ZED BGRA8 image to BGR8 required by AprilTag
136+
isaac_converter_node = ComposableNode(
137+
package='isaac_ros_image_proc',
138+
plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
139+
name='zed_image_converter',
140+
namespace=namespace_val,
141+
parameters=[
142+
{
143+
'image_width': int(image_width / rescale),
144+
'image_height': int(image_height / rescale),
145+
'encoding_desired': 'bgr8',
146+
'num_blocks': 40
147+
}
148+
],
149+
remappings=[
150+
('image_raw', 'zed/rgb/image_rect_color'),
151+
('image', 'zed/rgb/image_rect_color_bgr8')
152+
]
153+
)
154+
155+
# add parameters for conversion node -> https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_image_pipeline/isaac_ros_image_proc/index.html#imageformatconverternode
156+
157+
# AprilTag detection node
158+
isac_apriltag_node = ComposableNode(
159+
package='isaac_ros_apriltag',
160+
plugin='nvidia::isaac_ros::apriltag::AprilTagNode',
161+
name='apriltag',
162+
namespace=namespace_val,
163+
remappings=[
164+
('image', 'zed/rgb/image_rect_color_bgr8'),
165+
('camera_info', 'zed/rgb/camera_info')
166+
],
167+
parameters=[apriltag_config_path]
168+
)
169+
170+
container_full_name = namespace_val + '/' + container_name
171+
# Load the Converter node into the container
172+
load_converter_node = LoadComposableNodes(
173+
composable_node_descriptions=[isaac_converter_node],
174+
target_container=container_full_name
175+
)
176+
actions.append(load_converter_node)
177+
178+
# Load the AprilTag node into the container
179+
load_april_tag_node = LoadComposableNodes(
180+
composable_node_descriptions=[isac_apriltag_node],
181+
target_container=container_full_name
182+
)
183+
actions.append(load_april_tag_node)
184+
185+
return actions
186+
187+
def generate_launch_description():
188+
return LaunchDescription(
189+
[
190+
DeclareLaunchArgument(
191+
'camera_model',
192+
description='[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features.',
193+
choices=['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual', 'zedxonegs', 'zedxone4k']),
194+
DeclareLaunchArgument(
195+
'disable_tf',
196+
default_value='False',
197+
description='If `True` disable TF broadcasting for all the cameras in order to fuse visual odometry information externally.'),
198+
OpaqueFunction(function=launch_setup)
199+
]
200+
)

‎isaac_ros/zed_isaac_ros_april_tag/package.xml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>zed_isaac_ros_april_tag</name>
5+
<version>5.0.0</version>
6+
<description>Contains an example to integrate ZED camera with Isaac ROS for AprilTag detection leveraging NITROS communication.</description>
7+
<maintainer email="[email protected]">STEREOLABS</maintainer>
8+
<license>Apache License 2.0</license>
9+
<url type="website">https://www.stereolabs.com/</url>
10+
<url type="repository">https://github.com/stereolabs/zed-ros2-wrapper</url>
11+
<url type="bugtracker">https://github.com/stereolabs/zed-ros2-wrapper/issues</url>
12+
<buildtool_depend>ament_cmake</buildtool_depend>
13+
<build_depend>ament_cmake_auto</build_depend>
14+
<build_depend>rcutils</build_depend>
15+
<build_depend>builtin_interfaces</build_depend>
16+
17+
<exec_depend>zed_components</exec_depend>
18+
<exec_depend>zed_wrapper</exec_depend>
19+
<exec_depend>launch_ros</exec_depend>
20+
<exec_depend>rcutils</exec_depend>
21+
<exec_depend>builtin_interfaces</exec_depend>
22+
23+
<test_depend>ament_lint_common</test_depend>
24+
<test_depend>ament_cmake_clang_format</test_depend>
25+
<export>
26+
<build_type>ament_cmake</build_type>
27+
</export>
28+
</package>

0 commit comments

Comments
 (0)