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);