CHUNK_SIZEはもっと小さくてもおそらく平気。
# Copyright 2016 Open Source Robotics Foundation, Inc.
#
# 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.
# Copyright 2022 robot-creation-diary.com
#
# 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.
# チュートリアルの内容から、HMDのオイラー角を受け取りそれに合わせて
# サーボモータを制御するように変更してある
# 使用機材:サーボモータ：Miuzei MS18、サーボモータコントローラ：20軸サーボモータコントローラ WR-MSXX
import rclpy
from rclpy.node import Node
from cpp_pubsub.msg import RobotSpeakerMsg
import serial
import copy
import numpy as np
import pyaudio
import matplotlib.pyplot as plt
import time
CHANNELS = 1
SAMPLE_RATE = 44100
SAMPLE_WIDTH = 2
CHUNK_SIZE = 65536 # バッファーサイズ（2の累乗）
class RobotSpeakerSubscriber(Node):
def __init__(self):
super().__init__('robot_speaker_subscriber')
self.subscription = self.create_subscription(
RobotSpeakerMsg,
'robot_speaker_topic',
self.listener_callback,
10)
self.subscription # prevent unused variable warning
self.debuf_time=time.time()
self.pre_debug_count = 0
# 10のデータをまとめることで平滑化する
self.buffer_size = 3
self.audio_data_buffer = [None] * self.buffer_size
self.audio_data_read_count = 0
self.pre_audio_data_read_count = 0
self.audio_data_write_count = 0
self.volume = 5
# PyAudioの初期化
p = pyaudio.PyAudio()
# 出力ストリームを開く（スピーカに再生）
self.output_stream = p.open(format=p.get_format_from_width(SAMPLE_WIDTH),
channels=CHANNELS,
rate=SAMPLE_RATE,
output=True,
frames_per_buffer=CHUNK_SIZE)
print("pyaudio start")
def __del__(self):
pass
def listener_callback(self, msg):
# #audio#########################################
audio_data = copy.deepcopy(msg.audio_data)
if self.pre_debug_count == msg.debug_count:
#self.debuf_time=time.time()
time.sleep(0.001)
return
if 0 < len(audio_data):
self.audio_data_buffer[self.audio_data_write_count] = np.array(audio_data)
self.audio_data_buffer[self.audio_data_write_count]=\
np.clip( self.audio_data_buffer[self.audio_data_write_count]*self.volume, -32768, 32786).astype(np.int16)
wcbuf = (self.audio_data_write_count + 1) % self.buffer_size
if wcbuf != self.audio_data_read_count:
self.audio_data_write_count = wcbuf
if None is not self.audio_data_buffer[self.audio_data_read_count]:
rcbuf = (self.audio_data_read_count + 1) % self.buffer_size
if rcbuf != self.audio_data_write_count:
self.audio_data_read_count = rcbuf
if self.pre_audio_data_read_count != self.audio_data_read_count:
self.output_stream.write(
bytes(self.audio_data_buffer[self.audio_data_read_count]))
self.pre_audio_data_read_count = self.audio_data_read_count
print("debug_count,", msg.debug_count)
#self.debug_prot(audio_data)
print("interval 2",time.time()-self.debuf_time)
self.debuf_time=time.time()
#time.sleep(0.03)
self.pre_debug_count = msg.debug_count
def debug_prot(self, data):
# 左右チャンネルに分割
left_channel = data[::2] # 偶数番目が左チャンネル
right_channel = data[1::2] # 奇数番目が右チャンネル
# 波形をプロットする（左チャンネル）
plt.subplot(2, 1, 1)
plt.plot(left_channel)
plt.title("Left Channel")
plt.ylim(-32768, 32767)
# 波形をプロットする（右チャンネル）
plt.subplot(2, 1, 2)
plt.plot(right_channel)
plt.title("Right Channel")
plt.ylim(-32768, 32767)
#print ("right_channel",right_channel)
# プロットを更新
plt.pause(0.01)
plt.clf()
def main(args=None):
rclpy.init(args=args)
minimal_subscriber = RobotSpeakerSubscriber()
rclpy.spin(minimal_subscriber)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()