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 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 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 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 WavRecord::onInitialize()
{
RTC_INFO(("WavRecord : Audio record 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
m_in_dataIn.addConnectorDataListener(ON_BUFFER_WRITE, new DataListener("ON_BUFFER_WRITE", this));
m_in_dataIn.setDescription(_("Audio data input."));
registerInPort("AudioDataIn", m_in_dataIn);
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
bindParameter("SampleRate", m_rate, "16000");
bindParameter("ChannelNumbers", m_channels, "1");
//bindParameter("FileName", m_filename, "");
#if defined(__linux)
bindParameter("FileName", m_filename, "wavrecord-default.wav");
#ifdef SHARED_LIB
Gtk::FileChooserDialog diag( "ファイル選択", Gtk::FILE_CHOOSER_ACTION_SAVE );
// 開く、キャンセルボタン
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());
OpenDiaog(hwnd,"Wave Files(*.wav)\0*.wav\0All Files(*.*)\0*.*\0\0",
WaveFileName,OFN_CREATEPROMPT | OFN_OVERWRITEPROMPT);
#endif//SHARED_LIB
#endif//defined(_WIN32)
RTC_INFO(("onInitialize finish"));
is_active = false;
return RTC::RTC_OK;
}
RTC::ReturnCode_t ImageViewer::onActivated(RTC::UniqueId ec_id)
{
PortServiceList* portlist;
portlist = this->get_ports();
connection_check = new bool [portlist->length()];
for(unsigned int i=0; i<portlist->length(); ++i)
{
PortService_ptr port;
port = (*portlist)[i];
if(port->get_port_profile()->connector_profiles.length()!=0)
connection_check[i]=true;
else
connection_check[i]=false;
}
//サービスポート接続状態のチェック
if(connection_check[1])
{
//連続画像取得モードに設定
if(m_capture_frame_num == 0)
{
std::cout << "start continuous image streaming." << std::endl;
RTC_INFO(("Send command of \"start continuous\" via CameraCaptureService."));
m_CameraCaptureService->start_continuous();
}
//1shotキャプチャモードに設定
else if(m_capture_frame_num == 1)
{
std::cout << "take one frame." << std::endl;
RTC_INFO(("Send command of \"take one frame\" via CameraCaptureService."));
m_CameraCaptureService->take_one_frame();
}
//指定枚数キャプチャモードに設定
else if(m_capture_frame_num > 1)
{
std::cout << "take multi frames (" << m_capture_frame_num << " frames)." << std::endl;
RTC_INFO(("Send command of \"take multi frames\" via CameraCaptureService."));
m_CameraCaptureService->take_multi_frames(m_capture_frame_num);
}
else
{
std::cerr << "Please set capture_frame_num to more than 0" << std::endl;
RTC_ERROR(("Configuration Param <frames_num> should be over 0. [%d]",m_capture_frame_num));
return RTC::RTC_ERROR;
}
}
std::cout << "Start image view" << std::endl;
std::cout << "If you want to take a 1 shot image as image file, please push s on Captured Image Window!" << std::endl;
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;
}
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;
}
bool DcpsFuncPubImpl::write(octecSeq& oct_seq)
{
if (!writer_)
{
RTC_INFO(("Port is not connected; cannot write"));
return false;
}
// Convert sequence<octet> to DdsDataType
dds_datatype::DdsDataType value;
unsigned long length = oct_seq.size();
value.data.length(length);
for (unsigned int i = 0; i < length; i++)
{
value.data[i] = oct_seq[i];
}
// Write
DDS::ReturnCode_t res = type_writer_->write(value,
DDS::HANDLE_NIL);
if (res != DDS::RETCODE_OK)
{
RTC_ERROR(("Writer error %d", res));
return false;
}
return true;
}
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;
}
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;
}
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 ImageViewer::onDeactivated(RTC::UniqueId ec_id)
{
if(connection_check[1]){
//連続キャプチャモードの場合は、キャプチャを停止
if(m_capture_frame_num == 0)
{
std::cout << "Stop image streaming" << std::endl;
RTC_INFO(("Send command of \"stop continuous\" via CameraCaptureService."));
m_CameraCaptureService->stop_continuous();
}
}
if(connection_check != NULL)
delete [] connection_check;
//描画ウィンドウの消去
std::cout << "Stop image view" << std::endl;
//描画用画像メモリの解放
image.release();
return RTC::RTC_OK;
}
/*!
* @brief コンストラクタログレベルのテスト
*
* - コンストラクタ（name)の場合、Managerの設定ログレベル(INFO)で動作するか？
*/
void test_constract_name(void)
{
// std::cout << "test_constract_name() IN" << std::endl;
RTC::Manager* m_mgr;
m_mgr = RTC::Manager::init(0, NULL);
CPPUNIT_ASSERT(m_mgr != NULL);
RTC::Logger rtclog("TestName");
std::string log_level = m_mgr->getLogLevel();
CPPUNIT_ASSERT_EQUAL(std::string("INFO"), log_level);
coil::Properties m_config = m_mgr->getConfig();
std::vector<std::string> logouts = coil::split(m_config["logger.file_name"], ",");
// 汎用ログ出力マクロ、各種ログ出力マクロでファイル出力
RTC_LOG( ::RTC::Logger::RTL_PARANOID,("RTL_PARANOID tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_PARANOID, "RTL_PARANOID tests str");
RTC_PARANOID( ("Macro RTL_PARANOID tests %s","fmt"));
RTC_PARANOID_STR("Macro RTL_PARANOID tests str");
RTC_LOG( ::RTC::Logger::RTL_VERBOSE,("RTL_VERBOSE tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_VERBOSE, "RTL_VERBOSE tests str");
RTC_VERBOSE( ("Macro RTL_VERBOSE tests %s","fmt"));
RTC_VERBOSE_STR("Macro RTL_VERBOSE tests str");
RTC_LOG( ::RTC::Logger::RTL_TRACE,("RTL_TRACE tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_TRACE, "RTL_TRACE tests str");
RTC_TRACE( ("Macro RTL_TRACE tests %s","fmt"));
RTC_TRACE_STR("Macro RTL_TRACE tests str");
RTC_LOG( ::RTC::Logger::RTL_DEBUG,("RTL_DEBUG tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_DEBUG, "RTL_DEBUG tests str");
RTC_DEBUG( ("Macro RTL_DEBUG tests %s","fmt"));
RTC_DEBUG_STR("Macro RTL_DEBUG tests str");
RTC_LOG( ::RTC::Logger::RTL_INFO,("RTL_INFO tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_INFO, "RTL_INFO tests str");
RTC_INFO( ("Macro RTL_INFO tests %s","fmt"));
RTC_INFO_STR("Macro RTL_INFO tests str");
RTC_LOG( ::RTC::Logger::RTL_WARN,("RTL_WARN tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_WARN, "RTL_WARN tests str");
RTC_WARN( ("Macro RTL_WARN tests %s","fmt"));
RTC_WARN_STR("Macro RTL_WARN tests str");
RTC_LOG( ::RTC::Logger::RTL_ERROR,("RTL_ERROR tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_ERROR, "RTL_ERROR tests str");
RTC_ERROR( ("Macro RTL_ERROR tests %s","fmt"));
RTC_ERROR_STR("Macro RTL_ERROR tests str");
RTC_LOG( ::RTC::Logger::RTL_FATAL,("RTL_FATAL tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_FATAL, "RTL_FATAL tests str");
RTC_FATAL( ("Macro RTL_FATAL tests %s","fmt"));
RTC_FATAL_STR("Macro RTL_FATAL tests str");
RTC_LOG( ::RTC::Logger::RTL_SILENT,("RTL_SILENT tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_SILENT, "RTL_SILENT tests str");
m_mgr->terminate();
// rtc*.log ファイルが作成され、４列目のログレベルが INFO以下か？
// INFO WARNING ERROR FATAL SILENT だけが記録されているか？
// Aug 03 14:03:09 INFO: manager: OpenRTM-aist-1.0.0
// [0] 1 2 3 4 5
std::string rstr;
std::vector<std::string> vstr;
bool bret;
std::ifstream ifs(logouts[0].c_str());
while(getline(ifs, rstr))
{
if(rstr.size() == 0) break;
vstr = coil::split(rstr, " ");
// ログレベル判定
bret = false;
if( (vstr[3] == "INFO:") || (vstr[3] == "WARNING:") || (vstr[3] == "ERROR:") ||
(vstr[3] == "FATAL:") || (vstr[3] == "SILENT:") )
bret = true;
CPPUNIT_ASSERT(bret);
// name判定
bret = false;
if( (vstr[4] == "manager:") || (vstr[4] == "TestName:") ||
(vstr[4] == "NamingOnCorba:") || (vstr[4] == "NamingManager:") ||
(vstr[4] == "ManagerServant:") )
bret = true;
CPPUNIT_ASSERT(bret);
}
// std::cout << "test_constract_name() OUT" << std::endl;
}
//--------------------------------------------------
//**************** Public Functions ****************
//--------------------------------------------------
void DcpsFuncPubImpl::init(coil::Properties const& props,
std::string const portName)
{
// Get the host and port of DCPSInfoRepo service
std::string address = props.getProperty("dcpsir_ip_address");
std::string port_num = props.getProperty("dcpsir_port_number");
if (address.empty())
{
RTC_ERROR(("Invalid dcpsir_ip_address value"));
return;
}
if (port_num.empty())
{
RTC_ERROR(("Invalid dcpsir_port_number value"));
return;
}
std::vector<std::string> args;
args.push_back("-DCPSInfoRepo");
args.push_back(address + ":" + port_num);
// args.push_back("-ORBDebugLevel");
// args.push_back("10");
// args.push_back("-DCPSDebugLevel");
// args.push_back("10");
// args.push_back("-ORBLogFile");
// args.push_back(portName + ".log");
char** argv = coil::toArgv(args);
int argc(args.size());
// Create DomainParticipantFactory
fact_ = TheParticipantFactoryWithArgs(argc, argv);
DDS::DomainParticipantFactoryQos qos;
DDS::ReturnCode_t res = fact_->get_qos(qos);
if (res != DDS::RETCODE_OK)
{
RTC_ERROR(("Failed to get factory QoS (%d); cannot "
"set QoS properties", res));
}
if (!participant_)
{
int domain(0);
if (!coil::stringTo(domain, props.getProperty("domain",
"0").c_str()))
{
RTC_ERROR(("Invalid domain value"));
return;
}
RTC_INFO(("DDSPubPort %s joining domain %d", portName.c_str(), domain));
participant_ = fact_->create_participant(
domain,
PARTICIPANT_QOS_DEFAULT,
0,
::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (!participant_)
{
RTC_ERROR(("Failed to create domain participant"));
return;
}
}
// Register the Type
dds_datatype::DdsDataTypeTypeSupport_var ddtTs
= new dds_datatype::DdsDataTypeTypeSupportImpl();
if (DDS::RETCODE_OK !=
ddtTs->register_type(participant_, type_name_.c_str()))
{
RTC_ERROR(("Failed to register data type"));
return;
}
RTC_PARANOID(("DDSPubPort %s initialised succesfully", portName.c_str()));
}
bool DcpsFuncPubImpl::publishInterfaces(std::string const topic_name,
coil::Properties const& props)
{
RTC_TRACE(("publishInterfaces()"));
// TODO: Fix this
// Currently can only write to one topic at a time
if (topic_)
{
RTC_ERROR(("DDSPubPort can only write to one topic at a "
"time"));
return false;
}
// Make sure the port was initialised correctly
if (!participant_)
{
RTC_ERROR(("Port is not initialised"));
return false;
}
// Get the QoS
std::string partition_name = props.getProperty("partition_name", "rtmdds_data");
std::string dur_kind_str = props.getProperty("qos_durability_kind", "VOLATILE");
std::string rel_kind_str = props.getProperty("qos_reliability_kind", "BEST_EFFORT");
std::string rel_time_str = props.getProperty("qos_reliability_max_blocking_time", "100");
DDS::DurabilityQosPolicyKind dur_kind(DDS::VOLATILE_DURABILITY_QOS);
DDS::ReliabilityQosPolicyKind rel_kind(DDS::BEST_EFFORT_RELIABILITY_QOS);
long rel_time(100);
if (partition_name.empty())
{
RTC_ERROR(("Invalid partition_name value"));
return false;
}
if (dur_kind_str == "VOLATILE")
{
dur_kind = DDS::VOLATILE_DURABILITY_QOS;
}
else if (dur_kind_str == "TRANSIENT_LOCAL")
{
dur_kind = DDS::TRANSIENT_LOCAL_DURABILITY_QOS;
}
else if (dur_kind_str == "TRANSIENT")
{
dur_kind = DDS::TRANSIENT_DURABILITY_QOS;
}
else if (dur_kind_str == "PERSISTENT")
{
dur_kind = DDS::PERSISTENT_DURABILITY_QOS;
}
else
{
RTC_ERROR(("Invalid qos_durability_kind value"));
return false;
}
if (rel_kind_str == "RELIABLE")
{
rel_kind = DDS::RELIABLE_RELIABILITY_QOS;
}
else if (rel_kind_str == "BEST_EFFORT")
{
rel_kind = DDS::BEST_EFFORT_RELIABILITY_QOS;
}
else
{
RTC_ERROR(("Invalid qos_reliability_kind value"));
return false;
}
if (!coil::stringTo(rel_time, rel_time_str.c_str()))
{
RTC_ERROR(("Invalid qos_reliability_max_blocking_time value"));
return false;
}
DDS::Duration_t rel_time_st;
rel_time_st.sec = rel_time / 1000;
rel_time_st.nanosec = (rel_time % 1000) * 1000000; // mili -> nano
// Join the topic
DDS::TopicQos topic_qos;
participant_->get_default_topic_qos(topic_qos);
topic_qos.durability.kind = dur_kind;
topic_qos.reliability.kind = rel_kind;
topic_qos.reliability.max_blocking_time = rel_time_st;
RTC_INFO(("Creating and joining topic %s", topic_name.c_str()));
topic_ = participant_->create_topic(
topic_name.c_str(),
type_name_.c_str(),
topic_qos,
0,
::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (!topic_)
{
RTC_ERROR(("Failed to create topic %s", topic_name.c_str()));
return false;
}
// Create a publisher
DDS::PublisherQos pub_qos;
participant_->get_default_publisher_qos(pub_qos);
pub_qos.partition.name.length(1);
pub_qos.partition.name[0] = partition_name.c_str();
publisher_ = participant_->create_publisher(
pub_qos,
0,
::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (!publisher_)
{
RTC_ERROR(("Failed to create publisher"));
return false;
}
// Create a writer
DDS::DataWriterQos dw_qos;
publisher_->get_default_datawriter_qos(dw_qos);
dw_qos.durability.kind = dur_kind;
dw_qos.reliability.kind = rel_kind;
dw_qos.reliability.max_blocking_time = rel_time_st;
writer_ = publisher_->create_datawriter(
topic_,
dw_qos,
0,
::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (!writer_)
{
RTC_ERROR(("Failed to create data writer"));
return false;
}
type_writer_ = dds_datatype::DdsDataTypeDataWriter::_narrow(writer_);
if (!type_writer_)
{
RTC_ERROR(("Failed to narrow writer"));
return false;
}
return true;
}
/*!
* @brief logfile出力のテスト
*
* - ログレベルを ERROR にした場合のファイル出力が正しく行われるか？
*/
void test_logfile_ERROR(void)
{
// std::cout << "test_logfile_ERROR() IN" << std::endl;
coil::LogStreamBuffer logger;
std::string logfile("rtcERROR.log");
std::filebuf of;
of.open(logfile.c_str(), std::ios::out);
if (!of.is_open())
{
std::cerr << "Error: cannot open logfile: "
<< logfile << std::endl;
}
logger.addStream(&of, true);
RTC::Logger rtclog(&logger);
rtclog.setName("Test");
rtclog.setDateFormat("%b %d %H:%M:%S");
rtclog.setLevel("ERROR");
// 汎用ログ出力マクロ、各種ログ出力マクロで正しくファイル出力されるか？
RTC_LOG( ::RTC::Logger::RTL_PARANOID,("RTL_PARANOID tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_PARANOID, "RTL_PARANOID tests str");
RTC_PARANOID( ("Macro RTL_PARANOID tests %s","fmt"));
RTC_PARANOID_STR("Macro RTL_PARANOID tests str");
RTC_LOG( ::RTC::Logger::RTL_VERBOSE,("RTL_VERBOSE tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_VERBOSE, "RTL_VERBOSE tests str");
RTC_VERBOSE( ("Macro RTL_VERBOSE tests %s","fmt"));
RTC_VERBOSE_STR("Macro RTL_VERBOSE tests str");
RTC_LOG( ::RTC::Logger::RTL_TRACE,("RTL_TRACE tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_TRACE, "RTL_TRACE tests str");
RTC_TRACE( ("Macro RTL_TRACE tests %s","fmt"));
RTC_TRACE_STR("Macro RTL_TRACE tests str");
RTC_LOG( ::RTC::Logger::RTL_DEBUG,("RTL_DEBUG tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_DEBUG, "RTL_DEBUG tests str");
RTC_DEBUG( ("Macro RTL_DEBUG tests %s","fmt"));
RTC_DEBUG_STR("Macro RTL_DEBUG tests str");
RTC_LOG( ::RTC::Logger::RTL_INFO,("RTL_INFO tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_INFO, "RTL_INFO tests str");
RTC_INFO( ("Macro RTL_INFO tests %s","fmt"));
RTC_INFO_STR("Macro RTL_INFO tests str");
RTC_LOG( ::RTC::Logger::RTL_WARN,("RTL_WARN tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_WARN, "RTL_WARN tests str");
RTC_WARN( ("Macro RTL_WARN tests %s","fmt"));
RTC_WARN_STR("Macro RTL_WARN tests str");
RTC_LOG( ::RTC::Logger::RTL_ERROR,("RTL_ERROR tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_ERROR, "RTL_ERROR tests str");
RTC_ERROR( ("Macro RTL_ERROR tests %s","fmt"));
RTC_ERROR_STR("Macro RTL_ERROR tests str");
RTC_LOG( ::RTC::Logger::RTL_FATAL,("RTL_FATAL tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_FATAL, "RTL_FATAL tests str");
RTC_FATAL( ("Macro RTL_FATAL tests %s","fmt"));
RTC_FATAL_STR("Macro RTL_FATAL tests str");
RTC_LOG( ::RTC::Logger::RTL_SILENT,("RTL_SILENT tests %s","fmt"));
RTC_LOG_STR(::RTC::Logger::RTL_SILENT, "RTL_SILENT tests str");
of.close();
// ファイル出力があるか？
std::string rstr;
std::ifstream ifs(logfile.c_str());
ifs >> rstr;
CPPUNIT_ASSERT(rstr.size() > 0);
ifs >> rstr;
ifs >> rstr;
ifs >> rstr;
CPPUNIT_ASSERT_EQUAL(std::string("ERROR:"), rstr);
ifs >> rstr;
CPPUNIT_ASSERT_EQUAL(std::string("Test:"), rstr);
ifs >> rstr;
CPPUNIT_ASSERT_EQUAL(std::string("RTL_ERROR"), rstr);
ifs >> rstr;
CPPUNIT_ASSERT_EQUAL(std::string("tests"), rstr);
ifs >> rstr;
CPPUNIT_ASSERT_EQUAL(std::string("fmt"), rstr);
// std::cout << "test_logfile_ERROR() OUT" << std::endl;
}
