/*!
* @brief ログ出力のデッドロックテスト
*
* - RTC_LOG出力時、引数の関数内でログ出力がある場合にデッドロックしないか？
*/
void test_deadlock(void)
{
// std::cout << "test_deadlock() IN" << std::endl;
coil::LogStreamBuffer logger;
std::stringstream s0;
logger.addStream(s0.rdbuf());
LoggerMock rtclog(&logger);
rtclog.setName("Test");
rtclog.setDateFormat("");
rtclog.setLevel("TRACE");
s0.str("");
// ロックモード設定
rtclog.enableLock();
RTC_TRACE(("RTC_TRACE1 %s", rtclog.test_string().c_str()));
std::string rstr;
getline(s0, rstr);
CPPUNIT_ASSERT_EQUAL(std::string(" TRACE: Test: RTC_TRACE2 test_string()"), rstr);
getline(s0, rstr);
CPPUNIT_ASSERT_EQUAL(std::string(" TRACE: Test: RTC_TRACE1 TestString"), rstr);
// ロックモード解除
rtclog.disableLock();
// std::cout << "test_deadlock() OUT" << std::endl;
}
/*!
* @if jp
* @brief InterfaceProfile情報を公開する
* @else
* @brief Publish InterfaceProfile information
* @endif
*/
void InPortProvider::publishInterfaceProfile(SDOPackage::NVList& prop)
{
RTC_TRACE(("publishInterfaceProfile()"));
#ifdef ORB_IS_RTORB
NVUtil::appendStringValue(*prop.cobj(), "dataport.interface_type",
m_interfaceType.c_str());
#else // ORB_IS_RTORB
NVUtil::appendStringValue(prop, "dataport.interface_type",
m_interfaceType.c_str());
#endif // ORB_IS_RTORB
NVUtil::append(prop, m_properties);
}
/*!
* @if jp
* @brief Interface情報を公開する
* @else
* @brief Publish Interface information
* @endif
*/
bool InPortProvider::publishInterface(SDOPackage::NVList& prop)
{
RTC_TRACE(("publishInterface()"));
RTC_DEBUG_STR((NVUtil::toString(prop)));
if (!NVUtil::isStringValue(prop,
"dataport.interface_type",
m_interfaceType.c_str()))
{
return false;
}
NVUtil::append(prop, m_properties);
// NVUtil::dump(m_properties);
return true;
}
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;
}
void DcpsFuncSubImpl::unsubscribeInterfaces()
{
RTC_TRACE(("unsubscribeInterfaces()"));
if (participant_)
{
if (reader_)
{
subscriber_->delete_datareader(reader_);
reader_ = 0;
}
if (subscriber_)
{
participant_->delete_subscriber(subscriber_);
subscriber_ = 0;
}
if (topic_)
{
participant_->delete_topic(topic_);
topic_ = 0;
}
}
}
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;
}
void DcpsFuncPubImpl::unsubscribeInterfaces()
{
RTC_TRACE(("unsubscribeInterfaces()"));
if (participant_)
{
if (writer_)
{
publisher_->delete_datawriter(writer_);
writer_ = 0;
}
if (publisher_)
{
participant_->delete_publisher(publisher_);
publisher_ = 0;
}
if (topic_)
{
participant_->delete_topic(topic_);
topic_ = 0;
}
}
}
/*!
* @if jp
* @brief Connector 名取得
*
* Connector 名を取得する
*
* @else
* @brief Getting Connector name
*
* This operation returns Connector name
*
* @endif
*/
const char* OutPortConnector::name()
{
RTC_TRACE(("name() = %s", profile().name.c_str()));
return profile().name.c_str();
}
/*!
* @if jp
* @brief Connector ID 取得
*
* Connector ID を取得する
*
* @else
* @brief Getting Connector ID
*
* This operation returns Connector ID
*
* @endif
*/
const char* OutPortConnector::id()
{
RTC_TRACE(("id() = %s", profile().id.c_str()));
return profile().id.c_str();
}
/*!
* @if jp
* @brief ConnectorInfo 取得
*
* Connector ConnectorInfo を取得する
*
* @else
* @brief Getting ConnectorInfo
*
* This operation returns ConnectorInfo
*
* @endif
*/
const ConnectorInfo& OutPortConnector::profile()
{
RTC_TRACE(("profile()"));
return m_profile;
}
RTC::ReturnCode_t ImageViewer::onExecute(RTC::UniqueId ec_id) {
//Inport data check
if(m_ImageIn.isNew()) {
m_ImageIn.read();
width = m_Image.data.image.width;
height = m_Image.data.image.height;
channels = (m_Image.data.image.format == Img::CF_GRAY) ? 1 :
(m_Image.data.image.format == Img::CF_RGB || m_Image.data.image.format == Img::CF_PNG || m_Image.data.image.format == Img::CF_JPEG) ? 3 :
(m_Image.data.image.raw_data.length()/width/height);
RTC_TRACE(("Capture image size %d x %d", width, height));
RTC_TRACE(("Channels %d", channels));
if(channels == 3)
image.create(height, width, CV_8UC3);
else
image.create(height, width, CV_8UC1);
long data_length = m_Image.data.image.raw_data.length();
long image_size = width * height * channels;
std::cout<<"ColorFormat"<<m_Image.data.image.format<<std::endl;
if( m_Image.data.image.format == Img::CF_RGB ) {
for(int i=0; i<height; ++i)
memcpy(&image.data[i*image.step],&m_Image.data.image.raw_data[i*width*channels],sizeof(unsigned char)*width*channels);
if(channels == 3)
cv::cvtColor(image, image, CV_RGB2BGR);
}
else if( m_Image.data.image.format == Img::CF_JPEG || m_Image.data.image.format == Img::CF_PNG ) {
std::vector<uchar> compressed_image = std::vector<uchar>(data_length);
memcpy(&compressed_image[0], &m_Image.data.image.raw_data[0], sizeof(unsigned char) * data_length);
//Decode received compressed image
cv::Mat decoded_image;
if(channels == 3) {
decoded_image = cv::imdecode(cv::Mat(compressed_image), CV_LOAD_IMAGE_COLOR);
cv::cvtColor(decoded_image, image, CV_RGB2BGR);
}
else {
decoded_image = cv::imdecode(cv::Mat(compressed_image), CV_LOAD_IMAGE_GRAYSCALE);
image = decoded_image;
}
}
}
//画像データが入っている場合は画像を表示
if(!image.empty()) {
//Communication Time
coil::TimeValue tm(coil::gettimeofday());
std::cout<< "Communication Time: " << tm.usec() - (m_Image.tm.nsec / 1000) << "\r";
imageQueue.push( image );
//cv::imshow("Image Window", image);
}
//char key = cv::waitKey(3);
//Image save process
/**
if ( key == 's') {
char file[80];
sprintf( file, "CapturedImage%03d.png",++saved_image_counter);
cv::imwrite( file, image );
}
**/
return RTC::RTC_OK;
}
/*!
* @if jp
* @brief サブスクリプションタイプを設定する
* @else
* @brief Set the subscription type
* @endif
*/
void InPortProvider::setSubscriptionType(const char* subs_type)
{
RTC_TRACE(("setSubscriptionType(%s)", subs_type));
m_subscriptionType = subs_type;
}
/*!
* @if jp
* @brief データフロータイプを設定する
* @else
* @brief Set the dataflow type
* @endif
*/
void InPortProvider::setDataFlowType(const char* dataflow_type)
{
RTC_TRACE(("setDataFlowType(%s)", dataflow_type));
m_dataflowType = dataflow_type;
}
/*!
* @if jp
* @brief インターフェースタイプを設定する
* @else
* @brief Set the interface type
* @endif
*/
void InPortProvider::setInterfaceType(const char* interface_type)
{
RTC_TRACE(("setInterfaceType(%s)", interface_type));
m_interfaceType = interface_type;
}
/*!
* @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;
}
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;
}
/*!
* @if jp
* @brief endianタイプ設定
*
* endianタイプを設定する
*
* @else
* @brief Setting an endian type
*
* This operation set this connector's endian type
*
* @endif
*/
void OutPortConnector::setEndian(const bool endian_type)
{
RTC_TRACE(("setEndian() = %s", endian_type ? "little":"big"));
m_littleEndian = endian_type;
}
/*!
* @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;
}