From 04d3df41ec4d238d604b4b1b222fcc0811d24485 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 29 Dec 2023 23:18:02 +0530 Subject: [PATCH] Extending LibRS's GL support to RS ROS2 --- realsense2_camera/CMakeLists.txt | 4 ++ .../include/base_realsense_node.h | 9 +++ realsense2_camera/include/gl_window.h | 68 +++++++++++++++++++ realsense2_camera/launch/rs_launch.py | 1 + realsense2_camera/package.xml | 2 +- realsense2_camera/src/base_realsense_node.cpp | 9 ++- realsense2_camera/src/gl_gpu_processing.cpp | 40 +++++++++++ realsense2_camera/src/parameters.cpp | 4 ++ realsense2_camera/src/rs_node_setup.cpp | 2 + 9 files changed, 135 insertions(+), 4 deletions(-) create mode 100644 realsense2_camera/include/gl_window.h create mode 100644 realsense2_camera/src/gl_gpu_processing.cpp diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 67361a948c..02105687a5 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/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 ) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 34c5e8ebae..e70071640c 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -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& profiles); + void initOpenGLProcessing(bool use_gpu_processing); + void shutdownOpenGLProcessing(); + void glfwPollEventCallback(); + rs2::device _dev; std::map _sensors; std::map> _sensors_callback; @@ -342,6 +348,9 @@ namespace realsense2_camera std::shared_ptr _diagnostics_updater; rs2::stream_profile _base_profile; + GLwindow _app; + rclcpp::TimerBase::SharedPtr _timer; + data_processing_mode _data_processing_mode; };//end class } diff --git a/realsense2_camera/include/gl_window.h b/realsense2_camera/include/gl_window.h new file mode 100644 index 0000000000..fd0a50926a --- /dev/null +++ b/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 // Include RealSense Cross Platform API + +#define GL_SILENCE_DEPRECATION +#define GLFW_INCLUDE_GLU +#include +#include +#include + +#include // 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; +}; diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index c6c14db6c4..4a1ea19294 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/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]'}, diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 6db46cff68..59cd0c80f3 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -17,7 +17,7 @@ builtin_interfaces cv_bridge image_transport - librealsense2 + librealsense2-gl rclcpp rclcpp_components realsense2_camera_msgs diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index a3ebd0e675..90bc1c0e8d 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/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(std::make_shared(), _parameters, _logger); + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _filters.push_back(_colorizer_filter); - _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); + _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); _filters.push_back(_pc_filter); _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp new file mode 100644 index 0000000000..412e8f7a40 --- /dev/null +++ b/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 + +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(); +} diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 72523cb801..44779123bf 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -82,6 +82,10 @@ void BaseRealSenseNode::getParameters() _base_frame_id = _parameters->setParam(param_name, DEFAULT_BASE_FRAME_ID); _base_frame_id = (static_cast(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(param_name, int(data_processing_mode::NO_GPU))); + _parameters_names.push_back(param_name); } void BaseRealSenseNode::setDynamicParams() diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index d98157f6d9..f70eb31606 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/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();