diff --git a/README.md b/README.md index 7f2fe2e..ad6477f 100755 --- a/README.md +++ b/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 diff --git a/mini_pupper_dance/mini_pupper_dance/dance_client.py b/mini_pupper_dance/mini_pupper_dance/dance_client.py index d9f85f9..5aeddea 100644 --- a/mini_pupper_dance/mini_pupper_dance/dance_client.py +++ b/mini_pupper_dance/mini_pupper_dance/dance_client.py @@ -3,8 +3,10 @@ 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): @@ -12,7 +14,9 @@ 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() diff --git a/mini_pupper_dance/mini_pupper_dance/episode.py b/mini_pupper_dance/mini_pupper_dance/episode.py index 7857f5a..9311358 100644 --- a/mini_pupper_dance/mini_pupper_dance/episode.py +++ b/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', diff --git a/mini_pupper_interfaces/CMakeLists.txt b/mini_pupper_interfaces/CMakeLists.txt index d237220..0262fa6 100644 --- a/mini_pupper_interfaces/CMakeLists.txt +++ b/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 ) diff --git a/mini_pupper_interfaces/srv/PlayMusic.srv b/mini_pupper_interfaces/srv/PlayMusic.srv new file mode 100644 index 0000000..0cce0ae --- /dev/null +++ b/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 \ No newline at end of file diff --git a/mini_pupper_interfaces/srv/StopMusic.srv b/mini_pupper_interfaces/srv/StopMusic.srv new file mode 100644 index 0000000..1847da1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/mini_pupper_music/README.md b/mini_pupper_music/README.md new file mode 100644 index 0000000..747361b --- /dev/null +++ b/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 +``` + + + + + + diff --git a/mini_pupper_music/mini_pupper_music/music_player.py b/mini_pupper_music/mini_pupper_music/music_player.py new file mode 100644 index 0000000..70a845d --- /dev/null +++ b/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() diff --git a/mini_pupper_music/mini_pupper_music/music_server.py b/mini_pupper_music/mini_pupper_music/music_server.py index c6715bf..66232d9 100644 --- a/mini_pupper_music/mini_pupper_music/music_server.py +++ b/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() diff --git a/mini_pupper_music/resource/robot1.mp3 b/mini_pupper_music/resource/robot1.mp3 new file mode 100644 index 0000000..d027ddb Binary files /dev/null and b/mini_pupper_music/resource/robot1.mp3 differ