Add a test for the gripper's _fast stop_ service

Also add a function to set control bits in the dummy.
This avoids a lot of boilerplate code in tests.

schunk_egu_egk_gripper_dummy/src/dummy.py

@@ -223,6 +223,18 @@ def get_control_bit(self, bit: int) -> int | bool:
         byte_index, bit_index = divmod(bit, 8)
         return 1 if self.plc_output_buffer[byte_index] & (1 << bit_index) != 0 else 0
 
+    def set_control_bit(self, bit: int, value: bool) -> bool:
+        if bit < 0 or bit > 31:
+            return False
+        if bit in self.reserved_control_bits:
+            return False
+        byte_index, bit_index = divmod(bit, 8)
+        if value:
+            self.plc_output_buffer[byte_index] |= 1 << bit_index
+        else:
+            self.plc_output_buffer[byte_index] &= ~(1 << bit_index)
+        return True
+
     def get_target_position(self) -> float:
         return struct.unpack("f", self.plc_output_buffer[4:8])[0]
 
@@ -260,6 +272,11 @@ def process_control_bits(self) -> None:
         if self.get_control_bit(30) == 1:
             self.set_status_bit(bit=4, value=True)
 
+        # Fast stop
+        if self.get_control_bit(0) == 0:  # fail-safe behavior
+            self.set_status_bit(bit=7, value=True)
+            self.set_status_diagnostics("D9")
+
         # Move to absolute position
         if self.get_control_bit(13) == 1:
             self.move(

schunk_egu_egk_gripper_dummy/tests/test_dummy.py

@@ -44,15 +44,8 @@ def test_dummy_starts_in_error_state():
 
 def test_dummy_is_ready_after_acknowledge():
     dummy = Dummy()
-    control_double_word = "04000000"  # bit 2
-    set_position = "00000000"
-    set_speed = "00000000"
-    gripping_force = "00000000"
-    command = {
-        "inst": dummy.plc_output,
-        "value": control_double_word + set_position + set_speed + gripping_force,
-    }
-    dummy.post(command)
+    dummy.set_control_bit(bit=2, value=True)
+    dummy.process_control_bits()
     assert dummy.get_status_bit(0) == 1  # ready
     assert dummy.get_status_bit(7) == 0  # no error
     assert dummy.get_status_error() == "0"
@@ -62,15 +55,7 @@ def test_dummy_is_ready_after_acknowledge():
 def test_dummy_always_toggles_command_received_bit():
     dummy = Dummy()
     before = dummy.get_status_bit(bit=5)  # command received toggle
-    control_double_word = "00000000"
-    set_position = "00000000"
-    set_speed = "00000000"
-    gripping_force = "00000000"
-    command = {
-        "inst": dummy.plc_output,
-        "value": control_double_word + set_position + set_speed + gripping_force,
-    }
-    dummy.post(command)
+    dummy.process_control_bits()
     after = dummy.get_status_bit(bit=5)
     assert after != before
 
@@ -99,13 +84,14 @@ def test_dummy_moves_to_absolute_position():
 
 def test_dummy_performs_break_test():
     dummy = Dummy()
-    control_double_word = "00000040"  # Bit 30
-    set_position = "00000000"
-    set_speed = "00000000"
-    gripping_force = "00000000"
-    command = {
-        "inst": dummy.plc_output,
-        "value": control_double_word + set_position + set_speed + gripping_force,
-    }
-    dummy.post(command)
+    dummy.set_control_bit(bit=30, value=True)
+    dummy.process_control_bits()
     assert dummy.get_status_bit(bit=4) == 1  # command successfully processed
+
+
+def test_dummy_performs_fast_stop():
+    dummy = Dummy()
+    dummy.set_control_bit(bit=0, value=False)
+    dummy.process_control_bits()
+    assert dummy.get_status_bit(bit=7) == 1  # error
+    assert dummy.get_status_diagnostics() == "D9"  # ERR_FAST_STOP

schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py

@@ -81,11 +81,13 @@ def test_dummy_rejects_writing_invalid_status_diagnostics():
         assert not dummy.set_status_diagnostics(code)
 
 
-def test_dummy_supports_reading_bits_in_plc_control():
+def test_dummy_supports_reading_and_writing_bits_in_plc_control():
     dummy = Dummy()
     for bit in dummy.valid_control_bits:
+        dummy.set_control_bit(bit=bit, value=True)
         result = dummy.get_control_bit(bit=bit)
         assert isinstance(result, int)  # successful calls get the bit's value
+        assert result == 1
 
 
 def test_dummy_rejects_reading_reserved_control_bits():
@@ -95,6 +97,13 @@ def test_dummy_rejects_reading_reserved_control_bits():
         assert not dummy.get_control_bit(bit)
 
 
+def test_dummy_rejects_writing_reserved_control_bits():
+    dummy = Dummy()
+    invalid_bits = [-1, 999]
+    for bit in invalid_bits + dummy.reserved_control_bits:
+        assert not dummy.set_control_bit(bit, True)
+
+
 def test_dummy_supports_toggling_status_bits():
     dummy = Dummy()
     for bit in dummy.valid_status_bits:

schunk_egu_egk_gripper_tests/test/test_services.py

@@ -4,6 +4,7 @@
 from schunk_egu_egk_gripper_interfaces.srv import (  # type: ignore[attr-defined]
     Acknowledge,
     BrakeTest,
+    FastStop,
 )
 from test.helpers import ServiceReturnsResult
 
@@ -36,3 +37,10 @@ def test_driver_supports_break_test(running_driver):
     service = ServiceReturnsResult("/brake_test", BrakeTest, BrakeTest.Request())
     service.event.wait(timeout=1)
     assert service.result.brake_test_successful is True
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_fast_stop(running_driver):
+    service = ServiceReturnsResult("/fast_stop", FastStop, FastStop.Request())
+    service.event.wait(timeout=1)
+    assert service.result.fast_stopped is True