zero copy nitros ros

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

realsense2_camera/include/base_realsense_node.h

@@ -17,6 +17,9 @@
 #include <librealsense2/rs.hpp>
 #include <librealsense2/rsutil.h>
 #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<stream_index_pair, std::shared_ptr<image_publisher>>& 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<geometry_msgs::msg::TransformStamped> _static_tf_msgs;
         std::shared_ptr<std::thread> _tf_t;
 
-        bool _use_intra_process;      
+        bool _use_intra_process;
         std::map<stream_index_pair, std::shared_ptr<image_publisher>> _image_publishers;
+#ifdef BUILD_WITH_NITROS
+        bool _enable_color_nitros = false;
+        std::map<stream_index_pair, std::shared_ptr<NitrosImagePublisher>> _nitros_image_publishers;
+#endif
         rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _labeled_pointcloud_publisher;
         rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr _occupancy_publisher;
         std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;

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)

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 <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <functional>
+#include <memory>
+#include <string>
+
+namespace realsense2_camera
+{
+// Thin, GXF-free wrapper around nvidia::isaac_ros::nitros::ManagedNitrosPublisher<NitrosImage>.
+// 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> _impl;
+};
+}  // namespace realsense2_camera
+
+#endif  // BUILD_WITH_NITROS

realsense2_camera/package.xml

@@ -19,6 +19,10 @@
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>librealsense2</depend>
+  <!-- Optional NITROS GPU zero-copy publishing (-DBUILD_WITH_NITROS=ON), like the optional
+       realsense2-gl dep these are not declared here: they are provided by an Isaac ROS
+       workspace (isaac_ros_nitros, isaac_ros_managed_nitros, isaac_ros_nitros_image_type)
+       and only needed for that build. -->
   <depend>rclcpp</depend>
 
   <!-- Lifecycle dependencies are optional -DUSE_LIFECYCLE_NODE -->

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<size_t>(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,

realsense2_camera/src/nitros_image_publisher.cpp

@@ -0,0 +1,93 @@
+// 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 <isaac_ros_managed_nitros/managed_nitros_publisher.hpp>
+#include <isaac_ros_nitros_image_type/nitros_image.hpp>
+#include <isaac_ros_nitros_image_type/nitros_image_builder.hpp>
+
+#include <cuda_runtime.h>
+#include <exception>
+#include <utility>
+
+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<ManagedNitrosPublisher<NitrosImage>> pub;
+};
+
+NitrosImagePublisher::NitrosImagePublisher(
+    rclcpp::Node * node, const std::string & topic, const std::string & nitros_format)
+: _impl(std::make_unique<Impl>())
+{
+    // 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<ManagedNitrosPublisher<NitrosImage>>(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

realsense2_camera/src/parameters.cpp

@@ -91,6 +91,14 @@ void BaseRealSenseNode::getParameters()
     _tf_prefix = _parameters->setParam<std::string>(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<bool>(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<bool>(param_name, false,