Retry D585S service-mode switch to handle post-enumeration FW readiness race (realsenseai#15329)

Author: Nir-Az

unit-tests/conftest.py

@@ -895,14 +895,17 @@ def test_device_wrapped(test_device):
     is_d585s = dev.supports(rs.camera_info.name) and "D585S" in dev.get_info(rs.camera_info.name)
     safety_sensor = None
     if is_d585s:
+        from rspy import tests_wrapper  # local import: pulls in pyrealsense2, unavailable in infra-tests
         safety_sensor = dev.first_safety_sensor()
         if safety_sensor.get_option(rs.option.safety_mode) != rs.safety_mode.service:
             # Will throw on failure — intentional so we fail the test rather than run without service mode.
-            safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.service)
+            # Retries internally: the FW needs a few seconds after enumeration before it accepts the switch.
+            tests_wrapper.set_safety_mode(safety_sensor, rs.safety_mode.service)
     yield dev, ctx
     if safety_sensor is not None:
         try:
-            safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+            # tests_wrapper already imported in the setup block above (still bound across the yield)
+            tests_wrapper.set_safety_mode(safety_sensor, rs.safety_mode.run)
         except Exception as e:
             # Best-effort: don't mask test failures, and the device may already be reset by teardown time.
             log.warning(f"safety_mode restore skipped for {sn}: {e}")

unit-tests/live/d500/safety/pytest-interface-config-get-set.py

@@ -210,7 +210,7 @@ def test_verify_same_table_after_camera_reboot(test_device):
     safety_sensor = new_dev.first_safety_sensor()
 
     log.debug("Setting operational mode to service")
-    safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.service)
+    tw.set_safety_mode(safety_sensor, rs.safety_mode.service)
     assert safety_sensor.get_option(rs.option.safety_mode) == float(rs.safety_mode.service)
 
     config_after_reboot = safety_sensor.get_safety_interface_config(rs.calib_location.flash)

unit-tests/live/d500/safety/pytest-operational-mode-stress.py

@@ -4,6 +4,7 @@
 import pytest
 import pyrealsense2 as rs
 from pytest_check import check
+from rspy import tests_wrapper as tw
 import logging
 log = logging.getLogger(__name__)
 
@@ -25,13 +26,13 @@ def test_operational_mode_stress(test_device):
     for i in range(ITERATIONS_COUNT):
         log.debug("stress test iteration: %s", i)
         log.debug("command service mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.service)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.service)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.service))
 
         log.debug("command standby mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.standby)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.standby)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.standby))
 
         log.debug("command run mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.run)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.run))

unit-tests/live/d500/safety/pytest-operational-mode.py

@@ -5,6 +5,7 @@
 import time
 import pyrealsense2 as rs
 from pytest_check import check
+from rspy import tests_wrapper as tw
 import logging
 log = logging.getLogger(__name__)
 
@@ -41,7 +42,7 @@ def test_pause_resume_no_impact_on_streaming(test_context):
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.run))  # verify default
 
         log.debug("Command standby mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.standby)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.standby)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.standby))
         verify_frames_received(pipe, count=10)
 
@@ -51,7 +52,7 @@ def test_pause_resume_no_impact_on_streaming(test_context):
         verify_frames_received(pipe, count=10)
 
         log.debug("Command run mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.run)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.run))
         verify_frames_received(pipe, count=10)
     finally:
@@ -76,7 +77,7 @@ def test_resume_to_maintenance_keeps_video_streaming(test_context):
         safety_sensor = pipeline_device.first_safety_sensor()
 
         log.debug("Command run mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.run)
         log.debug(f"Current mode: {safety_sensor.get_option(rs.option.safety_mode)}")
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.run))
         # Verify that on RUN mode we get frames
@@ -86,14 +87,14 @@ def test_resume_to_maintenance_keeps_video_streaming(test_context):
         time.sleep(2)
 
         log.debug("Command service mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.service)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.service)
         log.debug(f"Current mode: {safety_sensor.get_option(rs.option.safety_mode)}")
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.service))
         verify_frames_received(pipe, count=10)
 
         # Restore Run mode
         log.debug("Command run mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.run)
         log.debug(f"Current mode: {safety_sensor.get_option(rs.option.safety_mode)}")
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.run))
         # Verify that on RUN mode we get frames
@@ -118,20 +119,20 @@ def test_resume_to_maintenance_keeps_safety_streaming(test_context):
         safety_sensor = pipeline_device.first_safety_sensor()
 
         log.debug("Command run mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.run)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.run))
         # Verify that on RUN mode we get frames
         verify_frames_received(pipe, count=10)
 
         log.debug("Command service mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.service)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.service)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.service))
         # Verify that on SERVICE mode we still get frames
         verify_frames_received(pipe, count=10)
 
         # Restore Run mode
         log.debug("Command run mode")
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+        tw.set_safety_mode(safety_sensor, rs.safety_mode.run)
         check.equal(safety_sensor.get_option(rs.option.safety_mode), float(rs.safety_mode.run))
 
         # We know that returning to run mode will not restart the safety stream.

unit-tests/py/rspy/tests_wrapper.py

@@ -4,19 +4,48 @@
 # Some future models might need to wrap the tests in setup and teardown steps.
 # Don't remove this file even if current implementation is empty...
 
+import time
 import pyrealsense2 as rs
 from rspy import log
+from rspy.stopwatch import Stopwatch
+
+
+# The D585S FW can transiently reject a safety_mode change (RuntimeError "Failed to set the
+# option to value N") for several seconds after USB enumeration and after disruptive operations
+# (heavy streaming, hardware_reset). Retry until it takes or we time out. Use this for every
+# D585S safety_mode transition so the tests don't rely on luck / pytest-retry.
+def set_safety_mode( safety_sensor, mode, timeout = 8, interval = 0.5 ):
+    sw = Stopwatch()
+    last_exc = None
+    attempt = 0
+    while True:
+        attempt += 1
+        last_exc = None  # reset so a stale error from an earlier iteration can't leak out at timeout
+        try:
+            safety_sensor.set_option( rs.option.safety_mode, mode )
+            if safety_sensor.get_option( rs.option.safety_mode ) == float( mode ):
+                log.i( f"safety_mode set to {mode} after {attempt} attempt(s), {sw.get_elapsed():.1f}s" )
+                return
+            log.w( f"safety_mode set to {mode} accepted but read-back mismatched on attempt {attempt}, retrying" )
+        except Exception as e:
+            last_exc = e
+        if sw.get_elapsed() >= timeout:
+            if last_exc:
+                raise last_exc
+            raise RuntimeError( f"failed to set safety_mode to {mode} within {timeout}s" )
+        time.sleep( interval )
+
 
 # Many operations, such as setting options, can take place only in safety service mode
 def start_wrapper( dev = None ):
     if "D585S" in dev.get_info(rs.camera_info.name):
         safety_sensor = dev.first_safety_sensor()
-        safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.service)
+        set_safety_mode( safety_sensor, rs.safety_mode.service )
 
 def stop_wrapper( dev = None ):
     if "D585S" in dev.get_info(rs.camera_info.name):
         try:
             safety_sensor = dev.first_safety_sensor()
-            safety_sensor.set_option(rs.option.safety_mode, rs.safety_mode.run)
+            set_safety_mode( safety_sensor, rs.safety_mode.run )
         except Exception as e:
-            log.e(f"Cleanup failed: could not set safety_mode back to run: {e}")
+            log.e(f"Cleanup failed: could not set safety_mode back to run: {e}")