void DdsDomainParticipantRepository::remove(std::string const& name) throw(ENotRegistered) { DDS::DomainParticipantFactory * dpf = DDS::DomainParticipantFactory::get_instance(); InstanceMap::iterator i = instances_.find(name); if (i != instances_.end()) { DDS::DomainParticipant * p = dynamic_cast<DDS::DomainParticipant *>(i->second); DDS::ReturnCode_t rc = p->delete_contained_entities(); if (rc != DDS::RETCODE_OK) { MIRO_LOG_OSTR(LL_ERROR, "Error deleting contained entities: " << DdsSupport::getError(rc)); } rc = dpf->delete_participant(p); if (rc != DDS_RETCODE_OK) { MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS domain particpant: " << DdsSupport::getError(rc)); } instances_.erase(i); if (instances_.size() == 0) SingletonType::ACE_Singleton_Type::close(); return; } throw ENotRegistered(name); }
DdsEntitiesFactory::~DdsEntitiesFactory() { MIRO_LOG_DTOR("kn::DdsEntitiesFactory"); // delete all topics we created { vector<DDS::Topic *>::const_iterator first, last = m_data->topics.end(); for (first = m_data->topics.begin(); first != last; ++first) { DDS::DomainParticipant * participant = (*first)->get_participant(); DDS::ReturnCode_t rc = participant->delete_topic(*first); if (rc != DDS::RETCODE_OK) { MIRO_LOG_OSTR(LL_ERROR , "Failed to delete topic: " << DdsSupport::getError(rc)); } } m_data->topics.clear(); } MIRO_LOG(LL_NOTICE, "Cleaning up DDS subscribers..."); DdsSubscriberRepository::SingletonType::ACE_Singleton_Type::close(); MIRO_LOG(LL_NOTICE, "Cleaning up DDS publishers..."); DdsPublisherRepository::SingletonType::ACE_Singleton_Type::close(); MIRO_LOG(LL_NOTICE, "Cleaning up DDS flow-controllers..."); { ParticipantVector::const_iterator first, last = m_params.participants.end(); for (first = m_params.participants.begin(); first != last; ++first) { DDS::DomainParticipant * dp = DdsDomainParticipantRepository::instance()->get(first->name); vector<DdsFlowControllerParameters>::const_iterator f, l = first->flowControllers.end(); for (f = first->flowControllers.begin(); f != l; ++f) { DDS::FlowController * fc = dp->lookup_flowcontroller(f->name.c_str()); dp->delete_flowcontroller(fc); } } } #if defined(KNDDS_HAS_RTI_DistLogger) if(m_params.enableDistLogger) { MIRO_LOG(LL_NOTICE, "Stop RTI Distributed Logger..."); RTI_DLDistLogger* dl = RTI_DLDistLogger::getInstance(); if(dl) { dl->finalizeInstance(); } } #endif MIRO_LOG(LL_NOTICE, "Cleaning up DDS participants..."); DdsDomainParticipantRepository::SingletonType::ACE_Singleton_Type::close(); DDS::DomainParticipantFactory * dpf = DDS::DomainParticipantFactory::get_instance(); dpf->finalize_instance(); delete m_data; }
DdsPublisherRepository::~DdsPublisherRepository() throw() { // just to make sure, the repository should actually be empty at this point. InstanceMap::iterator first, last = instances_.end(); for (first = instances_.begin(); first != last; ++first) { // delete first->second; DDS::Publisher * pub = dynamic_cast<DDS::Publisher *>(first->second); DDS::DomainParticipant * participant = pub->get_participant(); DDS::ReturnCode_t rc = participant->delete_publisher(pub); if (rc != DDS_RETCODE_OK) { MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS publisher: " << DdsSupport::getError(rc)); } } }
// specializations DdsDomainParticipantRepository::~DdsDomainParticipantRepository() throw() { MIRO_LOG_DTOR("DdsDomainParticipantRepository"); DDS::DomainParticipantFactory * dpf = DDS::DomainParticipantFactory::get_instance(); // just to make sure, the repository should actually be empty at this point. InstanceMap::iterator first, last = instances_.end(); for (first = instances_.begin(); first != last; ++first) { DDS::DomainParticipant * p = dynamic_cast<DDS::DomainParticipant *>(first->second); DDS::ReturnCode_t rc = p->delete_contained_entities(); if (rc != DDS::RETCODE_OK) { MIRO_LOG_OSTR(LL_ERROR, "Error deleting contained entities: " << DdsSupport::getError(rc)); } rc = dpf->delete_participant(dynamic_cast<DDS::DomainParticipant *>(p)); if (rc != DDS_RETCODE_OK) { MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS domain particpant: " << DdsSupport::getError(rc)); } } }
void DdsSubscriberRepository::remove(std::string const& name) throw(ENotRegistered) { InstanceMap::iterator i = instances_.find(name); if (i != instances_.end()) { //delete i->second; DDS::Subscriber * sub = dynamic_cast<DDS::Subscriber *>(i->second); DDS::DomainParticipant * participant = sub->get_participant(); DDS::ReturnCode_t rc = participant->delete_subscriber(sub); if (rc != DDS_RETCODE_OK) { MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS subscriber: " << DdsSupport::getError(rc)); } instances_.erase(i); if (instances_.size() == 0) SingletonType::ACE_Singleton_Type::close(); return; } throw ENotRegistered(name); }
rmw_node_t * rmw_create_node(const char * name, size_t domain_id) { if (!name) { RMW_SET_ERROR_MSG("name is null"); return nullptr; } DDS::DomainParticipantFactory_var dp_factory = DDS::DomainParticipantFactory::get_instance(); if (!dp_factory) { RMW_SET_ERROR_MSG("failed to get domain participant factory"); return nullptr; } DDS::DomainId_t domain = static_cast<DDS::DomainId_t>(domain_id); DDS::DomainParticipant * participant = nullptr; // Make sure that the OSPL_URI is set, otherwise node creation will fail. char * ospl_uri = nullptr; const char * ospl_uri_env = "OSPL_URI"; #ifndef _WIN32 ospl_uri = getenv(ospl_uri_env); #else size_t ospl_uri_size; _dupenv_s(&ospl_uri, &ospl_uri_size, ospl_uri_env); #endif if (!ospl_uri) { RMW_SET_ERROR_MSG("OSPL_URI not set"); return nullptr; } else { #ifdef _WIN32 free(ospl_uri); #endif } // Ensure the ROS_DOMAIN_ID env variable is set, otherwise parsing of the config may fail. // Also make sure the it is set to the domain_id passed in, otherwise it will fail. // But first backup the current ROS_DOMAIN_ID. char * ros_domain_id = nullptr; const char * env_var = "ROS_DOMAIN_ID"; #ifndef _WIN32 ros_domain_id = getenv(env_var); #else size_t ros_domain_id_size; _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); #endif // On Windows, setting the ROS_DOMAIN_ID does not fix the problem, so error early. #ifdef _WIN32 if (!ros_domain_id) { RMW_SET_ERROR_MSG("environment variable ROS_DOMAIN_ID is not set"); fprintf(stderr, "[rmw_opensplice_cpp]: error: %s\n", rmw_get_error_string_safe()); return nullptr; } #endif // Set the ROS_DOMAIN_ID explicitly (if not Windows). #ifndef _WIN32 auto domain_id_as_string = std::to_string(domain_id); int ret = 0; ret = setenv(env_var, domain_id_as_string.c_str(), 1); if (0 != ret) { RMW_SET_ERROR_MSG("failed to set the ROS_DOMAIN_ID"); return nullptr; } #endif participant = dp_factory->create_participant( domain, PARTICIPANT_QOS_DEFAULT, NULL, DDS::STATUS_MASK_NONE); if (!participant) { RMW_SET_ERROR_MSG("failed to create domain participant"); return NULL; } // Restore the ROS_DOMAIN_ID if necessary (and not Windows). #ifndef _WIN32 if (ros_domain_id) { ret = setenv(env_var, ros_domain_id, 1); if (0 != ret) { RMW_SET_ERROR_MSG("failed to reset the ROS_DOMAIN_ID"); return nullptr; } } else { // Otherwise unset it again. ret = unsetenv(env_var); if (0 != ret) { RMW_SET_ERROR_MSG("failed to unset the ROS_DOMAIN_ID"); return nullptr; } } #endif rmw_node_t * node = nullptr; OpenSpliceStaticNodeInfo * node_info = nullptr; CustomPublisherListener * publisher_listener = nullptr; CustomSubscriberListener * subscriber_listener = nullptr; void * buf = nullptr; DDS::DataReader * data_reader = nullptr; DDS::PublicationBuiltinTopicDataDataReader * builtin_publication_datareader = nullptr; DDS::SubscriptionBuiltinTopicDataDataReader * builtin_subscription_datareader = nullptr; DDS::Subscriber * builtin_subscriber = participant->get_builtin_subscriber(); if (!builtin_subscriber) { RMW_SET_ERROR_MSG("builtin subscriber handle is null"); goto fail; } // setup publisher listener data_reader = builtin_subscriber->lookup_datareader("DCPSPublication"); builtin_publication_datareader = DDS::PublicationBuiltinTopicDataDataReader::_narrow(data_reader); if (!builtin_publication_datareader) { RMW_SET_ERROR_MSG("builtin publication datareader handle is null"); goto fail; } buf = rmw_allocate(sizeof(CustomPublisherListener)); if (!buf) { RMW_SET_ERROR_MSG("failed to allocate memory"); goto fail; } RMW_TRY_PLACEMENT_NEW(publisher_listener, buf, goto fail, CustomPublisherListener) buf = nullptr; builtin_publication_datareader->set_listener(publisher_listener, DDS::DATA_AVAILABLE_STATUS); data_reader = builtin_subscriber->lookup_datareader("DCPSSubscription"); builtin_subscription_datareader = DDS::SubscriptionBuiltinTopicDataDataReader::_narrow(data_reader); if (!builtin_subscription_datareader) { RMW_SET_ERROR_MSG("builtin subscription datareader handle is null"); goto fail; } // setup subscriber listener buf = rmw_allocate(sizeof(CustomSubscriberListener)); if (!buf) { RMW_SET_ERROR_MSG("failed to allocate memory"); goto fail; } RMW_TRY_PLACEMENT_NEW(subscriber_listener, buf, goto fail, CustomSubscriberListener) buf = nullptr; builtin_subscription_datareader->set_listener(subscriber_listener, DDS::DATA_AVAILABLE_STATUS); node = rmw_node_allocate(); if (!node) { RMW_SET_ERROR_MSG("failed to allocate rmw_node_t"); goto fail; } node->name = reinterpret_cast<const char *>(rmw_allocate(sizeof(char) * strlen(name) + 1)); if (!node->name) { RMW_SET_ERROR_MSG("failed to allocate memory for node name"); goto fail; } memcpy(const_cast<char *>(node->name), name, strlen(name) + 1); buf = rmw_allocate(sizeof(OpenSpliceStaticNodeInfo)); if (!buf) { RMW_SET_ERROR_MSG("failed to allocate memory"); goto fail; } RMW_TRY_PLACEMENT_NEW(node_info, buf, goto fail, OpenSpliceStaticNodeInfo) buf = nullptr; node_info->participant = participant; node_info->publisher_listener = publisher_listener; node_info->subscriber_listener = subscriber_listener; node->implementation_identifier = opensplice_cpp_identifier; node->data = node_info; return node; fail: if (participant) { if (dp_factory->delete_participant(participant) != DDS::RETCODE_OK) { std::stringstream ss; ss << "leaking domain participant while handling failure at: " << __FILE__ << ":" << __LINE__ << '\n'; (std::cerr << ss.str()).flush(); } } if (publisher_listener) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( publisher_listener->~CustomPublisherListener(), CustomPublisherListener) rmw_free(publisher_listener); } if (subscriber_listener) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( subscriber_listener->~CustomSubscriberListener(), CustomSubscriberListener) rmw_free(subscriber_listener); } if (node_info) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( node_info->~OpenSpliceStaticNodeInfo(), OpenSpliceStaticNodeInfo) rmw_free(node_info); } if (buf) { rmw_free(buf); } if (node) { if (node->name) { rmw_free(const_cast<char *>(node->name)); } rmw_node_free(node); } return nullptr; }
DDS::DomainParticipant* BulkDataNTStream::createDDSParticipant() { AUTO_TRACE(__PRETTY_FUNCTION__); DDS::ReturnCode_t ret; DDS::DomainParticipantQos participant_qos; int domainID=0; //TBD: where to get domain ID DDS::DomainParticipant* domainParticipant; if (factory_m==NULL) { NullPointerExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__); ex.setVariable("factory_m"); throw ex; } // If the profileQoS set on the configuration corresponds to a default stream profile, // then we use the default library as the library to search for it. // Otherwise, it means that the user specified it, so it must belong // to the specified library too const char *library = 0; if( configuration_m.libraryQos.compare((const char*)DDSConfiguration::DEFAULT_LIBRARY) == 0 && (configuration_m.profileQos.compare((const char*)DDSConfiguration::DEFAULT_RECEIVER_STREAM_PROFILE) == 0 || configuration_m.profileQos.compare((const char*)DDSConfiguration::DEFAULT_SENDER_STREAM_PROFILE) == 0) ) library = DDSConfiguration::DEFAULT_LIBRARY; else library = configuration_m.libraryQos.c_str(); ret = factory_m->get_participant_qos_from_profile(participant_qos, library, configuration_m.profileQos.c_str()); if (ret!=DDS::RETCODE_OK) { DDSQoSSetProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__); ex.setDDSTypeCode(ret); ex.setQoS("get_participant_qos_from_profile"); throw ex; }//if // we make sure that what participant creates is created disabled participant_qos.entity_factory.autoenable_created_entities=DDS_BOOLEAN_FALSE; domainParticipant =factory_m->create_participant(domainID, participant_qos, NULL, DDS::STATUS_MASK_NONE ); if (domainParticipant==NULL) { DDSParticipantCreateProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__); ex.setDomainID(domainID); throw ex; } ret = domainParticipant->enable(); if (ret!=DDS::RETCODE_OK) { DDSParticipantEnableProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__); ex.setDDSTypeCode(ret); ex.setDomainID(domainID); throw ex; }//if const char* type_name = ACSBulkData::BulkDataNTFrameTypeSupport::get_type_name(); ret = ACSBulkData::BulkDataNTFrameTypeSupport::register_type(domainParticipant, type_name); if (ret != DDS::RETCODE_OK) { DDSRegisterTypeProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__); ex.setDDSTypeCode(ret); ex.setTypeName(type_name); throw ex; } return domainParticipant; }//createDDSParticipant
DdsEntitiesFactory::DdsEntitiesFactory(DdsEntitiesFactoryParameters const& params) : m_params(params), m_data(new Data()) { MIRO_LOG_CTOR("kn::DdsEntitiesFactory"); // for error reporting stringstream ostr; DDS::DomainParticipantFactory * dpf = DDS::DomainParticipantFactory::get_instance(); //--------------------------------------------------- // disable default locations for Qos policy lookup DDS::DomainParticipantFactoryQos dpfQos; DDS_DomainParticipantFactoryQos_initialize(&dpfQos); DDS::ReturnCode_t rc; if ((rc = dpf->get_qos(dpfQos)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to query Qos (" << ((int)rc) << "): " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } // start participants disabled, so we can set discovery modules dpfQos.entity_factory.autoenable_created_entities = DDS_BOOLEAN_FALSE; // if configuration file is given, disable all other config options if (!m_params.configFiles.empty()) { Miro::SearchPaths paths(KNDDS_INSTALL_PREFIX "/etc"); paths.addMiroEtcPaths(); dpfQos.profile.url_profile.maximum(m_params.configFiles.size()); dpfQos.profile.url_profile.length(m_params.configFiles.size()); dpfQos.profile.ignore_user_profile = true; dpfQos.profile.ignore_environment_profile = true; dpfQos.profile.ignore_resource_profile = true; for (unsigned int i = 0; i < m_params.configFiles.size(); ++i) { string absFilePath = paths.findFile(m_params.configFiles[i]); if (absFilePath.empty()) { ostr << "DdsSupport::init() - DDS config file given not found (" << m_params.configFiles[i] << ")"; throw Miro::Exception(ostr.str()); } dpfQos.profile.url_profile[i] = DDS_String_dup(("file://" + absFilePath).c_str()); } if ((rc = dpf->set_qos(dpfQos)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set Qos: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } } if (!m_params.defaultLibrary.empty() && (rc = dpf->set_default_library(m_params.defaultLibrary.c_str())) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set default library: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } if (!m_params.defaultProfile.empty() && (rc = dpf->set_default_profile(m_params.defaultLibrary.c_str(), m_params.defaultProfile.c_str())) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set default library: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } //------------------------------------------- // create all participants // (default is 1) DdsDomainParticipantRepository * partRepo = DdsDomainParticipantRepository::instance(); DdsTypeRegistratorRepository * typeRepo = DdsTypeRegistratorRepository::instance(); { ParticipantVector::const_iterator first, last = m_params.participants.end(); for (first = m_params.participants.begin(); first != last; ++first) { // create one participant char const * library = first->library.empty() ? NULL : first->library.c_str(); char const * profile = first->profile.empty() ? NULL : first->profile.c_str(); DDS::DomainParticipantQos qos; DDS_DomainParticipantQos_initialize(&qos); if (profile == NULL && (m_params.defaultLibrary.empty() || m_params.defaultProfile.empty())) { DDS::ReturnCode_t rc = dpf->get_default_participant_qos(qos); if (rc != DDS_RETCODE_OK) { ostr << "Get default participant Qos profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } } else { DDS::ReturnCode_t rc = dpf->get_participant_qos_from_profile(qos, library, profile); if (rc != DDS_RETCODE_OK) { ostr << "Load participant Qos profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } } // set participant name from Miro parameters if (!first->participantName.empty()) { qos.participant_name.name = DDS_String_dup(first->participantName.c_str()); } // handle discovery-peers processing // just print the stuff as a first step... if (!first->discoveryPeersFiles.empty()) { DDS_DiscoveryQosPolicy& discovery = qos.discovery; StringVector peers; StringVector::const_iterator f, l = first->discoveryPeersFiles.end(); for (f = first->discoveryPeersFiles.begin(); f != l; ++f) { StringVector const p = parseDiscoveryPeersFile(*f); peers.insert(peers.end(), p.begin(), p.end()); } // set peers list in qos if (discovery.initial_peers.maximum() < (int)peers.size()) { discovery.initial_peers.maximum(peers.size()); } discovery.initial_peers.length(peers.size()); for (int i = 0; i < discovery.initial_peers.length(); ++i) { assert (peers[i].length() < 100); DDS_String_free(discovery.initial_peers[i]); discovery.initial_peers[i] = DDS_String_dup(peers[i].c_str()); } MIRO_LOG_OSTR(LL_NOTICE, "DDS Initial peers layout for " << qos.participant_name.name); for (int i = 0; i < qos.discovery.initial_peers.length(); ++i) { MIRO_LOG_OSTR(LL_NOTICE, qos.discovery.initial_peers[i]); } } #ifdef KNDDS_HAS_DDS_Monitor if (first->enableMonitor) { int rc; rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "rti.monitor.library", "rtimonintoring", DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); char valueBuffer[17]; sprintf(valueBuffer, "%p", RTIDefaultMonitor_create); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "rti.monitor.create_function_ptr", valueBuffer, DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); int monitorId = (first->monitorDomainId >= 0) ? first->monitorDomainId : first->domainId + 1; sprintf(valueBuffer, "%i", monitorId); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "rti.monitor.config.new_participant_domain_id", valueBuffer, DDS_BOOLEAN_FALSE); if (!first->monitorLibrary.empty()) { rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "rti.monitor.config.qos_library", first->monitorLibrary.c_str(), DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "rti.monitor.config.qos_profile", first->monitorProfile.c_str(), DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); } } #endif #if defined(KNDDS_HAS_RTI_DistLogger) if(m_params.enableDistLogger) { // Initialize the distributed logger. If distLoggerDomainId is less than // 0, use the domain of the first participant int loggerId = (m_params.distLoggerDomainId >= 0) ? m_params.distLoggerDomainId : first->domainId; RTI_DLDistLogger* dl = NULL; RTI_DLOptions dlOptions; // RTI_DLOptions API changed in RTI DDS 5.1.0 #if (RTI_DDS_VERSION_MAJOR >= 5 && RTI_DDS_VERSION_MINOR > 0) rc = dlOptions.setDomainId(loggerId); assert (rc == DDS_RETCODE_OK); rc = dlOptions.setApplicationKind(qos.participant_name.name); assert (rc == DDS_RETCODE_OK); #else dlOptions.setDomainId(loggerId); dlOptions.setApplicationKind(qos.participant_name.name); #endif RTI_DLDistLogger::setOptions(dlOptions); dl = RTI_DLDistLogger::getInstance(); if(dl) { dl->info("DdsEntitiesFactory initialized RTI Distributed Logger"); } else { KN_ERROR("RTI Distributed Logger - getInstance() returned NULL"); } } #endif #if defined(KNDDS_HAS_DDS_LBPlugin) && defined(RTIDDS_LB_BETA_VERSION) // disable simple-endpoind discovery if static discovery modules are specified // we need to double-check if they can coexist peacefully... if (!first->lbpdFile.empty()) { qos.discovery_config.builtin_discovery_plugins &= ~DDS_DISCOVERYCONFIG_BUILTIN_SPDP; } if (!first->lbedFile.empty()) { qos.discovery_config.builtin_discovery_plugins &= ~DDS_DISCOVERYCONFIG_BUILTIN_SEDP; } #endif DDS::DomainParticipant * participant = dpf->create_participant(first->domainId, qos, NULL /* listener */, DDS::STATUS_MASK_NONE); if (participant == NULL) { throw Miro::Exception("DdsSupport::init() - Failed to create participant."); } // set default library/profile if (library != NULL && (rc = participant->set_default_library(library)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set participant default library: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } if (profile != NULL && (rc = participant->set_default_profile(library, profile)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set participant default profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } #if defined(KNDDS_HAS_DDS_LBPlugin) // add static discovery modules if specified if (!first->lbpdFile.empty()) { MIRO_LOG(LL_NOTICE, "DDS Static Participant Discovery"); # if defined(RTIDDS_LB_BETA_VERSION) DDSLBPDiscoveryPlugin *pPlugin = new DDSLBPDiscoveryPlugin(); pPlugin->parseXmlFile(first->lbpdFile.c_str()); pPlugin->registerPlugin(participant); # else rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.participant.lbpdiscovery.library", "rtilbpdisc", DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.participant.lbpdiscovery.create_function", "DDS_LBPDiscoveryPlugin_create", DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.participant.lbpdiscovery.config_file", first->lbpdFile.c_str(), DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.participant.load_plugin", "dds.discovery.endpoint.lbpdiscovery", DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); char valueBuffer[18]; sprintf(valueBuffer, "%i", first->lbedLogVerbosity); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.participant.lbpdiscovery.verbosity", valueBuffer, DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); # endif } if (!first->lbedFile.empty()) { MIRO_LOG(LL_NOTICE, "DDS Static Endpoint Discovery"); # if defined(RTIDDS_LB_BETA_VERSION) DDSLBEDiscoveryPlugin *ePlugin = new DDSLBEDiscoveryPlugin(); ePlugin->parseXmlFile(first->lbedFile.c_str()); ePlugin->registerPlugin(participant); # else rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.endpoint.lbediscovery.library", "rtilbedisc", DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.endpoint.lbediscovery.create_function", "DDS_LBEDiscoveryPlugin_create", DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.endpoint.lbediscovery.config_file", first->lbedFile.c_str(), DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.endpoint.load_plugin", "dds.discovery.endpoint.lbediscovery", DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); char valueBuffer[18]; sprintf(valueBuffer, "%i", first->lbedLogVerbosity); rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property, "dds.discovery.endpoint.lbediscovery.verbosity", valueBuffer, DDS_BOOLEAN_FALSE); assert (rc == DDS_RETCODE_OK); # endif } #endif // enable participant rc = participant->enable(); if (rc != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to enable participant: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } try { partRepo->add(first->name, participant); } catch (Miro::RepositoryBase::EAlreadyRegistered const&) { throw Miro::Exception("DdsSupport::init() - Duplicate name for participant at repo."); } //------------------------------------------------- // register all relevant types with the participant // by default assume, that all types are for all domains if (first->types.empty()) { DdsTypeRegistratorRepository::InstanceMap const& types = typeRepo->getMap(); DdsTypeRegistratorRepository::InstanceMap::const_iterator f, l = types.end(); for (f = types.begin(); f != l; ++f) { try { f->second->registerType(participant); } catch (Miro::Exception const& e) { ostr << "Error registering type" << f->first << " at participant " << first->name << ": " << e.what(); throw Miro::Exception(ostr.str()); } } } else { StringVector::const_iterator f, l = first->types.end(); for (f = first->types.begin(); f != l; ++f) { try { DdsTypeRegistratorBase * tReg = typeRepo->get(*f); tReg->registerType(participant); } catch (Miro::Exception const& e) { ostr << "Error registering type" << *f << " at participant " << first->name << ": " << e.what(); throw Miro::Exception(ostr.str()); } } } //------------------------------------------------- // create all flow controllers configured for the participant vector<DdsFlowControllerParameters>::const_iterator f, l = first->flowControllers.end(); for (f = first->flowControllers.begin(); f != l; ++f) { DDS::FlowControllerProperty_t property; property.scheduling_policy = (f->schedulingPolicy == "DDS_RR_FLOW_CONTROLLER_SCHED_POLICY")? DDS_RR_FLOW_CONTROLLER_SCHED_POLICY : DDS_EDF_FLOW_CONTROLLER_SCHED_POLICY; property.token_bucket.max_tokens = (f->tokenBucket.maxTokens < 0)? DDS_LENGTH_UNLIMITED : f->tokenBucket.maxTokens; property.token_bucket.tokens_added_per_period = (f->tokenBucket.tokensAddedPerPeriod < 0)? DDS_LENGTH_UNLIMITED : f->tokenBucket.tokensAddedPerPeriod; property.token_bucket.tokens_leaked_per_period = (f->tokenBucket.tokensLeakedPerPeriod < 0)? DDS_LENGTH_UNLIMITED : f->tokenBucket.tokensLeakedPerPeriod; if (f->tokenBucket.period > ACE_Time_Value::zero) { property.token_bucket.period.sec = f->tokenBucket.period.sec(); property.token_bucket.period.nanosec = f->tokenBucket.period.usec() * 1000; } else { property.token_bucket.period = DDS_DURATION_INFINITE; } property.token_bucket.bytes_per_token = (f->tokenBucket.bytesPerToken < 0)? DDS_LENGTH_UNLIMITED : f->tokenBucket.bytesPerToken; participant->create_flowcontroller(DDS::String_dup(f->name.c_str()), property); } DDS_DomainParticipantQos_finalize(&qos); } } //------------------------------------------------- // create all publishers in configuration file // (default is 1) DdsPublisherRepository * pubRepo = DdsPublisherRepository::instance(); { NodeVector::const_iterator first, last = m_params.publishers.end(); for (first = m_params.publishers.begin(); first != last; ++first) { // per publisher DDS::DomainParticipant * participant = NULL; try { participant = partRepo->get(first->participant); } catch (Miro::Exception const&) { ostr << "Publisher specified unknown participant: " << first->participant; throw Miro::Exception(ostr.str()); } char const * profile = first->profile.empty() ? NULL : first->profile.c_str(); char const * library = first->library.empty() ? NULL : first->library.c_str(); DDS::PublisherQos qos; DDS_PublisherQos_initialize(&qos); if (profile == NULL && (m_params.defaultLibrary.empty() || m_params.defaultProfile.empty())) { DDS::ReturnCode_t rc = participant->get_default_publisher_qos(qos); if (rc != DDS_RETCODE_OK) { ostr << "Get default publisher Qos profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } } else { DDS::ReturnCode_t rc = dpf->get_publisher_qos_from_profile(qos, library, profile); if (rc != DDS_RETCODE_OK) { ostr << "Load publisher Qos profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } } char const * partition = (first->partition.empty() || first->partition == "<NONE>")? NULL : first->partition.c_str(); if (partition != NULL) { qos.partition.name.maximum(1); qos.partition.name.length(1); qos.partition.name[0] = DDS_String_dup(partition); } DDS::Publisher * publisher = participant->create_publisher(qos, NULL /* listener */, DDS::STATUS_MASK_NONE); if (publisher == NULL) { throw Miro::Exception("Failed to create publisher."); } // set default library/profile if (library != NULL && (rc = publisher->set_default_library(library)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set publisher default library: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } if (profile != NULL && (rc = publisher->set_default_profile(library, profile)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set publisher default profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } try { pubRepo->add(first->name, publisher); } catch (Miro::RepositoryBase::EAlreadyRegistered const&) { throw Miro::Exception("DdsSupport::init() - Duplicate name for publisher at repo."); } DDS_PublisherQos_finalize(&qos); } } //------------------------------------------------- // create all subscribers in configuration file // (default is 2) DdsSubscriberRepository * subRepo = DdsSubscriberRepository::instance(); { NodeVector::const_iterator first, last = m_params.subscribers.end(); for (first = m_params.subscribers.begin(); first != last; ++first) { // per subscriber DDS::DomainParticipant * participant = NULL; try { participant = partRepo->get(first->participant); } catch (Miro::Exception const&) { ostr << "Subscriber specified unknown participant: " << first->participant; throw Miro::Exception(ostr.str()); } char const * profile = first->profile.empty() ? NULL : first->profile.c_str(); char const * library = first->library.empty() ? NULL : first->library.c_str(); DDS::SubscriberQos qos; DDS_SubscriberQos_initialize(&qos); if (profile == NULL && (m_params.defaultLibrary.empty() || m_params.defaultProfile.empty())) { DDS::ReturnCode_t rc = participant->get_default_subscriber_qos(qos); if (rc != DDS_RETCODE_OK) { ostr << "Get default subscriber Qos profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } } else { DDS::ReturnCode_t rc = dpf->get_subscriber_qos_from_profile(qos, library, profile); if (rc != DDS_RETCODE_OK) { ostr << "Load subscriber Qos profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } } char const * partition = (first->partition.empty() || first->partition == "<NONE>")? NULL : first->partition.c_str(); if (partition != NULL) { if (strcmp("<TEAM>", partition) == 0) { Miro::RobotParameters * rp = Miro::RobotParameters::instance(); if (!rp->teamMembers.empty()) { qos.partition.name.maximum(rp->teamMembers.size()); qos.partition.name.length(rp->teamMembers.size()); for (unsigned int i = 0; i < rp->teamMembers.size(); ++i) { qos.partition.name[i] = DDS_String_dup(rp->teamMembers[i].c_str()); } } else { MIRO_LOG(LL_WARNING, "Team partition requested, but RobotParameters::teamMembers is empty, assuming ALL."); qos.partition.name.maximum(1); qos.partition.name.length(1); qos.partition.name[0] = DDS_String_dup("*"); } } else { qos.partition.name.maximum(1); qos.partition.name.length(1); qos.partition.name[0] = DDS_String_dup(partition); } } DDS::Subscriber * subscriber = participant->create_subscriber(qos, NULL /* listener */, DDS::STATUS_MASK_NONE); if (subscriber == NULL) { throw Miro::Exception("Failed to create subscriber."); } // set default library/profile if (library != NULL && (rc = subscriber->set_default_library(library)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set subscriber default library: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } if (profile != NULL && (rc = subscriber->set_default_profile(library, profile)) != DDS_RETCODE_OK) { ostr << "DdsSupport::init() - Failed to set subscriber default profile: " << DdsSupport::getError(rc); throw Miro::Exception(ostr.str()); } try { subRepo->add(first->name, subscriber); } catch (Miro::RepositoryBase::EAlreadyRegistered const&) { throw Miro::Exception("DdsSupport::init() - Duplicate name for subscriber at repo."); } DDS_SubscriberQos_finalize(&qos); } } //------------------------------------------------- // create all topics in configuration file // (default is 0) { TopicVector::const_iterator first, last = m_params.topics.end(); for (first = m_params.topics.begin(); first != last; ++first) { DDS::DomainParticipant * participant = NULL; try { participant = partRepo->get(first->participant); } catch (Miro::Exception const&) { ostr << "Topic specified unknown participant: " << first->participant; throw Miro::Exception(ostr.str()); } char const * profile = first->profile.empty() ? NULL : first->profile.c_str(); char const * library = first->library.empty() ? NULL : first->library.c_str(); DDS::Topic * topic = (profile || (!m_params.defaultLibrary.empty() && !m_params.defaultProfile.empty())) ? participant->create_topic_with_profile(first->name.c_str(), first->typeName.c_str(), library, profile, NULL /* listener */, DDS::STATUS_MASK_NONE) : participant->create_topic(first->name.c_str(), first->typeName.c_str(), DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, DDS::STATUS_MASK_NONE); if (topic == NULL) { throw Miro::Exception("Failed to create topic."); } } } }