RTC::ReturnCode_t ImageFaceDetection::onActivated(RTC::UniqueId ec_id) { if (!m_FaceCascade.load(m_data_dir + "/haarcascade_frontalface_alt.xml")) { RTC_ERROR(("Load FaceCascade Failed(%s/%s)", m_data_dir.c_str(), "haarcascade_frontalface_alt.xml")); return RTC::RTC_ERROR; } if (!m_EyeCascade.load(m_data_dir + "/haarcascade_eye_tree_eyeglasses.xml")) { RTC_ERROR(("Load Eye Cascade Failed.")); return RTC::RTC_ERROR; } return RTC::RTC_OK; }
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; }
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; }
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 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; }
//-------------------------------------------------- //**************** 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; }
RTC::ReturnCode_t WebCamera::onActivated(RTC::UniqueId ec_id) { //Open camera device if(!cam_cap.open(m_camera_id)) { RTC_ERROR(("Unable to open selected video device ID:[%d].", m_camera_id)); return RTC::RTC_ERROR; } //Get and show the camera device properties cam_cap >> src_image; width = src_image.cols; height = src_image.rows; depth = src_image.depth(); std::cout << "Image size:" << width << " x " << height << std::endl; std::cout << "Depth :" << depth << std::endl; std::cout << "Channles :" << src_image.channels() << std::endl; //Check channels of camera device nchannels = (m_output_color_format == "GRAY") ? 1 : 3; if(nchannels > src_image.channels()) { if(m_output_color_format == "RGB" || m_output_color_format == "JPEG" || m_output_color_format == "PNG") { std::cout << "Convert GRAY image to RGB color image" <<std::endl; } } else if( nchannels < src_image.channels() ) { std::cout<< "Convert RGB color image to GRAY image" <<std::endl; } else { if(m_output_color_format == "RGB" || m_output_color_format == "JPEG" || m_output_color_format == "PNG") { std::cout << "Convert BGR color image to RGB color image" << std::endl; } else if(m_output_color_format == "GRAY") { std::cout << "Gray image" <<std::endl; } } //Load camera parameter //If camera parameter file could not be found, whole camera parameters are set to zero. std::cout<<"Loading camera parameter file: "<< m_camera_param_filename << std::endl; cv::FileStorage fs(m_camera_param_filename, cv::FileStorage::READ); if(fs.isOpened()) { fs["image_width"] >> cam_param.imageSize.width; fs["image_height"] >> cam_param.imageSize.height; fs["camera_matrix"] >> cam_param.cameraMatrix; fs["distortion_coefficients"] >> cam_param.distCoeffs; std::cout << "=================================================" << std::endl; std::cout << "Camera Parameter" <<std::endl; std::cout << "=================================================" << std::endl; std::cout << "Image size: " << cam_param.imageSize.width << "x" << cam_param.imageSize.height << std::endl; std::cout << "Camera Matrix: "<< cam_param.cameraMatrix << std::endl; std::cout << "Distortion coefficients: " << cam_param.distCoeffs << std::endl; //Set distortion coefficient to make rectify map CameraParam *param; param = &cam_param; cv::initUndistortRectifyMap(param->cameraMatrix, param->distCoeffs, cv::Mat(), cv::getOptimalNewCameraMatrix(param->cameraMatrix, param->distCoeffs, param->imageSize, 1, param->imageSize, 0), param->imageSize, CV_16SC2, param->map1, param->map2); //Set default capture mode m_CameraCaptureService.m_cap_continuous = coil::toBool(m_cap_continuous_flag, "true", "false"); std::cout << "Capture mode: " << m_cap_continuous_flag << std::endl; std::cout << "Capture start!!" << std::endl; }
/*! * @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; }
/*! * @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 ファイルが作成され、4列目のログレベルが 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; }