[GL-support] Dynamically switching b/w CPU & GPU processing

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.</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>
 

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<rs2::stream_profile>& 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

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

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

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<int>(param_name, int(accelerate_with_gpu::NO_GPU)));
+     _parameters->setParam<int>(param_name, int(accelerate_with_gpu::NO_GPU), 
+                    [this](const rclcpp::Parameter& parameter)
+                    {
+                        accelerate_with_gpu temp_value = accelerate_with_gpu(parameter.get_value<int>());
+                        if (_accelerate_with_gpu != temp_value)
+                        {
+                            _accelerate_with_gpu = temp_value;
+                            std::lock_guard<std::mutex> 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()

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<void()> func = [this, time_interval](){
         std::unique_lock<std::mutex> 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<std::mutex> 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<rs2::depth_sensor>() || sensor->is<rs2::color_sensor>());
 
-            // 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<stream_profile> 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<stream_profile> wanted_profiles;
+
+            bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles));
+            bool is_video_sensor = (sensor->is<rs2::depth_sensor>() || sensor->is<rs2::color_sensor>());
+
+            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);