Utilizing LRS's GL support in 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()
@@ -202,6 +203,7 @@ add_library(${PROJECT_NAME} SHARED
 
 target_link_libraries(${PROJECT_NAME}
     ${realsense2_LIBRARY}
+    ${realsense2-gl_LIBRARY}
     )
 
 set(dependencies
@@ -215,6 +217,7 @@ set(dependencies
   nav_msgs
   tf2
   realsense2
+  realsense2-gl
   tf2_ros
   diagnostic_updater
 )

realsense2_camera/include/base_realsense_node.h

@@ -49,6 +49,9 @@
 #include <ros_sensor.h>
 #include <named_filter.h>
 
+#include <gl_support.h>
+#include <chrono>
+
 #include <queue>
 #include <mutex>
 #include <atomic>
@@ -148,6 +151,8 @@ namespace realsense2_camera
         rclcpp::Service<realsense2_camera_msgs::srv::DeviceInfo>::SharedPtr _device_info_srv;
         std::shared_ptr<Parameters> _parameters;
         std::list<std::string> _parameters_names;
+        window _app;
+        rclcpp::TimerBase::SharedPtr _timer;
 
         void restartStaticTransformBroadcaster();
         void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex);
@@ -164,6 +169,8 @@ namespace realsense2_camera
                                  const std::string& child_frame_id);
         void eraseTransformMsgs(const stream_index_pair& sip, const rs2::stream_profile& profile);
         void setup();
+        void initOpenGLProcessing(bool use_gpu_processing);
+        void shutdownOpenGLProcessing();
 
     private:
         class CimuData
@@ -260,6 +267,7 @@ namespace realsense2_camera
         void startPublishers(const std::vector<rs2::stream_profile>& profiles, const RosSensor& sensor);
         void startRGBDPublisherIfNeeded();
         void stopPublishers(const std::vector<rs2::stream_profile>& profiles);
+        void glfwPollEventCallback();
 
         rs2::device _dev;
         std::map<stream_index_pair, rs2::sensor> _sensors;

realsense2_camera/include/gl_support.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 window
+{
+public:
+
+    window(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);
+
+    }
+
+    ~window()
+    {
+        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/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

@@ -25,6 +25,7 @@
 #include <tf2_ros/qos.hpp>
 
 using namespace realsense2_camera;
+using namespace std::chrono_literals;
 
 SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher, 
                                        std::size_t waiting_list_size):
@@ -92,6 +93,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _node(node),
     _logger(node.get_logger()),
     _parameters(parameters),
+    _app(1280, 720, "RS"),
     _dev(dev),
     _json_file_path(""),
     _depth_scale_meters(0),
@@ -127,6 +129,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
 
 BaseRealSenseNode::~BaseRealSenseNode()
 {
+    shutdownOpenGLProcessing();
+
     // Kill dynamic transform thread
     _is_running = false;
     _cv_tf.notify_one();
@@ -150,6 +154,27 @@ BaseRealSenseNode::~BaseRealSenseNode()
     }
 }
 
+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();
+}
+
 void BaseRealSenseNode::hardwareResetRequest()
 {
     ROS_ERROR_STREAM("Performing Hardware Reset.");
@@ -206,6 +231,8 @@ void BaseRealSenseNode::initializeFormatsMaps()
 
 void BaseRealSenseNode::setupFilters()
 {
+
+
     _filters.push_back(std::make_shared<NamedFilter>(std::make_shared<rs2::decimation_filter>(), _parameters, _logger));
     _filters.push_back(std::make_shared<NamedFilter>(std::make_shared<rs2::hdr_merge>(), _parameters, _logger));
     _filters.push_back(std::make_shared<NamedFilter>(std::make_shared<rs2::sequence_id_filter>(), _parameters, _logger));
@@ -229,13 +256,13 @@ 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);
+    _align_depth_filter = std::make_shared<AlignDepthFilter>(std::make_shared<rs2::align>(RS2_STREAM_COLOR),/* _node, */update_align_depth_func, _parameters, _logger);
     _filters.push_back(_align_depth_filter);
 }
 

realsense2_camera/src/rs_node_setup.cpp

@@ -22,6 +22,7 @@ using namespace rs2;
 
 void BaseRealSenseNode::setup()
 {
+    initOpenGLProcessing(true);
     setDynamicParams();
     startDiagnosticsUpdater();
     setAvailableSensors();