Introduce the get choreography status service (bdaiinstitute#212)

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: