RTC::ReturnCode_t EchoCanceler::onInitialize()
{
RTC_DEBUG(("onInitialize start"));
RTC_INFO(("EchoCanceler : Acoustic echo cancellation component using adaptive filter"));
RTC_INFO((" Copyright (C) 2010-2011 Yosuke Matsusaka and AIST-OpenHRI development team"));
RTC_INFO((" Version %s", VERSION));
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("AudioDataIn", m_ninIn);
addInPort("ReferenceAudioDataIn", m_finIn);
/* setiing datalistener event */
m_ninIn.addConnectorDataListener(ON_BUFFER_WRITE, new DataListener("ON_BUFFER_WRITE_N", this));
m_ninIn.setDescription(_("Audio data input (from mic)."));
m_finIn.addConnectorDataListener(ON_BUFFER_WRITE, new DataListener("ON_BUFFER_WRITE_F", this));
m_finIn.setDescription(_("Referenct audio data input (from AudioOuput component)."));
// Set OutPort buffer
registerOutPort("AudioDataOut", m_foutOut);
m_foutOut.setDescription(_("Audio data output."));
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
mp_sest = NULL;
RTC_DEBUG(("onInitialize finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t PulseAudioInput::onActivated(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onActivated start"));
try {
pa_cvolume cv;
// mp_vol = pa_cvolume_reset(&cv, 1);
// pa_cvolume_init(mp_vol);
m_spec.format = getFormat(m_formatstr);
m_spec.channels = (uint8_t)m_channels;
m_simple = pa_simple_new(
NULL, //!< Server name, or NULL for default
"PulseAudioInput", //!< A descriptive name for this client (application name, ...)
PA_STREAM_RECORD, //!< Open this stream for recording or playback?
NULL, //!< Sink (resp. source) name, or NULL for default
"record", //!< A descriptive name for this client (application name, song title, ...)
&m_spec, //!< The sample type to use
NULL, //!< The channel map to use, or NULL for default
NULL, //!< Buffering attributes, or NULL for default
&m_err ); //!< A pointer where the error code is stored when the routine returns NULL. It is OK to pass NULL here.
if ( m_simple == NULL ) {
throw m_err;
}
} catch (...) {
std::string error_str = pa_strerror(m_err);
RTC_WARN(("pa_simple_new() failed onActivated:%s", error_str.c_str()));
}
is_active = true;
RTC_DEBUG(("onActivated finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t PortAudioInput::onFinalize()
{
RTC_DEBUG(("onFinalize start"));
is_active = false;
m_mutex.lock();
RTC_DEBUG(("onFinalize:mutex lock"));
try {
m_out_dataOut.deactivateInterfaces();
if ( m_stream ) {
if ( !Pa_IsStreamStopped( m_stream ) ) {
m_err = Pa_AbortStream( m_stream );
if ( m_err != paNoError ) {
throw m_err;
}
}
m_err = Pa_CloseStream( m_stream );
if ( m_err != paNoError ) {
throw m_err;
}
m_stream = NULL;
}
} catch (...) {
std::string error_str = Pa_GetErrorText(m_err);
RTC_WARN(("PortAudio Stream close failed onFinalize:%s", error_str.c_str()));
return RTC::RTC_ERROR;
}
m_mutex.unlock();
RTC_DEBUG(("onFinalize:mutex unlock"));
RTC_DEBUG(("onFinalize finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t WavPlayer::onActivated(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onActivated start"));
#if defined(__linux)
RTC_INFO(("Linux platform is not supported yet."));
#elif defined(_WIN32)
sfinfo.samplerate = (int)m_samplerate;
sfinfo.channels = m_channels;
sfinfo.format = SF_FORMAT_WAV | SF_FORMAT_PCM_16;
try {
sfr = sf_open(WaveFileName, SFM_READ, &sfinfo);
RTC_INFO(("Wave File Name: %s\n", WaveFileName));
if (sf_format_check(&sfinfo) == 0) {
RTC_DEBUG(("invalid format"));
RTC_INFO(("Wave file invalid format"));
return RTC::RTC_ERROR;
}
if (sfr == NULL) {
//RTC_DEBUG(("unable to open file: %s", m_filename.c_str()));
RTC_DEBUG(("unable to open file: %s", WaveFileName));
return RTC::RTC_ERROR;
}
m_timer = coil::gettimeofday() - 1.0;
} catch (...) {
RTC_WARN(("%s", "error onActivated."));
return RTC::RTC_ERROR;
}
#endif
RTC_DEBUG(("onActivated finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t Gate::onInitialize()
{
RTC_DEBUG(("onInitialize start"));
RTC_INFO(("Gate : Gate component"));
RTC_INFO((" Copyright (C) 2010-2011 Yosuke Matsusaka and AIST-OpenHRI development team"));
RTC_INFO((" Version %s", VERSION));
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("AudioDataIn", m_inIn);
m_inIn.setDescription(_("Audio data input."));
addInPort("GateIn", m_gateIn);
m_gateIn.setDescription(_("Gate data input."));
// Set OutPort buffer
addOutPort("AudioDataOut", m_outOut);
m_outOut.setDescription(_("Audio data output."));
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
RTC_DEBUG(("onInitialize finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t PortAudioInput::onExecute(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onExecute start"));
//! The data read from the device is output.
m_mutex.lock();
RTC_DEBUG(("onExecute:mutex lock"));
if ( Pa_IsStreamActive(m_stream) ) {
m_out_data.data.length(m_totalframes); //!< set outport data length
m_err = Pa_ReadStream( m_stream, (void *)&(m_out_data.data[0]), FRAMES_PER_BUFFER );
// if( ( m_gain != 0 ) && ( m_gain != 1 ) && ( m_agc != 0 ) ) {
// AutoGainControl((void *)&(m_out_data.data[0]), (int)m_totalframes );
// }
if ( m_err != paNoError ) {
std::string error_str = Pa_GetErrorText(m_err);
RTC_WARN(("PortAudio ReadStream failed:%s", error_str.c_str()));
}
setTimestamp(m_out_data);
m_out_dataOut.write();
}
m_mutex.unlock();
RTC_DEBUG(("onExecute:mutex unlock"));
RTC_DEBUG(("onExecute finish"));
return RTC::RTC_OK;
}
/*!
* @if jp
* @brief デフォルトコンストラクタ
* @else
* @brief Default Constructor
* @endif
*/
RTPreemptEC::RTPreemptEC()
: ::RTC::PeriodicExecutionContext(),
m_priority(49), m_policy(SCHED_FIFO), m_waitoffset(0)
{
rtclog.setName("RTPreemptEC");
coil::Properties& prop(::RTC::Manager::instance().getConfig());
// Priority
getProperty(prop, "exec_cxt.periodic.priority", m_priority);
getProperty(prop, "exec_cxt.periodic.rtpreempt.priority", m_priority);
RTC_DEBUG(("Priority: %d", m_priority));
// Policy
{
std::string policy;
getProperty(prop, "exec_cxt.periodic.rtpreempt.sched_policy", policy);
if (!policy.empty())
{
coil::normalize(policy);
if (policy == "rr") { m_policy = SCHED_RR; }
if (policy == "fifo") { m_policy = SCHED_FIFO; }
RTC_DEBUG(("Scheduling policy: %s", policy.c_str()));
}
else
{
RTC_DEBUG(("Scheduling policy: fifo"));
}
}
// Wait offset
getProperty(prop, "exec_cxt.periodic.rtpreempt.wait_offset", m_waitoffset);
RTC_DEBUG(("Wait offset: %d [ns]", m_waitoffset));
}
RTC::ReturnCode_t PortAudioInput::onInitialize()
{
RTC_DEBUG(("onInitialize start"));
RTC_INFO(("PortAudioInput : Audio input component using portaudio library"));
RTC_INFO((" Copyright (C) 2010-2011 Yosuke Matsusaka and AIST-OpenHRI development team"));
RTC_INFO((" Version %s", VERSION));
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("GainDataIn", m_in_dataIn);
m_in_dataIn.setDescription(_("Gain."));
/* setting datalistener event */
m_in_dataIn.addConnectorDataListener(ON_BUFFER_WRITE, new PortAudioInputDataListener("ON_BUFFER_WRITE", this), false);
// Set OutPort buffer
registerOutPort("AudioDataOut", m_out_dataOut);
m_out_dataOut.setDescription(_("Audio data in packet."));
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
bindParameter("OutputSampleRate", m_samplerate, "16000");
bindParameter("OutputSampleByte", m_formatstr, "int16");
bindParameter("OutputChannelNumbers", m_channels, "1");
RTC_DEBUG(("onInitialize finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t WebRTCVAD::onExecute(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onExecute start"));
m_mutex.lock();
RTC_DEBUG(("onExecute:mutex lock"));
if (m_inbuffer.size() >= WINLEN) {
int i;
WebRtc_Word16 *data;//ADDED
data = new WebRtc_Word16[WINLEN];//CHANGED
std::list<short>::iterator pbuffer;
// sliding window with half overlap
for (i = 0; i < WINLEN/2; i++) {
data[i] = m_inbuffer.front();
m_inbuffer.pop_front();
}
pbuffer = m_inbuffer.begin();
for (i = WINLEN/2; i < WINLEN; i++) {
data[i] = *pbuffer;
pbuffer++;
}
WebRtc_Word16 vad = WebRtcVad_Process(handle, 16000, data, WINLEN);
m_filterdatabuffer.push_back(data);//ADDED
m_filterflagbuffer.push_back(vad);//ADDED
while (m_filterdatabuffer.size() > m_bufferlen) {//CHANGED Begin
WebRtc_Word16 vad1 = 0;
std::list<WebRtc_Word16>::iterator it = m_filterflagbuffer.begin();
for (i = 0; i < m_bufferlen; i++) {
WebRtc_Word16 vad2 = *it;
if (vad2 > 0) {
vad1 = vad2;
}
it++;
}
//RTC_INFO(("vad: %i, vad(filtered): %i", vad, vad1));
m_filterflagbuffer.pop_front();
data = m_filterdatabuffer.front();
m_filterdatabuffer.pop_front();
// output the resulting signal
m_fout.data.length(WINLEN);
if (vad1 > 0) {
for (i = 0; i < WINLEN/2; i++) {
m_fout.data[i*2] = (unsigned char)(data[i] & 0x00ff);
m_fout.data[i*2+1] = (unsigned char)((data[i] & 0xff00) >> 8);
}
} else {
for (i = 0; i < WINLEN/2; i++) {
m_fout.data[i*2] = i % 2; // avoid julius zero stripping problem
m_fout.data[i*2+1] = 0;
}
}
delete [] data;
setTimestamp(m_fout);
m_foutOut.write();
}//CHANGED End
RTC::ReturnCode_t PulseAudioOutput::onExecute(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t ret = RTC::RTC_OK;
RTC_DEBUG(("onExecute start"));
if (m_simple) {
ret = WriteBuffer();
}
RTC_DEBUG(("onExecute finish"));
return ret;
}
RTC::ReturnCode_t EchoCanceler::onActivated(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onActivated start"));
is_active = true;
BufferClr();
if ( mp_sest == NULL ) {
int sampleRate = 16000;
mp_sest = speex_echo_state_init(ECHOLEN, sampleRate * 2);
speex_echo_ctl(mp_sest, SPEEX_ECHO_SET_SAMPLING_RATE, &sampleRate);
}
RTC_DEBUG(("onActivated finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t PulseAudioOutput::onFinalize()
{
RTC_DEBUG(("onFinalize start"));
is_active = false;
m_mutex.lock();
RTC_DEBUG(("onFinalize:mutex lock"));
try {
if ( !m_data.empty() ) {
RTC_DEBUG(("onFinalize:queue buffer clear start."));
m_data.clear(); //!< queue buffer clear
RTC_DEBUG(("onFinalize:queue buffer clear finish."));
}
if ( m_simple ) {
RTC_DEBUG(("onFinalize:simple connection object free start."));
pa_simple_free( m_simple );
RTC_DEBUG(("onFinalize:simple connection object free finish."));
m_simple = NULL;
}
} catch (...) {
RTC_WARN(("pa_simple_free() failed onDFinalize."));
}
m_mutex.unlock();
RTC_DEBUG(("onFinalize:mutex unlock"));
RTC_DEBUG(("onFinalize finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t WavPlayer::onInitialize()
{
RTC_DEBUG(("onInitialize start"));
RTC_INFO(("WavPlayer : Wave player component"));
RTC_INFO((" Copyright (C) 2010-2011 Yosuke Matsusaka and AIST-OpenHRI development team"));
RTC_INFO((" Version %s", VERSION));
registerOutPort("AudioDataOut", m_out_dataOut);
m_out_dataOut.setDescription(_("Audio data out packet."));
bindParameter("OutputSampleRate", m_samplerate, "16000");
bindParameter("ChannelNumbers", m_channels, "1");
#if defined(__linux)
bindParameter("FileName", m_filename, "wavrecord-default.wav");
#ifdef SHARED_LIB
Gtk::FileChooserDialog diag( "ファイル選択", Gtk::FILE_CHOOSER_ACTION_OPEN );
// 開く、キャンセルボタン
diag.add_button(Gtk::Stock::OPEN, Gtk::RESPONSE_OK);
diag.add_button(Gtk::Stock::CANCEL, Gtk::RESPONSE_CANCEL);
switch( diag.run() ){
case Gtk::RESPONSE_OK:
strncpy(WaveFileName, (diag.get_filename()).c_str(), (diag.get_filename()).size());
break;
case Gtk::RESPONSE_CANCEL:
strncpy(WaveFileName, m_filename.c_str(), m_filename.size());
break;
}
Gtk::MessageDialog( WaveFileName ).run();
#endif //SHARED_LIB
#elif defined(_WIN32)
bindParameter("FileName", m_filename, "c:\\work\\wavrecord-default.wav");
#ifdef SHARED_LIB
HWND hwnd = GetWindow( NULL, GW_OWNER );
ZeroMemory(WaveFileName,MAX_PATH*2);
strncpy(WaveFileName, m_filename.c_str(), m_filename.size());
//printf("m_filename.c_str: %s\n", m_filename.c_str());
//printf("m_filename.size: %d\n", m_filename.size());
//printf("Wave File Name: %s\n", WaveFileName);
if (OpenDiaog(hwnd,"Wave Files(*.wav)\0*.wav\0All Files(*.*)\0*.*\0\0",
WaveFileName,OFN_PATHMUSTEXIST | OFN_FILEMUSTEXIST | OFN_HIDEREADONLY))
//MessageBox(hwnd,strcat(WaveFileName,"\nを選択しました。"),"情報",MB_OK);
#endif//SHARED_LIB
#endif//defined(_WIN32)
RTC_DEBUG(("onInitialize finish"));
RTC_INFO(("onInitialize finish"));
return RTC::RTC_OK;
}
void WavRecord::RcvBuffer(TimedOctetSeq data)
{
RTC_DEBUG(("RcvBuffer start"));
if (is_active == true) {
m_mutex.lock();
RTC_DEBUG(("RcvBuffer:mutex lock"));
sf_count_t frames = data.data.length();
sf_write_raw(sfw, &data.data[0], frames);
m_mutex.unlock();
RTC_DEBUG(("RcvBuffer:mutex unlock"));
}
RTC_DEBUG(("RcvBuffer finish"));
return;
}
void EchoCanceler::BufferClr(void)
{
RTC_DEBUG(("BufferClr start"));
m_mutex.lock();
RTC_DEBUG(("BufferClr:mutex lock"));
if (!m_outdata.empty()) {
m_outdata.clear(); //!< queue buffer clear
}
if (!m_indata.empty()) {
m_indata.clear(); //!< queue buffer clear
}
m_mutex.unlock();
RTC_DEBUG(("BufferClr:mutex unlock"));
RTC_DEBUG(("BufferClr finish"));
}
void PortAudioInput::AutoGainControl(void *data, int length)
{
RTC_DEBUG(("AutoGainControl start"));
int i,len;
if ( m_formatstr == "int8" ) {
len = length;
char *buffer = (char *)data;
for ( i = 0; i < len; i++ ) {
buffer[i] = (char)(buffer[i] * m_gain);
}
} else if ( m_formatstr == "int24" ) {
len = length / 3;
long *buffer = (long *)data;
for ( i = 0; i < len; i++ ) {
buffer[i] = (long)(buffer[i] * m_gain);
}
} else {
len = length / 2;
short *buffer = (short *)data;
for ( i = 0; i < len; i++ ) {
buffer[i] = (short)(buffer[i] * m_gain);
}
}
return;
}
RTC::ReturnCode_t WavPlayer::onExecute(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onExecute start"));
coil::TimeValue now = coil::gettimeofday();
long bufferlen = long((now - m_timer) * m_samplerate);
if ( bufferlen <= 0 ) return RTC::RTC_OK;
m_timer = now;
short *buffer = new short[bufferlen];
sf_readf_short(sfr, buffer, bufferlen) ;
m_out_data.data.length(bufferlen * 2); //!< set outport data length
memcpy((void *)&(m_out_data.data[0]), (void *)&(buffer[0]), bufferlen * 2);
setTimestamp(m_out_data);
m_out_dataOut.write();
delete [] buffer;
RTC_DEBUG(("onExecute finish"));
return RTC::RTC_OK;
}
void PulseAudioOutput::RcvBuffer(TimedOctetSeq data)
{
RTC_DEBUG(("AudioDataIn port:ON_BUFFER_WRITE"));
if ( is_active == true ) {
//! The queuing does input data.
m_mutex.lock();
RTC_DEBUG(("RcvBuffer:mutex lock"));
RTC_DEBUG(("Queuing input data process start."));
unsigned long len = data.data.length(); //!< inport data length
for (unsigned long i = 0; i < len; i++) {
m_data.push_back((unsigned char)data.data[i]); //!< enqueue
}
RTC_DEBUG(("Queuing input data process finish."));
m_mutex.unlock();
RTC_DEBUG(("RcvBuffer:mutex unlock"));
}
return;
}
RTC::ReturnCode_t PulseAudioOutput::onActivated(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onActivated start"));
try {
m_spec.format = getFormat(m_formatstr);
m_spec.channels = (uint8_t)m_channels;
m_spec.rate = (uint32_t)m_samplerate;
m_bufferattr.maxlength = m_bufferattr.minreq = (uint32_t)-1;
m_bufferattr.tlength = m_bufferattr.prebuf = (uint32_t)(m_bufferlen * m_samplerate * m_channels);
m_simple = pa_simple_new(NULL, //!< Server name, or NULL for default
"PulseAudioOutput", //!< A descriptive name for this client (application name, ...)
PA_STREAM_PLAYBACK, //!< Open this stream for recording or playback?
NULL, //!< Sink (resp. source) name, or NULL for default
"playback", //!< A descriptive name for this client (application name, song title, ...)
&m_spec, //!< The sample type to use
NULL, //!< The channel map to use, or NULL for default
&m_bufferattr, //!< Buffering attributes, or NULL for default
&m_err ); //!< A pointer where the error code is stored when the routine returns NULL. It is OK to pass NULL here.
if ( m_simple == NULL ) {
throw m_err;
}
} catch (...) {
std::string error_str = pa_strerror(m_err);
RTC_WARN(("pa_simple_new() failed onActivated:%s", error_str.c_str()));
return RTC::RTC_ERROR;
}
if (!m_data.empty()) {
RTC_DEBUG(("onActivated:queue buffer clear start."));
m_data.clear(); //!< queue buffer clear
RTC_DEBUG(("onActivated:queue buffer clear finish."));
}
is_active = true;
m_writezero = true;
RTC_DEBUG(("onActivated finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t WavRecord::onActivated(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onActivated start"));
RTC_INFO(("Wave File Name: %s\n", WaveFileName));
sfinfo.samplerate = m_rate;
sfinfo.channels = m_channels;
sfinfo.format = SF_FORMAT_WAV | SF_FORMAT_PCM_16;
if (sf_format_check(&sfinfo) == 0) {
RTC_DEBUG(("invalid format"));
return RTC::RTC_ERROR;
}
sfw = sf_open(WaveFileName, SFM_WRITE, &sfinfo);
if (sfw == NULL) {
RTC_DEBUG(("unable to open file: %s", WaveFileName));
return RTC::RTC_ERROR;
}
is_active = true;
RTC_DEBUG(("onActivated finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t EchoCanceler::onExecute(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onExecute start"));
if((m_indata.size() > BUFFER_MAX) || (m_outdata.size() > BUFFER_MAX)) {
RTC_INFO(("One of buffers exceeded the maximum value. Start clear buffers."));
BufferClr();
}
if (( m_indata.size() >= ECHOLEN) && (m_outdata.size() >= ECHOLEN)) {
m_mutex.lock();
RTC_DEBUG(("onExecute:mutex lock"));
int i;
short *inbuffer = new short[ECHOLEN];
short *outbuffer = new short[ECHOLEN];
short *result = new short[ECHOLEN];
for ( i = 0; i < ECHOLEN; i++ ) {
inbuffer[i] = m_indata.front();
m_indata.pop_front();
outbuffer[i] = m_outdata.front();
m_outdata.pop_front();
result[i] = 0;
}
m_mutex.unlock();
RTC_DEBUG(("onExecute:mutex unlock"));
speex_echo_cancellation(mp_sest, inbuffer, outbuffer, result);
delete[] inbuffer;
delete[] outbuffer;
m_fout.data.length(ECHOLEN * 2);
for ( i = 0; i < ECHOLEN; i++ ) {
short val = result[i];
m_fout.data[i*2] = (unsigned char)(val & 0x00ff);
m_fout.data[i*2+1] = (unsigned char)((val & 0xff00) >> 8);
}
delete[] result;
setTimestamp( m_fout );
m_foutOut.write();
RTC_DEBUG(("onExecute:writing %d samples", m_fout.data.length() / 2));
} else {
RTC::ReturnCode_t PortAudioInput::onDeactivated(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onDeactivated start"));
is_active = false;
m_mutex.lock();
RTC_DEBUG(("onDeactivated:mutex lock"));
try {
if ( m_stream ) {
if ( !Pa_IsStreamStopped( m_stream ) ) {
m_err = Pa_AbortStream( m_stream );
if ( m_err != paNoError ) {
throw m_err;
}
}
#ifdef HAVE_LIBPORTMIXER
Px_CloseMixer(m_mixer);
#elif defined(__linux)
if ( m_fd > 0 ) {
close( m_fd );
m_fd = -1;
}
#endif
m_err = Pa_CloseStream( m_stream );
if ( m_err != paNoError ) {
throw m_err;
}
m_stream = NULL;
}
} catch (...) {
std::string error_str = Pa_GetErrorText(m_err);
RTC_WARN(("PortAudio Stream close failed onDeactivated:%s", error_str.c_str()));
return RTC::RTC_ERROR;
}
m_mutex.unlock();
RTC_DEBUG(("onDeactivated:mutex unlock"));
RTC_DEBUG(("onDeactivated finish"));
return RTC::RTC_OK;
}
void EchoCanceler::RcvOutBuffer(TimedOctetSeq data)
{
RTC_DEBUG(("RcvOutBuffer start"));
if (is_active == true) {
m_mutex.lock();
RTC_DEBUG(("RcvOutBuffer:mutex lock"));
unsigned char wk0, wk1;
short wk;
int length = data.data.length(); //!< inport data length
for (int i = 0; i < length/2; i++) {
wk0 = (unsigned char)data.data[i*2];
wk1 = (unsigned char)data.data[i*2+1];
wk = (short)(wk1 << 8) + (short)wk0;
m_outdata.push_back(wk);
}
RTC_DEBUG(("Input2:size %d, time %d:%d, buffer %d", length/2, data.tm.sec, data.tm.nsec, m_outdata.size()));
m_mutex.unlock();
RTC_DEBUG(("RcvOutBuffer:mutex unlock"));
}
RTC_DEBUG(("RcvOutBuffer finish"));
return;
}
void WebRTCVAD::RcvInBuffer(TimedOctetSeq data)
{
RTC_DEBUG(("RcvInBuffer start"));
if ( is_active == true ) {
m_mutex.lock();
RTC_DEBUG(("RcvInBuffer:mutex lock"));
int length = data.data.length();
short wk;
unsigned char wk0, wk1;
for (int i = 0; i < length/2; i++) {
wk0 = (unsigned char)data.data[i*2];
wk1 = (unsigned char)data.data[i*2+1];
wk = (short)(wk1 << 8) + (short)wk0;
m_inbuffer.push_back(wk);
}
m_mutex.unlock();
RTC_DEBUG(("RcvInBuffer:mutex unlock"));
}
RTC_DEBUG(("RcvInBuffer finish"));
return;
}
RTC::ReturnCode_t WebRTCVAD::onActivated(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onActivated start"));
//RTC_INFO(("onActivated start0"));
//WebRtcVad_Create(&handle);
//WebRtcVad_Init(handle);
//WebRtcVad_set_mode(handle, 2); // agressive adaptation mode
m_mutex.lock();
if (!m_inbuffer.empty()) {
m_inbuffer.clear();
}
//RTC_INFO(("onActivated start1"));
if (!m_filterflagbuffer.empty()) {//ADDED Begin
m_filterflagbuffer.clear();
}
//RTC_INFO(("onActivated start2"));
if (!m_filterdatabuffer.empty()) {
std::list<WebRtc_Word16*>::iterator it = m_filterdatabuffer.begin();
while (it != m_filterdatabuffer.end()) {
delete [] *it;
//RTC_INFO(("delete *it"));
it++;
}
m_filterdatabuffer.clear();
}//ADDED End
m_mutex.unlock();
is_active = true;
RTC_DEBUG(("onActivated finish"));
//RTC_INFO(("onActivated finish"));
return RTC::RTC_OK;
}
RTC::ReturnCode_t WebRTCVAD::onInitialize()
{
RTC_DEBUG(("onInitialize start"));
RTC_INFO(("WebRTCVAD : WebRTC based noise reduction component"));
RTC_INFO((" Copyright (C) 2010-2011 Yosuke Matsusaka and AIST-OpenHRI development team"));
RTC_INFO((" Version %s", VERSION));
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("AudioDataIn", m_ninIn);
m_ninIn.setDescription(_("Audio data input."));
/* setiing datalistener event */
m_ninIn.addConnectorDataListener(ON_BUFFER_WRITE,
new DataListener("ON_BUFFER_WRITE", this));
// Set OutPort buffer
addOutPort("AudioDataOut", m_foutOut);
m_foutOut.setDescription(_("Audio data output."));
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
bindParameter("FilterLength", m_bufferlen, "5");//ADDED
WebRtcVad_Create(&handle);
WebRtcVad_Init(handle);
WebRtcVad_set_mode(handle, 2); // agressive adaptation mode
RTC_DEBUG(("onInitialize finish"));
return RTC::RTC_OK;
}
bool DcpsFuncSubImpl::read(octecSeq* oct_seq)
{
RTC_TRACE(("read(DataType*)"));
if (!reader_)
{
RTC_INFO(("Port is not connected; cannot read"));
return false;
}
// Attempt to take
// Use 'take' function and set the sample state to ANY_SAMPLE_STATE.
// (Don't use 'take_next_sample')
// Because the sample state becomes OLD by 'isNew' function.
DDS::SampleInfoSeq info(1);
dds_datatype::DdsDataTypeSeq value(1);
DDS::ReturnCode_t ret = type_reader_->take(
value,
info,
1,
DDS::ANY_SAMPLE_STATE,
DDS::ANY_VIEW_STATE,
DDS::ANY_INSTANCE_STATE);
if (ret == DDS::RETCODE_NO_DATA)
{
RTC_INFO(("No data available"));
return false;
}
else if (ret != DDS::RETCODE_OK)
{
RTC_ERROR(("Read error %d", ret));
return false;
}
if (!info[0].valid_data)
{
RTC_INFO(("Out-of-band data read"));
return false;
}
RTC_DEBUG(("Successfully read data"));
// Convert DdsDataType to sequence<octet>
unsigned long length = value[0].data.length();
for (unsigned int i = 0; i < length; i++)
{
oct_seq->push_back(value[0].data[i]);
}
return true;
}
hrpExecutionContext::hrpExecutionContext()
#ifndef OPENRTM_VERSION_TRUNK
: PeriodicExecutionContext(),
#else
: RTC_exp::PeriodicExecutionContext(),
#endif
m_priority(49)
{
resetProfile();
rtclog.setName("hrpEC");
coil::Properties& prop(Manager::instance().getConfig());
// Priority
getProperty(prop, "exec_cxt.periodic.priority", m_priority);
getProperty(prop, "exec_cxt.periodic.rtpreempt.priority", m_priority);
RTC_DEBUG(("Priority: %d", m_priority));
}
RTC::ReturnCode_t PulseAudioInput::onExecute(RTC::UniqueId ec_id)
{
RTC_DEBUG(("onExecute start"));
m_mutex.lock();
RTC_DEBUG(("onExecute:mutex lock"));
if( m_simple ) {
int r;
simple_recast *psimple = (simple_recast *)m_simple;
pa_threaded_mainloop_lock( psimple->mainloop );
RTC_DEBUG(("pa_threaded_mainloop_lock()"));
while ( !psimple->read_data ) {
r = pa_stream_peek( psimple->stream, &psimple->read_data, &psimple->read_length );
if ( !psimple->read_data ) {
RTC_DEBUG(("pa_stream_peek():no readable data. wait start."));
pa_threaded_mainloop_wait(psimple->mainloop);
}
}
m_out_data.data.length( psimple->read_length ); //!< set outport data length
memcpy((void *)&(m_out_data.data[0]), (const uint8_t*) psimple->read_data, psimple->read_length);
r = pa_stream_drop( psimple->stream );
if ( r < 0 ) {
RTC_WARN(("pa_stream_drop():capture stream drop failed."));
}
psimple->read_data = NULL;
psimple->read_length = 0;
psimple->read_index = 0;
setTimestamp( m_out_data );
m_out_dataOut.write();
RTC_DEBUG(("AudioDataOut port:ON_BUFFER_WRITE"));
pa_threaded_mainloop_unlock( psimple->mainloop );
RTC_DEBUG(("pa_threaded_mainloop_unlock()"));
}
m_mutex.unlock();
RTC_DEBUG(("onExecute:mutex unlock"));
RTC_DEBUG(("onExecute finish"));
return RTC::RTC_OK;
}
bool DcpsFuncSubImpl::isNew()
{
RTC_TRACE(("isNew()"));
if (!reader_)
{
RTC_INFO(("Port is not connected; cannot read"));
return false;
}
DDS::SampleInfoSeq info(1);
// Attempt to read
dds_datatype::DdsDataTypeSeq value(1);
DDS::ReturnCode_t ret = type_reader_->read(
value,
info,
1,
DDS::NOT_READ_SAMPLE_STATE,
DDS::ANY_VIEW_STATE,
DDS::ANY_INSTANCE_STATE);
if (ret == DDS::RETCODE_NO_DATA)
{
RTC_INFO(("No data available"));
return false;
}
else if (ret != DDS::RETCODE_OK)
{
RTC_ERROR(("Read error %d", ret));
return false;
}
if (!info[0].valid_data)
{
RTC_INFO(("Out-of-band data read"));
return false;
}
RTC_DEBUG(("Successfully read data"));
// type_reader_->return_loan(value, info);
return true;
}
