diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 67361a948c..3092c6d33e 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() @@ -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 ) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 34c5e8ebae..5718105dd7 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -49,6 +49,9 @@ #include #include +#include +#include + #include #include #include @@ -148,6 +151,8 @@ namespace realsense2_camera rclcpp::Service::SharedPtr _device_info_srv; std::shared_ptr _parameters; std::list _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& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); void stopPublishers(const std::vector& profiles); + void glfwPollEventCallback(); rs2::device _dev; std::map _sensors; diff --git a/realsense2_camera/include/gl_support.h b/realsense2_camera/include/gl_support.h new file mode 100644 index 0000000000..417c2022d2 --- /dev/null +++ b/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 // 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 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; +}; \ No newline at end of file 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..c0d7a3b7bc 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -25,6 +25,7 @@ #include using namespace realsense2_camera; +using namespace std::chrono_literals; SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher::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(std::make_shared(), _parameters, _logger)); _filters.push_back(std::make_shared(std::make_shared(), _parameters, _logger)); _filters.push_back(std::make_shared(std::make_shared(), _parameters, _logger)); @@ -229,13 +256,13 @@ 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); + _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR),/* _node, */update_align_depth_func, _parameters, _logger); _filters.push_back(_align_depth_filter); } diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index d98157f6d9..2aa88163fa 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -22,6 +22,7 @@ using namespace rs2; void BaseRealSenseNode::setup() { + initOpenGLProcessing(true); setDynamicParams(); startDiagnosticsUpdater(); setAvailableSensors();