diff --git a/README.md b/README.md index 3a7bdc7998..52af27c039 100644 --- a/README.md +++ b/README.md @@ -306,6 +306,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**: @@ -355,15 +369,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' - ```
diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 67156240c7..3d808f5e1c 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 startUpdatedSensors(); + void stopRequiredSensors(); void publishServices(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); @@ -356,6 +358,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 ef689f1a70..d35112a968 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..ad4f8abe8e 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,8 +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 (_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 @@ -57,6 +70,10 @@ void BaseRealSenseNode::monitoringProfileChanges() } _is_profile_changed = false; _is_align_depth_changed = false; + + #if defined (ACCELERATE_WITH_GPU) + _is_accelerate_with_gpu_changed = false; + #endif } } }; @@ -334,6 +351,35 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded() void BaseRealSenseNode::updateSensors() { std::lock_guard lock_guard(_update_sensor_mutex); + try{ + stopRequiredSensors(); + + #if defined (ACCELERATE_WITH_GPU) + if (_is_accelerate_with_gpu_changed) + { + shutdownOpenGLProcessing(); + + bool use_gpu_processing = (_accelerate_with_gpu == accelerate_with_gpu::GL_GPU); + initOpenGLProcessing(use_gpu_processing); + } + #endif + + startUpdatedSensors(); + } + 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::stopRequiredSensors() +{ try{ for(auto&& sensor : _available_ros_sensors) { @@ -344,21 +390,61 @@ void BaseRealSenseNode::updateSensors() 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_profile_changed || (_is_align_depth_changed && 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(is_profile_changed) + if (is_profile_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ) { - // Start/stop sensors only if profile was changed + // 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); + } + } + } + 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::startUpdatedSensors() +{ + 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()); + + if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ))) + { if (!wanted_profiles.empty()) { startPublishers(wanted_profiles, *sensor); @@ -372,9 +458,13 @@ void BaseRealSenseNode::updateSensors() } } - if(is_profile_changed) + if (is_profile_changed + #if defined (ACCELERATE_WITH_GPU) + || _is_accelerate_with_gpu_changed + #endif + ) { - // Start/stop sensors only if profile was changed + // 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);