simplify music server with playsound package and support mp3 files

mini_pupper_dance/mini_pupper_dance/dance_client.py

@@ -3,16 +3,16 @@
 import rclpy
 from rclpy.node import Node
 from mini_pupper_interfaces.srv import DanceCommand
-from std_srvs.srv import SetBool
-from .episode import dance_commands
+from mini_pupper_interfaces.srv import MusicCommand
+from .episode import dance_song_file_name, dance_commands
 
 
 class MiniPupperDanceClientAsync(Node):
 
     def __init__(self):
         super().__init__('mini_pupper_dance_client_async')
         self.dance_cli = self.create_client(DanceCommand, 'dance_command')
-        self.music_cli = self.create_client(SetBool, 'music_command')
+        self.music_cli = self.create_client(MusicCommand, 'music_command')
         while not self.dance_cli.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('service not available, waiting again...')
 
@@ -25,9 +25,10 @@ def send_dance_request(self, dance_command):
         rclpy.spin_until_future_complete(self, future)
         return future.result()
 
-    def send_music_request(self, trigger):
-        req = SetBool.Request()
-        req.data = trigger
+    def send_music_request(self, file_name):
+        req = MusicCommand.Request()
+        req.command = 'play'
+        req.file_name = file_name
         self.music_cli.call_async(req)  # Fire-and-forget style communication
 
 
@@ -39,18 +40,13 @@ def main():
         # Start music for the first command
         if index == 0:
             minimal_client.get_logger().info('Starting music...')
-            minimal_client.send_music_request(True)
+            minimal_client.send_music_request(dance_song_file_name)
 
         # Send movemoment comment for the robot to dance
         response = minimal_client.send_dance_request(command)
         if response.executed:
             minimal_client.get_logger().info('Command Executed!')
 
-        # Stop music after the last command
-        if index == len(minimal_client.dance_commands) - 1:
-            minimal_client.get_logger().info('Stopping music...')
-            minimal_client.send_music_request(False)
-
     minimal_client.destroy_node()
     rclpy.shutdown()
 

mini_pupper_dance/mini_pupper_dance/episode.py

@@ -12,6 +12,8 @@
 # look_middle: the robot will return to the default standing posture
 # stay: the robot will keep the last command
 
+dance_song_file_name = 'robot1.mp3'
+
 dance_commands = [
     'move_forward',
     'look_middle',

mini_pupper_interfaces/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/DanceCommand.srv"
+  "srv/MusicCommand.srv"
   DEPENDENCIES std_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
 )
 

mini_pupper_interfaces/srv/MusicCommand.srv

@@ -0,0 +1,5 @@
+string command  # The command to execute ("play")
+string file_name  # The name of the song file
+---
+bool success  # Indicates whether the command was executed successfully
+string message  # Additional information or error message

mini_pupper_music/mini_pupper_music/music_server.py

@@ -17,91 +17,43 @@
 
 import rclpy
 from rclpy.node import Node
-from std_srvs.srv import SetBool
-import sounddevice as sd
-import soundfile as sf
-import threading
+from mini_pupper_interfaces.srv import MusicCommand
+from playsound import playsound
 import os
 from ament_index_python.packages import get_package_share_directory
-import ctypes
 
 
 class SoundPlayerNode(Node):
     def __init__(self):
         super().__init__('mini_pupper_music_service')
         self.service = self.create_service(
-            SetBool,
+            MusicCommand,
             'music_command',
             self.play_sound_callback
         )
-        self.is_playing = False
-        self.playback_thread = None
-        self.lock = threading.Lock()
-        self.load_sound_data()
-
-    def load_sound_data(self):
-        package_name = 'mini_pupper_music'
-        file_name = 'resource/robot1.wav'
-        package_path = get_package_share_directory(package_name)
-        sound_file = os.path.join(package_path, file_name)
-        try:
-            self.sound_data, self.sound_fs = sf.read(sound_file, dtype='float32')
-        except Exception as e:
-            self.get_logger().error('Failed to load sound data: {}'.format(str(e)))
+        self.song_pool = {'robot1.mp3', 'robot1.wav'}
 
     def play_sound_callback(self, request, response):
-        if request.data:
-            with self.lock:
-                if not self.is_playing:
-                    self.play_sound()
-                    response.success = True
-                    response.message = 'Sound playback started.'
-                else:
-                    response.success = False
-                    response.message = 'Sound is already playing.'
-        else:
-            with self.lock:
-                if self.is_playing:
-                    self.stop_sound()
-                    response.success = True
-                    response.message = 'Sound playback stopped.'
-                else:
-                    response.success = False
-                    response.message = 'No sound is currently playing.'
-        return response
-
-    def play_sound(self):
-        self.is_playing = True
-        self.playback_thread = threading.Thread(target=self.play_sound_thread)
-        self.playback_thread.start()
+        if request.command == 'play':
+            if request.file_name in self.song_pool:
+                self.play_sound_file(request.file_name)
+                response.success = True
+                response.message = 'Sound playback started.'
+            else:
+                response.success = False
+                response.message = f'File {request.file_name} is not found.'
 
-    def play_sound_thread(self):
-        while self.is_playing:
-            self.get_logger().info('Playing the song from the beginning')
-            sd.play(self.sound_data, self.sound_fs)
-            sd.wait()
-
-    def stop_sound(self):
-        self.is_playing = False
-        if self.playback_thread is not None:
-            self.playback_thread.join(timeout=1.0)  # Wait for 1 second for the thread to finish
-            if self.playback_thread.is_alive():
-                # If the thread is still running, terminate it forcefully
-                self.get_logger().warning('Playback thread did not terminate gracefully.')
-                self.get_logger().warning('Terminating forcefully.')
-                self.terminate_thread(self.playback_thread)
+        else:
+            response.success = False
+            response.message = f'Command {request.command} is not supported.'
 
-    def terminate_thread(self, thread):
-        if not thread.is_alive():
-            return
+        return response
 
-        thread_id = thread.ident
-        # Terminate the thread using ctypes
-        ctypes.pythonapi.PyThreadState_SetAsyncExc(
-            ctypes.c_long(thread_id),
-            ctypes.py_object(SystemExit)
-        )
-        self.get_logger().warning('Playback thread terminated forcefully.')
+    def play_sound_file(self, file_name):
+        package_name = 'mini_pupper_music'
+        package_path = get_package_share_directory(package_name)
+        sound_path = os.path.join(package_path, 'resource', file_name)
+        playsound(sound_path, block=False)
 
 
 def main(args=None):