simplify music server with playsound package and support mp3 files

README.md

@@ -71,6 +71,14 @@ cd mini_pupper_ros
 ./install.sh
 ```
 
+Install packages for playing music
+```sh
+sudo apt-get install ffmpeg portaudio19-dev -y
+pip install pydub
+pip install pyaudio
+
+```
+
 ## 2. Quick Start
 
 ## 2.1 Mini Pupper

mini_pupper_dance/mini_pupper_dance/dance_client.py

@@ -3,16 +3,20 @@
 import rclpy
 from rclpy.node import Node
 from mini_pupper_interfaces.srv import DanceCommand
-from std_srvs.srv import SetBool
+from mini_pupper_interfaces.srv import PlayMusic, StopMusic
 from .episode import dance_commands
+from .episode import dance_song_file_name
+from .episode import dance_song_start_second
 
 
 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.play_music_cli = self.create_client(PlayMusic, 'play_music')
+        self.stop_music_cli = self.create_client(StopMusic, 'stop_music')
+
         while not self.dance_cli.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('service not available, waiting again...')
 
@@ -25,10 +29,15 @@ 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
-        self.music_cli.call_async(req)  # Fire-and-forget style communication
+    def send_play_music_request(self, file_name, start_second):
+        req = PlayMusic.Request()
+        req.file_name = file_name
+        req.start_second = start_second
+        self.play_music_cli.call_async(req)  # Fire-and-forget style communication
+
+    def send_stop_music_request(self):
+        req = StopMusic.Request()
+        self.stop_music_cli.call_async(req)  # Fire-and-forget style communication
 
 
 def main():
@@ -39,7 +48,8 @@ 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_play_music_request(dance_song_file_name,
+                                                   dance_song_start_second)
 
         # Send movemoment comment for the robot to dance
         response = minimal_client.send_dance_request(command)
@@ -49,7 +59,7 @@ def main():
         # 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.send_stop_music_request()
 
     minimal_client.destroy_node()
     rclpy.shutdown()

mini_pupper_dance/mini_pupper_dance/episode.py

@@ -12,6 +12,9 @@
 # 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_song_start_second = 5.0
+
 dance_commands = [
     'move_forward',
     'look_middle',

mini_pupper_interfaces/CMakeLists.txt

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

mini_pupper_interfaces/srv/PlayMusic.srv

@@ -0,0 +1,6 @@
+string file_name  # The name of the song file
+float32 start_second  # Optional offset (in seconds) to start loading the audio file
+float32 duration  # Optional duration to be played
+---
+bool success  # Indicates whether the command was executed successfully
+string message  # Additional information or error message

mini_pupper_interfaces/srv/StopMusic.srv

@@ -0,0 +1,3 @@
+---
+bool success  # Indicates whether the command was executed successfully
+string message  # Additional information or error message

mini_pupper_music/README.md

@@ -0,0 +1,19 @@
+## Quick Guide
+
+You can call service in terminal as below:
+
+```
+# To play the music
+ros2 service call /play_music mini_pupper_interfaces/srv/PlayMusic "{file_name: 'robot1.mp3', start_second: 3}"
+```
+
+```
+# To stop the music
+ros2 service call /stop_music mini_pupper_interfaces/srv/StopMusic
+```
+
+
+
+
+
+

mini_pupper_music/mini_pupper_music/music_player.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+#
+# Copyright 2023 MangDang
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+# @Author  : Cullen SUN
+
+import threading
+import pyaudio
+from pydub import AudioSegment
+from pydub.utils import make_chunks
+import sounddevice
+
+
+class MusicPlayer:
+    def __init__(self):
+        # sounddevice is not used in any logics, just print something here
+        # See: https://stackoverflow.com/a/76305661/663645
+        print(sounddevice.query_devices())
+
+        self.audio = pyaudio.PyAudio()
+        self.playing = False
+        self.play_thread = None
+
+    def _play_music(self, file_path, start_second, duration):
+        file_extension = file_path.split(".")[-1]
+        if duration == 0.0:
+            duration = None
+
+        self.playing = True
+
+        audio_seg = AudioSegment.from_file(
+            file=file_path,
+            format=file_extension,
+            start_second=start_second,
+            duration=duration
+        )
+
+        stream = self.audio.open(
+            format=self.audio.get_format_from_width(audio_seg.sample_width),
+            channels=audio_seg.channels,
+            rate=audio_seg.frame_rate,
+            output=True
+        )
+
+        try:
+            for chunk in make_chunks(audio_seg, 500):
+                if self.playing:
+                    stream.write(chunk._data)
+                else:
+                    break
+        finally:
+            stream.stop_stream()
+            stream.close()
+            self.playing = False
+
+    def start_music(self, file_path, start_second=0, duration=0.0):
+        self.play_thread = threading.Thread(
+            target=self._play_music,
+            args=(file_path, start_second, duration),
+            daemon=True
+        )
+        self.play_thread.start()
+
+    def stop_music(self):
+        self.playing = False
+
+    def destroy(self):
+        self.stop_music()
+        self.audio.terminate()
+        self.play_thread.join()

mini_pupper_music/mini_pupper_music/music_server.py

@@ -17,98 +17,74 @@
 
 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 PlayMusic, StopMusic
+from .music_player import MusicPlayer
 import os
 from ament_index_python.packages import get_package_share_directory
-import ctypes
 
 
-class SoundPlayerNode(Node):
+class MusicServiceNode(Node):
     def __init__(self):
         super().__init__('mini_pupper_music_service')
-        self.service = self.create_service(
-            SetBool,
-            'music_command',
-            self.play_sound_callback
+        self.music_player = MusicPlayer()
+        self.play_service = self.create_service(
+            PlayMusic,
+            'play_music',
+            self.play_music_callback
+        )
+        self.stop_service = self.create_service(
+            StopMusic,
+            'stop_music',
+            self.stop_music_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)))
+    def play_music_callback(self, request, response):
+        file_path = self.get_valid_file_path(request.file_name)
+        if file_path is not None:
+            if self.music_player.playing:
+                response.success = False
+                response.message = 'Another music is being played.'
+            else:
+                self.music_player.start_music(file_path,
+                                              request.start_second,
+                                              request.duration)
+                response.success = True
+                response.message = 'Music started playing.'
+                self.get_logger().info(f"playing music at {file_path}")
 
-    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.'
+            response.success = False
+            response.message = f'File {request.file_name} is not found.'
         return response
 
-    def play_sound(self):
-        self.is_playing = True
-        self.playback_thread = threading.Thread(target=self.play_sound_thread)
-        self.playback_thread.start()
-
-    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)
+    def stop_music_callback(self, request, response):
+        if self.music_player.playing:
+            self.music_player.stop_music()
+            response.success = True
+            response.message = 'Music playback stopped.'
+        else:
+            response.success = False
+            response.message = 'No music is being played.'
+        return response
 
-    def terminate_thread(self, thread):
-        if not thread.is_alive():
-            return
+    def get_valid_file_path(self, file_name):
+        package_name = 'mini_pupper_music'
+        package_path = get_package_share_directory(package_name)
+        file_path = os.path.join(package_path, 'resource', file_name)
+        if os.path.isfile(file_path):
+            return file_path
+        else:
+            return None
 
-        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 destroy_node(self):
+        self.music_player.destroy()
+        super().destroy_node()
 
 
 def main(args=None):
     rclpy.init(args=args)
-    sound_player_node = SoundPlayerNode()
-    rclpy.spin(sound_player_node)
-    sound_player_node.destroy_node()
+    music_service_node = MusicServiceNode()
+    rclpy.spin(music_service_node)
     rclpy.shutdown()