From 7a9d25132f9559e12f626ce0aebe84ec93953208 Mon Sep 17 00:00:00 2001 From: Ashraf Kattoura Date: Wed, 15 Jul 2026 14:33:09 +0300 Subject: [PATCH 1/2] zero copy nitros ros --- realsense2_camera/CMakeLists.txt | 42 +++++++++ .../include/base_realsense_node.h | 19 +++- realsense2_camera/include/constants.h | 1 + .../include/nitros_image_publisher.h | 63 +++++++++++++ realsense2_camera/package.xml | 4 + realsense2_camera/src/base_realsense_node.cpp | 62 +++++++++++++ .../src/nitros_image_publisher.cpp | 92 +++++++++++++++++++ realsense2_camera/src/parameters.cpp | 8 ++ realsense2_camera/src/rs_node_setup.cpp | 24 +++++ 9 files changed, 314 insertions(+), 1 deletion(-) create mode 100644 realsense2_camera/include/nitros_image_publisher.h create mode 100644 realsense2_camera/src/nitros_image_publisher.cpp diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 920fa1d7e..707cb9dc7 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -32,6 +32,10 @@ option(BUILD_WITH_OPENMP "Use OpenMP" OFF) option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OFF) # Define an option to enable or disable lifecycle nodes option(USE_LIFECYCLE_NODE "Enable lifecycle nodes (ON/OFF)" OFF) +# NITROS-native GPU zero-copy publishing (Jetson / Isaac ROS). Requires an Isaac ROS workspace +# (isaac_ros_nitros, isaac_ros_managed_nitros, isaac_ros_nitros_image_type) and a librealsense +# built with BUILD_WITH_CUDA_ZEROCOPY. Default OFF so standard builds are unaffected. +option(BUILD_WITH_NITROS "Enable NITROS-native GPU zero-copy publishing (Jetson / Isaac ROS)" OFF) # Compiler Defense Flags if(UNIX OR APPLE) @@ -160,6 +164,17 @@ if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n RealSense SDK 2.0 is missing, please install it from https://github.com/realsenseai/librealsense/releases\n\n") endif() +if (BUILD_WITH_NITROS) + if (USE_LIFECYCLE_NODE) + message(FATAL_ERROR "BUILD_WITH_NITROS is incompatible with USE_LIFECYCLE_NODE: ManagedNitrosPublisher requires an rclcpp::Node.") + endif() + find_package(CUDA REQUIRED) + find_package(isaac_ros_nitros REQUIRED) + find_package(isaac_ros_managed_nitros REQUIRED) + find_package(isaac_ros_nitros_image_type REQUIRED) + message(STATUS "🚀 BUILD_WITH_NITROS enabled — NITROS GPU zero-copy color publishing") +endif() + #set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories(include) @@ -190,6 +205,10 @@ if (BUILD_ACCELERATE_GPU_WITH_GLSL) list(APPEND SOURCES src/gl_gpu_processing.cpp) endif() +if (BUILD_WITH_NITROS) + list(APPEND SOURCES src/nitros_image_publisher.cpp) +endif() + if(NOT DEFINED ENV{ROS_DISTRO}) message(FATAL_ERROR "ROS_DISTRO is not defined." ) endif() @@ -240,6 +259,10 @@ if (BUILD_ACCELERATE_GPU_WITH_GLSL) add_definitions(-DACCELERATE_GPU_WITH_GLSL) endif() +if (BUILD_WITH_NITROS) + add_definitions(-DBUILD_WITH_NITROS) +endif() + set(INCLUDES include/context_singleton_wrapper.h include/constants.h @@ -260,6 +283,10 @@ if (BUILD_ACCELERATE_GPU_WITH_GLSL) list(APPEND INCLUDES include/gl_window.h) endif() +if (BUILD_WITH_NITROS) + list(APPEND INCLUDES include/nitros_image_publisher.h) +endif() + if (BUILD_TOOLS) include_directories(tools) @@ -325,6 +352,10 @@ else() list(APPEND targets realsense2::realsense2) endif() +if (BUILD_WITH_NITROS) + list(APPEND dependencies isaac_ros_nitros isaac_ros_managed_nitros isaac_ros_nitros_image_type) +endif() + # If the flag is enabled, define the macro if(USE_LIFECYCLE_NODE) find_package(rclcpp_lifecycle REQUIRED) @@ -351,6 +382,17 @@ target_link_libraries(${PROJECT_NAME} ${targets} ) +# NITROS packages export their include dirs and libraries the ament way; use +# ament_target_dependencies so GXF / Isaac ROS transitive deps are picked up. +if (BUILD_WITH_NITROS) + ament_target_dependencies(${PROJECT_NAME} + isaac_ros_nitros + isaac_ros_managed_nitros + isaac_ros_nitros_image_type) + target_include_directories(${PROJECT_NAME} PRIVATE ${CUDA_INCLUDE_DIRS}) + target_link_libraries(${PROJECT_NAME} ${CUDA_LIBRARIES}) +endif() + # Workaround: librealsense built with bundled DDS exports its own fastcdr # symbols from librealsense2.so, clashing at runtime with ROS 2's fastdds. # rclcpp_components dlopens the plugin .so with RTLD_LOCAL, so we must link diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index fa4bc4aca..57e130805 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -17,6 +17,9 @@ #include #include #include "constants.h" +#ifdef BUILD_WITH_NITROS +#include "nitros_image_publisher.h" +#endif // cv_bridge.h last supported version is humble #if defined(CV_BRDIGE_HAS_HPP) @@ -304,6 +307,16 @@ namespace realsense2_camera const std::map>& image_publishers, const bool is_publishMetadata = true); +#ifdef BUILD_WITH_NITROS + // Map an rs2 pixel format to a NITROS supported-type name + sensor_msgs encoding string + + // bytes-per-pixel. Returns false for formats not (yet) supported by the NITROS color path. + bool getNitrosImageFormat(const rs2_format& format, std::string& nitros_format, + std::string& encoding, unsigned int& bpp); + // Publish a video frame as a NITROS image using its GPU device pointer (zero-copy). + void publishNitrosFrame(rs2::frame f, const rclcpp::Time& t, const stream_index_pair& stream, + unsigned int width, unsigned int height, const rs2_format& stream_format); +#endif + void publishRGBD( const cv::Mat& rgb_cv_matrix, const rs2_format& color_format, @@ -369,8 +382,12 @@ namespace realsense2_camera std::vector _static_tf_msgs; std::shared_ptr _tf_t; - bool _use_intra_process; + bool _use_intra_process; std::map> _image_publishers; +#ifdef BUILD_WITH_NITROS + bool _enable_color_nitros = false; + std::map> _nitros_image_publishers; +#endif rclcpp::Publisher::SharedPtr _labeled_pointcloud_publisher; rclcpp::Publisher::SharedPtr _occupancy_publisher; std::map::SharedPtr> _imu_publishers; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 4f0b86404..b01a09c03 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -39,6 +39,7 @@ #define ROS_FATAL_STREAM(msg) RCLCPP_FATAL_STREAM(_logger, msg) #define ROS_DEBUG_STREAM_ONCE(msg) RCLCPP_DEBUG_STREAM_ONCE(_logger, msg) #define ROS_INFO_STREAM_ONCE(msg) RCLCPP_INFO_STREAM_ONCE(_logger, msg) +#define ROS_WARN_STREAM_ONCE(msg) RCLCPP_WARN_STREAM_ONCE(_logger, msg) #define ROS_WARN_STREAM_COND(cond, msg) RCLCPP_WARN_STREAM_EXPRESSION(_logger, cond, msg) #define ROS_WARN_ONCE(msg) RCLCPP_WARN_ONCE(_logger, msg) diff --git a/realsense2_camera/include/nitros_image_publisher.h b/realsense2_camera/include/nitros_image_publisher.h new file mode 100644 index 000000000..a4f246721 --- /dev/null +++ b/realsense2_camera/include/nitros_image_publisher.h @@ -0,0 +1,63 @@ +// Copyright 2026 RealSense, Inc. 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. + +#pragma once + +#ifdef BUILD_WITH_NITROS + +#include +#include +#include +#include +#include + +namespace realsense2_camera +{ +// Thin, GXF-free wrapper around nvidia::isaac_ros::nitros::ManagedNitrosPublisher. +// The heavy Isaac ROS / GXF / CUDA headers are confined to nitros_image_publisher.cpp, so the +// rest of the wrapper (base_realsense_node.*) only sees this handle and never pulls in NITROS. +// +// NOTE on ownership (Isaac ROS release-3.2): NitrosImageBuilder::WithGpuData() hands the pointer +// to a GXF VideoBuffer whose release callback calls cudaFree(). GXF therefore OWNS the buffer and +// frees it once downstream is done. We must NOT pass the librealsense frame-pool pointer directly +// (GXF would cudaFree memory the SDK owns). Instead publish() cudaMalloc's a fresh device buffer, +// device-to-device copies the frame's GPU pixels into it, and hands that to GXF. This still avoids +// the GPU->CPU->GPU round-trip to the perception graph (the goal); the D2D copy is the cost of +// release-3.2's owning model (release-3.4+/main add WithReleaseCallback for a true alias). +class NitrosImagePublisher +{ +public: + // nitros_format is a NITROS supported-type name (e.g. "nitros_image_rgb8"). + NitrosImagePublisher(rclcpp::Node * node, const std::string & topic, const std::string & nitros_format); + ~NitrosImagePublisher(); + + // gpu_src : CUDA device pointer to the frame pixels (aliases the SDK frame buffer). + // size_bytes: number of bytes to copy (width * height * bytes_per_pixel). + // encoding : sensor_msgs::image_encodings string matching nitros_format (e.g. "rgb8"). + // The frame's pixels are D2D-copied into a GXF-owned buffer; the caller need not keep the + // frame alive after this returns (the copy is synchronous). + void publish(const void * gpu_src, + uint32_t width, + uint32_t height, + size_t size_bytes, + const std::string & encoding, + const std_msgs::msg::Header & header); + +private: + struct Impl; + std::unique_ptr _impl; +}; +} // namespace realsense2_camera + +#endif // BUILD_WITH_NITROS diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 5853a74b5..46bc65263 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -19,6 +19,10 @@ cv_bridge image_transport librealsense2 + rclcpp diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 99156a689..3bbb91aee 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1252,6 +1252,12 @@ void BaseRealSenseNode::publishFrame( return; } +#ifdef BUILD_WITH_NITROS + // Additive: hand the color frame's GPU device pointer straight to NITROS (no CPU copy). + // Independent of the sensor_msgs publishers below, which keep working unchanged. + publishNitrosFrame(f, t, stream, width, height, stream_format); +#endif + // Publish stream image if (image_publishers.find(stream) != image_publishers.end()) { @@ -1346,6 +1352,62 @@ void BaseRealSenseNode::publishFrame( } } +#ifdef BUILD_WITH_NITROS +bool BaseRealSenseNode::getNitrosImageFormat( + const rs2_format& format, std::string& nitros_format, std::string& encoding, unsigned int& bpp) +{ + // Only the common color formats for now (color-only first cut). NITROS supported-type + // name (used by ManagedNitrosPublisher / negotiation) + matching sensor_msgs encoding + bpp. + switch (format) + { + case RS2_FORMAT_RGB8: nitros_format = "nitros_image_rgb8"; encoding = sensor_msgs::image_encodings::RGB8; bpp = 3; return true; + case RS2_FORMAT_BGR8: nitros_format = "nitros_image_bgr8"; encoding = sensor_msgs::image_encodings::BGR8; bpp = 3; return true; + case RS2_FORMAT_RGBA8: nitros_format = "nitros_image_rgba8"; encoding = sensor_msgs::image_encodings::RGBA8; bpp = 4; return true; + case RS2_FORMAT_BGRA8: nitros_format = "nitros_image_bgra8"; encoding = sensor_msgs::image_encodings::BGRA8; bpp = 4; return true; + default: return false; + } +} + +void BaseRealSenseNode::publishNitrosFrame( + rs2::frame f, const rclcpp::Time& t, const stream_index_pair& stream, + unsigned int width, unsigned int height, const rs2_format& stream_format) +{ + auto it = _nitros_image_publishers.find(stream); + if (it == _nitros_image_publishers.end()) + return; + + std::string nitros_format, encoding; + unsigned int bpp = 0; + if (!getNitrosImageFormat(stream_format, nitros_format, encoding, bpp)) + return; + + // Get a CUDA device pointer for the frame. On a librealsense built with + // BUILD_WITH_CUDA_ZEROCOPY running on an integrated GPU (Jetson), this aliases the + // GPU-mapped frame buffer with no host->device copy (copied=false). Otherwise the SDK + // uploads (copied=true) so the path still works; either way we get a device pointer. + bool copied = false; + const void* gpu_ptr = f.get_gpu_data_or_upload(&copied); + if (!gpu_ptr) + { + ROS_WARN_STREAM_ONCE("NITROS: no GPU pointer available for " << STREAM_NAME(stream) + << " (librealsense not built with CUDA?). Skipping NITROS publish."); + return; + } + + std_msgs::msg::Header header; + header.stamp = t; + header.frame_id = OPTICAL_FRAME_ID(stream); + + // The publisher D2D-copies the pixels into a GXF-owned buffer synchronously, so `f` (and the + // frame-pool buffer gpu_ptr aliases) only needs to stay alive for the duration of this call. + const size_t size_bytes = static_cast(width) * height * bpp; + it->second->publish(gpu_ptr, width, height, size_bytes, encoding, header); + + ROS_DEBUG_STREAM("NITROS " << STREAM_NAME(stream) << " published (" + << (copied ? "UPLOAD (host->device copy)" : "ZERO-COPY source") << ")"); +} +#endif + void BaseRealSenseNode::publishRGBD( const cv::Mat& rgb_cv_matrix, diff --git a/realsense2_camera/src/nitros_image_publisher.cpp b/realsense2_camera/src/nitros_image_publisher.cpp new file mode 100644 index 000000000..57bdb84c8 --- /dev/null +++ b/realsense2_camera/src/nitros_image_publisher.cpp @@ -0,0 +1,92 @@ +// Copyright 2026 RealSense, Inc. 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. + +#ifdef BUILD_WITH_NITROS + +#include "nitros_image_publisher.h" + +#include +#include +#include + +#include +#include + +namespace realsense2_camera +{ +using nvidia::isaac_ros::nitros::ManagedNitrosPublisher; +using nvidia::isaac_ros::nitros::NitrosImage; +using nvidia::isaac_ros::nitros::NitrosImageBuilder; + +struct NitrosImagePublisher::Impl +{ + std::shared_ptr> pub; +}; + +NitrosImagePublisher::NitrosImagePublisher( + rclcpp::Node * node, const std::string & topic, const std::string & nitros_format) +: _impl(std::make_unique()) +{ + // ManagedNitrosPublisher performs REP-2007/2009 type negotiation internally; we only + // choose the compatible data format (e.g. "nitros_image_rgb8"). + _impl->pub = std::make_shared>(node, topic, nitros_format); +} + +NitrosImagePublisher::~NitrosImagePublisher() = default; + +void NitrosImagePublisher::publish( + const void * gpu_src, + uint32_t width, + uint32_t height, + size_t size_bytes, + const std::string & encoding, + const std_msgs::msg::Header & header) +{ + // GXF will cudaFree() this buffer once downstream is done, so it must be a fresh cudaMalloc. + void * dev = nullptr; + cudaError_t err = cudaMalloc(&dev, size_bytes); + if (err != cudaSuccess) { + RCLCPP_WARN( + rclcpp::get_logger("NitrosImagePublisher"), + "cudaMalloc(%zu) failed: %s; dropping NITROS frame", size_bytes, cudaGetErrorString(err)); + return; + } + // Synchronous device-to-device copy from the frame's GPU pixels (cudaMemcpyDefault lets UVA + // resolve the source, which may be device-mapped pinned host memory on a zero-copy build). + err = cudaMemcpy(dev, gpu_src, size_bytes, cudaMemcpyDefault); + if (err != cudaSuccess) { + RCLCPP_WARN( + rclcpp::get_logger("NitrosImagePublisher"), + "cudaMemcpy(%zu) failed: %s; dropping NITROS frame", size_bytes, cudaGetErrorString(err)); + cudaFree(dev); + return; + } + + try { + NitrosImage img = NitrosImageBuilder() + .WithHeader(header) + .WithEncoding(encoding) + .WithDimensions(height, width) + .WithGpuData(dev) // ownership transfers to GXF (frees via cudaFree on release) + .Build(); + _impl->pub->publish(std::move(img)); + } catch (const std::exception & e) { + // Build() throws (e.g. odd dimensions / unsupported encoding) before taking ownership. + cudaFree(dev); + RCLCPP_WARN(rclcpp::get_logger("NitrosImagePublisher"), "NitrosImage Build failed: %s", e.what()); + } +} +} // namespace realsense2_camera + +#endif // BUILD_WITH_NITROS diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 02b22031b..76000d300 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -91,6 +91,14 @@ void BaseRealSenseNode::getParameters() _tf_prefix = _parameters->setParam(param_name, ""); _parameters_names.push_back(param_name); +#ifdef BUILD_WITH_NITROS + // Enable NITROS-native GPU zero-copy publishing of the color stream (Jetson / Isaac ROS). + // Read once at construction (publisher creation happens in rs_node_setup); default off. + param_name = std::string("enable_color_nitros"); + _enable_color_nitros = _parameters->setParam(param_name, false); + _parameters_names.push_back(param_name); +#endif + #if defined (ACCELERATE_GPU_WITH_GLSL) param_name = std::string("accelerate_gpu_with_glsl"); _parameters->setParam(param_name, false, diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 3eccadce0..abf1b6eaf 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -300,6 +300,30 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi } #endif +#ifdef BUILD_WITH_NITROS + // Additive NITROS-native GPU zero-copy publisher for the color stream. + if (_enable_color_nitros && profile.stream_type() == RS2_STREAM_COLOR) + { + std::string nitros_format, encoding; + unsigned int bpp = 0; + if (getNitrosImageFormat(profile.format(), nitros_format, encoding, bpp)) + { + std::stringstream nitros_topic; + nitros_topic << "~/" << stream_name << "/nitros_image"; + _nitros_image_publishers[sip] = + std::make_shared(&_node, nitros_topic.str(), nitros_format); + ROS_INFO_STREAM("NITROS color publisher created on topic " << nitros_topic.str() + << " (format " << nitros_format << ")"); + } + else + { + ROS_WARN_STREAM("NITROS color publishing requested but format " + << rs2_format_to_string(profile.format()) + << " is not supported; skipping NITROS for this stream."); + } + } +#endif + // create cameraInfo publishers only for non-SC streams if(shouldPublishCameraInfo(sip)) { From fbd0068fa2dad106e0dca28eba7187df153f121e Mon Sep 17 00:00:00 2001 From: Ashraf Kattoura Date: Wed, 15 Jul 2026 18:25:51 +0300 Subject: [PATCH 2/2] nitros_image_publisher: include explicitly Addresses Copilot review: we catch std::exception, so include the header directly instead of relying on a transitive include. Co-Authored-By: Claude Opus 4.8 --- realsense2_camera/src/nitros_image_publisher.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense2_camera/src/nitros_image_publisher.cpp b/realsense2_camera/src/nitros_image_publisher.cpp index 57bdb84c8..d8860b04b 100644 --- a/realsense2_camera/src/nitros_image_publisher.cpp +++ b/realsense2_camera/src/nitros_image_publisher.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace realsense2_camera