Skip to content

Commit

Permalink
PR #2956 from Arun-Prasad-V: Extending LibRS's GL support to RS ROS2
Browse files Browse the repository at this point in the history
  • Loading branch information
Nir-Az authored Feb 1, 2024
2 parents 7bd5c60 + 4943275 commit 58593f6
Show file tree
Hide file tree
Showing 9 changed files with 209 additions and 6 deletions.
9 changes: 9 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down
31 changes: 28 additions & 3 deletions realsense2_camera/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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()
Expand Down Expand Up @@ -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
Expand All @@ -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)

Expand All @@ -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
Expand All @@ -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}
)
Expand Down
16 changes: 16 additions & 0 deletions realsense2_camera/include/base_realsense_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
}
Expand Down
84 changes: 84 additions & 0 deletions realsense2_camera/include/gl_window.h
Original file line number Diff line number Diff line change
@@ -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
1 change: 1 addition & 0 deletions realsense2_camera/launch/rs_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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]'},
Expand Down
18 changes: 15 additions & 3 deletions realsense2_camera/src/base_realsense_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
{
Expand All @@ -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();
Expand Down Expand Up @@ -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);
Expand Down
46 changes: 46 additions & 0 deletions realsense2_camera/src/gl_gpu_processing.cpp
Original file line number Diff line number Diff line change
@@ -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
6 changes: 6 additions & 0 deletions realsense2_camera/src/parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
4 changes: 4 additions & 0 deletions realsense2_camera/src/rs_node_setup.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit 58593f6

Please sign in to comment.