diff --git a/README.md b/README.md index a77621972e..5625929f09 100644 --- a/README.md +++ b/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**: diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 9b889d8561..d387dc7e80 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/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'"}, diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 5d676b5032..208b87b167 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -130,13 +130,48 @@ void BaseRealSenseNode::setAvailableSensors() std::function 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();}); + 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; - if (sensor.is() || - sensor.is() || + if (sensor.is()) + { + 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>(&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(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(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is()); + } + else if (sensor.is() || sensor.is() || sensor.is()) { diff --git a/realsense2_camera/test/live_camera/test_d585s_safety_mode.py b/realsense2_camera/test/live_camera/test_d585s_safety_mode.py new file mode 100644 index 0000000000..bcf2e44b91 --- /dev/null +++ b/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() + diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 0a0069115f..5c6b950d36 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/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, ""