hi,
I’m testing NVIDIA NITROS like:
- Create local camera capturing video NITROS node, publis to topic, second node grab the message and republish, view the raw image.
Custom camera NITROS Node (NITROS message) → Custom forward node (NITROS message, enable_ros_publish=True) → Image View
- Create local camera capturing video NITROS node, create launch file for Resize node, second node grab the message and republish, view the raw image
Custom camera NITROS Node (NITROS message) → ResizeNode → Custom forward node (NITROS message, enable_ros_publish=True) → Image View
In the first case all works fine messages are published correctly and I can view the image.
In the second case I do have error and ResizeNode looks like not reciving the messages and nore resizing
Custom image publisher
# local_camera_publisher_node.py
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from isaac_ros_pynitros.isaac_ros_pynitros_publisher import PyNitrosPublisher
from isaac_ros_pynitros.pynitros_type_builders.pynitros_image_builder import PyNitrosImageBuilder
from isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeImage
from isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeImage, NitrosBridgeTensorList
from std_msgs.msg import Header
import cv2
import cupy as cp
class LocalCameraPublisherNode(Node):
def __init__(self):
super().__init__('local_camera_publisher_node')
# Initialize OpenCV VideoCapture for direct camera access
# self.camera = cv2.VideoCapture(0) # Use camera index 0 (default camera)
self.camera = cv2.VideoCapture("video/test.mp4") # Use camera index 0 (default camera)
if not self.camera.isOpened():
self.get_logger().error("Failed to open the camera!")
return
# Initialize image builder
self.pynitros_image_builder = PyNitrosImageBuilder()
# Initialize NITROS Publisher for scaled images
# self.pynitros_publisher = PyNitrosPublisher(self, NitrosBridgeImage, '/camera/image', '/camera/image_raw')
self.pynitros_publisher = PyNitrosPublisher(self, NitrosBridgeImage, '/input_image', '/input_image_raw')
# Create a timer to periodically capture and process frames
# self.timer = self.create_timer(0.033, self.capture_and_process_frame) # ~30 FPS
self.timer = self.create_timer(0.016, self.capture_and_process_frame) # ~30 FPS
def capture_and_process_frame(self):
"""Capture a frame from the camera, process it on the GPU, and publish it."""
# ret, frame = self.camera.read()
ret, frame = self.camera.read()
if not ret:
self.get_logger().error("Failed to capture frame from the camera!")
return
try:
# Convert frame to GPU array (BGR to RGB conversion)
rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
gpu_frame = cp.asarray(rgb_frame, dtype=cp.uint8)
# Build and publish NITROS message
nitros_msg = self.build_nitros_image(gpu_frame)
self.pynitros_publisher.publish(nitros_msg)
except Exception as e:
self.get_logger().error(f"Error processing image: {e}")
def gpu_resize(self, frame, target_size):
"""Resize the image on the GPU."""
return cp.resize(frame, target_size)
def build_nitros_image(self, gpu_data):
"""Build a NITROS-compatible Image message."""
header = Header()
header.frame_id = "camera_frame" # Set frame ID
header.stamp = self.get_clock().now().to_msg() # Set timestamp
built_image = self.pynitros_image_builder.build(gpu_data[0].data.ptr,
int(gpu_data.shape[0]),
int(gpu_data.shape[1]),
int(gpu_data.shape[1] * 3),
# 'rgb8',
'nitros_image_bgr8',
header,
0,
False)
return built_image
def destroy_node(self):
"""Release resources when shutting down the node."""
if hasattr(self, 'camera') and self.camera.isOpened():
self.camera.release()
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = LocalCameraPublisherNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
# only shut down if context is active
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()
import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
def generate_launch_description():
resize_node = ComposableNode(
package='isaac_ros_image_proc',
plugin='nvidia::isaac_ros::image_proc::ResizeNode',
name='resize',
parameters=[{
'output_width': 480,
'output_height': 288,
'enable_isp': False
}],
remappings=[
('image', 'input_image'),
('camera_info', 'input_image_info'),
('resize/image', '/resized_image'),
])
resize_container = ComposableNodeContainer(
package='rclcpp_components',
name='resize_container',
namespace='',
executable='component_container_mt',
composable_node_descriptions=[
resize_node,
],
output='screen'
)
return launch.LaunchDescription([resize_container])
Node forwarding the image
# 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
from isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeImage
from isaac_ros_pynitros.isaac_ros_pynitros_publisher import PyNitrosPublisher
from isaac_ros_pynitros.isaac_ros_pynitros_subscriber import PyNitrosSubscriber
from isaac_ros_pynitros.pynitros_type_builders.pynitros_image_builder import PyNitrosImageBuilder
import rclpy
from rclpy.node import Node
from std_msgs.msg import Header
import torch
class PyNITROSImageForwardNode(Node):
"""A PyNITROS node listens to image message and publish it again."""
def __init__(self, name='local_camera_crop_publisher_node'):
super().__init__(name)
self.pynitros_subscriber = PyNitrosSubscriber(
self, NitrosBridgeImage, '/resized_image',
enable_ros_subscribe=False)
self.pynitros_subscriber.create_subscription(self.listener_callback)
# self.pynitros_publisher = PyNitrosPublisher(
# self, NitrosBridgeImage, '/camera/forward/image', '/camera/forward/image_raw')
self.pynitros_publisher = PyNitrosPublisher(
self, NitrosBridgeImage, '/forward_image', '/forward_image_raw')
self.pynitros_image_builder = PyNitrosImageBuilder(
num_buffer=40, timeout=5)
def listener_callback(self, pynitros_image_view):
# Build Image
header = Header()
header.frame_id = pynitros_image_view.get_frame_id()
header.stamp.sec = pynitros_image_view.get_timestamp_seconds()
header.stamp.nanosec = pynitros_image_view.get_timestamp_nanoseconds()
# Get data as tensor, or any other data type supports __cuda_array_interface__
# image_tensor = torch.as_tensor(pynitros_image_view, dtype=torch.uint8, device=gpu)
built_image = self.pynitros_image_builder.build(pynitros_image_view.get_buffer(),
pynitros_image_view.get_height(),
pynitros_image_view.get_width(),
pynitros_image_view.get_stride(),
pynitros_image_view.get_encoding(),
header,
0,
True)
# Publish
self.pynitros_publisher.publish(built_image)
def main(args=None):
try:
rclpy.init(args=args)
node = PyNITROSImageForwardNode()
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
# only shut down if context is active
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()
and the error
ros2 launch test_suite test_suite.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2025-03-24-15-45-30-946514-atoMix-54525
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container_mt-1]: process started with pid [54537]
[component_container_mt-1] [INFO] [1742827531.255290320] [resize_container]: Load Library: /home/user/workspaces/isaac_ros-dev/install/isaac_ros_image_proc/lib/libresize_node.so
[component_container_mt-1] [INFO] [1742827531.272925262] [resize_container]: Found class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::ResizeNode>
[component_container_mt-1] [INFO] [1742827531.272955478] [resize_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::ResizeNode>
[component_container_mt-1] [INFO] [1742827531.300563007] [resize]: [NitrosNode] Initializing NitrosNode
[component_container_mt-1] [INFO] [1742827531.300968872] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/std/libgxf_std.so
[component_container_mt-1] [INFO] [1742827531.301786731] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_gxf_helpers.so
[component_container_mt-1] [INFO] [1742827531.303415861] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_sight.so
[component_container_mt-1] [INFO] [1742827531.306175445] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_atlas.so
[component_container_mt-1] 2025-03-24 15:45:31.311 WARN gxf/std/program.cpp@538: No GXF scheduler specified.
[component_container_mt-1] [INFO] [1742827531.311454549] [resize]: [NitrosNode] Starting NitrosNode
[component_container_mt-1] [INFO] [1742827531.316294430] [resize]: [NitrosNode] Loading extensions
[component_container_mt-1] [INFO] [1742827531.316566185] [resize]: [NitrosContext] Loading extension: gxf/lib/multimedia/libgxf_multimedia.so
[component_container_mt-1] [INFO] [1742827531.316816811] [resize]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_message_compositor.so
[component_container_mt-1] [INFO] [1742827531.317189871] [resize]: [NitrosContext] Loading extension: gxf/lib/cuda/libgxf_cuda.so
[component_container_mt-1] [INFO] [1742827531.318703945] [resize]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_tensorops.so
[component_container_mt-1] [INFO] [1742827531.321281178] [resize]: [NitrosNode] Loading graph to the optimizer
[component_container_mt-1] [INFO] [1742827531.323157165] [resize]: [NitrosNode] Running optimization
[component_container_mt-1] [INFO] [1742827531.428042102] [resize]: [NitrosNode] Obtaining graph IO group info from the optimizer
[component_container_mt-1] [INFO] [1742827531.433741423] [resize]: [NitrosNode] Starting negotiation...
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/resize' in container '/resize_container'
[component_container_mt-1] [INFO] [1742827532.434250317] [resize]: [NitrosNode] Starting post negotiation setup
[component_container_mt-1] [INFO] [1742827532.434393958] [resize]: [NitrosNode] Getting data format negotiation results
[component_container_mt-1] [INFO] [1742827532.434430127] [resize]: [NitrosPublisher] Negotiation ended with no results
[component_container_mt-1] [INFO] [1742827532.434447084] [resize]: [NitrosPublisher] Use only the compatible publisher: topic_name="/resized_image", data_format="nitros_image_bgr8"
[component_container_mt-1] [INFO] [1742827532.434463143] [resize]: [NitrosPublisher] Negotiation ended with no results
[component_container_mt-1] [INFO] [1742827532.434475937] [resize]: [NitrosPublisher] Use only the compatible publisher: topic_name="/resize/camera_info", data_format="nitros_camera_info"
[component_container_mt-1] [INFO] [1742827532.434497427] [resize]: [NitrosSubscriber] Negotiation ended with no results
[component_container_mt-1] [INFO] [1742827532.434512038] [resize]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/input_image", data_format="nitros_image_bgr8"
[component_container_mt-1] [INFO] [1742827532.434530921] [resize]: [NitrosSubscriber] Negotiation ended with no results
[component_container_mt-1] [INFO] [1742827532.434543030] [resize]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/input_image_info", data_format="nitros_camera_info"
[component_container_mt-1] [INFO] [1742827532.434885703] [resize]: [NitrosNode] Exporting the final graph based on the negotiation results
[component_container_mt-1] [INFO] [1742827532.441369046] [resize]: [NitrosNode] Wrote the final top level YAML graph to "/tmp/isaac_ros_nitros/graphs/JONFMGFGAI/JONFMGFGAI.yaml"
[component_container_mt-1] [INFO] [1742827532.441390308] [resize]: [NitrosNode] Loading application
[component_container_mt-1] [INFO] [1742827532.442987107] [resize]: [ResizeNode] postLoadGraphCallback().
[component_container_mt-1] [INFO] [1742827532.443031528] [resize]: [NitrosNode] Initializing and running GXF graph
[component_container_mt-1] [INFO] [1742827532.562866051] [resize]: [NitrosNode] Node was started
Any advise what the problem can be?