Set service mode before updating Depth controls

README.md

@@ -305,6 +305,10 @@ 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.
+- **safety_camera.safety_mode**:
+  - 0 -> **Run** mode
+  - 1 -> **Standby** mode
+  - 2 -> **Service** mode
 
 #### Parameters that cannot be changed in runtime:
 - **serial_no**:

realsense2_camera/launch/rs_launch.py

@@ -82,6 +82,7 @@
                            {'name': 'wait_for_device_timeout',      'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},
                            {'name': 'reconnect_timeout',            'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},
                            {'name': 'enable_safety',                'default': 'false', 'description': "'enable safety stream'"},                                                      
+                           {'name': 'safety_camera.safety_mode',    'default': '0', 'description': '[int] 0-Run, 1-Standby, 2-Service'},                                                       
                            {'name': 'enable_labeled_point_cloud',   'default': 'false', 'description': "'enable labeled point cloud stream'"},
                            {'name': 'enable_occupancy',             'default': 'false', 'description': "'enable occupancy stream'"},
                            {'name': 'depth_mapping_camera.profile', 'default': '0,0,0', 'description': "'depth mapping camera profile'"},

realsense2_camera/src/rs_node_setup.cpp

@@ -130,13 +130,48 @@ void BaseRealSenseNode::setAvailableSensors()
     std::function<void()> hardware_reset_func = [this](){hardwareResetRequest();};
 
     _dev_sensors = _dev.query_sensors();
+    rs2::sensor* safety_sensor = nullptr;
+
+    // Find if the Safety Sensor is available.
+    auto iter = std::find_if(_dev_sensors.begin(), _dev_sensors.end(), 
+                            [](rs2::sensor sensor){return sensor.is<rs2::safety_sensor>();});
+    if (iter != _dev_sensors.end())
+    {
+        safety_sensor = &(*iter);
+    }
 
     for(auto&& sensor : _dev_sensors)
     {
         const std::string module_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME)));
         std::unique_ptr<RosSensor> rosSensor;
-        if (sensor.is<rs2::depth_sensor>() || 
-            sensor.is<rs2::color_sensor>() ||
+        if (sensor.is<rs2::depth_sensor>())
+        {
+            auto safety_mode = RS2_SAFETY_MODE_RUN;
+
+            // Deleter to revert the safety mode to its original value.
+            auto deleter_to_revert_safety_mode = std::unique_ptr<rs2_safety_mode, std::function<void(rs2_safety_mode*)>>(&safety_mode, 
+                                            [&](rs2_safety_mode* revert_safety_mode_to){
+                                                    if (revert_safety_mode_to && safety_sensor)
+                                                    {
+                                                        safety_sensor->set_option(RS2_OPTION_SAFETY_MODE, *revert_safety_mode_to);
+                                                    }
+                                                });
+
+            // Few Depth controls can only be updated when the safety mode is set to SERVICE.
+            // So, during INIT, setting the safety mode to SERVICE and reverting back to its original value later.
+            if (safety_sensor)
+            {
+                safety_mode = static_cast<rs2_safety_mode>(safety_sensor->get_option(RS2_OPTION_SAFETY_MODE));
+                if (safety_mode != RS2_SAFETY_MODE_SERVICE)
+                {
+                    safety_sensor->set_option(RS2_OPTION_SAFETY_MODE, RS2_SAFETY_MODE_SERVICE);
+                }
+            }
+
+            ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor.");
+            rosSensor = std::make_unique<RosSensor>(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is<playback>());
+        }
+        else if (sensor.is<rs2::color_sensor>() ||
             sensor.is<rs2::safety_sensor>() ||
             sensor.is<rs2::depth_mapping_sensor>())
         {