Extending LibRS's GL support to RS ROS2

README.md

@@ -354,6 +354,15 @@ User can set the camera name and camera namespace, to distinguish between camera
   - double, positive values set the period between diagnostics updates on the `/diagnostics` topic.
   - 0 or negative values mean no diagnostics topic is published. Defaults to 0.</br>
 The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams.
+- **accelerate_with_gpu**:
+  - GPU accelerated processing of PointCloud and Colorizer filters.
+  - integer:
+    - 0 --> **NO_GPU**: use only CPU for proccessing PointCloud and Colorizer filters
+    - 1 --> **GL_GPU**: use GLSL to accelerate GPU for processing PointCloud and Colorizer filters
+  - Note: To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build:
+    ```bash
+    colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON'
+    ```
 
 <hr>
 

realsense2_camera/CMakeLists.txt

@@ -117,6 +117,9 @@ find_package(tf2 REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 
 find_package(realsense2 2.54.1)
+if (BUILD_ACCELERATE_WITH_GPU)
+  find_package(realsense2-gl 2.54.1)
+endif()
 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()
@@ -141,6 +144,10 @@ set(SOURCES
     src/tfs.cpp
   )
 
+if (BUILD_ACCELERATE_WITH_GPU)
+  list(APPEND SOURCES src/gl_gpu_processing.cpp)
+endif()
+
 if(NOT DEFINED ENV{ROS_DISTRO})
   message(FATAL_ERROR "ROS_DISTRO is not defined." )
 endif()
@@ -171,6 +178,10 @@ if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "17.0")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRCLCPP_HAS_OnSetParametersCallbackType")
 endif()
 
+if (BUILD_ACCELERATE_WITH_GPU)
+  add_definitions(-DACCELERATE_WITH_GPU)
+endif()
+
 set(INCLUDES
     include/constants.h
     include/realsense_node_factory.h
@@ -184,6 +195,9 @@ set(INCLUDES
     include/profile_manager.h
     include/image_publisher.h)
 
+if (BUILD_ACCELERATE_WITH_GPU)
+  list(APPEND INCLUDES include/gl_window.h)
+endif()
 
 if (BUILD_TOOLS)
 
@@ -200,9 +214,15 @@ add_library(${PROJECT_NAME} SHARED
     ${SOURCES}
 )
 
+if (BUILD_ACCELERATE_WITH_GPU)
+  set(link_libraries ${realsense2-gl_LIBRARY})
+else()
+  set(link_libraries ${realsense2_LIBRARY})
+endif()
+
 target_link_libraries(${PROJECT_NAME}
-    ${realsense2_LIBRARY}
-    )
+    ${link_libraries}
+)
 
 set(dependencies
   cv_bridge
@@ -214,11 +234,16 @@ set(dependencies
   sensor_msgs
   nav_msgs
   tf2
-  realsense2
   tf2_ros
   diagnostic_updater
 )
 
+if (BUILD_ACCELERATE_WITH_GPU)
+  list(APPEND dependencies realsense2-gl)
+else()
+  list(APPEND dependencies realsense2)
+endif()
+
 ament_target_dependencies(${PROJECT_NAME}
   ${dependencies}
 )

realsense2_camera/include/base_realsense_node.h

@@ -49,6 +49,10 @@
 #include <ros_sensor.h>
 #include <named_filter.h>
 
+#if defined (ACCELERATE_WITH_GPU)
+#include <gl_window.h>
+#endif
+
 #include <queue>
 #include <mutex>
 #include <atomic>
@@ -115,6 +119,7 @@ namespace realsense2_camera
 
     public:
         enum class imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION};
+        enum class accelerate_with_gpu {NO_GPU, GL_GPU};
 
     protected:
         class float3
@@ -261,6 +266,12 @@ namespace realsense2_camera
         void startRGBDPublisherIfNeeded();
         void stopPublishers(const std::vector<rs2::stream_profile>& profiles);
 
+#if defined (ACCELERATE_WITH_GPU)
+        void initOpenGLProcessing(bool use_gpu_processing);
+        void shutdownOpenGLProcessing();
+        void glfwPollEventCallback();
+#endif
+
         rs2::device _dev;
         std::map<stream_index_pair, rs2::sensor> _sensors;
         std::map<std::string, std::function<void(rs2::frame)>> _sensors_callback;
@@ -342,6 +353,11 @@ namespace realsense2_camera
         std::shared_ptr<diagnostic_updater::Updater> _diagnostics_updater;
         rs2::stream_profile _base_profile;
 
+#if defined (ACCELERATE_WITH_GPU)
+        GLwindow _app;
+        rclcpp::TimerBase::SharedPtr _timer;
+        accelerate_with_gpu _accelerate_with_gpu;
+#endif
 
     };//end class
 }

realsense2_camera/include/gl_window.h

@@ -0,0 +1,84 @@
+// 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.
+
+#pragma once
+
+#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
+
+#if defined (ACCELERATE_WITH_GPU)
+
+#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();
+        glfwWindowHint(GLFW_VISIBLE, 0);
+        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;
+};
+
+#endif

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': 'accelerate_with_gpu',          '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/src/base_realsense_node.cpp

@@ -115,6 +115,9 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _imu_sync_method(imu_sync_method::NONE),
     _is_profile_changed(false),
     _is_align_depth_changed(false)
+#if defined (ACCELERATE_WITH_GPU)
+    ,_app(1280, 720, "RS_GLFW_Window")
+#endif
 {
     if ( use_intra_process )
     {
@@ -127,6 +130,10 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
 
 BaseRealSenseNode::~BaseRealSenseNode()
 {
+#if defined (ACCELERATE_WITH_GPU)
+    shutdownOpenGLProcessing();
+#endif
+
     // Kill dynamic transform thread
     _is_running = false;
     _cv_tf.notify_one();
@@ -229,10 +236,15 @@ void BaseRealSenseNode::setupFilters()
         _cv_mpc.notify_one();
     };
 
-    _colorizer_filter = std::make_shared<NamedFilter>(std::make_shared<rs2::colorizer>(), _parameters, _logger); 
-    _filters.push_back(_colorizer_filter);
-
+#if defined (ACCELERATE_WITH_GPU)
+    _colorizer_filter = std::make_shared<NamedFilter>(std::make_shared<rs2::gl::colorizer>(), _parameters, _logger); 
+    _pc_filter = std::make_shared<PointcloudFilter>(std::make_shared<rs2::gl::pointcloud>(), _node, _parameters, _logger);
+#else
+    _colorizer_filter = std::make_shared<NamedFilter>(std::make_shared<rs2::colorizer>(), _parameters, _logger);
     _pc_filter = std::make_shared<PointcloudFilter>(std::make_shared<rs2::pointcloud>(), _node, _parameters, _logger);
+#endif
+
+    _filters.push_back(_colorizer_filter);
     _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,46 @@
+// 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;
+
+#if defined (ACCELERATE_WITH_GPU)
+
+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);
+    if (use_gpu_processing)
+        rs2::gl::init_rendering();
+
+    _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()
+{
+
+}
+
+#endif

realsense2_camera/src/parameters.cpp

@@ -82,6 +82,12 @@ 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);
+
+#if defined (ACCELERATE_WITH_GPU)
+    param_name = std::string("accelerate_with_gpu");
+    _accelerate_with_gpu = accelerate_with_gpu(_parameters->setParam<int>(param_name, int(accelerate_with_gpu::NO_GPU)));
+    _parameters_names.push_back(param_name);
+#endif
 }
 
 void BaseRealSenseNode::setDynamicParams()

realsense2_camera/src/rs_node_setup.cpp

@@ -22,6 +22,10 @@ using namespace rs2;
 
 void BaseRealSenseNode::setup()
 {
+#if defined (ACCELERATE_WITH_GPU)
+    bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU);
+    initOpenGLProcessing(use_gpu_processing);
+#endif
     setDynamicParams();
     startDiagnosticsUpdater();
     setAvailableSensors();