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):