From 16df1c5e421f63e207d4d05fa0421141fbc6394f Mon Sep 17 00:00:00 2001
From: ad-daniel <ad-daniel@github.com>
Date: Wed, 7 Aug 2024 04:11:46 +0000
Subject: [PATCH] Sync submodule

---
 .../webots/src/controller/c/speaker.c         | 53 +++++++++++++++++++
 1 file changed, 53 insertions(+)

diff --git a/webots_ros2_driver/webots/src/controller/c/speaker.c b/webots_ros2_driver/webots/src/controller/c/speaker.c
index 306d63bbb..fdf8472fa 100644
--- a/webots_ros2_driver/webots/src/controller/c/speaker.c
+++ b/webots_ros2_driver/webots/src/controller/c/speaker.c
@@ -40,6 +40,8 @@ struct Sound {
   bool need_update;
   bool need_stop;
   bool is_playing;
+  unsigned char *upload_data;  // sound data for uploading to webots
+  int upload_size;             // size of sound data
   struct Sound *next;
 };
 
@@ -257,6 +259,18 @@ static void speaker_write_request(WbDevice *d, WbRequest *r) {
         else
           request_write_char(r, 0);
       }
+
+      request_write_int32(r, sound->upload_size);
+      if (sound->upload_size) {
+        /* Sound data available to stream, send it one time and discard it.
+         * Webots will cache the data inside the device.
+         */
+        request_write_data(r, sound->upload_data, sound->upload_size);
+        free(sound->upload_data);
+        sound->upload_data = NULL;
+        sound->upload_size = 0;
+      }
+
       sound->need_update = false;
       sound = sound->next;
     }
@@ -298,6 +312,43 @@ static void speaker_toggle_remote(WbDevice *d, WbRequest *r) {
   // nothing to do.
 }
 
+/* Try to read sound file contents if it is found by given path. */
+static void speaker_try_load_sound_local(Sound *sound, const char *sound_file_name) {
+  FILE *fp;
+
+  sound->upload_data = NULL;
+  sound->upload_size = 0;
+
+  if ((fp = fopen(sound_file_name, "rb")) != NULL) {
+    if (fseek(fp, 0L, SEEK_END) == 0) {
+      const long file_size = ftell(fp);
+      if (file_size != -1) {
+        if (fseek(fp, 0L, SEEK_SET) == 0) {
+          size_t consumed = 0U;
+          sound->upload_data = malloc(file_size);
+          sound->upload_size = (int)file_size;
+          while (consumed < file_size) {
+            const size_t read = fread(&sound->upload_data[consumed], sizeof(unsigned char), file_size - consumed, fp);
+            if (ferror(fp)) {
+              free(sound->upload_data);
+              sound->upload_data = NULL;
+              sound->upload_size = 0;
+              break;
+            }
+            consumed += read;
+          }
+        }
+      }
+    }
+
+    if (sound->upload_data == NULL) {
+      fprintf(stderr, "Warning: %s() failed to read sound file '%s' :", __FUNCTION__, sound_file_name);
+      perror(NULL);
+    }
+    fclose(fp);
+  }
+}
+
 static void speaker_play_sound(WbDevice *device, const char *sound_name, double volume, double pitch, double balance, bool loop,
                                int side) {
   Sound *sound = speaker_get_sound_if_exist(device, sound_name);
@@ -309,6 +360,8 @@ static void speaker_play_sound(WbDevice *device, const char *sound_name, double
     memcpy(sound->sound_file, sound_name, l);
     sound->next = ((Speaker *)device->pdata)->sound_list;
     ((Speaker *)device->pdata)->sound_list = sound;
+
+    speaker_try_load_sound_local(sound, sound_name);
   }
   sound->volume = volume;
   sound->pitch = pitch;