[SW-948] Add mission client interfaces to spot_ros2

spot_driver/spot_driver/spot_ros2.py

@@ -109,6 +109,8 @@
     GetGripperCameraParameters,
     GetLEDBrightness,
     GetLogpointStatus,
+    GetMissionInfo,
+    GetMissionState,
     GetPtzPosition,
     GetVolume,
     GraphNavClearGraph,
@@ -124,8 +126,11 @@
     ListPtz,
     ListSounds,
     ListWorldObjects,
+    LoadMission,
     LoadSound,
+    PlayMission,
     PlaySound,
+    RestartMission,
     RetrieveLogpoint,
     SetGripperCameraParameters,
     SetLEDBrightness,
@@ -583,6 +588,58 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
             callback_group=self.group,
         )
 
+        # Mission services
+        self.create_service(
+            LoadMission,
+            "load_mission",
+            lambda request, response: self.service_wrapper("load_mission", self.handle_load_mission, request, response),
+            callback_group=self.group,
+        )
+        self.create_service(
+            PlayMission,
+            "play_mission",
+            lambda request, response: self.service_wrapper("play_mission", self.handle_play_mission, request, response),
+            callback_group=self.group,
+        )
+        self.create_service(
+            Trigger,
+            "pause_mission",
+            lambda request, response: self.service_wrapper(
+                "pause_mission", self.handle_pause_mission, request, response
+            ),
+            callback_group=self.group,
+        )
+        self.create_service(
+            Trigger,
+            "stop_mission",
+            lambda request, response: self.service_wrapper("stop_mission", self.handle_stop_mission, request, response),
+            callback_group=self.group,
+        )
+        self.create_service(
+            RestartMission,
+            "restart_mission",
+            lambda request, response: self.service_wrapper(
+                "restart_mission", self.handle_restart_mission, request, response
+            ),
+            callback_group=self.group,
+        )
+        self.create_service(
+            GetMissionInfo,
+            "get_mission_info",
+            lambda request, response: self.service_wrapper(
+                "get_mission_info", self.handle_get_mission_info, request, response
+            ),
+            callback_group=self.group,
+        )
+        self.create_service(
+            GetMissionState,
+            "get_mission_state",
+            lambda request, response: self.service_wrapper(
+                "get_mission_state", self.handle_get_mission_state, request, response
+            ),
+            callback_group=self.group,
+        )
+
         if has_arm:
             self.create_service(
                 Trigger,
@@ -1380,6 +1437,93 @@ def handle_clear_behavior_fault(
         response.message = message
         return response
 
+    # Mission services
+    def handle_load_mission(self, request: LoadMission.Request, response: LoadMission.Response) -> LoadMission.Response:
+        """ROS service handler to load the mission."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        proto_response = self.spot_wrapper.spot_mission.load_mission(request.request.root, request.request.leases)
+        convert(proto_response, response.response)
+        return response
+
+    def handle_play_mission(self, request: PlayMission.Request, response: PlayMission.Response) -> PlayMission.Response:
+        """ROS service handler to play the loaded mission."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        proto_response = self.spot_wrapper.spot_mission.play_mission(
+            request.request.pause_time, request.request.leases, request.request.settings
+        )
+        convert(proto_response, response.response)
+        return response
+
+    def handle_pause_mission(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
+        """ROS service handler to pause the mission."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        success, msg = self.spot_wrapper.spot_mission.pause_mission()
+        response.success = success
+        response.message = msg
+        return response
+
+    def handle_stop_mission(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
+        """ROS service handler to stop the mission."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        success, msg = self.spot_wrapper.spot_mission.stop_mission()
+        response.success = success
+        response.message = msg
+        return response
+
+    def handle_restart_mission(
+        self, request: RestartMission.Request, response: RestartMission.Response
+    ) -> RestartMission.Response:
+        """ROS service handler to restart the loaded mission."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        proto_response = self.spot_wrapper.spot_mission.restart_mission(
+            request.request.pause_time, request.request.leases, request.request.settings
+        )
+        convert(proto_response, response.response)
+        return response
+
+    def handle_get_mission_info(
+        self, request: GetMissionInfo.Request, response: GetMissionInfo.Response
+    ) -> GetMissionInfo.Response:
+        """ROS service handler to get the loaded mission's info."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        proto_response = self.spot_wrapper.spot_mission.get_mission_info()
+        convert(proto_response, response.response)
+        return response
+
+    def handle_get_mission_state(
+        self, request: GetMissionState.Request, response: GetMissionState.Response
+    ) -> GetMissionState.Response:
+        """ROS service handler to get the loaded mission's state."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        proto_response = self.spot_wrapper.spot_mission.get_mission_state(
+            request.request.history_upper_tick_bound,
+            request.request.history_lower_tick_bound,
+            request.request.history_past_ticks,
+        )
+        convert(proto_response, response.response)
+        return response
+
     def handle_stop_dance(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
         """ROS service handler to stop the robot's dancing."""
         if self.spot_wrapper is None:

spot_driver/test/pytests/test_spot_mission.py

@@ -0,0 +1,169 @@
+# Copyright (c) 2024 The AI Institute LLC. See LICENSE file for more info.
+
+"""
+Test for the Mission commands.
+"""
+
+# 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.mission.mission_pb2 import (
+    GetInfoResponse,
+    GetStateResponse,
+    LoadMissionResponse,
+    PlayMissionResponse,
+    RestartMissionResponse,
+)
+from std_srvs.srv import Trigger
+
+from spot_msgs.srv import (  # type: ignore
+    GetMissionInfo,
+    GetMissionState,
+    LoadMission,
+    PlayMission,
+    RestartMission,
+)
+from spot_wrapper.testing.fixtures import SpotFixture
+
+
+@pytest.mark.usefixtures("spot_node")
+def test_mission_services(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
+
+    # Test LoadMission
+    client = ros.node.create_client(LoadMission, "load_mission")
+    assert client.wait_for_service(timeout_sec=2.0)
+    load_req = LoadMission.Request()
+    future = client.call_async(load_req)
+
+    call = simple_spot.api.LoadMission.serve(timeout=2.0)
+    assert call is not None
+
+    response = LoadMissionResponse()
+    response.status = LoadMissionResponse.Status.STATUS_OK
+    call.returns(response)
+
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert response.success
+
+    # Test PlayMission
+    client = ros.node.create_client(PlayMission, "play_mission")
+    assert client.wait_for_service(timeout_sec=2.0)
+    load_req = PlayMission.Request()
+    future = client.call_async(load_req)
+
+    call = simple_spot.api.PlayMission.serve(timeout=2.0)
+    assert call is not None
+
+    response = PlayMissionResponse()
+    response.status = PlayMissionResponse.Status.STATUS_OK
+    call.returns(response)
+
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success
+
+    # Test PauseMission
+    client = ros.node.create_client(Trigger, "pause_mission")
+    assert client.wait_for_service(timeout_sec=2.0)
+    load_req = Trigger.Request()
+    future = client.call_async(load_req)
+
+    call = simple_spot.api.PauseMission.serve(timeout=2.0)
+    assert call is not None
+
+    response = Trigger.Response()
+    response.success = True
+    call.returns(response)
+
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success
+
+    # Test RestartMission
+    client = ros.node.create_client(RestartMission, "restart_mission")
+    assert client.wait_for_service(timeout_sec=2.0)
+    load_req = RestartMission.Request()
+    future = client.call_async(load_req)
+
+    call = simple_spot.api.RestartMission.serve(timeout=2.0)
+    assert call is not None
+
+    response = RestartMissionResponse()
+    response.status = RestartMissionResponse.Status.STATUS_OK
+    call.returns(response)
+
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success
+
+    # Test StopMission
+    client = ros.node.create_client(Trigger, "stop_mission")
+    assert client.wait_for_service(timeout_sec=2.0)
+    load_req = Trigger.Request()
+    future = client.call_async(load_req)
+
+    call = simple_spot.api.StopMission.serve(timeout=2.0)
+    assert call is not None
+
+    response = Trigger.Response()
+    response.success = True
+    call.returns(response)
+
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success
+
+    # Test GetMissionInfo
+    client = ros.node.create_client(GetMissionInfo, "get_mission_info")
+    assert client.wait_for_service(timeout_sec=2.0)
+    load_req = GetMissionInfo.Request()
+    future = client.call_async(load_req)
+
+    call = simple_spot.api.GetMissionInfo.serve(timeout=2.0)
+    assert call is not None
+
+    response = GetInfoResponse()
+    response.status = GetInfoResponse.Status.STATUS_OK
+    call.returns(response)
+
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success
+
+    # Test GetMissionState
+    client = ros.node.create_client(GetMissionState, "get_mission_state")
+    assert client.wait_for_service(timeout_sec=2.0)
+    load_req = GetMissionState.Request()
+    future = client.call_async(load_req)
+
+    call = simple_spot.api.GetMissionState.serve(timeout=2.0)
+    assert call is not None
+
+    response = GetStateResponse()
+    response.status = GetStateResponse.Status.STATUS_OK
+    call.returns(response)
+
+    assert wait_for_future(future, timeout_sec=2.0)
+    result = future.result()
+    assert result.success

spot_msgs/CMakeLists.txt

@@ -29,6 +29,7 @@ find_package(builtin_interfaces REQUIRED)
 find_package(bosdyn_api_msgs REQUIRED)
 find_package(bosdyn_spot_api_msgs REQUIRED)
 find_package(bosdyn_spot_cam_api_msgs REQUIRED)
+find_package(bosdyn_mission_api_msgs REQUIRED)
 find_package(common_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -97,6 +98,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Dock.srv"
   "srv/GetGripperCameraParameters.srv"
   "srv/SetGripperCameraParameters.srv"
+  "srv/LoadMission.srv"
+  "srv/PlayMission.srv"
+  "srv/RestartMission.srv"
+  "srv/GetMissionInfo.srv"
+  "srv/GetMissionState.srv"
   "action/ExecuteDance.action"
   "action/NavigateTo.action"
   "action/RobotCommand.action"
@@ -106,6 +112,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     bosdyn_api_msgs
     bosdyn_spot_api_msgs
     bosdyn_spot_cam_api_msgs
+    bosdyn_mission_api_msgs
     builtin_interfaces
     common_interfaces
     geometry_msgs

spot_msgs/srv/GetMissionInfo.srv

@@ -0,0 +1,5 @@
+
+---
+bosdyn_mission_api_msgs/GetInfoResponse response
+bool success
+string message

spot_msgs/srv/GetMissionState.srv

@@ -0,0 +1,5 @@
+
+---
+bosdyn_mission_api_msgs/GetStateResponse response
+bool success
+string message

spot_msgs/srv/LoadMission.srv

@@ -0,0 +1,5 @@
+bosdyn_mission_api_msgs/LoadMissionRequest request
+---
+bosdyn_mission_api_msgs/LoadMissionResponse response
+bool success
+string message

spot_msgs/srv/PlayMission.srv

@@ -0,0 +1,5 @@
+bosdyn_mission_api_msgs/PlayMissionRequest request
+---
+bosdyn_mission_api_msgs/PlayMissionResponse response
+bool success
+string message