diff --git a/README.md b/README.md index a77621972e..731dd42c2c 100644 --- a/README.md +++ b/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.
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' + ```
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 67361a948c..1d3ad2ac81 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/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} ) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 34c5e8ebae..7aa736c2c6 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -49,6 +49,10 @@ #include #include +#if defined (ACCELERATE_WITH_GPU) +#include +#endif + #include #include #include @@ -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& profiles); +#if defined (ACCELERATE_WITH_GPU) + void initOpenGLProcessing(bool use_gpu_processing); + void shutdownOpenGLProcessing(); + void glfwPollEventCallback(); +#endif + rs2::device _dev; std::map _sensors; std::map> _sensors_callback; @@ -342,6 +353,11 @@ namespace realsense2_camera std::shared_ptr _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 } diff --git a/realsense2_camera/include/gl_window.h b/realsense2_camera/include/gl_window.h new file mode 100644 index 0000000000..a1cebb7af8 --- /dev/null +++ b/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 // Include RealSense Cross Platform API + +#if defined (ACCELERATE_WITH_GPU) + +#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(); + 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 diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index c6c14db6c4..fa6dfaabf2 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': '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]'}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index a3ebd0e675..62493d8896 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/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(std::make_shared(), _parameters, _logger); - _filters.push_back(_colorizer_filter); - +#if defined (ACCELERATE_WITH_GPU) + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); + _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); +#else + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); +#endif + + _filters.push_back(_colorizer_filter); _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..a43f333a9b --- /dev/null +++ b/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 + +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 diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 72523cb801..4ee2caee06 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -82,6 +82,12 @@ 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); + +#if defined (ACCELERATE_WITH_GPU) + param_name = std::string("accelerate_with_gpu"); + _accelerate_with_gpu = accelerate_with_gpu(_parameters->setParam(param_name, int(accelerate_with_gpu::NO_GPU))); + _parameters_names.push_back(param_name); +#endif } void BaseRealSenseNode::setDynamicParams() diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index d98157f6d9..76243425b5 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/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();