Fix /clear_behavior_fault service (bdaiinstitute#376)

Signed-off-by: Michel Hidalgo <[email protected]>

spot_driver/spot_driver/spot_ros2.py

@@ -1372,7 +1372,12 @@ def handle_clear_behavior_fault(
             response.success = False
             response.message = "Spot wrapper is undefined"
             return response
-        response.success, response.message = self.spot_wrapper.clear_behavior_fault(request.id)
+        success, message, cleared = self.spot_wrapper.clear_behavior_fault(request.id)
+        if not cleared:
+            success = False
+            message = "No behavior fault cleared"
+        response.success = success
+        response.message = message
         return response
 
     def handle_stop_dance(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:

spot_driver/test/pytests/test_clear_behavior_fault.py

@@ -0,0 +1,56 @@
+# Copyright (c) 2024 Boston Dynamics AI Institute LLC. See LICENSE file for more info.
+
+"""
+Test for the Clear Behavior Fault command.
+"""
+
+# We disable Pylint warnings for all Protobuf files which contain objects with
+# dynamically added member attributes.
+# pylint: disable=no-member
+
+import pytest
+from bdai_ros2_wrappers.futures import wait_for_future
+from bdai_ros2_wrappers.scope import ROSAwareScope
+from bosdyn.api.robot_command_pb2 import ClearBehaviorFaultResponse
+from std_srvs.srv import Trigger
+
+from spot_msgs.srv import ClearBehaviorFault  # type: ignore
+from spot_wrapper.testing.fixtures import SpotFixture
+
+
+@pytest.mark.usefixtures("spot_node")
+def test_clear_behavior_fault(ros: ROSAwareScope, simple_spot: SpotFixture) -> None:
+    """
+    This integration test checks if the "clear_behavior_fault" service infrastructure is
+    setup correctly.
+
+    Args:
+        ros: A ROS2 scope that can be used to create clients.
+        simple_spot: a programmable fake Spot robot running on a local
+            GRPC server.
+    """
+    # Satisfy driver prerequisites.
+    client = ros.node.create_client(Trigger, "claim")
+    assert client.wait_for_service(timeout_sec=2.0)
+    future = client.call_async(Trigger.Request())
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success, result.message
+
+    # Send ROS request.
+    client = ros.node.create_client(ClearBehaviorFault, "clear_behavior_fault")
+    assert client.wait_for_service(timeout_sec=2.0)
+    future = client.call_async(ClearBehaviorFault.Request(id=127))
+
+    # Serve fault clear service.
+    call = simple_spot.api.ClearBehaviorFault.serve(timeout=5.0)
+    assert call is not None
+    assert call.request.behavior_fault_id == 127
+    response = ClearBehaviorFaultResponse()
+    response.status = ClearBehaviorFaultResponse.Status.STATUS_CLEARED
+    call.returns(response)
+
+    # Wait for ROS response.
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success, result.message