Set Safety mode to service before updating Depth controls during launch

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

realsense2_camera/test/live_camera/test_d585s_safety_mode.py

@@ -0,0 +1,105 @@
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import os
+import sys
+
+import pytest
+
+from realsense2_camera_msgs.msg import Metadata as msg_Metadata
+
+sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
+import pytest_rs_utils
+from pytest_rs_utils import launch_descr_with_parameters, get_node_heirarchy
+
+import pytest_live_camera_utils
+
+test_params_test_default_run_mode = {
+    'camera_name': 'D585S',
+    'device_type': 'D585S',
+    'initial_reset': 'true',
+    'safety_camera.safety_mode': 0,
+    'depth_module.exposure': 1000,
+    'depth_module.enable_auto_exposure': 'false',
+    }
+test_params_test_run_to_standby_mode = {
+    'camera_name': 'D585S',
+    'device_type': 'D585S',
+    'safety_camera.safety_mode': 1,
+    'depth_module.exposure': 2000,
+    'depth_module.enable_auto_exposure': 'false',
+    }
+test_params_test_standby_to_service_mode = {
+    'camera_name': 'D585S',
+    'device_type': 'D585S',
+    'safety_camera.safety_mode': 2,
+    'depth_module.exposure': 3000,
+    'depth_module.enable_auto_exposure': 'false',
+    }
+'''
+The test was implemented to check whether ROS wrapper can automatically switch to service mode during
+launch of the node and configure the launch params and again switch back to user requested mode.
+'''
+@pytest.mark.parametrize("launch_descr_with_parameters", [    
+    pytest.param(test_params_test_default_run_mode, marks=pytest.mark.d585s),
+    pytest.param(test_params_test_run_to_standby_mode, marks=pytest.mark.d585s),
+    pytest.param(test_params_test_standby_to_service_mode, marks=pytest.mark.d585s),
+    ],indirect=True)
+@pytest.mark.launch(fixture=launch_descr_with_parameters)
+class TestD585s_TestSafetyMode(pytest_rs_utils.RsTestBaseClass):
+    def test_d585s_test_safety_mode(self,launch_descr_with_parameters):
+        params = launch_descr_with_parameters[1]
+        if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False:
+            print("Device not found? : " + params['device_type'])
+            assert False
+            return
+
+        try:
+            ''' 
+            initialize, run and check the data 
+            '''
+            print("Starting camera test...")
+            self.init_test("RsTest"+params['camera_name'])
+            self.wait_for_node(params['camera_name'])
+            self.create_param_ifs(get_node_heirarchy(params))
+
+            if params['initial_reset'] == 'true':
+                self.spin_for_time(wait_time=10)
+            else:
+                self.spin_for_time(wait_time=5)
+
+            assert self.get_integer_param('safety_camera.safety_mode') == params['safety_camera.safety_mode']
+            assert self.get_integer_param('depth_module.exposure') == params['depth_module.exposure']
+
+            depth_metadata = msg_Metadata()
+            depth_metadata.json_data = '{"actual_exposure":'+str(params['depth_module.exposure']) +'}'
+
+            themes = [
+                {'topic':get_node_heirarchy(params)+'/depth/metadata',
+                'msg_type':msg_Metadata,
+                'expected_data_chunks':1,
+                'data':depth_metadata
+                }
+            ]
+
+            ret = self.run_test(themes)
+            assert ret[0], ret[1]
+            assert self.process_data(themes)
+
+        finally:
+            #this step is important because the test will fail next time
+            pytest_rs_utils.kill_realsense2_camera_node()
+            self.shutdown()
+

realsense2_camera/test/utils/pytest_rs_utils.py

@@ -384,13 +384,14 @@ def extrinsicsTest(data, gt_data):
 def metadatTest(data, gt_data):
     jdata = json.loads(data.json_data)
     gt_jdata = json.loads(gt_data.json_data)
-    if jdata['frame_number'] != gt_jdata['frame_number']:
+
+    if 'frame_number' in gt_jdata and jdata['frame_number'] != gt_jdata['frame_number']:
         msg = 'Frame no not matching: ' + str(jdata['frame_number']) + " and " + str(gt_jdata['frame_number'])
         return False, msg
-    if jdata['clock_domain'] != gt_jdata['clock_domain']:
+    if 'clock_domain' in gt_jdata and jdata['clock_domain'] != gt_jdata['clock_domain']:
         msg = 'clock_domain not matching: ' + str(jdata['clock_domain']) + " and " + str(gt_jdata['clock_domain'])
         return False, msg
-    if jdata['frame_timestamp'] != gt_jdata['frame_timestamp']:
+    if 'frame_timestamp' in gt_jdata and jdata['frame_timestamp'] != gt_jdata['frame_timestamp']:
         msg = 'frame_timestamp not matching: ' + str(jdata['frame_timestamp']) + " and " + str(gt_jdata['frame_timestamp'])
         return False, msg
     '''
@@ -401,9 +402,13 @@ def metadatTest(data, gt_data):
         msg = 'frame_counter not matching: ' + str(jdata['frame_counter']) + " and " + str(gt_jdata['frame_counter'])
         return False, msg
     '''
-    if jdata['time_of_arrival'] != gt_jdata['time_of_arrival']:
+    if 'time_of_arrival' in gt_jdata and jdata['time_of_arrival'] != gt_jdata['time_of_arrival']:
         msg = 'time_of_arrival not matching: ' + str(jdata['time_of_arrival']) + " and " + str(gt_jdata['time_of_arrival'])
         return False, msg
+    if 'actual_exposure' in gt_jdata and jdata['actual_exposure'] != gt_jdata['actual_exposure']:
+        msg = 'actual_exposure not matching: ' + str(jdata['actual_exposure']) + " and " + str(gt_jdata['actual_exposure'])
+        return False, msg
+
     return True, ""