From 6036e6020729365f356f670ee098d5a3b660f24a Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 2 Feb 2024 04:53:06 +0530 Subject: [PATCH 1/3] Added support for dynamically switching b/w CPU / GPU processing --- .../include/base_realsense_node.h | 3 + realsense2_camera/src/base_realsense_node.cpp | 8 +- realsense2_camera/src/gl_gpu_processing.cpp | 3 +- realsense2_camera/src/parameters.cpp | 14 +- realsense2_camera/src/rs_node_setup.cpp | 130 +++++++++++++++++- 5 files changed, 150 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 7aa736c2c6..d54cecd7dc 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -261,6 +261,8 @@ namespace realsense2_camera void setAvailableSensors(); void setCallbackFunctions(); void updateSensors(); + void startVideoSensors(); + void stopVideoSensors(); void publishServices(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); @@ -357,6 +359,7 @@ namespace realsense2_camera GLwindow _app; rclcpp::TimerBase::SharedPtr _timer; accelerate_with_gpu _accelerate_with_gpu; + bool _is_accelerate_with_gpu_changed; #endif };//end class diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 62493d8896..6c610b7171 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -116,7 +116,9 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_profile_changed(false), _is_align_depth_changed(false) #if defined (ACCELERATE_WITH_GPU) - ,_app(1280, 720, "RS_GLFW_Window") + ,_app(1280, 720, "RS_GLFW_Window"), + _accelerate_with_gpu(accelerate_with_gpu::NO_GPU), + _is_accelerate_with_gpu_changed(false) #endif { if ( use_intra_process ) @@ -130,10 +132,6 @@ 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(); diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp index a43f333a9b..8bbf8c4b44 100644 --- a/realsense2_camera/src/gl_gpu_processing.cpp +++ b/realsense2_camera/src/gl_gpu_processing.cpp @@ -40,7 +40,8 @@ void BaseRealSenseNode::glfwPollEventCallback() void BaseRealSenseNode::shutdownOpenGLProcessing() { - + rs2::gl::shutdown_rendering(); + rs2::gl::shutdown_processing(); } #endif diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 4ee2caee06..0747c49c42 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -85,9 +85,21 @@ void BaseRealSenseNode::getParameters() #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->setParam(param_name, int(accelerate_with_gpu::NO_GPU), + [this](const rclcpp::Parameter& parameter) + { + accelerate_with_gpu temp_value = accelerate_with_gpu(parameter.get_value()); + if (_accelerate_with_gpu != temp_value) + { + _accelerate_with_gpu = temp_value; + std::lock_guard lock_guard(_profile_changes_mutex); + _is_accelerate_with_gpu_changed = true; + } + _cv_mpc.notify_one(); + }); _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 76243425b5..198384bebc 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -25,6 +25,7 @@ void BaseRealSenseNode::setup() #if defined (ACCELERATE_WITH_GPU) bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); initOpenGLProcessing(use_gpu_processing); + _is_accelerate_with_gpu_changed = false; #endif setDynamicParams(); startDiagnosticsUpdater(); @@ -43,7 +44,11 @@ void BaseRealSenseNode::monitoringProfileChanges() std::function func = [this, time_interval](){ std::unique_lock lock(_profile_changes_mutex); while(_is_running) { - _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed);}); + _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed +#if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed +#endif + );}); if (_is_running && (_is_profile_changed || _is_align_depth_changed)) { ROS_DEBUG("Profile has changed"); @@ -58,6 +63,27 @@ void BaseRealSenseNode::monitoringProfileChanges() _is_profile_changed = false; _is_align_depth_changed = false; } +#if defined (ACCELERATE_WITH_GPU) + if (_is_running && _is_accelerate_with_gpu_changed) + { + ROS_DEBUG("Accelerate_with_gpu changed"); + try + { + stopVideoSensors(); + shutdownOpenGLProcessing(); + + bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); + initOpenGLProcessing(use_gpu_processing); + + startVideoSensors(); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("Error updating the sensors: " << e.what()); + } + _is_accelerate_with_gpu_changed = false; + } +#endif } }; _monitoring_pc = std::make_shared(func); @@ -406,6 +432,108 @@ void BaseRealSenseNode::updateSensors() } } +void BaseRealSenseNode::stopVideoSensors() +{ + std::lock_guard lock_guard(_update_sensor_mutex); + try{ + for(auto&& sensor : _available_ros_sensors) + { + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + + bool is_video_sensor = (sensor->is() || sensor->is()); + + // do all updates if profile has been changed, or if the align depth filter status has been changed + // and we are on a video sensor. TODO: explore better options to monitor and update changes + // without resetting the whole sensors and topics. + if (is_video_sensor) + { + std::vector active_profiles = sensor->get_active_streams(); + + // Start/stop sensors only if profile was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Stopping Sensor: " << module_name); + sensor->stop(); + + stopPublishers(active_profiles); + } + } + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "Unknown exception has occured!"); + throw; + } +} + +void BaseRealSenseNode::startVideoSensors() +{ + std::lock_guard lock_guard(_update_sensor_mutex); + try{ + for(auto&& sensor : _available_ros_sensors) + { + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + // if active_profiles != wanted_profiles: stop sensor. + std::vector wanted_profiles; + + sensor->getUpdatedProfiles(wanted_profiles); + bool is_video_sensor = (sensor->is() || sensor->is()); + + // do all updates if profile has been changed, or if the align depth filter status has been changed + // and we are on a video sensor. TODO: explore better options to monitor and update changes + // without resetting the whole sensors and topics. + if (is_video_sensor) + { + std::vector active_profiles = sensor->get_active_streams(); + + if (!wanted_profiles.empty()) + { + startPublishers(wanted_profiles, *sensor); + updateProfilesStreamCalibData(wanted_profiles); + if (_publish_tf) + { + std::lock_guard lock_guard(_publish_tf_mutex); + for (auto &profile : wanted_profiles) + { + calcAndAppendTransformMsgs(profile, _base_profile); + } + } + + // Start/stop sensors only if profile was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Starting Sensor: " << module_name); + sensor->start(wanted_profiles); + + if (sensor->rs2::sensor::is()) + { + _depth_scale_meters = sensor->as().get_depth_scale(); + } + } + } + } + if (_publish_tf) + { + std::lock_guard lock_guard(_publish_tf_mutex); + publishStaticTransforms(); + } + startRGBDPublisherIfNeeded(); + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "Unknown exception has occured!"); + throw; + } +} + void BaseRealSenseNode::publishServices() { // adding "~/" to the service name will add node namespace and node name to the service From 57f819ab68f77f34dde9d965b63930d0ea7cf215 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 9 Feb 2024 04:18:53 +0530 Subject: [PATCH 2/3] Updated updateSensors() function --- .../include/base_realsense_node.h | 4 +- realsense2_camera/src/rs_node_setup.cpp | 168 +++++++----------- 2 files changed, 67 insertions(+), 105 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d54cecd7dc..903d70f805 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -261,8 +261,8 @@ namespace realsense2_camera void setAvailableSensors(); void setCallbackFunctions(); void updateSensors(); - void startVideoSensors(); - void stopVideoSensors(); + void startUpdatedSensors(); + void stopRequiredSensors(); void publishServices(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 198384bebc..ad4f8abe8e 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -44,12 +44,20 @@ void BaseRealSenseNode::monitoringProfileChanges() std::function func = [this, time_interval](){ std::unique_lock lock(_profile_changes_mutex); while(_is_running) { - _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed -#if defined (ACCELERATE_WITH_GPU) - || _is_accelerate_with_gpu_changed -#endif - );}); - if (_is_running && (_is_profile_changed || _is_align_depth_changed)) + _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), + [&]{return (!_is_running || _is_profile_changed + || _is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + );}); + + if (_is_running && (_is_profile_changed + || _is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + )) { ROS_DEBUG("Profile has changed"); try @@ -62,28 +70,11 @@ void BaseRealSenseNode::monitoringProfileChanges() } _is_profile_changed = false; _is_align_depth_changed = false; - } -#if defined (ACCELERATE_WITH_GPU) - if (_is_running && _is_accelerate_with_gpu_changed) - { - ROS_DEBUG("Accelerate_with_gpu changed"); - try - { - stopVideoSensors(); - shutdownOpenGLProcessing(); - - bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); - initOpenGLProcessing(use_gpu_processing); - startVideoSensors(); - } - catch(const std::exception& e) - { - ROS_ERROR_STREAM("Error updating the sensors: " << e.what()); - } - _is_accelerate_with_gpu_changed = false; + #if defined (ACCELERATE_WITH_GPU) + _is_accelerate_with_gpu_changed = false; + #endif } -#endif } }; _monitoring_pc = std::make_shared(func); @@ -361,64 +352,19 @@ void BaseRealSenseNode::updateSensors() { std::lock_guard lock_guard(_update_sensor_mutex); try{ - for(auto&& sensor : _available_ros_sensors) - { - std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); - // if active_profiles != wanted_profiles: stop sensor. - std::vector wanted_profiles; - - bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); - bool is_video_sensor = (sensor->is() || sensor->is()); + stopRequiredSensors(); - // do all updates if profile has been changed, or if the align depth filter status has been changed - // and we are on a video sensor. TODO: explore better options to monitor and update changes - // without resetting the whole sensors and topics. - if (is_profile_changed || (_is_align_depth_changed && is_video_sensor)) + #if defined (ACCELERATE_WITH_GPU) + if (_is_accelerate_with_gpu_changed) { - std::vector active_profiles = sensor->get_active_streams(); - if(is_profile_changed) - { - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Stopping Sensor: " << module_name); - sensor->stop(); - } - stopPublishers(active_profiles); - - if (!wanted_profiles.empty()) - { - startPublishers(wanted_profiles, *sensor); - updateProfilesStreamCalibData(wanted_profiles); - if (_publish_tf) - { - std::lock_guard lock_guard(_publish_tf_mutex); - for (auto &profile : wanted_profiles) - { - calcAndAppendTransformMsgs(profile, _base_profile); - } - } + shutdownOpenGLProcessing(); - if(is_profile_changed) - { - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Starting Sensor: " << module_name); - sensor->start(wanted_profiles); - } - - if (sensor->rs2::sensor::is()) - { - _depth_scale_meters = sensor->as().get_depth_scale(); - } - } + bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); + initOpenGLProcessing(use_gpu_processing); } - } - if (_publish_tf) - { - std::lock_guard lock_guard(_publish_tf_mutex); - publishStaticTransforms(); - } - startRGBDPublisherIfNeeded(); + #endif + + startUpdatedSensors(); } catch(const std::exception& ex) { @@ -432,28 +378,39 @@ void BaseRealSenseNode::updateSensors() } } -void BaseRealSenseNode::stopVideoSensors() +void BaseRealSenseNode::stopRequiredSensors() { - std::lock_guard lock_guard(_update_sensor_mutex); try{ for(auto&& sensor : _available_ros_sensors) { std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + // if active_profiles != wanted_profiles: stop sensor. + std::vector wanted_profiles; + bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); bool is_video_sensor = (sensor->is() || sensor->is()); - // do all updates if profile has been changed, or if the align depth filter status has been changed + // do all updates if profile has been changed, or if the align depth filter or gpu acceleration status has been changed // and we are on a video sensor. TODO: explore better options to monitor and update changes // without resetting the whole sensors and topics. - if (is_video_sensor) + if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ))) { std::vector active_profiles = sensor->get_active_streams(); - - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Stopping Sensor: " << module_name); - sensor->stop(); - + if (is_profile_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ) + { + // Start/stop sensors only if profile or gpu acceleration status was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Stopping Sensor: " << module_name); + sensor->stop(); + } stopPublishers(active_profiles); } } @@ -470,9 +427,8 @@ void BaseRealSenseNode::stopVideoSensors() } } -void BaseRealSenseNode::startVideoSensors() +void BaseRealSenseNode::startUpdatedSensors() { - std::lock_guard lock_guard(_update_sensor_mutex); try{ for(auto&& sensor : _available_ros_sensors) { @@ -480,16 +436,15 @@ void BaseRealSenseNode::startVideoSensors() // if active_profiles != wanted_profiles: stop sensor. std::vector wanted_profiles; - sensor->getUpdatedProfiles(wanted_profiles); + bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); bool is_video_sensor = (sensor->is() || sensor->is()); - // do all updates if profile has been changed, or if the align depth filter status has been changed - // and we are on a video sensor. TODO: explore better options to monitor and update changes - // without resetting the whole sensors and topics. - if (is_video_sensor) + if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ))) { - std::vector active_profiles = sensor->get_active_streams(); - if (!wanted_profiles.empty()) { startPublishers(wanted_profiles, *sensor); @@ -503,10 +458,17 @@ void BaseRealSenseNode::startVideoSensors() } } - // Start/stop sensors only if profile was changed - // No need to start/stop sensors if align_depth was changed - ROS_INFO_STREAM("Starting Sensor: " << module_name); - sensor->start(wanted_profiles); + if (is_profile_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ) + { + // Start/stop sensors only if profile or gpu acceleration was changed + // No need to start/stop sensors if align_depth was changed + ROS_INFO_STREAM("Starting Sensor: " << module_name); + sensor->start(wanted_profiles); + } if (sensor->rs2::sensor::is()) { From c86312d370d0663212c83ef25b404695ca8b5cd3 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 14 Feb 2024 22:06:18 +0530 Subject: [PATCH 3/3] updated readme --- README.md | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 731dd42c2c..a29107bfc7 100644 --- a/README.md +++ b/README.md @@ -305,6 +305,20 @@ User can set the camera name and camera namespace, to distinguish between camera - 1 -> **copy**: Every gyro message will be attached by the last accel message. - 2 -> **linear_interpolation**: Every gyro message will be attached by an accel message which is interpolated to gyro's timestamp. - Note: When the param *unite_imu_method* is dynamically updated, re-enable either gyro or accel stream for the change to take effect. +- **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 have smooth transition between the processing blocks when this parameter is updated dynamically, the node will: + - Stop the video sensors + - Do necessary GLSL configuration + - And then, start the video sensors + - To enable GPU acceleration, turn ON `BUILD_ACCELERATE_WITH_GPU` during build: + ```bash + colcon build --cmake-args '-DBUILD_ACCELERATE_WITH_GPU=ON' + ``` #### Parameters that cannot be changed in runtime: - **serial_no**: @@ -354,15 +368,6 @@ 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' - ```