diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index aa4020660..001a684a8 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -114,6 +114,7 @@ DeleteSound, Dock, ExecuteDance, + GetChoreographyStatus, GetGripperCameraParameters, GetVolume, GraphNavClearGraph, @@ -622,6 +623,14 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw ), callback_group=self.group, ) + self.create_service( + GetChoreographyStatus, + "get_choreography_status", + lambda request, response: self.service_wrapper( + "get_choreography_status", self.handle_get_choreography_status, request, response + ), + callback_group=self.group, + ) self.create_service( ListSounds, "list_sounds", @@ -1261,6 +1270,19 @@ def handle_stop_recording_state( response.success, response.message, _ = self.spot_wrapper.stop_recording_state() return response + def handle_get_choreography_status( + self, request: GetChoreographyStatus.Request, response: GetChoreographyStatus.Response + ) -> GetChoreographyStatus.Response: + """ROS service handler for getting current status of choreography playback.""" + if self.spot_wrapper is None: + response.success = False + response.message = "Spot wrapper is undefined" + return response + response.success, response.message, choreography_status = self.spot_wrapper.get_choreography_status() + response.status = choreography_status.status + response.execution_id = choreography_status.execution_id + return response + def handle_upload_animation( self, request: UploadAnimation.Request, response: UploadAnimation.Response ) -> UploadAnimation.Response: