Extending LibRS's GL support to RS ROS2

realsense2_camera/CMakeLists.txt

@@ -117,6 +117,7 @@ find_package(tf2 REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 
 find_package(realsense2 2.54.1)
+find_package(realsense2-gl 2.54.1)
 if(NOT realsense2_FOUND)
     message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n")
 endif()
@@ -139,6 +140,7 @@ set(SOURCES
     src/profile_manager.cpp
     src/image_publisher.cpp
     src/tfs.cpp
+    src/gl_gpu_processing.cpp
   )
 
 if(NOT DEFINED ENV{ROS_DISTRO})
@@ -202,6 +204,7 @@ add_library(${PROJECT_NAME} SHARED
 
 target_link_libraries(${PROJECT_NAME}
     ${realsense2_LIBRARY}
+    ${realsense2-gl_LIBRARY}
     )
 
 set(dependencies
@@ -215,6 +218,7 @@ set(dependencies
   nav_msgs
   tf2
   realsense2
+  realsense2-gl
   tf2_ros
   diagnostic_updater
 )

realsense2_camera/include/base_realsense_node.h

@@ -48,6 +48,7 @@
 
 #include <ros_sensor.h>
 #include <named_filter.h>
+#include <gl_window.h>
 
 #include <queue>
 #include <mutex>
@@ -115,6 +116,7 @@ namespace realsense2_camera
 
     public:
         enum class imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION};
+        enum class data_processing_mode {NO_GPU, GL_GPU};
 
     protected:
         class float3
@@ -261,6 +263,10 @@ namespace realsense2_camera
         void startRGBDPublisherIfNeeded();
         void stopPublishers(const std::vector<rs2::stream_profile>& profiles);
 
+        void initOpenGLProcessing(bool use_gpu_processing);
+        void shutdownOpenGLProcessing();
+        void glfwPollEventCallback();
+
         rs2::device _dev;
         std::map<stream_index_pair, rs2::sensor> _sensors;
         std::map<std::string, std::function<void(rs2::frame)>> _sensors_callback;
@@ -342,6 +348,9 @@ namespace realsense2_camera
         std::shared_ptr<diagnostic_updater::Updater> _diagnostics_updater;
         rs2::stream_profile _base_profile;
 
+        GLwindow _app;
+        rclcpp::TimerBase::SharedPtr _timer;
+        data_processing_mode _data_processing_mode;
 
     };//end class
 }

realsense2_camera/include/gl_window.h

@@ -0,0 +1,68 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2015 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
+
+#define GL_SILENCE_DEPRECATION
+#define GLFW_INCLUDE_GLU
+#include <GLFW/glfw3.h>
+#include <GL/gl.h>
+#include <iostream>
+
+#include <librealsense2-gl/rs_processing_gl.hpp> // Include GPU-Processing API
+
+
+#ifndef PI
+#define PI  3.14159265358979323846
+#define PI_FL  3.141592f
+#endif
+
+
+class GLwindow
+{
+public:
+
+    GLwindow(int width, int height, const char* title)
+        : _width(width), _height(height)
+    {
+        glfwInit();
+        win = glfwCreateWindow(width, height, title, nullptr, nullptr);
+        if (!win)
+            throw std::runtime_error("Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools");
+        glfwMakeContextCurrent(win);
+
+        glfwSetWindowUserPointer(win, this);
+
+    }
+
+    ~GLwindow()
+    {
+        glfwDestroyWindow(win);
+        glfwTerminate();
+    }
+
+    void close()
+    {
+        glfwSetWindowShouldClose(win, 1);
+    }
+
+    float width() const { return float(_width); }
+    float height() const { return float(_height); }
+
+    operator bool()
+    {
+        auto res = !glfwWindowShouldClose(win);
+
+        glfwPollEvents();
+
+        return res;
+    }
+
+    operator GLFWwindow* () { return win; }
+
+private:
+    GLFWwindow* win;
+    int _width, _height;
+};

realsense2_camera/launch/rs_launch.py

@@ -29,6 +29,7 @@
                            {'name': 'config_file',                  'default': "''", 'description': 'yaml config file'},
                            {'name': 'json_file_path',               'default': "''", 'description': 'allows advanced configuration'},
                            {'name': 'initial_reset',                'default': 'false', 'description': "''"},
+                           {'name': 'data_processing_mode',         'default': "0", 'description': '[0-No_GPU, 1-GL_GPU]'},
                            {'name': 'rosbag_filename',              'default': "''", 'description': 'A realsense bagfile to run from as a device'},
                            {'name': 'log_level',                    'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
                            {'name': 'output',                       'default': 'screen', 'description': 'pipe node output [screen|log]'},

realsense2_camera/package.xml

@@ -17,7 +17,7 @@
   <depend>builtin_interfaces</depend>
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
-  <depend>librealsense2</depend>
+  <depend>librealsense2-gl</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>realsense2_camera_msgs</depend>

realsense2_camera/src/base_realsense_node.cpp

@@ -114,7 +114,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _pointcloud(false),
     _imu_sync_method(imu_sync_method::NONE),
     _is_profile_changed(false),
-    _is_align_depth_changed(false)
+    _is_align_depth_changed(false),
+    _app(1280, 720, "RS_GLFW_Window")
 {
     if ( use_intra_process )
     {
@@ -127,6 +128,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
 
 BaseRealSenseNode::~BaseRealSenseNode()
 {
+    shutdownOpenGLProcessing();
+
     // Kill dynamic transform thread
     _is_running = false;
     _cv_tf.notify_one();
@@ -229,10 +232,10 @@ void BaseRealSenseNode::setupFilters()
         _cv_mpc.notify_one();
     };
 
-    _colorizer_filter = std::make_shared<NamedFilter>(std::make_shared<rs2::colorizer>(), _parameters, _logger); 
+    _colorizer_filter = std::make_shared<NamedFilter>(std::make_shared<rs2::gl::colorizer>(), _parameters, _logger); 
     _filters.push_back(_colorizer_filter);
 
-    _pc_filter = std::make_shared<PointcloudFilter>(std::make_shared<rs2::pointcloud>(), _node, _parameters, _logger);
+    _pc_filter = std::make_shared<PointcloudFilter>(std::make_shared<rs2::gl::pointcloud>(), _node, _parameters, _logger);
     _filters.push_back(_pc_filter);
 
     _align_depth_filter = std::make_shared<AlignDepthFilter>(std::make_shared<rs2::align>(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger);

realsense2_camera/src/gl_gpu_processing.cpp

@@ -0,0 +1,40 @@
+// Copyright 2023 Intel Corporation. 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.
+
+#include "../include/base_realsense_node.h"
+#include <chrono>
+
+using namespace realsense2_camera;
+using namespace std::chrono_literals;
+
+void BaseRealSenseNode::initOpenGLProcessing(bool use_gpu_processing)
+{
+    // Once we have a window, initialize GL module
+    // Pass our window to enable sharing of textures between processed frames and the window
+    // The "use_gpu_processing" is going to control if we will use CPU or GPU for data processing
+    rs2::gl::init_processing(_app, use_gpu_processing);
+
+    _timer = _node.create_wall_timer(1000ms, std::bind(&BaseRealSenseNode::glfwPollEventCallback, this));
+}
+
+void BaseRealSenseNode::glfwPollEventCallback()
+{
+    // Must poll the GLFW events perodically, else window will hang or crash
+    glfwPollEvents();
+}
+
+void BaseRealSenseNode::shutdownOpenGLProcessing()
+{
+    rs2::gl::shutdown_processing();
+}

realsense2_camera/src/parameters.cpp

@@ -82,6 +82,10 @@ void BaseRealSenseNode::getParameters()
     _base_frame_id = _parameters->setParam<std::string>(param_name, DEFAULT_BASE_FRAME_ID);
     _base_frame_id = (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str();
     _parameters_names.push_back(param_name);
+
+    param_name = std::string("data_processing_mode");
+    _data_processing_mode = data_processing_mode(_parameters->setParam<int>(param_name, int(data_processing_mode::NO_GPU)));
+    _parameters_names.push_back(param_name);
 }
 
 void BaseRealSenseNode::setDynamicParams()

realsense2_camera/src/rs_node_setup.cpp

@@ -22,6 +22,8 @@ using namespace rs2;
 
 void BaseRealSenseNode::setup()
 {
+    bool use_gpu_processing = (_data_processing_mode == data_processing_mode::GL_GPU);
+    initOpenGLProcessing(use_gpu_processing);
     setDynamicParams();
     startDiagnosticsUpdater();
     setAvailableSensors();